public void ProcessTriangle(IndexedVector3[] triangle, int partId, int triangleIndex) { //do a swept sphere for now IndexedMatrix ident = IndexedMatrix.Identity; CastResult castResult = BulletGlobals.CastResultPool.Get(); castResult.m_fraction = m_hitFraction; SphereShape pointShape = BulletGlobals.SphereShapePool.Get(); pointShape.Initialize(m_ccdSphereRadius); using (TriangleShape triShape = BulletGlobals.TriangleShapePool.Get()) { triShape.Initialize(ref triangle[0], ref triangle[1], ref triangle[2]); VoronoiSimplexSolver simplexSolver = BulletGlobals.VoronoiSimplexSolverPool.Get(); SubSimplexConvexCast convexCaster = BulletGlobals.SubSimplexConvexCastPool.Get(); convexCaster.Initialize(pointShape, triShape, simplexSolver); //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver); //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0); //local space? if (convexCaster.CalcTimeOfImpact(ref m_ccdSphereFromTrans, ref m_ccdSphereToTrans, ref ident, ref ident, castResult)) { if (m_hitFraction > castResult.m_fraction) { m_hitFraction = castResult.m_fraction; } } BulletGlobals.SubSimplexConvexCastPool.Free(convexCaster); BulletGlobals.VoronoiSimplexSolverPool.Free(simplexSolver); BulletGlobals.SphereShapePool.Free(pointShape); castResult.Cleanup(); } }
public virtual void ProcessTriangle(IndexedVector3[] triangle, int partId, int triangleIndex) { TriangleShape triangleShape = new TriangleShape(ref triangle[0], ref triangle[1], ref triangle[2]); triangleShape.SetMargin(m_triangleCollisionMargin); VoronoiSimplexSolver simplexSolver = BulletGlobals.VoronoiSimplexSolverPool.Get(); GjkEpaPenetrationDepthSolver gjkEpaPenetrationSolver = new GjkEpaPenetrationDepthSolver(); //#define USE_SUBSIMPLEX_CONVEX_CAST 1 //if you reenable USE_SUBSIMPLEX_CONVEX_CAST see commented ref code below #if USE_SUBSIMPLEX_CONVEX_CAST SubsimplexConvexCast convexCaster = new SubsimplexConvexCast(m_convexShape, triangleShape, simplexSolver); #else //btGjkConvexCast convexCaster(m_convexShape,&triangleShape,&simplexSolver); ContinuousConvexCollision convexCaster = BulletGlobals.ContinuousConvexCollisionPool.Get(); convexCaster.Initialize(m_convexShape, triangleShape, simplexSolver, gjkEpaPenetrationSolver); #endif //#USE_SUBSIMPLEX_CONVEX_CAST CastResult castResult = BulletGlobals.CastResultPool.Get(); castResult.m_fraction = 1f; if (convexCaster.CalcTimeOfImpact(ref m_convexShapeFrom, ref m_convexShapeTo, ref m_triangleToWorld, ref m_triangleToWorld, castResult)) { //add hit if (castResult.m_normal.LengthSquared() > 0.0001f) { if (castResult.m_fraction < m_hitFraction) { /* btContinuousConvexCast's normal is already in world space */ /* #ifdef USE_SUBSIMPLEX_CONVEX_CAST * //rotate normal into worldspace * castResult.m_normal = m_convexShapeFrom.getBasis() * castResult.m_normal; #endif //USE_SUBSIMPLEX_CONVEX_CAST */ castResult.m_normal.Normalize(); ReportHit(ref castResult.m_normal, ref castResult.m_hitPoint, castResult.m_fraction, partId, triangleIndex); } } } BulletGlobals.ContinuousConvexCollisionPool.Free(convexCaster); BulletGlobals.VoronoiSimplexSolverPool.Free(simplexSolver); castResult.Cleanup(); }
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); }