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