public void ConvexSweepTest(ConvexShape castShape, ref Matrix convexFromWorld, ref Matrix convexToWorld, ConvexResultCallback resultCallback, float allowedCcdPenetration)
{
Matrix convexFromTrans = convexFromWorld;
Matrix convexToTrans = convexToWorld;
Vector3 castShapeAabbMin = Vector3.Zero, castShapeAabbMax = Vector3.Zero;
/* Compute AABB that encompasses angular movement */
Vector3 linVel = Vector3.Zero, angVel = Vector3.Zero;
TransformUtil.CalculateVelocity (ref convexFromTrans, ref convexToTrans, 1.0f, ref linVel, ref angVel);
Matrix R = MathUtil.BasisMatrix(ref convexFromTrans);
castShape.CalculateTemporalAabb (ref R, ref linVel, ref angVel, 1.0f, ref castShapeAabbMin, ref castShapeAabbMax);
/// 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<m_overlappingObjects.Count;i++)
{
CollisionObject collisionObject = m_overlappingObjects[i];
//only perform raycast if filterMask matches
if(resultCallback.NeedsCollision(collisionObject.GetBroadphaseHandle()))
{
//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
Vector3 collisionObjectAabbMin = Vector3.Zero, collisionObjectAabbMax = Vector3.Zero;
Matrix t = collisionObject.GetWorldTransform();
collisionObject.GetCollisionShape().GetAabb(ref t,ref collisionObjectAabbMin,ref collisionObjectAabbMax);
AabbUtil2.AabbExpand (ref collisionObjectAabbMin, ref collisionObjectAabbMax, ref castShapeAabbMin, ref castShapeAabbMax);
float hitLambda = 1f; //could use resultCallback.m_closestHitFraction, but needs testing
Vector3 hitNormal = Vector3.Zero;
if (AabbUtil2.RayAabb(convexFromWorld.Translation, convexToWorld.Translation, collisionObjectAabbMin, collisionObjectAabbMax, hitLambda, hitNormal))
{
Matrix wt = collisionObject.GetWorldTransform();
CollisionWorld.ObjectQuerySingle(castShape, ref convexFromTrans,ref convexToTrans,
collisionObject,
collisionObject.GetCollisionShape(),
ref wt,
resultCallback,
allowedCcdPenetration);
}
}
}
}
public virtual void ConvexSweepTest (ConvexShape castShape, ref Matrix convexFromWorld, ref Matrix convexToWorld, ConvexResultCallback resultCallback, float allowedCcdPenetration)
{
//BT_PROFILE("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
Matrix convexFromTrans = new Matrix();
Matrix convexToTrans = new Matrix();
convexFromTrans = convexFromWorld;
convexToTrans = convexToWorld;
Vector3 castShapeAabbMin = new Vector3();
Vector3 castShapeAabbMax = new Vector3();
/* Compute AABB that encompasses angular movement */
{
Vector3 linVel = new Vector3();
Vector3 angVel = new Vector3();
TransformUtil.CalculateVelocity (ref convexFromTrans, ref convexToTrans, 1.0f, ref linVel, ref angVel);
Vector3 zeroLinVel = new Vector3();
Matrix R = MathUtil.BasisMatrix(ref convexFromTrans);
castShape.CalculateTemporalAabb (ref R, ref zeroLinVel, ref angVel, 1.0f, ref castShapeAabbMin, ref castShapeAabbMax);
}
#if !USE_BRUTEFORCE_RAYBROADPHASE
SingleSweepCallback convexCB = new SingleSweepCallback(castShape,ref convexFromWorld,ref convexToWorld,this,resultCallback,allowedCcdPenetration);
Vector3 tempFrom = convexFromTrans.Translation;
Vector3 tempTo = convexToTrans.Translation;
m_broadphasePairCache.RayTest(ref tempFrom,ref tempTo,convexCB,ref castShapeAabbMin,ref castShapeAabbMax);
convexCB.Cleanup();
#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++)
{
CollisionObject collisionObject= m_collisionObjects[i];
//only perform raycast if filterMask matches
if(resultCallback.NeedsCollision(collisionObject.GetBroadphaseHandle()))
{
//RigidcollisionObject* collisionObject = ctrl.GetRigidcollisionObject();
Vector3 collisionObjectAabbMin = new Vector3();
Vector3 collisionObjectAabbMax = new Vector3();
collisionObject.GetCollisionShape().GetAabb(collisionObject.GetWorldTransform(),ref collisionObjectAabbMin,ref collisionObjectAabbMax);
AabbUtil2.AabbExpand(ref collisionObjectAabbMin, ref collisionObjectAabbMax, ref castShapeAabbMin, ref castShapeAabbMax);
float hitLambda = 1f; //could use resultCallback.m_closestHitFraction, but needs testing
Vector3 hitNormal = new Vector3();
Vector3 fromOrigin = convexFromWorld.Translation;
Vector3 toOrigin = convexToWorld.Translation;
if (AabbUtil2.RayAabb(ref fromOrigin, ref toOrigin, ref collisionObjectAabbMin, ref collisionObjectAabbMax, ref hitLambda, ref hitNormal))
{
Matrix trans = collisionObject.GetWorldTransform();
ObjectQuerySingle(castShape, ref convexFromTrans,ref convexToTrans,
collisionObject,
collisionObject.GetCollisionShape(),
ref trans,
resultCallback,
allowedCcdPenetration);
}
}
}
#endif //USE_BRUTEFORCE_RAYBROADPHASE
}