public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
CollisionObject convexbody = m_isSwapped ? bodyB : bodyA;
CollisionObject triBody = m_isSwapped ? bodyA : bodyB;
//quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast)
//only perform CCD above a certain threshold, this prevents blocking on the long run
//because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame...
float squareMot0 = (convexbody.GetInterpolationWorldTransform()._origin - convexbody.GetWorldTransform()._origin).LengthSquared();
if (squareMot0 < convexbody.GetCcdSquareMotionThreshold())
{
return(1);
}
//IndexedMatrix triInv = MathHelper.InvertMatrix(triBody.getWorldTransform());
IndexedMatrix triInv = triBody.GetWorldTransform().Inverse();
IndexedMatrix convexFromLocal = triInv * convexbody.GetWorldTransform();
IndexedMatrix convexToLocal = triInv * convexbody.GetInterpolationWorldTransform();
if (triBody.GetCollisionShape().IsConcave())
{
IndexedVector3 rayAabbMin = convexFromLocal._origin;
MathUtil.VectorMin(convexToLocal._origin, ref rayAabbMin);
IndexedVector3 rayAabbMax = convexFromLocal._origin;
MathUtil.VectorMax(convexToLocal._origin, ref rayAabbMax);
IndexedVector3 ccdRadius0 = new IndexedVector3(convexbody.GetCcdSweptSphereRadius());
rayAabbMin -= ccdRadius0;
rayAabbMax += ccdRadius0;
float curHitFraction = 1f; //is this available?
using (LocalTriangleSphereCastCallback raycastCallback = BulletGlobals.LocalTriangleSphereCastCallbackPool.Get())
{
raycastCallback.Initialize(ref convexFromLocal, ref convexToLocal,
convexbody.GetCcdSweptSphereRadius(), curHitFraction);
raycastCallback.m_hitFraction = convexbody.GetHitFraction();
CollisionObject concavebody = triBody;
ConcaveShape triangleMesh = concavebody.GetCollisionShape() as ConcaveShape;
if (triangleMesh != null)
{
triangleMesh.ProcessAllTriangles(raycastCallback, ref rayAabbMin, ref rayAabbMax);
}
if (raycastCallback.m_hitFraction < convexbody.GetHitFraction())
{
convexbody.SetHitFraction(raycastCallback.m_hitFraction);
return(raycastCallback.m_hitFraction);
}
}
}
return(1);
}
public override float CalculateTimeOfImpact(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
//(void)resultOut;
//(void)dispatchInfo;
///Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold
///Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold
///body0.m_worldTransform,
float resultFraction = 1.0f;
float squareMot0 = (body0.GetInterpolationWorldTransform()._origin - body0.GetWorldTransform()._origin).LengthSquared();
float squareMot1 = (body1.GetInterpolationWorldTransform()._origin - body1.GetWorldTransform()._origin).LengthSquared();
if (squareMot0 < body0.GetCcdSquareMotionThreshold() &&
squareMot1 < body1.GetCcdSquareMotionThreshold())
{
return resultFraction;
}
if (disableCcd)
{
return 1f;
}
//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 = body0.GetCollisionShape() as ConvexShape;
SphereShape sphere1 = BulletGlobals.SphereShapePool.Get();
sphere1.Initialize(body1.GetCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
CastResult result = BulletGlobals.CastResultPool.Get();
VoronoiSimplexSolver voronoiSimplex = BulletGlobals.VoronoiSimplexSolverPool.Get();
//SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
///Simplification, one object is simplified as a sphere
using (GjkConvexCast ccd1 = BulletGlobals.GjkConvexCastPool.Get())
{
ccd1.Initialize(convex0, sphere1, voronoiSimplex);
//ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
if (ccd1.CalcTimeOfImpact(body0.GetWorldTransform(), body0.GetInterpolationWorldTransform(),
body1.GetWorldTransform(), body1.GetInterpolationWorldTransform(), result))
{
//store result.m_fraction in both bodies
if (body0.GetHitFraction() > result.m_fraction)
{
body0.SetHitFraction(result.m_fraction);
}
if (body1.GetHitFraction() > result.m_fraction)
{
body1.SetHitFraction(result.m_fraction);
}
if (resultFraction > result.m_fraction)
{
resultFraction = result.m_fraction;
}
}
BulletGlobals.VoronoiSimplexSolverPool.Free(voronoiSimplex);
BulletGlobals.SphereShapePool.Free(sphere1);
result.Cleanup();
}
}
/// Sphere (for convex0) against Convex1
{
ConvexShape convex1 = body1.GetCollisionShape() as ConvexShape;
SphereShape sphere0 = BulletGlobals.SphereShapePool.Get();
sphere0.Initialize(body0.GetCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
CastResult result = BulletGlobals.CastResultPool.Get();
VoronoiSimplexSolver voronoiSimplex = BulletGlobals.VoronoiSimplexSolverPool.Get();
//SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
///Simplification, one object is simplified as a sphere
using (GjkConvexCast ccd1 = BulletGlobals.GjkConvexCastPool.Get())
{
ccd1.Initialize(sphere0, convex1, voronoiSimplex);
//ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
if (ccd1.CalcTimeOfImpact(body0.GetWorldTransform(), body0.GetInterpolationWorldTransform(),
body1.GetWorldTransform(), body1.GetInterpolationWorldTransform(), result))
{
//store result.m_fraction in both bodies
if (body0.GetHitFraction() > result.m_fraction)
{
body0.SetHitFraction(result.m_fraction);
}
if (body1.GetHitFraction() > result.m_fraction)
{
body1.SetHitFraction(result.m_fraction);
}
if (resultFraction > result.m_fraction)
{
resultFraction = result.m_fraction;
}
}
BulletGlobals.VoronoiSimplexSolverPool.Free(voronoiSimplex);
BulletGlobals.SphereShapePool.Free(sphere0);
result.Cleanup();
}
}
return resultFraction;
}
public override float CalculateTimeOfImpact(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
///Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold
///Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold
///body0.m_worldTransform,
float resultFraction = 1.0f;
float squareMot0 = (body0.GetInterpolationWorldTransform()._origin - body0.GetWorldTransform()._origin).LengthSquared();
float squareMot1 = (body1.GetInterpolationWorldTransform()._origin - body1.GetWorldTransform()._origin).LengthSquared();
if (squareMot0 < body0.GetCcdSquareMotionThreshold() &&
squareMot1 < body1.GetCcdSquareMotionThreshold())
{
return(resultFraction);
}
//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 = body0.GetCollisionShape() as ConvexShape;
SphereShape sphere1 = BulletGlobals.SphereShapePool.Get();
sphere1.Initialize(body1.GetCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
CastResult result = BulletGlobals.CastResultPool.Get();
VoronoiSimplexSolver voronoiSimplex = BulletGlobals.VoronoiSimplexSolverPool.Get();
//SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
///Simplification, one object is simplified as a sphere
using (GjkConvexCast ccd1 = BulletGlobals.GjkConvexCastPool.Get())
{
ccd1.Initialize(convex0, sphere1, voronoiSimplex);
//ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
if (ccd1.CalcTimeOfImpact(body0.GetWorldTransform(), body0.GetInterpolationWorldTransform(),
body1.GetWorldTransform(), body1.GetInterpolationWorldTransform(), result))
{
//store result.m_fraction in both bodies
if (body0.GetHitFraction() > result.m_fraction)
{
body0.SetHitFraction(result.m_fraction);
}
if (body1.GetHitFraction() > result.m_fraction)
{
body1.SetHitFraction(result.m_fraction);
}
if (resultFraction > result.m_fraction)
{
resultFraction = result.m_fraction;
}
}
BulletGlobals.VoronoiSimplexSolverPool.Free(voronoiSimplex);
BulletGlobals.SphereShapePool.Free(sphere1);
result.Cleanup();
}
}
/// Sphere (for convex0) against Convex1
{
ConvexShape convex1 = body1.GetCollisionShape() as ConvexShape;
SphereShape sphere0 = BulletGlobals.SphereShapePool.Get();
sphere0.Initialize(body0.GetCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
CastResult result = BulletGlobals.CastResultPool.Get();
VoronoiSimplexSolver voronoiSimplex = BulletGlobals.VoronoiSimplexSolverPool.Get();
//SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
///Simplification, one object is simplified as a sphere
using (GjkConvexCast ccd1 = BulletGlobals.GjkConvexCastPool.Get())
{
ccd1.Initialize(sphere0, convex1, voronoiSimplex);
//ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
if (ccd1.CalcTimeOfImpact(body0.GetWorldTransform(), body0.GetInterpolationWorldTransform(),
body1.GetWorldTransform(), body1.GetInterpolationWorldTransform(), result))
{
//store result.m_fraction in both bodies
if (body0.GetHitFraction() > result.m_fraction)
{
body0.SetHitFraction(result.m_fraction);
}
if (body1.GetHitFraction() > result.m_fraction)
{
body1.SetHitFraction(result.m_fraction);
}
if (resultFraction > result.m_fraction)
{
resultFraction = result.m_fraction;
}
}
BulletGlobals.VoronoiSimplexSolverPool.Free(voronoiSimplex);
BulletGlobals.SphereShapePool.Free(sphere0);
result.Cleanup();
}
}
return(resultFraction);
}
