internal static void CollDetectSphereStaticMeshSweep(BoundingSphere oldSphere, BoundingSphere newSphere, TriangleMesh mesh, CollDetectInfo info, float collTolerance, CollisionFunctor collisionFunctor) { // really use a swept test - or overlap? Vector3 delta = newSphere.Center - oldSphere.Center; if (delta.LengthSquared() < (0.25f * newSphere.Radius * newSphere.Radius)) { CollDetectSphereStaticMeshOverlap(oldSphere, newSphere, mesh, info, collTolerance, collisionFunctor); } else { Vector3 body0Pos = (info.Skin0.Owner != null) ? info.Skin0.Owner.OldPosition : Vector3.Zero; Vector3 body1Pos = (info.Skin1.Owner != null) ? info.Skin1.Owner.OldPosition : Vector3.Zero; float sphereTolR = collTolerance + oldSphere.Radius; float sphereToR2 = sphereTolR * sphereTolR; Vector3 collNormal = Vector3.Zero; BoundingBox bb = BoundingBoxHelper.InitialBox; BoundingBoxHelper.AddSphere(oldSphere, ref bb); BoundingBoxHelper.AddSphere(newSphere, ref bb); // get the spheres centers in triangle mesh space Vector3 newSphereCen = Vector3.TransformCoordinate(newSphere.Center, mesh.InverseTransformMatrix); Vector3 oldSphereCen = Vector3.TransformCoordinate(oldSphere.Center, mesh.InverseTransformMatrix); unsafe { #if USE_STACKALLOC SmallCollPointInfo* collPts = stackalloc SmallCollPointInfo[MaxLocalStackSCPI]; int* potentialTriangles = stackalloc int[MaxLocalStackTris]; { { #else SmallCollPointInfo[] collPtArray = SCPIStackAlloc(); fixed (SmallCollPointInfo* collPts = collPtArray) { int[] potTriArray = IntStackAlloc(); fixed( int* potentialTriangles = potTriArray) { #endif int numCollPts = 0; int numTriangles = mesh.GetTrianglesIntersectingtAABox(potentialTriangles, MaxLocalStackTris, ref bb); for (int iTriangle = 0; iTriangle < numTriangles; ++iTriangle) { // first test the old sphere for being on the wrong side IndexedTriangle meshTriangle = mesh.GetTriangle(potentialTriangles[iTriangle]); float distToCentreOld = SlimDX.Plane.DotCoordinate(meshTriangle.Plane, oldSphereCen); if (distToCentreOld <= 0.0f) continue; // now test the new sphere for being clear float distToCentreNew = SlimDX.Plane.DotCoordinate(meshTriangle.Plane, newSphereCen); if (distToCentreNew > sphereTolR) continue; int i0, i1, i2; meshTriangle.GetVertexIndices(out i0, out i1, out i2); Triangle triangle = new Triangle(mesh.GetVertex(i0), mesh.GetVertex(i1), mesh.GetVertex(i2)); // If the old sphere is intersecting, just use that result float s, t; float d2 = Distance.PointTriangleDistanceSq(out s, out t, oldSphereCen, triangle); if (d2 < sphereToR2) { float dist = (float)System.Math.Sqrt(d2); float depth = oldSphere.Radius - dist; Vector3 triangleN = triangle.Normal; Vector3 normSafe = oldSphereCen - triangle.GetPoint(s, t); JiggleMath.NormalizeSafe(ref normSafe); Vector3 collisionN = (dist > float.Epsilon) ? normSafe : triangleN; // since impulse gets applied at the old position Vector3 pt = oldSphere.Center - oldSphere.Radius * collisionN; if (numCollPts < MaxLocalStackSCPI) { collPts[numCollPts++] = new SmallCollPointInfo(pt - body0Pos, pt - body1Pos, depth); } collNormal += collisionN; } else if (distToCentreNew < distToCentreOld) { // old sphere is not intersecting - do a sweep, but only if the sphere is moving into the // triangle Vector3 pt, N; // CHECK THIS float depth; if (Intersection.SweptSphereTriangleIntersection(out pt, out N, out depth, oldSphere, newSphere, triangle, distToCentreOld, distToCentreNew, Intersection.EdgesToTest.EdgeAll, Intersection.CornersToTest.CornerAll)) { // collision point etc must be relative to the old position because that's //where the impulses are applied float dist = (float)System.Math.Sqrt(d2); float depth2 = oldSphere.Radius - dist; Vector3 triangleN = triangle.Normal; Vector3 normSafe = oldSphereCen - triangle.GetPoint(s, t); JiggleMath.NormalizeSafe(ref normSafe); Vector3 collisionN = (dist > JiggleMath.Epsilon) ? normSafe : triangleN; // since impulse gets applied at the old position Vector3 pt2 = oldSphere.Center - oldSphere.Radius * collisionN; if (numCollPts < MaxLocalStackSCPI) { collPts[numCollPts++] = new SmallCollPointInfo(pt2 - body0Pos, pt2 - body1Pos, depth); } collNormal += collisionN; } } } if (numCollPts > 0) { JiggleMath.NormalizeSafe(ref collNormal); collisionFunctor.CollisionNotify(ref info, ref collNormal, collPts, numCollPts); } } #if USE_STACKALLOC } } #else FreeStackAlloc(potTriArray); } FreeStackAlloc(collPtArray); } #endif } }
public static void CollDetectSphereStaticMeshOverlap(BoundingSphere oldSphere, BoundingSphere newSphere, TriangleMesh mesh, CollDetectInfo info, float collTolerance, CollisionFunctor collisionFunctor) { Vector3 body0Pos = (info.Skin0.Owner != null) ? info.Skin0.Owner.OldPosition : Vector3.Zero; Vector3 body1Pos = (info.Skin1.Owner != null) ? info.Skin1.Owner.OldPosition : Vector3.Zero; float sphereTolR = collTolerance + newSphere.Radius; float sphereTolR2 = sphereTolR * sphereTolR; unsafe { #if USE_STACKALLOC SmallCollPointInfo* collPts = stackalloc SmallCollPointInfo[MaxLocalStackSCPI]; int* potentialTriangles = stackalloc int[MaxLocalStackTris]; { { #else SmallCollPointInfo[] collPtArray = SCPIStackAlloc(); fixed (SmallCollPointInfo* collPts = collPtArray) { int[] potTriArray = IntStackAlloc(); fixed( int* potentialTriangles = potTriArray) { #endif int numCollPts = 0; Vector3 collNormal = Vector3.Zero; BoundingBox bb = BoundingBoxHelper.InitialBox; BoundingBoxHelper.AddSphere(newSphere, ref bb); int numTriangles = mesh.GetTrianglesIntersectingtAABox(potentialTriangles, MaxLocalStackTris, ref bb); // Deano : get the spheres centers in triangle mesh space Vector3 newSphereCen = Vector3.TransformCoordinate(newSphere.Center, mesh.InverseTransformMatrix); Vector3 oldSphereCen = Vector3.TransformCoordinate(oldSphere.Center, mesh.InverseTransformMatrix); for (int iTriangle = 0; iTriangle < numTriangles; ++iTriangle) { IndexedTriangle meshTriangle = mesh.GetTriangle(potentialTriangles[iTriangle]); float distToCentre = SlimDX.Plane.DotCoordinate(meshTriangle.Plane, newSphereCen); //meshTriangle.Plane.DotCoordinate(newSphereCen); if (distToCentre <= 0.0f) continue; if (distToCentre >= sphereTolR) continue; int i0, i1, i2; meshTriangle.GetVertexIndices(out i0, out i1, out i2); Triangle triangle = new Triangle(mesh.GetVertex(i0), mesh.GetVertex(i1), mesh.GetVertex(i2)); float s, t; float newD2 = Distance.PointTriangleDistanceSq(out s, out t, newSphereCen, triangle); if (newD2 < sphereTolR2) { // have overlap - but actually report the old intersection float oldD2 = Distance.PointTriangleDistanceSq(out s, out t, oldSphereCen, triangle); float dist = (float)System.Math.Sqrt((float)oldD2); float depth = oldSphere.Radius - dist; Vector3 triPointSTNorm = oldSphereCen - triangle.GetPoint(s, t); JiggleMath.NormalizeSafe(ref triPointSTNorm); Vector3 collisionN = (dist > float.Epsilon) ? triPointSTNorm : triangle.Normal; // since impulse get applied at the old position Vector3 pt = oldSphere.Center - oldSphere.Radius * collisionN; if (numCollPts < MaxLocalStackSCPI) { collPts[numCollPts++] = new SmallCollPointInfo(pt - body0Pos, pt - body1Pos, depth); } collNormal += collisionN; } } if (numCollPts > 0) { JiggleMath.NormalizeSafe(ref collNormal); collisionFunctor.CollisionNotify(ref info, ref collNormal, collPts, numCollPts); } #if USE_STACKALLOC } } #else FreeStackAlloc(potTriArray); } FreeStackAlloc(collPtArray); } #endif } }
private static bool CollDetectBoxStaticMeshOverlap(Box oldBox, Box newBox, TriangleMesh mesh, ref CollDetectInfo info, float collTolerance, CollisionFunctor collisionFunctor) { float boxRadius = newBox.GetBoundingRadiusAroundCentre(); #region REFERENCE: Vector3 boxCentre = newBox.GetCentre(); Vector3 boxCentre; newBox.GetCentre(out boxCentre); // Deano need to trasnform the box center into mesh space Matrix invTransformMatrix = mesh.InverseTransformMatrix; Vector3.TransformCoordinate(ref boxCentre, ref invTransformMatrix, out boxCentre); #endregion BoundingBox bb = BoundingBoxHelper.InitialBox; BoundingBoxHelper.AddBox(newBox, ref bb); unsafe { bool collision = false; #if USE_STACKALLOC int* potentialTriangles = stackalloc int[MaxLocalStackTris]; { #else int[] potTriArray = IntStackAlloc(); fixed( int* potentialTriangles = potTriArray) { #endif // aabox is done in mesh space and handles the mesh transform correctly int numTriangles = mesh.GetTrianglesIntersectingtAABox(potentialTriangles, MaxLocalStackTris, ref bb); for (int iTriangle = 0; iTriangle < numTriangles; ++iTriangle) { IndexedTriangle meshTriangle = mesh.GetTriangle(potentialTriangles[iTriangle]); // quick early test is done in mesh space float dist = SlimDX.Plane.DotCoordinate(meshTriangle.Plane, boxCentre); if (dist > boxRadius || dist < 0.0f) continue; if (DoOverlapBoxTriangleTest( oldBox, newBox, ref meshTriangle, mesh, ref info, collTolerance, collisionFunctor)) { collision = true; } } #if USE_STACKALLOC } #else } FreeStackAlloc(potTriArray); #endif return collision; } }