private static bool DoOverlapBoxTriangleTest(Box oldBox, Box newBox,
ref IndexedTriangle triangle, TriangleMesh mesh,
ref CollDetectInfo info, float collTolerance,
CollisionFunctor collisionFunctor)
{
Matrix dirs0 = newBox.Orientation;
#region REFERENCE: Triangle tri = new Triangle(mesh.GetVertex(triangle.GetVertexIndex(0)),mesh.GetVertex(triangle.GetVertexIndex(1)),mesh.GetVertex(triangle.GetVertexIndex(2)));
Vector3 triVec0;
Vector3 triVec1;
Vector3 triVec2;
mesh.GetVertex(triangle.GetVertexIndex(0), out triVec0);
mesh.GetVertex(triangle.GetVertexIndex(1), out triVec1);
mesh.GetVertex(triangle.GetVertexIndex(2), out triVec2);
// Deano move tri into world space
Matrix transformMatrix = mesh.TransformMatrix;
Vector3.TransformCoordinate(ref triVec0, ref transformMatrix, out triVec0);
Vector3.TransformCoordinate(ref triVec1, ref transformMatrix, out triVec1);
Vector3.TransformCoordinate(ref triVec2, ref transformMatrix, out triVec2);
Triangle tri = new Triangle(ref triVec0,ref triVec1,ref triVec2);
#endregion
#region REFERENCE Vector3 triEdge0 = (tri.GetPoint(1) - tri.GetPoint(0));
Vector3 pt0;
Vector3 pt1;
tri.GetPoint(0, out pt0);
tri.GetPoint(1, out pt1);
Vector3 triEdge0;
Vector3.Subtract(ref pt1, ref pt0, out triEdge0);
#endregion
#region REFERENCE Vector3 triEdge1 = (tri.GetPoint(2) - tri.GetPoint(1));
Vector3 pt2;
tri.GetPoint(2, out pt2);
Vector3 triEdge1;
Vector3.Subtract(ref pt2, ref pt1, out triEdge1);
#endregion
#region REFERENCE Vector3 triEdge2 = (tri.GetPoint(0) - tri.GetPoint(2));
Vector3 triEdge2;
Vector3.Subtract(ref pt0, ref pt2, out triEdge2);
#endregion
triEdge0.Normalize();
triEdge1.Normalize();
triEdge2.Normalize();
Vector3 triNormal = triangle.Plane.Normal;
// the 15 potential separating axes
const int numAxes = 13;
Vector3[] axes = new Vector3[numAxes];
axes[0] = triNormal;
MatrixHelper.GetRight(ref dirs0, out axes[1]);
MatrixHelper.GetUp(ref dirs0, out axes[2]);
MatrixHelper.GetBackward(ref dirs0, out axes[3]);
Vector3.Cross(ref axes[1], ref triEdge0, out axes[4]);
Vector3.Cross(ref axes[1], ref triEdge1, out axes[5]);
Vector3.Cross(ref axes[1], ref triEdge2, out axes[6]);
Vector3.Cross(ref axes[2], ref triEdge0, out axes[7]);
Vector3.Cross(ref axes[2], ref triEdge1, out axes[8]);
Vector3.Cross(ref axes[2], ref triEdge2, out axes[9]);
Vector3.Cross(ref axes[3], ref triEdge0, out axes[10]);
Vector3.Cross(ref axes[3], ref triEdge1, out axes[11]);
Vector3.Cross(ref axes[3], ref triEdge2, out axes[12]);
// the overlap depths along each axis
float[] overlapDepths = new float[numAxes];
// see if the boxes are separate along any axis, and if not keep a
// record of the depths along each axis
int i;
for (i = 0; i < numAxes; ++i)
{
overlapDepths[i] = 1.0f;
if (Disjoint(out overlapDepths[i], axes[i], newBox, tri, collTolerance))
return false;
}
// The box overlap, find the separation depth closest to 0.
float minDepth = float.MaxValue;
int minAxis = -1;
for (i = 0; i < numAxes; ++i)
{
// If we can't normalise the axis, skip it
float l2 = axes[i].LengthSquared();
if (l2 < JiggleMath.Epsilon)
continue;
// Normalise the separation axis and the depth
float invl = 1.0f / (float)System.Math.Sqrt(l2);
axes[i] *= invl;
overlapDepths[i] *= invl;
// If this axis is the minimum, select it
if (overlapDepths[i] < minDepth)
{
minDepth = overlapDepths[i];
minAxis = i;
}
}
if (minAxis == -1)
return false;
// Make sure the axis is facing towards the 0th box.
// if not, invert it
Vector3 D = newBox.GetCentre() - tri.Centre;
Vector3 N = axes[minAxis];
float depth = overlapDepths[minAxis];
if (Vector3.Dot(D, N) < 0.0f)
N *= -1;
Vector3 boxOldPos = (info.Skin0.Owner != null) ? info.Skin0.Owner.OldPosition : Vector3.Zero;
Vector3 boxNewPos = (info.Skin0.Owner != null) ? info.Skin0.Owner.Position : Vector3.Zero;
Vector3 meshPos = (info.Skin1.Owner != null) ? info.Skin1.Owner.OldPosition : Vector3.Zero;
List<Vector3> pts = new List<Vector3>();
//pts.Clear();
const float combinationDist = 0.05f;
GetBoxTriangleIntersectionPoints(pts, newBox, tri, depth + combinationDist);
// adjust the depth
#region REFERENCE: Vector3 delta = boxNewPos - boxOldPos;
Vector3 delta;
Vector3.Subtract(ref boxNewPos, ref boxOldPos, out delta);
#endregion
#region REFERENCE: float oldDepth = depth + Vector3.Dot(delta, N);
float oldDepth = Vector3.Dot(delta, N);
oldDepth += depth;
#endregion
unsafe
{
// report collisions
int numPts = pts.Count;
#if USE_STACKALLOC
SmallCollPointInfo* collPts = stackalloc SmallCollPointInfo[MaxLocalStackSCPI];
#else
SmallCollPointInfo[] collPtArray = SCPIStackAlloc();
fixed (SmallCollPointInfo* collPts = collPtArray)
#endif
{
if (numPts > 0)
{
if (numPts >= MaxLocalStackSCPI)
{
numPts = MaxLocalStackSCPI - 1;
}
// adjust positions
for (i = 0; i < numPts; ++i)
{
collPts[i] = new SmallCollPointInfo(pts[i] - boxNewPos, pts[i] - meshPos, oldDepth);
}
collisionFunctor.CollisionNotify(ref info, ref N, collPts, numPts);
#if !USE_STACKALLOC
FreeStackAlloc(collPtArray);
#endif
return true;
}
else
{
#if !USE_STACKALLOC
FreeStackAlloc(collPtArray);
#endif
return false;
}
}
}
}
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;
}
}
private static void GetSupportPoint(out Vector3 p, Box box,Vector3 axis)
{
#region INLINE: Vector3 orient0 = box.Orientation.Right;
Vector3 orient0 = new Vector3(
box.transform.Orientation.M11,
box.transform.Orientation.M12,
box.transform.Orientation.M13);
#endregion
#region INLINE: Vector3 orient1 = box.Orientation.Up;
Vector3 orient1 = new Vector3(
box.transform.Orientation.M21,
box.transform.Orientation.M22,
box.transform.Orientation.M23);
#endregion
#region INLINE: Vector3 orient2 = box.Orientation.Backward;
Vector3 orient2 = new Vector3(
box.transform.Orientation.M31,
box.transform.Orientation.M32,
box.transform.Orientation.M33);
#endregion
#region INLINE: float ass = Vector3.Dot(axis,orient0);
float ass = axis.X * orient0.X + axis.Y * orient0.Y + axis.Z * orient0.Z;
#endregion
#region INLINE: float au = Vector3.Dot(axis,orient1);
float au = axis.X * orient1.X + axis.Y * orient1.Y + axis.Z * orient1.Z;
#endregion
#region INLINE: float ad = Vector3.Dot(axis,orient2);
float ad = axis.X * orient2.X + axis.Y * orient2.Y + axis.Z * orient2.Z;
#endregion
float threshold = JiggleMath.Epsilon;
box.GetCentre(out p);
if (ass < -threshold)
{
#region INLINE: p += orient0 * (0.5 * box.SideLength.X);
p.X += orient0.X * (0.5f * box.sideLengths.X);
p.Y += orient0.Y * (0.5f * box.sideLengths.X);
p.Z += orient0.Z * (0.5f * box.sideLengths.X);
#endregion
}
else if (ass >= threshold)
{
#region INLINE: p -= orient0 * (0.5 * box.SideLength.X);
p.X -= orient0.X * (0.5f * box.sideLengths.X);
p.Y -= orient0.Y * (0.5f * box.sideLengths.X);
p.Z -= orient0.Z * (0.5f * box.sideLengths.X);
#endregion
}
if (au < -threshold)
{
#region INLINE: p += orient1 * (0.5 * box.SideLength.Y);
p.X += orient1.X * (0.5f * box.sideLengths.Y);
p.Y += orient1.Y * (0.5f * box.sideLengths.Y);
p.Z += orient1.Z * (0.5f * box.sideLengths.Y);
#endregion
}
else if (au >= threshold)
{
#region INLINE: p -= orient1 * (0.5 * box.SideLength.Y);
p.X -= orient1.X * (0.5f * box.sideLengths.Y);
p.Y -= orient1.Y * (0.5f * box.sideLengths.Y);
p.Z -= orient1.Z * (0.5f * box.sideLengths.Y);
#endregion
}
if (ad < -threshold)
{
#region INLINE: p += orient2 * (0.5 * box.SideLength.Z);
p.X += orient2.X * (0.5f * box.sideLengths.Z);
p.Y += orient2.Y * (0.5f * box.sideLengths.Z);
p.Z += orient2.Z * (0.5f * box.sideLengths.Z);
#endregion
}
else if (ad >= threshold)
{
#region INLINE: p -= orient2 * (0.5 * box.SideLength.Z);
p.X -= orient2.X * (0.5f * box.sideLengths.Z);
p.Y -= orient2.Y * (0.5f * box.sideLengths.Z);
p.Z -= orient2.Z * (0.5f * box.sideLengths.Z);
#endregion
}
}