public void ConvexSweepTest(ConvexShape castShape, ref IndexedMatrix convexFromWorld, ref IndexedMatrix convexToWorld, ConvexResultCallback resultCallback, float allowedCcdPenetration)
{
IndexedMatrix convexFromTrans = convexFromWorld;
IndexedMatrix convexToTrans = convexToWorld;
IndexedVector3 castShapeAabbMin;
IndexedVector3 castShapeAabbMax;
/* Compute AABB that encompasses angular movement */
IndexedVector3 linVel, angVel;
TransformUtil.CalculateVelocity(ref convexFromTrans, ref convexToTrans, 1.0f, out linVel, out angVel);
// FIXME MAN check this - should be a get/set rotation call, basis copy like this may break with scale?
IndexedMatrix R = IndexedMatrix.Identity;
R.SetRotation(convexFromTrans.GetRotation());
castShape.CalculateTemporalAabb(ref R, ref linVel, ref angVel, 1.0f, out castShapeAabbMin, out 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();
IndexedVector3 collisionObjectAabbMin;
IndexedVector3 collisionObjectAabbMax;
IndexedMatrix t = collisionObject.GetWorldTransform();
collisionObject.GetCollisionShape().GetAabb(ref t, out collisionObjectAabbMin, out collisionObjectAabbMax);
AabbUtil2.AabbExpand(ref collisionObjectAabbMin, ref collisionObjectAabbMax, ref castShapeAabbMin, ref castShapeAabbMax);
float hitLambda = 1f; //could use resultCallback.m_closestHitFraction, but needs testing
IndexedVector3 hitNormal;
if (AabbUtil2.RayAabb(convexFromWorld._origin, convexToWorld._origin, ref collisionObjectAabbMin, ref collisionObjectAabbMax, ref hitLambda, out hitNormal))
{
IndexedMatrix wt = collisionObject.GetWorldTransform();
CollisionWorld.ObjectQuerySingle(castShape, ref convexFromTrans, ref convexToTrans,
collisionObject,
collisionObject.GetCollisionShape(),
ref wt,
resultCallback,
allowedCcdPenetration);
}
}
}
}
public virtual void ConvexSweepTest(ConvexShape castShape, ref IndexedMatrix convexFromWorld, ref IndexedMatrix convexToWorld, ConvexResultCallback resultCallback, float allowedCcdPenetration)
{
BulletGlobals.StartProfile("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
IndexedMatrix convexFromTrans;
IndexedMatrix convexToTrans;
convexFromTrans = convexFromWorld;
convexToTrans = convexToWorld;
IndexedVector3 castShapeAabbMin;
IndexedVector3 castShapeAabbMax;
/* Compute AABB that encompasses angular movement */
{
IndexedVector3 linVel;
IndexedVector3 angVel;
TransformUtil.CalculateVelocity(ref convexFromTrans, ref convexToTrans, 1.0f, out linVel, out angVel);
IndexedVector3 zeroLinVel = new IndexedVector3();
IndexedMatrix R = IndexedMatrix.Identity;
R.SetRotation(convexFromTrans.GetRotation());
castShape.CalculateTemporalAabb(ref R, ref zeroLinVel, ref angVel, 1.0f, out castShapeAabbMin, out castShapeAabbMax);
}
#if !USE_BRUTEFORCE_RAYBROADPHASE
SingleSweepCallback convexCB = BulletGlobals.SingleSweepCallbackPool.Get();
convexCB.Initialize(castShape, ref convexFromWorld, ref convexToWorld, this, resultCallback, allowedCcdPenetration);
IndexedVector3 tempFrom = convexFromTrans._origin;
IndexedVector3 tempTo = convexToTrans._origin;
m_broadphasePairCache.RayTest(ref tempFrom, ref tempTo, convexCB, ref castShapeAabbMin, ref castShapeAabbMax);
convexCB.Cleanup();
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++)
{
CollisionObject collisionObject= m_collisionObjects[i];
//only perform raycast if filterMask matches
if(resultCallback.NeedsCollision(collisionObject.GetBroadphaseHandle()))
{
//RigidcollisionObject* collisionObject = ctrl.GetRigidcollisionObject();
IndexedVector3 collisionObjectAabbMin = new IndexedVector3();
IndexedVector3 collisionObjectAabbMax = new IndexedVector3();
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
IndexedVector3 hitNormal = new IndexedVector3();
IndexedVector3 fromOrigin = convexFromWorld._origin;
IndexedVector3 toOrigin = convexToWorld._origin;
if (AabbUtil2.RayAabb(ref fromOrigin, ref toOrigin, ref collisionObjectAabbMin, ref collisionObjectAabbMax, ref hitLambda, ref hitNormal))
{
IndexedMatrix trans = collisionObject.GetWorldTransform();
ObjectQuerySingle(castShape, ref convexFromTrans,ref convexToTrans,
collisionObject,
collisionObject.GetCollisionShape(),
ref trans,
resultCallback,
allowedCcdPenetration);
}
}
}
#endif //USE_BRUTEFORCE_RAYBROADPHASE
BulletGlobals.StopProfile();
}
// 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.
public virtual void ConvexSweepTest(ConvexShape castShape, IndexedMatrix convexFromWorld, IndexedMatrix convexToWorld, ConvexResultCallback resultCallback, float allowedCcdPenetration)
{
ConvexSweepTest(castShape, ref convexFromWorld, ref convexToWorld, resultCallback, allowedCcdPenetration);
}
public LocalInfoAdder(int i, ConvexResultCallback user)
{
m_userCallback = user;
m_i = i;
m_closestHitFraction = user.m_closestHitFraction;
}
public void Initialize(ConvexShape castShape, ref IndexedMatrix convexFromTrans, ref IndexedMatrix convexToTrans, CollisionWorld world, ConvexResultCallback resultCallback, float allowedPenetration)
{
m_convexFromTrans = convexFromTrans;
m_convexToTrans = convexToTrans;
m_world = world;
m_resultCallback = resultCallback;
m_allowedCcdPenetration = allowedPenetration;
m_castShape = castShape;
IndexedVector3 unnormalizedRayDir = (m_convexToTrans._origin - m_convexFromTrans._origin);
IndexedVector3 rayDir = unnormalizedRayDir;
rayDir.Normalize();
///what about division by zero? -. just set rayDirection[i] to INF/1e30
m_rayDirectionInverse.X = MathUtil.CompareFloat(rayDir.X, 0.0f) ? float.MaxValue : 1f / rayDir.X;
m_rayDirectionInverse.Y = MathUtil.CompareFloat(rayDir.Y, 0.0f) ? float.MaxValue : 1f / rayDir.Y;
m_rayDirectionInverse.Z = MathUtil.CompareFloat(rayDir.Z, 0.0f) ? float.MaxValue : 1f / rayDir.Z;
m_signs[0] = m_rayDirectionInverse.X < 0.0;
m_signs[1] = m_rayDirectionInverse.Y < 0.0;
m_signs[2] = m_rayDirectionInverse.Z < 0.0;
m_lambda_max = rayDir.Dot(ref unnormalizedRayDir);
}
public SingleSweepCallback() { } // for pool
public SingleSweepCallback(ConvexShape castShape, ref IndexedMatrix convexFromTrans, ref IndexedMatrix convexToTrans, CollisionWorld world, ConvexResultCallback resultCallback, float allowedPenetration)
{
Initialize(castShape, ref convexFromTrans, ref convexToTrans, world, resultCallback, allowedPenetration);
}
public void Initialize(ConvexShape castShape, ref IndexedMatrix from, ref IndexedMatrix to,
ConvexResultCallback resultCallback, CollisionObject collisionObject, ConcaveShape triangleMesh, ref IndexedMatrix triangleToWorld)
{
base.Initialize(castShape, ref from, ref to, ref triangleToWorld, triangleMesh.GetMargin());
m_resultCallback = resultCallback;
m_collisionObject = collisionObject;
m_triangleMesh = triangleMesh;
}
public BridgeTriangleConvexcastCallback() { } // for pool
public BridgeTriangleConvexcastCallback(ConvexShape castShape, ref IndexedMatrix from, ref IndexedMatrix to,
ConvexResultCallback resultCallback, CollisionObject collisionObject, ConcaveShape triangleMesh, ref IndexedMatrix triangleToWorld) :
base(castShape, ref from, ref to, ref triangleToWorld, triangleMesh.GetMargin())
{
m_resultCallback = resultCallback;
m_collisionObject = collisionObject;
m_triangleMesh = triangleMesh;
}
/// objectQuerySingle performs a collision detection query and calls the resultCallback. It is used internally by rayTest.
public static void ObjectQuerySingle(ConvexShape castShape, ref IndexedMatrix convexFromTrans, ref IndexedMatrix convexToTrans,
CollisionObject collisionObject, CollisionShape collisionShape,
ref IndexedMatrix colObjWorldTransform,
ConvexResultCallback resultCallback, float allowedPenetration)
{
if (collisionShape.IsConvex())
{
BulletGlobals.StartProfile("convexSweepConvex");
CastResult castResult = BulletGlobals.CastResultPool.Get();
castResult.m_allowedPenetration = allowedPenetration;
castResult.m_fraction = resultCallback.m_closestHitFraction;//float(1.);//??
ConvexShape convexShape = collisionShape as ConvexShape;
VoronoiSimplexSolver simplexSolver = BulletGlobals.VoronoiSimplexSolverPool.Get();
GjkEpaPenetrationDepthSolver gjkEpaPenetrationSolver = BulletGlobals.GjkEpaPenetrationDepthSolverPool.Get();
ContinuousConvexCollision convexCaster1 = BulletGlobals.ContinuousConvexCollisionPool.Get();
convexCaster1.Initialize(castShape, convexShape, simplexSolver, gjkEpaPenetrationSolver);
//btGjkConvexCast convexCaster2(castShape,convexShape,&simplexSolver);
//btSubsimplexConvexCast convexCaster3(castShape,convexShape,&simplexSolver);
IConvexCast castPtr = convexCaster1;
if (castPtr.CalcTimeOfImpact(ref convexFromTrans, ref convexToTrans, ref colObjWorldTransform, ref colObjWorldTransform, castResult))
{
//add hit
if (castResult.m_normal.LengthSquared() > 0.0001f)
{
if (castResult.m_fraction < resultCallback.m_closestHitFraction)
{
castResult.m_normal.Normalize();
LocalConvexResult localConvexResult = new LocalConvexResult
(
collisionObject,
//null, // updated to allow different ctor on struct
ref castResult.m_normal,
ref castResult.m_hitPoint,
castResult.m_fraction
);
bool normalInWorldSpace = true;
resultCallback.AddSingleResult(ref localConvexResult, normalInWorldSpace);
}
}
}
BulletGlobals.ContinuousConvexCollisionPool.Free(convexCaster1);
BulletGlobals.GjkEpaPenetrationDepthSolverPool.Free(gjkEpaPenetrationSolver);
BulletGlobals.VoronoiSimplexSolverPool.Free(simplexSolver);
castResult.Cleanup();
BulletGlobals.StopProfile();
}
else
{
if (collisionShape.IsConcave())
{
if (collisionShape.GetShapeType() == BroadphaseNativeTypes.TRIANGLE_MESH_SHAPE_PROXYTYPE)
{
BulletGlobals.StartProfile("convexSweepbtBvhTriangleMesh");
BvhTriangleMeshShape triangleMesh = (BvhTriangleMeshShape)collisionShape;
IndexedMatrix worldTocollisionObject = colObjWorldTransform.Inverse();
IndexedVector3 convexFromLocal = worldTocollisionObject * convexFromTrans._origin;
IndexedVector3 convexToLocal = worldTocollisionObject * convexToTrans._origin;
// rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation
IndexedMatrix rotationXform = new IndexedMatrix(worldTocollisionObject._basis * convexToTrans._basis,new IndexedVector3(0));
using (BridgeTriangleConvexcastCallback tccb = BulletGlobals.BridgeTriangleConvexcastCallbackPool.Get())
{
tccb.Initialize(castShape, ref convexFromTrans, ref convexToTrans, resultCallback, collisionObject, triangleMesh, ref colObjWorldTransform);
tccb.m_hitFraction = resultCallback.m_closestHitFraction;
tccb.m_allowedPenetration = allowedPenetration;
IndexedVector3 boxMinLocal;
IndexedVector3 boxMaxLocal;
castShape.GetAabb(ref rotationXform, out boxMinLocal, out boxMaxLocal);
triangleMesh.PerformConvexCast(tccb, ref convexFromLocal, ref convexToLocal, ref boxMinLocal, ref boxMaxLocal);
}
BulletGlobals.StopProfile();
}
else
{
if (collisionShape.GetShapeType() == BroadphaseNativeTypes.STATIC_PLANE_PROXYTYPE)
{
CastResult castResult = BulletGlobals.CastResultPool.Get();
castResult.m_allowedPenetration = allowedPenetration;
castResult.m_fraction = resultCallback.m_closestHitFraction;
StaticPlaneShape planeShape = collisionShape as StaticPlaneShape;
ContinuousConvexCollision convexCaster1 = new ContinuousConvexCollision(castShape, planeShape);
if (convexCaster1.CalcTimeOfImpact(ref convexFromTrans, ref convexToTrans, ref colObjWorldTransform, ref colObjWorldTransform, castResult))
{
//add hit
if (castResult.m_normal.LengthSquared() > 0.0001f)
{
if (castResult.m_fraction < resultCallback.m_closestHitFraction)
{
castResult.m_normal.Normalize();
LocalConvexResult localConvexResult = new LocalConvexResult
(
collisionObject,
//null, // updated to allow different ctor on struct
ref castResult.m_normal,
ref castResult.m_hitPoint,
castResult.m_fraction
);
bool normalInWorldSpace = true;
resultCallback.AddSingleResult(ref localConvexResult, normalInWorldSpace);
}
}
}
castResult.Cleanup();
}
else
{
BulletGlobals.StartProfile("convexSweepConcave");
ConcaveShape concaveShape = (ConcaveShape)collisionShape;
IndexedMatrix worldTocollisionObject = colObjWorldTransform.Inverse();
IndexedVector3 convexFromLocal = worldTocollisionObject * convexFromTrans._origin;
IndexedVector3 convexToLocal = worldTocollisionObject * convexToTrans._origin;
// rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation
IndexedMatrix rotationXform = new IndexedMatrix(worldTocollisionObject._basis * convexToTrans._basis, new IndexedVector3(0));
using (BridgeTriangleConvexcastCallback tccb = BulletGlobals.BridgeTriangleConvexcastCallbackPool.Get())
{
tccb.Initialize(castShape, ref convexFromTrans, ref convexToTrans, resultCallback, collisionObject, concaveShape, ref colObjWorldTransform);
tccb.m_hitFraction = resultCallback.m_closestHitFraction;
tccb.m_allowedPenetration = allowedPenetration;
IndexedVector3 boxMinLocal;
IndexedVector3 boxMaxLocal;
castShape.GetAabb(ref rotationXform, out boxMinLocal, out boxMaxLocal);
IndexedVector3 rayAabbMinLocal = convexFromLocal;
MathUtil.VectorMin(ref convexToLocal, ref rayAabbMinLocal);
//rayAabbMinLocal.setMin(convexToLocal);
IndexedVector3 rayAabbMaxLocal = convexFromLocal;
//rayAabbMaxLocal.setMax(convexToLocal);
MathUtil.VectorMax(ref convexToLocal, ref rayAabbMaxLocal);
rayAabbMinLocal += boxMinLocal;
rayAabbMaxLocal += boxMaxLocal;
concaveShape.ProcessAllTriangles(tccb, ref rayAabbMinLocal, ref rayAabbMaxLocal);
BulletGlobals.StopProfile();
}
}
}
}
else
{
///@todo : use AABB tree or other BVH acceleration structure!
if (collisionShape.IsCompound())
{
BulletGlobals.StartProfile("convexSweepCompound");
CompoundShape compoundShape = (CompoundShape)collisionShape;
for (int i = 0; i < compoundShape.GetNumChildShapes(); i++)
{
IndexedMatrix childTrans = compoundShape.GetChildTransform(i);
CollisionShape childCollisionShape = compoundShape.GetChildShape(i);
IndexedMatrix childWorldTrans = colObjWorldTransform * childTrans;
// replace collision shape so that callback can determine the triangle
CollisionShape saveCollisionShape = collisionObject.GetCollisionShape();
collisionObject.InternalSetTemporaryCollisionShape(childCollisionShape);
LocalInfoAdder my_cb = new LocalInfoAdder(i, resultCallback);
my_cb.m_closestHitFraction = resultCallback.m_closestHitFraction;
ObjectQuerySingle(castShape, ref convexFromTrans, ref convexToTrans,
collisionObject,
childCollisionShape,
ref childWorldTrans,
my_cb, allowedPenetration);
// restore
collisionObject.InternalSetTemporaryCollisionShape(saveCollisionShape);
}
BulletGlobals.StopProfile();
}
}
}
}