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; } } } } } }