use of com.bulletphysics.collision.narrowphase.GjkEpaPenetrationDepthSolver in project bdx by GoranM.
the class CollisionWorld method objectQuerySingle.
/**
* objectQuerySingle performs a collision detection query and calls the resultCallback. It is used internally by rayTest.
*/
public static void objectQuerySingle(ConvexShape castShape, Transform convexFromTrans, Transform convexToTrans, CollisionObject collisionObject, CollisionShape collisionShape, Transform colObjWorldTransform, ConvexResultCallback resultCallback, float allowedPenetration) {
Stack stack = Stack.enter();
if (collisionShape.isConvex()) {
CastResult castResult = new CastResult();
castResult.allowedPenetration = allowedPenetration;
// ??
castResult.fraction = 1f;
ConvexShape convexShape = (ConvexShape) collisionShape;
VoronoiSimplexSolver simplexSolver = new VoronoiSimplexSolver();
GjkEpaPenetrationDepthSolver gjkEpaPenetrationSolver = new GjkEpaPenetrationDepthSolver();
// JAVA TODO: should be convexCaster1
//ContinuousConvexCollision convexCaster1(castShape,convexShape,&simplexSolver,&gjkEpaPenetrationSolver);
GjkConvexCast convexCaster2 = new GjkConvexCast(castShape, convexShape, simplexSolver);
//btSubsimplexConvexCast convexCaster3(castShape,convexShape,&simplexSolver);
ConvexCast castPtr = convexCaster2;
if (castPtr.calcTimeOfImpact(convexFromTrans, convexToTrans, colObjWorldTransform, colObjWorldTransform, castResult)) {
// add hit
if (castResult.normal.lengthSquared() > 0.0001f) {
if (castResult.fraction < resultCallback.closestHitFraction) {
castResult.normal.normalize();
LocalConvexResult localConvexResult = new LocalConvexResult(collisionObject, null, castResult.normal, castResult.hitPoint, castResult.fraction);
boolean normalInWorldSpace = true;
resultCallback.addSingleResult(localConvexResult, normalInWorldSpace);
}
}
}
} else {
if (collisionShape.isConcave()) {
if (collisionShape.getShapeType() == BroadphaseNativeType.TRIANGLE_MESH_SHAPE_PROXYTYPE) {
BvhTriangleMeshShape triangleMesh = (BvhTriangleMeshShape) collisionShape;
Transform worldTocollisionObject = stack.allocTransform();
worldTocollisionObject.inverse(colObjWorldTransform);
Vector3f convexFromLocal = stack.allocVector3f();
convexFromLocal.set(convexFromTrans.origin);
worldTocollisionObject.transform(convexFromLocal);
Vector3f convexToLocal = stack.allocVector3f();
convexToLocal.set(convexToTrans.origin);
worldTocollisionObject.transform(convexToLocal);
// rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation
Transform rotationXform = stack.allocTransform();
Matrix3f tmpMat = stack.allocMatrix3f();
tmpMat.mul(worldTocollisionObject.basis, convexToTrans.basis);
rotationXform.set(tmpMat);
BridgeTriangleConvexcastCallback tccb = new BridgeTriangleConvexcastCallback(castShape, convexFromTrans, convexToTrans, resultCallback, collisionObject, triangleMesh, colObjWorldTransform);
tccb.hitFraction = resultCallback.closestHitFraction;
tccb.normalInWorldSpace = true;
Vector3f boxMinLocal = stack.allocVector3f();
Vector3f boxMaxLocal = stack.allocVector3f();
castShape.getAabb(rotationXform, boxMinLocal, boxMaxLocal);
triangleMesh.performConvexcast(tccb, convexFromLocal, convexToLocal, boxMinLocal, boxMaxLocal);
} else {
BvhTriangleMeshShape triangleMesh = (BvhTriangleMeshShape) collisionShape;
Transform worldTocollisionObject = stack.allocTransform();
worldTocollisionObject.inverse(colObjWorldTransform);
Vector3f convexFromLocal = stack.allocVector3f();
convexFromLocal.set(convexFromTrans.origin);
worldTocollisionObject.transform(convexFromLocal);
Vector3f convexToLocal = stack.allocVector3f();
convexToLocal.set(convexToTrans.origin);
worldTocollisionObject.transform(convexToLocal);
// rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation
Transform rotationXform = stack.allocTransform();
Matrix3f tmpMat = stack.allocMatrix3f();
tmpMat.mul(worldTocollisionObject.basis, convexToTrans.basis);
rotationXform.set(tmpMat);
BridgeTriangleConvexcastCallback tccb = new BridgeTriangleConvexcastCallback(castShape, convexFromTrans, convexToTrans, resultCallback, collisionObject, triangleMesh, colObjWorldTransform);
tccb.hitFraction = resultCallback.closestHitFraction;
tccb.normalInWorldSpace = false;
Vector3f boxMinLocal = stack.allocVector3f();
Vector3f boxMaxLocal = stack.allocVector3f();
castShape.getAabb(rotationXform, boxMinLocal, boxMaxLocal);
Vector3f rayAabbMinLocal = stack.alloc(convexFromLocal);
VectorUtil.setMin(rayAabbMinLocal, convexToLocal);
Vector3f rayAabbMaxLocal = stack.alloc(convexFromLocal);
VectorUtil.setMax(rayAabbMaxLocal, convexToLocal);
rayAabbMinLocal.add(boxMinLocal);
rayAabbMaxLocal.add(boxMaxLocal);
triangleMesh.processAllTriangles(tccb, rayAabbMinLocal, rayAabbMaxLocal);
}
} else {
// todo: use AABB tree or other BVH acceleration structure!
if (collisionShape.isCompound()) {
CompoundShape compoundShape = (CompoundShape) collisionShape;
for (int i = 0; i < compoundShape.getNumChildShapes(); i++) {
Transform childTrans = compoundShape.getChildTransform(i, stack.allocTransform());
CollisionShape childCollisionShape = compoundShape.getChildShape(i);
Transform childWorldTrans = stack.allocTransform();
childWorldTrans.mul(colObjWorldTransform, childTrans);
// replace collision shape so that callback can determine the triangle
CollisionShape saveCollisionShape = collisionObject.getCollisionShape();
collisionObject.internalSetTemporaryCollisionShape(childCollisionShape);
objectQuerySingle(castShape, convexFromTrans, convexToTrans, collisionObject, childCollisionShape, childWorldTrans, resultCallback, allowedPenetration);
// restore
collisionObject.internalSetTemporaryCollisionShape(saveCollisionShape);
}
}
}
}
stack.leave();
}
Aggregations