/// <summary>
/// DoOverlapBoxTriangleTest
/// </summary>
/// <param name="oldBox"></param>
/// <param name="newBox"></param>
/// <param name="triangle"></param>
/// <param name="mesh"></param>
/// <param name="info"></param>
/// <param name="collTolerance"></param>
/// <param name="collisionFunctor"></param>
/// <returns>bool</returns>
private static bool DoOverlapBoxTriangleTest(Box oldBox, Box newBox,
ref IndexedTriangle triangle, TriangleMesh mesh,
ref CollDetectInfo info, float collTolerance,
CollisionFunctor collisionFunctor)
{
Matrix4 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
Matrix4 transformMatrix = mesh.TransformMatrix;
Vector3.Transform(ref triVec0, ref transformMatrix, out triVec0);
Vector3.Transform(ref triVec1, ref transformMatrix, out triVec1);
Vector3.Transform(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();
// BEN-OPTIMISATION: Replaced loops with code that requires no looping.
// The new code is faster, has less allocations and math especially
// since the method returns as soon as it finds a non-overlapping axis,
// i.e. Before irreleveat allocations occur.
#region "Old (less efficient) code"
/*Vector3 triNormal = triangle.Plane.Normal;
// the 15 potential separating axes
const int numAxes = 13;
Vector3[] axes = new Vector3[numAxes];
axes[0] = triNormal;
axes[1] = dirs0.Right;
axes[2] = dirs0.Up;
axes[3] = dirs0.Backward;
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];*/
#endregion
#region "Optimised code"
Vector3 triNormal = triangle.Plane.Normal;
Vector3 right = dirs0.Right();
Vector3 up = dirs0.Up();
Vector3 backward = dirs0.Backward();
float testDepth;
if (Disjoint(out testDepth, ref triNormal, newBox, ref tri, collTolerance))
return (false);
float depth = testDepth;
Vector3 N = triNormal;
if (Disjoint(out testDepth, ref right, newBox, ref tri, collTolerance))
return (false);
if (testDepth < depth)
{
depth = testDepth;
N = right;
}
if (Disjoint(out testDepth, ref up, newBox, ref tri, collTolerance))
return (false);
if (testDepth < depth)
{
depth = testDepth;
N = up;
}
if (Disjoint(out testDepth, ref backward, newBox, ref tri, collTolerance))
return (false);
if (testDepth < depth)
{
depth = testDepth;
N = backward;
}
Vector3 axis;
Vector3.Cross(ref right, ref triEdge0, out axis);
if (Disjoint(out testDepth, ref axis, newBox, ref tri, collTolerance))
return (false);
testDepth *= 1.0f / (float)System.Math.Sqrt(axis.X * axis.X + axis.Y * axis.Y + axis.Z * axis.Z);
if (testDepth < depth)
{
depth = testDepth;
N = axis;
}
Vector3.Cross(ref right, ref triEdge1, out axis);
if (Disjoint(out testDepth, ref axis, newBox, ref tri, collTolerance))
return (false);
testDepth *= 1.0f / (float)System.Math.Sqrt(axis.X * axis.X + axis.Y * axis.Y + axis.Z * axis.Z);
if (testDepth < depth)
{
depth = testDepth;
N = axis;
}
Vector3.Cross(ref right, ref triEdge2, out axis);
if (Disjoint(out testDepth, ref axis, newBox, ref tri, collTolerance))
return (false);
testDepth *= 1.0f / (float)System.Math.Sqrt(axis.X * axis.X + axis.Y * axis.Y + axis.Z * axis.Z);
if (testDepth < depth)
{
depth = testDepth;
N = axis;
}
Vector3.Cross(ref up, ref triEdge0, out axis);
if (Disjoint(out testDepth, ref axis, newBox, ref tri, collTolerance))
return (false);
testDepth *= 1.0f / (float)System.Math.Sqrt(axis.X * axis.X + axis.Y * axis.Y + axis.Z * axis.Z);
if (testDepth < depth)
{
depth = testDepth;
N = axis;
}
Vector3.Cross(ref up, ref triEdge1, out axis);
if (Disjoint(out testDepth, ref axis, newBox, ref tri, collTolerance))
return (false);
testDepth *= 1.0f / (float)System.Math.Sqrt(axis.X * axis.X + axis.Y * axis.Y + axis.Z * axis.Z);
if (testDepth < depth)
{
depth = testDepth;
N = axis;
}
Vector3.Cross(ref up, ref triEdge2, out axis);
if (Disjoint(out testDepth, ref axis, newBox, ref tri, collTolerance))
return (false);
testDepth *= 1.0f / (float)System.Math.Sqrt(axis.X * axis.X + axis.Y * axis.Y + axis.Z * axis.Z);
if (testDepth < depth)
{
depth = testDepth;
N = axis;
}
Vector3.Cross(ref backward, ref triEdge0, out axis);
if (Disjoint(out testDepth, ref axis, newBox, ref tri, collTolerance))
return (false);
testDepth *= 1.0f / (float)System.Math.Sqrt(axis.X * axis.X + axis.Y * axis.Y + axis.Z * axis.Z);
if (testDepth < depth)
{
depth = testDepth;
N = axis;
}
Vector3.Cross(ref backward, ref triEdge1, out axis);
if (Disjoint(out testDepth, ref axis, newBox, ref tri, collTolerance))
return (false);
testDepth *= 1.0f / (float)System.Math.Sqrt(axis.X * axis.X + axis.Y * axis.Y + axis.Z * axis.Z);
if (testDepth < depth)
{
depth = testDepth;
N = axis;
}
Vector3.Cross(ref backward, ref triEdge2, out axis);
if (Disjoint(out testDepth, ref axis, newBox, ref tri, collTolerance))
return (false);
testDepth *= 1.0f / (float)System.Math.Sqrt(axis.X * axis.X + axis.Y * axis.Y + axis.Z * axis.Z);
if (testDepth < depth)
{
depth = testDepth;
N = axis;
}
/*if (N == Vector3.Zero)
return (false);*/
Vector3 D = newBox.GetCentre() - tri.Centre;
N.Normalize();
int i;
#endregion
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(ref delta, ref N, out oldDepth);
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)
{
// BEN-OPTIMISATION: Reused existing SmallCollPointInfo and inlined vector substraction.
collPts[i].R0.X = pts[i].X - boxNewPos.X;
collPts[i].R0.Y = pts[i].Y - boxNewPos.Y;
collPts[i].R0.Z = pts[i].Z - boxNewPos.Z;
collPts[i].R1.X = pts[i].X - meshPos.X;
collPts[i].R1.Y = pts[i].Y - meshPos.Y;
collPts[i].R1.Z = pts[i].Z - meshPos.Z;
collPts[i].InitialPenetration = 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;
}
}
}
}
/// <summary>
/// CollDetectBoxStaticMeshOverlap
/// </summary>
/// <param name="oldBox"></param>
/// <param name="newBox"></param>
/// <param name="mesh"></param>
/// <param name="info"></param>
/// <param name="collTolerance"></param>
/// <param name="collisionFunctor"></param>
/// <returns>bool</returns>
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
Matrix4 invTransformMatrix = mesh.InverseTransformMatrix;
Vector3.Transform(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 = meshTriangle.Plane.DotCoordinate(boxCentre);
// BEN-BUG-FIX: Fixed by chaning 0.0F to -boxRadius.
if (dist > boxRadius || dist < -boxRadius)
continue;
if (DoOverlapBoxTriangleTest(
oldBox, newBox,
ref meshTriangle,
mesh,
ref info,
collTolerance,
collisionFunctor))
{
collision = true;
}
}
#if USE_STACKALLOC
}
#else
}
FreeStackAlloc(potTriArray);
#endif
return collision;
}
}
/// <summary>
/// SqrDistance
/// </summary>
/// <param name="line"></param>
/// <param name="box"></param>
/// <param name="pfLParam"></param>
/// <param name="pfBParam0"></param>
/// <param name="pfBParam1"></param>
/// <param name="pfBParam2"></param>
/// <returns>float</returns>
public static float SqrDistance(Line line, Box box, out float pfLParam,
out float pfBParam0, out float pfBParam1, out float pfBParam2)
{
// compute coordinates of line in box coordinate system
Vector3 diff = line.Origin - box.GetCentre();
Vector3 pnt = new Vector3(Vector3.Dot(diff, box.Orientation.Right),
Vector3.Dot(diff, box.Orientation.Up),
Vector3.Dot(diff, box.Orientation.Backward));
Vector3 kDir = new Vector3(Vector3.Dot(line.Dir, box.Orientation.Right),
Vector3.Dot(line.Dir, box.Orientation.Up),
Vector3.Dot(line.Dir, box.Orientation.Backward));
// Apply reflections so that direction vector has nonnegative components.
bool reflect0 = false;
bool reflect1 = false;
bool reflect2 = false;
pfLParam = 0;
if (kDir.X < 0.0f)
{
pnt.X = -pnt.X;
kDir.X = -kDir.X;
reflect0 = true;
}
if (kDir.Y < 0.0f)
{
pnt.Y = -pnt.Y;
kDir.Y = -kDir.Y;
reflect1 = true;
}
if (kDir.Z < 0.0f)
{
pnt.Z = -pnt.Z;
kDir.Z = -kDir.Z;
reflect2 = true;
}
float sqrDistance = 0.0f;
if (kDir.X > 0.0f)
{
if (kDir.Y > 0.0f)
{
if (kDir.Z > 0.0f)
{
// (+,+,+)
Vector3 kPmE = pnt - box.GetHalfSideLengths();
float prodDxPy = kDir.X * kPmE.Y;
float prodDyPx = kDir.Y * kPmE.X;
float prodDzPx, prodDxPz, prodDzPy, prodDyPz;
if (prodDyPx >= prodDxPy)
{
prodDzPx = kDir.Z * kPmE.X;
prodDxPz = kDir.X * kPmE.Z;
if (prodDzPx >= prodDxPz)
{
//Face(0,1,2)
FaceA(ref pnt, kDir, box, kPmE, out pfLParam, ref sqrDistance);
}
else
{
//Face(2,0,1)
FaceB(ref pnt, kDir, box, kPmE, out pfLParam, ref sqrDistance);
}
}
else
{
prodDzPy = kDir.Z * kPmE.Y;
prodDyPz = kDir.Y * kPmE.Z;
if (prodDzPy >= prodDyPz)
{
//Face(1,2,0)
FaceC(ref pnt, kDir, box, kPmE, out pfLParam, ref sqrDistance);
}
else
{
//Face(2,0,1)
FaceB(ref pnt, kDir, box, kPmE, out pfLParam, ref sqrDistance);
}
}
}
else
{
// (+,+,0)
float pmE0 = pnt.X - box.GetHalfSideLengths().X;
float pmE1 = pnt.Y - box.GetHalfSideLengths().Y;
float prod0 = kDir.Y * pmE0;
float prod1 = kDir.X * pmE1;
float delta, invLSqur, inv;
if (prod0 >= prod1)
{
// line intersects P[i0] = e[i0]
pnt.X = box.GetHalfSideLengths().X;
float ppE1 = pnt.Y + box.GetHalfSideLengths().Y;
delta = prod0 - kDir.X * ppE1;
if (delta >= 0.0f)
{
invLSqur = 1.0f / (kDir.X * kDir.X + kDir.Y * kDir.Y);
sqrDistance += delta * delta * invLSqur;
pnt.Y = -box.GetHalfSideLengths().Y;
pfLParam = -(kDir.X * pmE0 + kDir.Y * ppE1) * invLSqur;
}
else
{
inv = 1.0f / kDir.X;
pnt.Y -= prod0 * inv;
pfLParam = -pmE0 * inv;
}
}
else
{
// line intersects P[i1] = e[i1]
pnt.Y = box.GetHalfSideLengths().Y;
float ppE0 = pnt.X + box.GetHalfSideLengths().X;
delta = prod1 - kDir.Y * ppE0;
if (delta >= 0.0f)
{
invLSqur = 1.0f / (kDir.X * kDir.X + kDir.Y * kDir.Y);
sqrDistance += delta * delta * invLSqur;
pnt.X = -box.GetHalfSideLengths().X;
pfLParam = -(kDir.X * ppE0 + kDir.Y * pmE1) * invLSqur;
}
else
{
inv = 1.0f / kDir.Y;
pnt.X -= prod1 * inv;
pfLParam = -pmE1 * inv;
}
}
if (pnt.Z < -box.GetHalfSideLengths().Z)
{
delta = pnt.Z + box.GetHalfSideLengths().Z;
sqrDistance += delta * delta;
pnt.Z = -box.GetHalfSideLengths().Z;
}
else if (pnt.Z > box.GetHalfSideLengths().Z)
{
delta = pnt.Z - box.GetHalfSideLengths().Z;
sqrDistance += delta * delta;
pnt.Z = box.GetHalfSideLengths().Z;
}
}
}
else
{
if (kDir.Z > 0.0f)
{
// (+,0,+)
float pmE0 = pnt.X - box.GetHalfSideLengths().X;
float pmE1 = pnt.Z - box.GetHalfSideLengths().Z;
float prod0 = kDir.Z * pmE0;
float prod1 = kDir.X * pmE1;
float delta, invLSqur, inv;
if (prod0 >= prod1)
{
// line intersects P[i0] = e[i0]
pnt.X = box.GetHalfSideLengths().X;
float ppE1 = pnt.Z + box.GetHalfSideLengths().Z;
delta = prod0 - kDir.X * ppE1;
if (delta >= 0.0f)
{
invLSqur = 1.0f / (kDir.X * kDir.X + kDir.Z * kDir.Z);
sqrDistance += delta * delta * invLSqur;
pnt.Z = -box.GetHalfSideLengths().Z;
pfLParam = -(kDir.X * pmE0 + kDir.Z * ppE1) * invLSqur;
}
else
{
inv = 1.0f / kDir.X;
pnt.Z -= prod0 * inv;
pfLParam = -pmE0 * inv;
}
}
else
{
// line intersects P[i1] = e[i1]
pnt.Z = box.GetHalfSideLengths().Z;
float ppE0 = pnt.X + box.GetHalfSideLengths().X;
delta = prod1 - kDir.Z * ppE0;
if (delta >= 0.0f)
{
invLSqur = 1.0f / (kDir.X * kDir.X + kDir.Z * kDir.Z);
sqrDistance += delta * delta * invLSqur;
pnt.X = -box.GetHalfSideLengths().X;
pfLParam = -(kDir.X * ppE0 + kDir.Z * pmE1) * invLSqur;
}
else
{
inv = 1.0f / kDir.Z;
pnt.X -= prod1 * inv;
pfLParam = -pmE1 * inv;
}
}
if (pnt.Y < -box.GetHalfSideLengths().Y)
{
delta = pnt.Y + box.GetHalfSideLengths().Y;
sqrDistance += delta * delta;
pnt.Y = -box.GetHalfSideLengths().Y;
}
else if (pnt.Y > box.GetHalfSideLengths().Y)
{
delta = pnt.Y - box.GetHalfSideLengths().Y;
sqrDistance += delta * delta;
pnt.Y = box.GetHalfSideLengths().Y;
}
}
else
{
// (+,0,0)
float pmE0 = pnt.X - box.GetHalfSideLengths().X;
float pmE1 = pnt.Y - box.GetHalfSideLengths().Y;
float prod0 = kDir.Y * pmE0;
float prod1 = kDir.X * pmE1;
float delta, invLSqur, inv;
if (prod0 >= prod1)
{
// line intersects P[i0] = e[i0]
pnt.X = box.GetHalfSideLengths().X;
float ppE1 = pnt.Y + box.GetHalfSideLengths().Y;
delta = prod0 - kDir.X * ppE1;
if (delta >= 0.0f)
{
invLSqur = 1.0f / (kDir.X * kDir.X + kDir.Y * kDir.Y);
sqrDistance += delta * delta * invLSqur;
pnt.Y = -box.GetHalfSideLengths().Y;
pfLParam = -(kDir.X * pmE0 + kDir.Y * ppE1) * invLSqur;
}
else
{
inv = 1.0f / kDir.X;
pnt.Y -= prod0 * inv;
pfLParam = -pmE0 * inv;
}
}
else
{
// line intersects P[i1] = e[i1]
pnt.Y = box.GetHalfSideLengths().Y;
float ppE0 = pnt.X + box.GetHalfSideLengths().X;
delta = prod1 - kDir.Y * ppE0;
if (delta >= 0.0f)
{
invLSqur = 1.0f / (kDir.X * kDir.X + kDir.Y * kDir.Y);
sqrDistance += delta * delta * invLSqur;
pnt.X = -box.GetHalfSideLengths().X;
pfLParam = -(kDir.X * ppE0 + kDir.Y * pmE1) * invLSqur;
}
else
{
inv = 1.0f / kDir.Y;
pnt.X -= prod1 * inv;
pfLParam = -pmE1 * inv;
}
}
if (pnt.Z < -box.GetHalfSideLengths().Z)
{
delta = pnt.Z + box.GetHalfSideLengths().Z;
sqrDistance += delta * delta;
pnt.Z = -box.GetHalfSideLengths().Z;
}
else if (pnt.Z > box.GetHalfSideLengths().Z)
{
delta = pnt.Z - box.GetHalfSideLengths().Z;
sqrDistance += delta * delta;
pnt.Z = box.GetHalfSideLengths().Z;
}
}
}
}
else
{
if (kDir.Y > 0.0f)
{
if (kDir.Z > 0.0f)
{
float pmE0 = pnt.Y - box.GetHalfSideLengths().Y;
float pmE1 = pnt.Z - box.GetHalfSideLengths().Z;
float prod0 = kDir.Z * pmE0;
float prod1 = kDir.Y * pmE1;
float delta, invLSqur, inv;
if (prod0 >= prod1)
{
// line intersects P[i0] = e[i0]
pnt.Y = box.GetHalfSideLengths().Y;
float ppE1 = pnt.Z + box.GetHalfSideLengths().Z;
delta = prod0 - kDir.Y * ppE1;
if (delta >= 0.0f)
{
invLSqur = 1.0f / (kDir.Y * kDir.Y + kDir.Z * kDir.Z);
sqrDistance += delta * delta * invLSqur;
pnt.Z = -box.GetHalfSideLengths().Z;
pfLParam = -(kDir.Y * pmE0 + kDir.Z * ppE1) * invLSqur;
}
else
{
inv = 1.0f / kDir.Y;
pnt.Z -= prod0 * inv;
pfLParam = -pmE0 * inv;
}
}
else
{
// line intersects P[i1] = e[i1]
pnt.Z = box.GetHalfSideLengths().Z;
float ppE0 = pnt.Y + box.GetHalfSideLengths().Y;
delta = prod1 - kDir.Z * ppE0;
if (delta >= 0.0f)
{
invLSqur = 1.0f / (kDir.Y * kDir.Y + kDir.Z * kDir.Z);
sqrDistance += delta * delta * invLSqur;
pnt.Y = -box.GetHalfSideLengths().Y;
pfLParam = -(kDir.Y * ppE0 + kDir.Z * pmE1) * invLSqur;
}
else
{
inv = 1.0f / kDir.Z;
pnt.Y -= prod1 * inv;
pfLParam = -pmE1 * inv;
}
}
if (pnt.X < -box.GetHalfSideLengths().X)
{
delta = pnt.X + box.GetHalfSideLengths().X;
sqrDistance += delta * delta;
pnt.X = -box.GetHalfSideLengths().X;
}
else if (pnt.X > box.GetHalfSideLengths().X)
{
delta = pnt.X - box.GetHalfSideLengths().X;
sqrDistance += delta * delta;
pnt.X = box.GetHalfSideLengths().X;
}
}
else
{
// (0,+,0)
float delta;
pfLParam = (box.GetHalfSideLengths().Y - pnt.Y) / kDir.Y;
pnt.Y = box.GetHalfSideLengths().Y;
if (pnt.X < -box.GetHalfSideLengths().X)
{
delta = pnt.X + box.GetHalfSideLengths().X;
sqrDistance += delta * delta;
pnt.X = -box.GetHalfSideLengths().X;
}
else if (pnt.X > box.GetHalfSideLengths().X)
{
delta = pnt.X - box.GetHalfSideLengths().X;
sqrDistance += delta * delta;
pnt.X = box.GetHalfSideLengths().X;
}
if (pnt.Z < -box.GetHalfSideLengths().Z)
{
delta = pnt.Z + box.GetHalfSideLengths().Z;
sqrDistance += delta * delta;
pnt.Z = -box.GetHalfSideLengths().Z;
}
else if (pnt.Z > box.GetHalfSideLengths().Z)
{
delta = pnt.Z - box.GetHalfSideLengths().Z;
sqrDistance += delta * delta;
pnt.Z = box.GetHalfSideLengths().Z;
}
}
}
else
{
if (kDir.Z > 0.0f)
{
float delta;
pfLParam = (box.GetHalfSideLengths().Z - pnt.Z) / kDir.Z;
pnt.Z = box.GetHalfSideLengths().Z;
if (pnt.X < -box.GetHalfSideLengths().X)
{
delta = pnt.X + box.GetHalfSideLengths().X;
sqrDistance += delta * delta;
pnt.X = -box.GetHalfSideLengths().X;
}
else if (pnt.X > box.GetHalfSideLengths().X)
{
delta = pnt.X - box.GetHalfSideLengths().X;
sqrDistance += delta * delta;
pnt.X = box.GetHalfSideLengths().X;
}
if (pnt.Y < -box.GetHalfSideLengths().Y)
{
delta = pnt.Y + box.GetHalfSideLengths().Y;
sqrDistance += delta * delta;
pnt.Y = -box.GetHalfSideLengths().Y;
}
else if (pnt.Y > box.GetHalfSideLengths().Y)
{
delta = pnt.Y - box.GetHalfSideLengths().Y;
sqrDistance += delta * delta;
pnt.Y = box.GetHalfSideLengths().Y;
}
}
else
{
// (0,0,0)
float delta;
if (pnt.X < -box.GetHalfSideLengths().X)
{
delta = pnt.X + box.GetHalfSideLengths().X;
sqrDistance += delta * delta;
pnt.X = -box.GetHalfSideLengths().X;
}
else if (pnt.X > box.GetHalfSideLengths().X)
{
delta = pnt.X - box.GetHalfSideLengths().X;
sqrDistance += delta * delta;
pnt.X = box.GetHalfSideLengths().X;
}
if (pnt.Y < -box.GetHalfSideLengths().Y)
{
delta = pnt.Y + box.GetHalfSideLengths().Y;
sqrDistance += delta * delta;
pnt.Y = -box.GetHalfSideLengths().Y;
}
else if (pnt.Y > box.GetHalfSideLengths().Y)
{
delta = pnt.Y - box.GetHalfSideLengths().Y;
sqrDistance += delta * delta;
pnt.Y = box.GetHalfSideLengths().Y;
}
if (pnt.Z < -box.GetHalfSideLengths().Z)
{
delta = pnt.Z + box.GetHalfSideLengths().Z;
sqrDistance += delta * delta;
pnt.Z = -box.GetHalfSideLengths().Z;
}
else if (pnt.Z > box.GetHalfSideLengths().Z)
{
delta = pnt.Z - box.GetHalfSideLengths().Z;
sqrDistance += delta * delta;
pnt.Z = box.GetHalfSideLengths().Z;
}
}
}
}
// undo reflections
if (reflect0) pnt.X = -pnt.X;
if (reflect1) pnt.Y = -pnt.Y;
if (reflect2) pnt.Z = -pnt.Z;
pfBParam0 = pnt.X;
pfBParam1 = pnt.Y;
pfBParam2 = pnt.Z;
return MathHelper.Max(sqrDistance, 0.0f);
}
/// <summary>
/// SqrDistance
/// </summary>
/// <param name="point"></param>
/// <param name="box"></param>
/// <param name="pfBParam0"></param>
/// <param name="pfBParam1"></param>
/// <param name="pfBParam2"></param>
/// <returns>float</returns>
public static float SqrDistance(Vector3 point, Box box,
out float pfBParam0, out float pfBParam1, out float pfBParam2)
{
// compute coordinates of point in box coordinate system
Vector3 kDiff = point - box.GetCentre();
Vector3 kClosest = new Vector3(Vector3.Dot(kDiff, box.Orientation.Right),
Vector3.Dot(kDiff, box.Orientation.Up),
Vector3.Dot(kDiff, box.Orientation.Backward));
// project test point onto box
float fSqrDistance = 0.0f;
float fDelta;
if (kClosest.X < -box.GetHalfSideLengths().X)
{
fDelta = kClosest.X + box.GetHalfSideLengths().X;
fSqrDistance += fDelta * fDelta;
kClosest.X = -box.GetHalfSideLengths().X;
}
else if (kClosest.X > box.GetHalfSideLengths().X)
{
fDelta = kClosest.X - box.GetHalfSideLengths().X;
fSqrDistance += fDelta * fDelta;
kClosest.X = box.GetHalfSideLengths().X;
}
if (kClosest.Y < -box.GetHalfSideLengths().Y)
{
fDelta = kClosest.Y + box.GetHalfSideLengths().Y;
fSqrDistance += fDelta * fDelta;
kClosest.Y = -box.GetHalfSideLengths().Y;
}
else if (kClosest.Y > box.GetHalfSideLengths().Y)
{
fDelta = kClosest.Y - box.GetHalfSideLengths().Y;
fSqrDistance += fDelta * fDelta;
kClosest.Y = box.GetHalfSideLengths().Y;
}
if (kClosest.Z < -box.GetHalfSideLengths().Z)
{
fDelta = kClosest.Z + box.GetHalfSideLengths().Z;
fSqrDistance += fDelta * fDelta;
kClosest.Z = -box.GetHalfSideLengths().Z;
}
else if (kClosest.Z > box.GetHalfSideLengths().Z)
{
fDelta = kClosest.Z - box.GetHalfSideLengths().Z;
fSqrDistance += fDelta * fDelta;
kClosest.Z = box.GetHalfSideLengths().Z;
}
pfBParam0 = kClosest.X;
pfBParam1 = kClosest.Y;
pfBParam2 = kClosest.Z;
return MathHelper.Max(fSqrDistance, 0.0f);
}
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.Transform(ref triVec0, ref transformMatrix, out triVec0);
Vector3.Transform(ref triVec1, ref transformMatrix, out triVec1);
Vector3.Transform(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;
axes[1] = dirs0.Right;
axes[2] = dirs0.Up;
axes[3] = dirs0.Backward;
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(ref delta, ref N, out oldDepth);
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;
}
}
}
}
/// <summary>
/// GetSupportPoint
/// </summary>
/// <param name="p"></param>
/// <param name="box"></param>
/// <param name="axis"></param>
private static void GetSupportPoint(out Vector3 p, Box box,Vector3 axis)
{
// BEN-OPTIMISATION: Replaced following inlines with actual inlined code.
#region INLINE: Vector3 orient0 = box.Orientation.Right;
Vector3 orient0 = new Vector3();
orient0.X = box.transform.Orientation.M11;
orient0.Y = box.transform.Orientation.M12;
orient0.Z = box.transform.Orientation.M13;
#endregion
#region INLINE: Vector3 orient1 = box.Orientation.Up;
Vector3 orient1 = new Vector3();
orient1.X = box.transform.Orientation.M21;
orient1.Y = box.transform.Orientation.M22;
orient1.Z = box.transform.Orientation.M23;
#endregion
#region INLINE: Vector3 orient2 = box.Orientation.Backward;
Vector3 orient2 = new Vector3();
orient2.X = box.transform.Orientation.M31;
orient2.Y = box.transform.Orientation.M32;
orient2.Z = 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
}
}
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
}
}