/**
   * Processes the given convex shape to retrieve a correctly ordered FloatBuffer to construct the
   * shape from with a TriMesh.
   *
   * @param convexShape the shape to retreieve the vertices from.
   * @return the vertices as a FloatBuffer, ordered as Triangles.
   */
  private static FloatBuffer getVertices(ConvexShape convexShape) {
    // Check there is a hull shape to render
    if (convexShape.getUserPointer() == null) {
      // create a hull approximation
      ShapeHull hull = new ShapeHull(convexShape);
      float margin = convexShape.getMargin();
      hull.buildHull(margin);
      convexShape.setUserPointer(hull);
    }

    // Assert state - should have a pointer to a hull (shape) that'll be drawn
    assert convexShape.getUserPointer() != null
        : "Should have a shape for the userPointer, instead got null";
    ShapeHull hull = (ShapeHull) convexShape.getUserPointer();

    // Assert we actually have a shape to render
    assert hull.numTriangles() > 0 : "Expecting the Hull shape to have triangles";
    int numberOfTriangles = hull.numTriangles();

    // The number of bytes needed is: (floats in a vertex) * (vertices in a triangle) * (# of
    // triangles) * (size of float in bytes)
    final int numberOfFloats = 3 * 3 * numberOfTriangles;
    FloatBuffer vertices = BufferUtils.createFloatBuffer(numberOfFloats);

    // Force the limit, set the cap - most number of floats we will use the buffer for
    vertices.limit(numberOfFloats);

    // Loop variables
    final IntArrayList hullIndicies = hull.getIndexPointer();
    final List<Vector3f> hullVertices = hull.getVertexPointer();
    Vector3f vertexA, vertexB, vertexC;
    int index = 0;

    for (int i = 0; i < numberOfTriangles; i++) {
      // Grab the data for this triangle from the hull
      vertexA = hullVertices.get(hullIndicies.get(index++));
      vertexB = hullVertices.get(hullIndicies.get(index++));
      vertexC = hullVertices.get(hullIndicies.get(index++));

      // Put the verticies into the vertex buffer
      vertices.put(vertexA.x).put(vertexA.y).put(vertexA.z);
      vertices.put(vertexB.x).put(vertexB.y).put(vertexB.z);
      vertices.put(vertexC.x).put(vertexC.y).put(vertexC.z);
    }

    vertices.clear();
    return vertices;
  }
  protected void stepUp(CollisionWorld world) {
    // phase 1: up
    Transform start = new Transform();
    Transform end = new Transform();
    targetPosition.scaleAdd(
        stepHeight + (verticalOffset > 0.0 ? verticalOffset : 0.0f),
        upAxisDirection[upAxis],
        currentPosition);

    start.setIdentity();
    end.setIdentity();

    /* FIXME: Handle penetration properly */
    start.origin.scaleAdd(
        convexShape.getMargin() + addedMargin, upAxisDirection[upAxis], currentPosition);
    end.origin.set(targetPosition);

    // Find only sloped/flat surface hits, avoid wall and ceiling hits...
    Vector3d up = new Vector3d();
    up.scale(-1f, upAxisDirection[upAxis]);
    KinematicClosestNotMeConvexResultCallback callback =
        new KinematicClosestNotMeConvexResultCallback(ghostObject, up, 0.0f);
    callback.collisionFilterGroup = getGhostObject().getBroadphaseHandle().collisionFilterGroup;
    callback.collisionFilterMask = getGhostObject().getBroadphaseHandle().collisionFilterMask;

    if (useGhostObjectSweepTest) {
      ghostObject.convexSweepTest(
          convexShape, start, end, callback, world.getDispatchInfo().allowedCcdPenetration);
    } else {
      world.convexSweepTest(convexShape, start, end, callback);
    }

    if (callback.hasHit()) {
      // we moved up only a fraction of the step height
      currentStepOffset = stepHeight * callback.closestHitFraction;
      currentPosition.interpolate(currentPosition, targetPosition, callback.closestHitFraction);
      verticalVelocity = 0.0f;
      verticalOffset = 0.0f;
    } else {
      currentStepOffset = stepHeight;
      currentPosition.set(targetPosition);
    }
  }
  protected void stepForwardAndStrafe(CollisionWorld collisionWorld, Vector3d walkMove) {
    // printf("m_normalizedDirection=%f,%f,%f\n",
    // 	m_normalizedDirection[0],m_normalizedDirection[1],m_normalizedDirection[2]);
    // phase 2: forward and strafe
    Transform start = new Transform();
    Transform end = new Transform();
    targetPosition.add(currentPosition, walkMove);
    start.setIdentity();
    end.setIdentity();

    double fraction = 1.0f;
    Vector3d distance2Vec = new Vector3d();
    distance2Vec.sub(currentPosition, targetPosition);
    double distance2 = distance2Vec.lengthSquared();
    // printf("distance2=%f\n",distance2);

    /*if (touchingContact) {
    	if (normalizedDirection.dot(touchingNormal) > 0.0f) {
    		updateTargetPositionBasedOnCollision(touchingNormal);
    	}
    }*/

    int maxIter = 10;

    while (fraction > 0.01f && maxIter-- > 0) {
      start.origin.set(currentPosition);
      end.origin.set(targetPosition);

      KinematicClosestNotMeConvexResultCallback callback =
          new KinematicClosestNotMeConvexResultCallback(
              ghostObject, upAxisDirection[upAxis], -1.0f);
      callback.collisionFilterGroup = getGhostObject().getBroadphaseHandle().collisionFilterGroup;
      callback.collisionFilterMask = getGhostObject().getBroadphaseHandle().collisionFilterMask;

      double margin = convexShape.getMargin();
      convexShape.setMargin(margin + addedMargin);

      if (useGhostObjectSweepTest) {
        ghostObject.convexSweepTest(
            convexShape,
            start,
            end,
            callback,
            collisionWorld.getDispatchInfo().allowedCcdPenetration);
      } else {
        collisionWorld.convexSweepTest(convexShape, start, end, callback);
      }

      convexShape.setMargin(margin);

      fraction -= callback.closestHitFraction;

      if (callback.hasHit()) {
        // we moved only a fraction
        Vector3d hitDistanceVec = new Vector3d();
        hitDistanceVec.sub(callback.hitPointWorld, currentPosition);
        // double hitDistance = hitDistanceVec.length();

        // if the distance is farther than the collision margin, move
        // if (hitDistance > addedMargin) {
        //	//printf("callback.m_closestHitFraction=%f\n",callback.m_closestHitFraction);
        //	currentPosition.interpolate(currentPosition, targetPosition,
        // callback.closestHitFraction);
        // }

        updateTargetPositionBasedOnCollision(callback.hitNormalWorld);

        Vector3d currentDir = new Vector3d();
        currentDir.sub(targetPosition, currentPosition);
        distance2 = currentDir.lengthSquared();
        if (distance2 > BulletGlobals.SIMD_EPSILON) {
          currentDir.normalize();
          // see Quake2: "If velocity is against original velocity, stop ead to avoid tiny
          // oscilations in sloping corners."
          if (currentDir.dot(normalizedDirection) <= 0.0f) {
            break;
          }
        } else {
          // printf("currentDir: don't normalize a zero vector\n");
          break;
        }
      } else {
        // we moved whole way
        currentPosition.set(targetPosition);
      }

      // if (callback.m_closestHitFraction == 0.f)
      //    break;
    }
  }