/**
* 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;
}
}
public void convexSweepTest(
ConvexShape castShape,
Transform convexFromWorld,
Transform convexToWorld,
CollisionWorld.ConvexResultCallback resultCallback,
float allowedCcdPenetration) {
Transform convexFromTrans = new Transform();
Transform convexToTrans = new Transform();
convexFromTrans.set(convexFromWorld);
convexToTrans.set(convexToWorld);
Vector3f castShapeAabbMin = new Vector3f();
Vector3f castShapeAabbMax = new Vector3f();
// compute AABB that encompasses angular movement
{
Vector3f linVel = new Vector3f();
Vector3f angVel = new Vector3f();
TransformUtil.calculateVelocity(convexFromTrans, convexToTrans, 1f, linVel, angVel);
Transform R = new Transform();
R.setIdentity();
R.setRotation(convexFromTrans.getRotation(new Quat4f()));
castShape.calculateTemporalAabb(R, linVel, angVel, 1f, castShapeAabbMin, castShapeAabbMax);
}
Transform tmpTrans = new Transform();
// go over all objects, and if the ray intersects their aabb + cast shape aabb,
// do a ray-shape query using convexCaster (CCD)
for (int i = 0; i < overlappingObjects.size(); i++) {
CollisionObject collisionObject = overlappingObjects.getQuick(i);
// only perform raycast if filterMask matches
if (resultCallback.needsCollision(collisionObject.getBroadphaseHandle())) {
// RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
Vector3f collisionObjectAabbMin = new Vector3f();
Vector3f collisionObjectAabbMax = new Vector3f();
collisionObject
.getCollisionShape()
.getAabb(
collisionObject.getWorldTransform(tmpTrans),
collisionObjectAabbMin,
collisionObjectAabbMax);
AabbUtil2.aabbExpand(
collisionObjectAabbMin, collisionObjectAabbMax, castShapeAabbMin, castShapeAabbMax);
float[] hitLambda =
new float[] {1f}; // could use resultCallback.closestHitFraction, but needs testing
Vector3f hitNormal = new Vector3f();
if (AabbUtil2.rayAabb(
convexFromWorld.origin,
convexToWorld.origin,
collisionObjectAabbMin,
collisionObjectAabbMax,
hitLambda,
hitNormal)) {
CollisionWorld.objectQuerySingle(
castShape,
convexFromTrans,
convexToTrans,
collisionObject,
collisionObject.getCollisionShape(),
collisionObject.getWorldTransform(tmpTrans),
resultCallback,
allowedCcdPenetration);
}
}
}
}
