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 void UpdateSingleAabb(CollisionObject colObj)
{
IndexedVector3 minAabb;
IndexedVector3 maxAabb;
IndexedMatrix wt = colObj.GetWorldTransform();
colObj.GetCollisionShape().GetAabb(ref wt, out minAabb, out maxAabb);
//need to increase the aabb for contact thresholds
IndexedVector3 contactThreshold = new IndexedVector3(BulletGlobals.gContactBreakingThreshold);
minAabb -= contactThreshold;
maxAabb += contactThreshold;
if (GetDispatchInfo().m_useContinuous && colObj.GetInternalType() == CollisionObjectTypes.CO_RIGID_BODY && !colObj.IsStaticOrKinematicObject())
{
IndexedVector3 minAabb2,maxAabb2;
colObj.GetCollisionShape().GetAabb(colObj.GetInterpolationWorldTransform(),out minAabb2 ,out maxAabb2);
minAabb2 -= contactThreshold;
maxAabb2 += contactThreshold;
MathUtil.VectorMin(ref minAabb2,ref minAabb);
MathUtil.VectorMax(ref maxAabb2, ref maxAabb);
}
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugCollisionWorld)
{
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "updateSingleAabbMin", minAabb);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "updateSingleAabbMax", maxAabb);
}
IBroadphaseInterface bp = m_broadphasePairCache as IBroadphaseInterface;
//moving objects should be moderately sized, probably something wrong if not
if (colObj.IsStaticObject() || ((maxAabb - minAabb).LengthSquared() < 1e12f))
{
bp.SetAabb(colObj.GetBroadphaseHandle(), ref minAabb, ref maxAabb, m_dispatcher1);
}
else
{
//something went wrong, investigate
//this assert is unwanted in 3D modelers (danger of loosing work)
colObj.SetActivationState(ActivationState.DISABLE_SIMULATION);
//static bool reportMe = true;
bool reportMe = true;
if (reportMe && m_debugDrawer != null)
{
reportMe = false;
m_debugDrawer.ReportErrorWarning("Overflow in AABB, object removed from simulation");
m_debugDrawer.ReportErrorWarning("If you can reproduce this, please email [email protected]\n");
m_debugDrawer.ReportErrorWarning("Please include above information, your Platform, version of OS.\n");
m_debugDrawer.ReportErrorWarning("Thanks.\n");
}
}
}
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);
}
public void ProcessChildShape(CollisionShape childShape, int index)
{
Debug.Assert(index >= 0);
CompoundShape compoundShape = (CompoundShape)(m_compoundColObj.GetCollisionShape());
Debug.Assert(index < compoundShape.GetNumChildShapes());
//backup
IndexedMatrix orgTrans = m_compoundColObj.GetWorldTransform();
IndexedMatrix orgInterpolationTrans = m_compoundColObj.GetInterpolationWorldTransform();
IndexedMatrix childTrans = compoundShape.GetChildTransform(index);
IndexedMatrix newChildWorldTrans = orgTrans * childTrans;
//perform an AABB check first
IndexedVector3 aabbMin0;
IndexedVector3 aabbMax0;
IndexedVector3 aabbMin1;
IndexedVector3 aabbMax1;
childShape.GetAabb(ref newChildWorldTrans, out aabbMin0, out aabbMax0);
m_otherObj.GetCollisionShape().GetAabb(m_otherObj.GetWorldTransform(), out aabbMin1, out aabbMax1);
if (AabbUtil2.TestAabbAgainstAabb2(ref aabbMin0, ref aabbMax0, ref aabbMin1, ref aabbMax1))
{
m_compoundColObj.SetWorldTransform(ref newChildWorldTrans);
m_compoundColObj.SetInterpolationWorldTransform(ref newChildWorldTrans);
//the contactpoint is still projected back using the original inverted worldtrans
CollisionShape tmpShape = m_compoundColObj.GetCollisionShape();
m_compoundColObj.InternalSetTemporaryCollisionShape(childShape);
if (m_childCollisionAlgorithms[index] == null)
{
m_childCollisionAlgorithms[index] = m_dispatcher.FindAlgorithm(m_compoundColObj, m_otherObj, m_sharedManifold);
if (m_childCollisionAlgorithms[index] == m_parent)
{
int ibreak = 0;
}
}
///detect swapping case
if (m_resultOut.GetBody0Internal() == m_compoundColObj)
{
m_resultOut.SetShapeIdentifiersA(-1, index);
}
else
{
m_resultOut.SetShapeIdentifiersB(-1, index);
}
m_childCollisionAlgorithms[index].ProcessCollision(m_compoundColObj, m_otherObj, m_dispatchInfo, m_resultOut);
if (m_dispatchInfo.getDebugDraw() != null && (((m_dispatchInfo.getDebugDraw().GetDebugMode() & DebugDrawModes.DBG_DrawAabb)) != 0))
{
IndexedVector3 worldAabbMin = IndexedVector3.Zero, worldAabbMax = IndexedVector3.Zero;
m_dispatchInfo.getDebugDraw().DrawAabb(aabbMin0, aabbMax0, new IndexedVector3(1, 1, 1));
m_dispatchInfo.getDebugDraw().DrawAabb(aabbMin1, aabbMax1, new IndexedVector3(1, 1, 1));
}
//revert back transform
m_compoundColObj.InternalSetTemporaryCollisionShape(tmpShape);
m_compoundColObj.SetWorldTransform(ref orgTrans);
m_compoundColObj.SetInterpolationWorldTransform(ref orgInterpolationTrans);
}
}
public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
//resultOut = null;
CollisionObject colObj = m_isSwapped ? body1 : body0;
CollisionObject otherObj = m_isSwapped ? body0 : body1;
Debug.Assert(colObj.GetCollisionShape().IsCompound());
CompoundShape compoundShape = (CompoundShape)(colObj.GetCollisionShape());
///btCompoundShape might have changed:
////make sure the internal child collision algorithm caches are still valid
if (compoundShape.GetUpdateRevision() != m_compoundShapeRevision)
{
///clear and update all
RemoveChildAlgorithms();
PreallocateChildAlgorithms(body0, body1);
}
Dbvt tree = compoundShape.GetDynamicAabbTree();
//use a dynamic aabb tree to cull potential child-overlaps
using (CompoundLeafCallback callback = BulletGlobals.CompoundLeafCallbackPool.Get())
{
callback.Initialize(colObj, otherObj, m_dispatcher, dispatchInfo, resultOut, this, m_childCollisionAlgorithms, m_sharedManifold);
///we need to refresh all contact manifolds
///note that we should actually recursively traverse all children, btCompoundShape can nested more then 1 level deep
///so we should add a 'refreshManifolds' in the btCollisionAlgorithm
{
m_manifoldArray.Clear();
for (int i = 0; i < m_childCollisionAlgorithms.Count; i++)
{
if (m_childCollisionAlgorithms[i] != null)
{
m_childCollisionAlgorithms[i].GetAllContactManifolds(m_manifoldArray);
for (int m = 0; m < m_manifoldArray.Count; m++)
{
if (m_manifoldArray[m].GetNumContacts() > 0)
{
resultOut.SetPersistentManifold(m_manifoldArray[m]);
resultOut.RefreshContactPoints();
resultOut.SetPersistentManifold(null);//??necessary?
}
}
m_manifoldArray.Clear();
}
}
}
if (tree != null)
{
IndexedVector3 localAabbMin;
IndexedVector3 localAabbMax;
IndexedMatrix otherInCompoundSpace;
//otherInCompoundSpace = MathUtil.BulletMatrixMultiply(colObj.GetWorldTransform(),otherObj.GetWorldTransform());
otherInCompoundSpace = colObj.GetWorldTransform().Inverse() * otherObj.GetWorldTransform();
otherObj.GetCollisionShape().GetAabb(ref otherInCompoundSpace, out localAabbMin, out localAabbMax);
DbvtAabbMm bounds = DbvtAabbMm.FromMM(ref localAabbMin, ref localAabbMax);
//process all children, that overlap with the given AABB bounds
Dbvt.CollideTV(tree.m_root, ref bounds, callback, tree.CollideTVStack, ref tree.CollideTVCount);
}
else
{
//iterate over all children, perform an AABB check inside ProcessChildShape
int numChildren = m_childCollisionAlgorithms.Count;
for (int i = 0; i < numChildren; i++)
{
callback.ProcessChildShape(compoundShape.GetChildShape(i), i);
}
}
{
//iterate over all children, perform an AABB check inside ProcessChildShape
int numChildren = m_childCollisionAlgorithms.Count;
m_manifoldArray.Clear();
CollisionShape childShape = null;
IndexedMatrix orgTrans;
IndexedMatrix orgInterpolationTrans;
IndexedMatrix newChildWorldTrans;
for (int i = 0; i < numChildren; i++)
{
if (m_childCollisionAlgorithms[i] != null)
{
childShape = compoundShape.GetChildShape(i);
//if not longer overlapping, remove the algorithm
orgTrans = colObj.GetWorldTransform();
orgInterpolationTrans = colObj.GetInterpolationWorldTransform();
IndexedMatrix childTrans = compoundShape.GetChildTransform(i);
newChildWorldTrans = orgTrans * childTrans;
//perform an AABB check first
IndexedVector3 aabbMin0;
IndexedVector3 aabbMax0;
IndexedVector3 aabbMin1;
IndexedVector3 aabbMax1;
childShape.GetAabb(ref newChildWorldTrans, out aabbMin0, out aabbMax0);
otherObj.GetCollisionShape().GetAabb(otherObj.GetWorldTransform(), out aabbMin1, out aabbMax1);
if (!AabbUtil2.TestAabbAgainstAabb2(ref aabbMin0, ref aabbMax0, ref aabbMin1, ref aabbMax1))
{
m_dispatcher.FreeCollisionAlgorithm(m_childCollisionAlgorithms[i]);
m_childCollisionAlgorithms[i] = null;
}
}
}
}
}
}
