public void Initialize(SphereShape sphere, TriangleShape triangle, float contactBreakingThreshold) { m_sphere = sphere; m_triangle = triangle; m_contactBreakingThreshold = contactBreakingThreshold; }
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); }
} // for pool public SphereTriangleDetector(SphereShape sphere, TriangleShape triangle, float contactBreakingThreshold) { m_sphere = sphere; m_triangle = triangle; m_contactBreakingThreshold = contactBreakingThreshold; }
public virtual void GetAabbNonVirtual(ref IndexedMatrix t, ref IndexedVector3 aabbMin, ref IndexedVector3 aabbMax) { switch (m_shapeType) { case BroadphaseNativeTypes.SPHERE_SHAPE_PROXYTYPE: { SphereShape sphereShape = this as SphereShape; float radius = sphereShape.GetImplicitShapeDimensions().X;// * convexShape->getLocalScaling().getX(); float margin = radius + sphereShape.GetMarginNonVirtual(); IndexedVector3 center = t._origin; IndexedVector3 extent = new IndexedVector3(margin); aabbMin = center - extent; aabbMax = center + extent; } break; case BroadphaseNativeTypes.CYLINDER_SHAPE_PROXYTYPE: /* fall through */ case BroadphaseNativeTypes.BOX_SHAPE_PROXYTYPE: { BoxShape convexShape = this as BoxShape; float margin = convexShape.GetMarginNonVirtual(); IndexedVector3 halfExtents = convexShape.GetImplicitShapeDimensions(); halfExtents += new IndexedVector3(margin); IndexedBasisMatrix abs_b = t._basis.Absolute(); IndexedVector3 center = t._origin; IndexedVector3 extent = new IndexedVector3(abs_b._el0.Dot(ref halfExtents), abs_b._el1.Dot(ref halfExtents), abs_b._el2.Dot(ref halfExtents)); aabbMin = center - extent; aabbMax = center + extent; break; } case BroadphaseNativeTypes.TRIANGLE_SHAPE_PROXYTYPE: { TriangleShape triangleShape = (TriangleShape)this; float margin = triangleShape.GetMarginNonVirtual(); for (int i = 0; i < 3; i++) { IndexedVector3 vec = new IndexedVector3(); vec[i] = 1f; IndexedVector3 sv = LocalGetSupportVertexWithoutMarginNonVirtual(vec * t._basis); IndexedVector3 tmp = t * sv; aabbMax[i] = tmp[i] + margin; vec[i] = -1.0f; tmp = t * (LocalGetSupportVertexWithoutMarginNonVirtual(vec * t._basis)); aabbMin[i] = tmp[i] - margin; } } break; case BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE: { CapsuleShape capsuleShape = this as CapsuleShape; float r = capsuleShape.GetRadius(); IndexedVector3 halfExtents = new IndexedVector3(r); int m_upAxis = capsuleShape.GetUpAxis(); halfExtents[m_upAxis] = r + capsuleShape.GetHalfHeight(); float nvMargin = capsuleShape.GetMarginNonVirtual(); halfExtents += new IndexedVector3(nvMargin); IndexedBasisMatrix abs_b = t._basis.Absolute(); IndexedVector3 center = t._origin; IndexedVector3 extent = new IndexedVector3(abs_b._el0.Dot(ref halfExtents), abs_b._el1.Dot(ref halfExtents), abs_b._el2.Dot(ref halfExtents)); aabbMin = center - extent; aabbMax = center + extent; } break; case BroadphaseNativeTypes.CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE: case BroadphaseNativeTypes.CONVEX_HULL_SHAPE_PROXYTYPE: { PolyhedralConvexAabbCachingShape convexHullShape = (PolyhedralConvexAabbCachingShape)this; float margin = convexHullShape.GetMarginNonVirtual(); convexHullShape.GetNonvirtualAabb(ref t, out aabbMin, out aabbMax, margin); } break; default: GetAabb(ref t, out aabbMin, out aabbMax); break; } // should never reach here Debug.Assert(false); }