/// convexTest performs a swept convex cast on all objects in the btCollisionWorld, and calls the resultCallback
/// This allows for several queries: first hit, all hits, any hit, dependent on the value return by the callback.
internal void convexSweepTest( btConvexShape castShape, ref btTransform convexFromWorld, ref btTransform convexToWorld, ConvexResultCallback resultCallback, double allowedCcdPenetration = btScalar.BT_ZERO )
{
CProfileSample sample = new CProfileSample( "convexSweepTest" );
/// use the broadphase to accelerate the search for objects, based on their aabb
/// and for each object with ray-aabb overlap, perform an exact ray test
/// unfortunately the implementation for rayTest and convexSweepTest duplicated, albeit practically identical
btTransform convexFromTrans, convexToTrans;
convexFromTrans = convexFromWorld;
convexToTrans = convexToWorld;
btVector3 castShapeAabbMin, castShapeAabbMax;
/* Compute AABB that encompasses angular movement */
{
btVector3 linVel, angVel;
btTransformUtil.calculateVelocity( ref convexFromWorld, ref convexToWorld, 1.0f, out linVel, out angVel );
btVector3 zeroLinVel = btVector3.Zero;
btTransform R = btTransform.Identity;
R.m_basis = convexFromWorld.m_basis;
castShape.calculateTemporalAabb( ref R, ref zeroLinVel, ref angVel, 1.0f, out castShapeAabbMin, out castShapeAabbMax );
}
#if !USE_BRUTEFORCE_RAYBROADPHASE
btSingleSweepCallback convexCB = BulletGlobals.SingleSweepCallbackPool.Get();
convexCB.Initialize( castShape, ref convexFromWorld, ref convexToWorld, this, resultCallback, allowedCcdPenetration );
m_broadphasePairCache.rayTest( ref convexFromTrans.m_origin, ref convexToTrans.m_origin, convexCB, ref castShapeAabbMin, ref castShapeAabbMax );
BulletGlobals.SingleSweepCallbackPool.Free( convexCB );
#else
/// go over all objects, and if the ray intersects their aabb + cast shape aabb,
// do a ray-shape query using convexCaster (CCD)
int i;
for( i = 0; i < m_collisionObjects.Count; i++ )
{
btCollisionObject collisionObject = m_collisionObjects[i];
//only perform raycast if filterMask matches
if( resultCallback.needsCollision( collisionObject.getBroadphaseHandle() ) )
{
//RigidcollisionObject collisionObject = ctrl.GetRigidcollisionObject();
btVector3 collisionObjectAabbMin, collisionObjectAabbMax;
collisionObject.getCollisionShape().getAabb( collisionObject.getWorldTransform(), collisionObjectAabbMin, collisionObjectAabbMax );
AabbExpand( collisionObjectAabbMin, collisionObjectAabbMax, castShapeAabbMin, castShapeAabbMax );
double hitLambda = (double)( 1.0 ); //could use resultCallback.m_closestHitFraction, but needs testing
btVector3 hitNormal;
if( btRayAabb( convexFromWorld.getOrigin(), convexToWorld.getOrigin(), collisionObjectAabbMin, collisionObjectAabbMax, hitLambda, hitNormal ) )
{
objectQuerySingle( castShape, convexFromTrans, convexToTrans,
collisionObject,
collisionObject.getCollisionShape(),
collisionObject.getWorldTransform(),
resultCallback,
allowedCcdPenetration );
}
}
}
#endif //USE_BRUTEFORCE_RAYBROADPHASE
}
static public double capsuleCapsuleDistance(
out btVector3 normalOnB,
out btVector3 pointOnB,
double capsuleLengthA,
double capsuleRadiusA,
double capsuleLengthB,
double capsuleRadiusB,
int capsuleAxisA,
int capsuleAxisB,
ref btTransform transformA,
ref btTransform transformB,
double distanceThreshold )
{
btVector3 directionA; transformA.m_basis.getColumn( capsuleAxisA, out directionA );
btVector3 translationA; transformA.getOrigin( out translationA );
btVector3 directionB; transformB.m_basis.getColumn( capsuleAxisB, out directionB );
btVector3 translationB; transformB.getOrigin( out translationB );
// translation between centers
btVector3 translation; translationB.Sub( ref translationA, out translation );
// compute the closest points of the capsule line segments
btVector3 ptsVector; // the vector between the closest points
btVector3 offsetA, offsetB; // offsets from segment centers to their closest points
double tA, tB; // parameters on line segment
segmentsClosestPoints( out ptsVector, out offsetA, out offsetB, out tA, out tB, ref translation,
ref directionA, capsuleLengthA, ref directionB, capsuleLengthB );
double distance = ptsVector.length() - capsuleRadiusA - capsuleRadiusB;
if( distance > distanceThreshold )
{
pointOnB = btVector3.Zero;
normalOnB = btVector3.xAxis;
return distance;
}
double lenSqr = ptsVector.length2();
if( lenSqr <= ( btScalar.SIMD_EPSILON * btScalar.SIMD_EPSILON ) )
{
//degenerate case where 2 capsules are likely at the same location: take a vector tangential to 'directionA'
btVector3 q;
btVector3.btPlaneSpace1( ref directionA, out normalOnB, out q );
}
else
{
// compute the contact normal
ptsVector.Mult( -btScalar.btRecipSqrt( lenSqr ), out normalOnB );
}
btVector3 tmp;
btVector3 tmp2;
translationB.Add( ref offsetB, out tmp );
normalOnB.Mult( capsuleRadiusB, out tmp2 );
tmp.Add( ref tmp2, out pointOnB );
//pointOnB = transformB.getOrigin() + offsetB + normalOnB * capsuleRadiusB;
return distance;
}
public static void rayTestSingleInternal( ref btTransform rayFromTrans, ref btTransform rayToTrans,
btCollisionShape collisionShape,
btCollisionObject collisionObject,
ref btTransform useTransform,
RayResultCallback resultCallback )
{
btSphereShape pointShape = BulletGlobals.SphereShapePool.Get();
pointShape.Initialize( btScalar.BT_ZERO );
pointShape.setMargin( 0f );
btConvexShape castShape = pointShape;
//btCollisionShape collisionShape = collisionObjectWrap.getCollisionShape();
btTransform colObjWorldTransform = useTransform;// collisionObjectWrap.m_worldTransform;
if( collisionShape.isConvex() )
{
// CProfileSample sample = new CProfileSample("rayTestConvex");
btConvexCast.CastResult castResult = new btConvexCast.CastResult();
castResult.m_fraction = resultCallback.m_closestHitFraction;
btConvexShape convexShape = (btConvexShape)collisionShape;
btVoronoiSimplexSolver simplexSolver = BulletGlobals.VoronoiSimplexSolverPool.Get();
//new btVoronoiSimplexSolver();
btSubsimplexConvexCast subSimplexConvexCaster = BulletGlobals.SubSimplexConvexCastPool.Get();
subSimplexConvexCaster.Initialize( castShape, convexShape, simplexSolver );
btGjkConvexCast gjkConvexCaster = BulletGlobals.GjkConvexCastPool.Get();
gjkConvexCaster.Initialize( castShape, convexShape, simplexSolver );
BulletGlobals.SubSimplexConvexCastPool.Free( subSimplexConvexCaster );
BulletGlobals.VoronoiSimplexSolverPool.Free( simplexSolver );
//btContinuousConvexCollision convexCaster(castShape,convexShape,&simplexSolver,0);
btConvexCast convexCasterPtr = null;
//use kF_UseSubSimplexConvexCastRaytest by default
if( ( resultCallback.m_flags & (uint)btTriangleRaycastCallback.EFlags.kF_UseGjkConvexCastRaytest ) != 0 )
convexCasterPtr = gjkConvexCaster;
else
convexCasterPtr = subSimplexConvexCaster;
btConvexCast convexCaster = convexCasterPtr;
if( convexCaster.calcTimeOfImpact( ref rayFromTrans, ref rayToTrans
, ref useTransform/*collisionObjectWrap.m_worldTransform*/
, ref useTransform/*collisionObjectWrap.m_worldTransform*/
, castResult ) )
{
//add hit
if( castResult.m_normal.length2() > (double)( 0.0001 ) )
{
if( castResult.m_fraction < resultCallback.m_closestHitFraction )
{
//todo: figure out what this is about. When is rayFromTest.getBasis() not identity?
#if USE_SUBSIMPLEX_CONVEX_CAST
//rotate normal into worldspace
castResult.m_normal = rayFromTrans.getBasis() castResult.m_normal;
#endif //USE_SUBSIMPLEX_CONVEX_CAST
castResult.m_normal.normalize();
LocalRayResult localRayResult = new LocalRayResult
(
collisionObject,
null,
castResult.m_normal,
castResult.m_fraction
);
bool normalInWorldSpace = true;
resultCallback.addSingleResult( localRayResult, normalInWorldSpace );
}
}
}
BulletGlobals.GjkConvexCastPool.Free( gjkConvexCaster );
}
else
{
if( collisionShape.isConcave() )
{
btTransform worldTocollisionObject; colObjWorldTransform.inverse( out worldTocollisionObject );
btVector3 tmp;
rayFromTrans.getOrigin( out tmp );
btVector3 rayFromLocal; worldTocollisionObject.Apply( ref tmp, out rayFromLocal );
rayToTrans.getOrigin( out tmp );
btVector3 rayToLocal; worldTocollisionObject.Apply( ref tmp, out rayToLocal );
// CProfileSample sample = new CProfileSample("rayTestConcave");
#if SUPPORT_TRIANGLE_MESH
if( collisionShape.getShapeType() == BroadphaseNativeTypes.TRIANGLE_MESH_SHAPE_PROXYTYPE )
{
///optimized version for btBvhTriangleMeshShape
btBvhTriangleMeshShape triangleMesh = (btBvhTriangleMeshShape)collisionShape;
BridgeTriangleRaycastCallback rcb( rayFromLocal, rayToLocal,&resultCallback, collisionObjectWrap.getCollisionObject(), triangleMesh, colObjWorldTransform);
rcb.m_hitFraction = resultCallback.m_closestHitFraction;
triangleMesh.performRaycast( &rcb, rayFromLocal, rayToLocal );
}
else
#endif
{
//generic (slower) case
btConcaveShape concaveShape = (btConcaveShape)collisionShape;
//ConvexCast::CastResult
BridgeTriangleRaycastCallback rcb = new BridgeTriangleRaycastCallback( ref rayFromLocal, ref rayToLocal, resultCallback
, collisionObject, concaveShape, ref colObjWorldTransform );
rcb.m_hitFraction = resultCallback.m_closestHitFraction;
btVector3 rayAabbMinLocal = rayFromLocal;
rayAabbMinLocal.setMin( ref rayToLocal );
btVector3 rayAabbMaxLocal = rayFromLocal;
rayAabbMaxLocal.setMax( ref rayToLocal );
concaveShape.processAllTriangles( rcb, ref rayAabbMinLocal, ref rayAabbMaxLocal );
}
}
else
{
// CProfileSample sample = new CProfileSample("rayTestCompound");
if( collisionShape.isCompound() )
{
btCompoundShape compoundShape = (btCompoundShape)( collisionShape );
btDbvt dbvt = compoundShape.getDynamicAabbTree();
RayTester rayCB = new RayTester(
collisionObject,
compoundShape,
ref colObjWorldTransform,
ref rayFromTrans,
ref rayToTrans,
resultCallback );
#if !DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION
if( dbvt != null )
{
btTransform tmp;
colObjWorldTransform.inverseTimes( ref rayFromTrans, out tmp );
btVector3 localRayFrom; tmp.getOrigin( out localRayFrom );
colObjWorldTransform.inverseTimes( ref rayToTrans, out tmp );
btVector3 localRayTo; tmp.getOrigin( out localRayTo );
btDbvt.rayTest( dbvt.m_root, ref localRayFrom, ref localRayTo, rayCB );
}
else
#endif //DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION
{
for( int i = 0, n = compoundShape.getNumChildShapes(); i < n; ++i )
{
rayCB.Process( i );
}
}
}
}
}
BulletGlobals.SphereShapePool.Free( pointShape );
}
