use of com.bulletphysics.collision.narrowphase.VoronoiSimplexSolver in project bdx by GoranM.
the class CollisionWorld method rayTestSingle.
// TODO
public static void rayTestSingle(Transform rayFromTrans, Transform rayToTrans, CollisionObject collisionObject, CollisionShape collisionShape, Transform colObjWorldTransform, RayResultCallback resultCallback) {
Stack stack = Stack.enter();
SphereShape pointShape = new SphereShape(0f);
pointShape.setMargin(0f);
ConvexShape castShape = pointShape;
if (collisionShape.isConvex()) {
CastResult castResult = new CastResult();
castResult.fraction = resultCallback.closestHitFraction;
ConvexShape convexShape = (ConvexShape) collisionShape;
VoronoiSimplexSolver simplexSolver = new VoronoiSimplexSolver();
//#define USE_SUBSIMPLEX_CONVEX_CAST 1
//#ifdef USE_SUBSIMPLEX_CONVEX_CAST
SubsimplexConvexCast convexCaster = new SubsimplexConvexCast(castShape, convexShape, simplexSolver);
if (convexCaster.calcTimeOfImpact(rayFromTrans, rayToTrans, colObjWorldTransform, colObjWorldTransform, castResult)) {
//add hit
if (castResult.normal.lengthSquared() > 0.0001f) {
if (castResult.fraction < resultCallback.closestHitFraction) {
//#ifdef USE_SUBSIMPLEX_CONVEX_CAST
//rotate normal into worldspace
rayFromTrans.basis.transform(castResult.normal);
//#endif //USE_SUBSIMPLEX_CONVEX_CAST
castResult.normal.normalize();
LocalRayResult localRayResult = new LocalRayResult(collisionObject, null, castResult.normal, castResult.fraction);
boolean normalInWorldSpace = true;
resultCallback.addSingleResult(localRayResult, normalInWorldSpace);
}
}
}
} else {
if (collisionShape.isConcave()) {
if (collisionShape.getShapeType() == BroadphaseNativeType.TRIANGLE_MESH_SHAPE_PROXYTYPE) {
// optimized version for BvhTriangleMeshShape
BvhTriangleMeshShape triangleMesh = (BvhTriangleMeshShape) collisionShape;
Transform worldTocollisionObject = stack.allocTransform();
worldTocollisionObject.inverse(colObjWorldTransform);
Vector3f rayFromLocal = stack.alloc(rayFromTrans.origin);
worldTocollisionObject.transform(rayFromLocal);
Vector3f rayToLocal = stack.alloc(rayToTrans.origin);
worldTocollisionObject.transform(rayToLocal);
BridgeTriangleRaycastCallback rcb = new BridgeTriangleRaycastCallback(rayFromLocal, rayToLocal, resultCallback, collisionObject, triangleMesh);
rcb.hitFraction = resultCallback.closestHitFraction;
triangleMesh.performRaycast(rcb, rayFromLocal, rayToLocal);
} else {
ConcaveShape triangleMesh = (ConcaveShape) collisionShape;
Transform worldTocollisionObject = stack.allocTransform();
worldTocollisionObject.inverse(colObjWorldTransform);
Vector3f rayFromLocal = stack.alloc(rayFromTrans.origin);
worldTocollisionObject.transform(rayFromLocal);
Vector3f rayToLocal = stack.alloc(rayToTrans.origin);
worldTocollisionObject.transform(rayToLocal);
BridgeTriangleRaycastCallback rcb = new BridgeTriangleRaycastCallback(rayFromLocal, rayToLocal, resultCallback, collisionObject, triangleMesh);
rcb.hitFraction = resultCallback.closestHitFraction;
Vector3f rayAabbMinLocal = stack.alloc(rayFromLocal);
VectorUtil.setMin(rayAabbMinLocal, rayToLocal);
Vector3f rayAabbMaxLocal = stack.alloc(rayFromLocal);
VectorUtil.setMax(rayAabbMaxLocal, rayToLocal);
triangleMesh.processAllTriangles(rcb, rayAabbMinLocal, rayAabbMaxLocal);
}
} else {
// todo: use AABB tree or other BVH acceleration structure!
if (collisionShape.isCompound()) {
CompoundShape compoundShape = (CompoundShape) collisionShape;
int i = 0;
Transform childTrans = stack.allocTransform();
for (i = 0; i < compoundShape.getNumChildShapes(); i++) {
compoundShape.getChildTransform(i, childTrans);
CollisionShape childCollisionShape = compoundShape.getChildShape(i);
Transform childWorldTrans = stack.alloc(colObjWorldTransform);
childWorldTrans.mul(childTrans);
// replace collision shape so that callback can determine the triangle
CollisionShape saveCollisionShape = collisionObject.getCollisionShape();
collisionObject.internalSetTemporaryCollisionShape(childCollisionShape);
rayTestSingle(rayFromTrans, rayToTrans, collisionObject, childCollisionShape, childWorldTrans, resultCallback);
// restore
collisionObject.internalSetTemporaryCollisionShape(saveCollisionShape);
}
}
}
}
stack.leave();
}
use of com.bulletphysics.collision.narrowphase.VoronoiSimplexSolver 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();
}
use of com.bulletphysics.collision.narrowphase.VoronoiSimplexSolver in project bdx by GoranM.
the class ConvexConvexAlgorithm method calculateTimeOfImpact.
@Override
public float calculateTimeOfImpact(CollisionObject col0, CollisionObject col1, DispatcherInfo dispatchInfo, ManifoldResult resultOut) {
Stack stack = Stack.enter();
Vector3f tmp = stack.allocVector3f();
Transform tmpTrans1 = stack.allocTransform();
Transform tmpTrans2 = stack.allocTransform();
// Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold
// Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold
// col0->m_worldTransform,
float resultFraction = 1f;
tmp.sub(col0.getInterpolationWorldTransform(tmpTrans1).origin, col0.getWorldTransform(tmpTrans2).origin);
float squareMot0 = tmp.lengthSquared();
tmp.sub(col1.getInterpolationWorldTransform(tmpTrans1).origin, col1.getWorldTransform(tmpTrans2).origin);
float squareMot1 = tmp.lengthSquared();
if (squareMot0 < col0.getCcdSquareMotionThreshold() && squareMot1 < col1.getCcdSquareMotionThreshold()) {
return resultFraction;
}
if (disableCcd) {
stack.leave();
return 1f;
}
Transform tmpTrans3 = stack.allocTransform();
Transform tmpTrans4 = stack.allocTransform();
// An adhoc way of testing the Continuous Collision Detection algorithms
// One object is approximated as a sphere, to simplify things
// Starting in penetration should report no time of impact
// For proper CCD, better accuracy and handling of 'allowed' penetration should be added
// also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies)
// Convex0 against sphere for Convex1
{
ConvexShape convex0 = (ConvexShape) col0.getCollisionShape();
// todo: allow non-zero sphere sizes, for better approximation
SphereShape sphere1 = new SphereShape(col1.getCcdSweptSphereRadius());
ConvexCast.CastResult result = new ConvexCast.CastResult();
VoronoiSimplexSolver voronoiSimplex = new VoronoiSimplexSolver();
//SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
///Simplification, one object is simplified as a sphere
GjkConvexCast ccd1 = new GjkConvexCast(convex0, sphere1, voronoiSimplex);
//ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
if (ccd1.calcTimeOfImpact(col0.getWorldTransform(tmpTrans1), col0.getInterpolationWorldTransform(tmpTrans2), col1.getWorldTransform(tmpTrans3), col1.getInterpolationWorldTransform(tmpTrans4), result)) {
if (col0.getHitFraction() > result.fraction) {
col0.setHitFraction(result.fraction);
}
if (col1.getHitFraction() > result.fraction) {
col1.setHitFraction(result.fraction);
}
if (resultFraction > result.fraction) {
resultFraction = result.fraction;
}
}
}
// Sphere (for convex0) against Convex1
{
ConvexShape convex1 = (ConvexShape) col1.getCollisionShape();
// todo: allow non-zero sphere sizes, for better approximation
SphereShape sphere0 = new SphereShape(col0.getCcdSweptSphereRadius());
ConvexCast.CastResult result = new ConvexCast.CastResult();
VoronoiSimplexSolver voronoiSimplex = new VoronoiSimplexSolver();
//SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
///Simplification, one object is simplified as a sphere
GjkConvexCast ccd1 = new GjkConvexCast(sphere0, convex1, voronoiSimplex);
//ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
if (ccd1.calcTimeOfImpact(col0.getWorldTransform(tmpTrans1), col0.getInterpolationWorldTransform(tmpTrans2), col1.getWorldTransform(tmpTrans3), col1.getInterpolationWorldTransform(tmpTrans4), result)) {
if (col0.getHitFraction() > result.fraction) {
col0.setHitFraction(result.fraction);
}
if (col1.getHitFraction() > result.fraction) {
col1.setHitFraction(result.fraction);
}
if (resultFraction > result.fraction) {
resultFraction = result.fraction;
}
}
}
stack.leave();
return resultFraction;
}
Aggregations