public BridgeTriangleConvexcastCallback(ConvexShape castShape, ref Matrix from,ref Matrix to,
ConvexResultCallback resultCallback, CollisionObject collisionObject,TriangleMeshShape triangleMesh, ref Matrix triangleToWorld):
base(castShape, ref from,ref to, ref triangleToWorld, triangleMesh.Margin)
{
m_resultCallback = resultCallback;
m_collisionObject = collisionObject;
m_triangleMesh = triangleMesh;
}
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 SingleSweepCallback(ConvexShape castShape, ref Matrix convexFromTrans,ref Matrix 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;
Vector3 unnormalizedRayDir = (m_convexToTrans.Translation-m_convexFromTrans.Translation);
Vector3 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 = Vector3.Dot(rayDir,unnormalizedRayDir);
}
/// objectQuerySingle performs a collision detection query and calls the resultCallback. It is used internally by rayTest.
public static void ObjectQuerySingle(ConvexShape castShape,ref Matrix convexFromTrans,ref Matrix convexToTrans,
CollisionObject collisionObject,CollisionShape collisionShape,
ref Matrix colObjWorldTransform,
ConvexResultCallback resultCallback, float allowedPenetration)
{
if (collisionShape.IsConvex())
{
//BT_PROFILE("convexSweepConvex");
CastResult castResult = new CastResult();
castResult.m_allowedPenetration = allowedPenetration;
castResult.m_fraction = resultCallback.m_closestHitFraction;//float(1.);//??
ConvexShape convexShape = (ConvexShape) collisionShape;
VoronoiSimplexSolver simplexSolver = new VoronoiSimplexSolver();
GjkEpaPenetrationDepthSolver gjkEpaPenetrationSolver = new GjkEpaPenetrationDepthSolver();
ContinuousConvexCollision convexCaster1 = new ContinuousConvexCollision(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,
ref castResult.m_normal,
ref castResult.m_hitPoint,
castResult.m_fraction
);
bool normalInWorldSpace = true;
resultCallback.AddSingleResult(localConvexResult, normalInWorldSpace);
}
}
}
}
else
{
if (collisionShape.IsConcave())
{
if (collisionShape.ShapeType==BroadphaseNativeTypes.TRIANGLE_MESH_SHAPE_PROXYTYPE)
{
//BT_PROFILE("convexSweepbtBvhTriangleMesh");
BvhTriangleMeshShape triangleMesh = (BvhTriangleMeshShape)collisionShape;
Matrix worldTocollisionObject = Matrix.Invert(colObjWorldTransform);
Vector3 convexFromLocal = Vector3.Transform(convexFromTrans.Translation,worldTocollisionObject);
Vector3 convexToLocal = Vector3.Transform(convexToTrans.Translation,worldTocollisionObject);
// rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation
Matrix rotationXform = MathUtil.BasisMatrix(ref worldTocollisionObject) * MathUtil.BasisMatrix(ref convexToTrans);
BridgeTriangleConvexcastCallback tccb = new BridgeTriangleConvexcastCallback(castShape, ref convexFromTrans,ref convexToTrans,resultCallback,collisionObject,triangleMesh, ref colObjWorldTransform);
tccb.m_hitFraction = resultCallback.m_closestHitFraction;
Vector3 boxMinLocal = new Vector3();
Vector3 boxMaxLocal = new Vector3();
castShape.GetAabb(ref rotationXform, ref boxMinLocal, ref boxMaxLocal);
triangleMesh.PerformConvexCast(tccb,ref convexFromLocal,ref convexToLocal,ref boxMinLocal, ref boxMaxLocal);
}
else
{
//BT_PROFILE("convexSweepConcave");
ConcaveShape concaveShape = (ConcaveShape)collisionShape;
Matrix worldTocollisionObject = Matrix.Invert(colObjWorldTransform);
Vector3 convexFromLocal = Vector3.Transform(convexFromTrans.Translation,worldTocollisionObject);
Vector3 convexToLocal = Vector3.Transform(convexToTrans.Translation,worldTocollisionObject);
// rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation
Matrix rotationXform = MathUtil.BasisMatrix(ref worldTocollisionObject) * MathUtil.BasisMatrix(ref convexToTrans);
BridgeTriangleConvexcastCallback2 tccb = new BridgeTriangleConvexcastCallback2(castShape, ref convexFromTrans, ref convexToTrans, resultCallback, collisionObject, concaveShape, ref colObjWorldTransform);
tccb.m_hitFraction = resultCallback.m_closestHitFraction;
Vector3 boxMinLocal = new Vector3();
Vector3 boxMaxLocal = new Vector3();
castShape.GetAabb(ref rotationXform, ref boxMinLocal, ref boxMaxLocal);
Vector3 rayAabbMinLocal = convexFromLocal;
MathUtil.VectorMin(ref convexToLocal,ref rayAabbMinLocal);
//rayAabbMinLocal.setMin(convexToLocal);
Vector3 rayAabbMaxLocal = convexFromLocal;
//rayAabbMaxLocal.setMax(convexToLocal);
MathUtil.VectorMax(ref convexToLocal,ref rayAabbMaxLocal);
rayAabbMinLocal += boxMinLocal;
rayAabbMaxLocal += boxMaxLocal;
concaveShape.ProcessAllTriangles(tccb,ref rayAabbMinLocal,ref rayAabbMaxLocal);
}
}
else
{
///@todo : use AABB tree or other BVH acceleration structure!
if (collisionShape.IsCompound())
{
//BT_PROFILE("convexSweepCompound");
CompoundShape compoundShape = (CompoundShape)collisionShape;
for (int i=0;i<compoundShape.GetNumChildShapes();i++)
{
Matrix childTrans = compoundShape.GetChildTransform(i);
CollisionShape childCollisionShape = compoundShape.GetChildShape(i);
Matrix childWorldTrans = MathUtil.BulletMatrixMultiply(colObjWorldTransform,childTrans);
// replace collision shape so that callback can determine the triangle
CollisionShape saveCollisionShape = collisionObject.GetCollisionShape();
collisionObject.InternalSetTemporaryCollisionShape((CollisionShape)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);
}
}
}
}
}
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
}
// 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, Matrix convexFromWorld, Matrix convexToWorld, ConvexResultCallback resultCallback, float allowedCcdPenetration)
{
ConvexSweepTest(castShape, ref convexFromWorld, ref convexToWorld, resultCallback, allowedCcdPenetration);
}
