private bool DoOverlapBoxTriangleTest(MyBox box, ref MyColDetVoxelTriangle triangle)
{
Matrix dirs0 = box.Orientation;
Vector3 triEdge0;
Vector3 triEdge1;
Vector3 triEdge2;
triEdge0 = MyMwcUtils.Normalize(triangle.Edge0);
triEdge1 = MyMwcUtils.Normalize(triangle.Edge1);
triEdge2 = MyMwcUtils.Normalize(triangle.Edge2);
Vector3 triNormal = triangle.Plane.Normal;
// the 15 potential separating axes (comment by Marek Rosa: note says 15 but code uses 13... I don't know why, mistake in the note??)
const int NUM_AXES = 13;
MyVector3Array13 axes = new MyVector3Array13();
axes[0] = triNormal;
axes[1] = dirs0.Right;
axes[2] = dirs0.Up;
axes[3] = dirs0.Backward;
axes[4] = Vector3.Cross(axes[1], triEdge0);
axes[5] = Vector3.Cross(axes[1], triEdge1);
axes[6] = Vector3.Cross(axes[1], triEdge2);
axes[7] = Vector3.Cross(axes[2], triEdge0);
axes[8] = Vector3.Cross(axes[2], triEdge1);
axes[9] = Vector3.Cross(axes[2], triEdge2);
axes[10] = Vector3.Cross(axes[3], triEdge0);
axes[11] = Vector3.Cross(axes[3], triEdge1);
axes[12] = Vector3.Cross(axes[3], triEdge2);
// the overlap depths along each axis
MyFloatArray13 overlapDepths = new MyFloatArray13();
// 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 < NUM_AXES; ++i)
{
overlapDepths[i] = 1.0f;
bool b;
overlapDepths[i] = Disjoint(out b, axes[i], box, triangle, MyPhysics.physicsSystem.GetRigidBodyModule().CollisionEpsilon);
if (b)
{
return(false);
}
}
// The box overlap, find the separation depth closest to 0.
float minDepth = float.MaxValue;
int minAxis = -1;
for (i = 0; i < NUM_AXES; ++i)
{
// If we can't normalise the axis, skip it
float l2 = axes[i].LengthSquared();
if (l2 < MyPhysicsConfig.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 = box.GetCentre() - triangle.Centre;
Vector3 N = axes[minAxis];
float depth = overlapDepths[minAxis];
if (Vector3.Dot(D, N) < 0.0f)
{
N *= -1;
}
MyRigidBody rbo0 = GetRigidBody1();
MyRigidBody rbo1 = GetRigidBody2();
float dt = MyPhysics.physicsSystem.GetRigidBodyModule().CurrentTimeStep;
Vector3 boxOldPos = rbo0.Position;
Vector3 boxNewPos = rbo0.Position + rbo0.LinearVelocity * dt;
Vector3 meshPos = rbo1.Position;
m_CPList.Clear();
GetBoxTriangleIntersectionPoints(m_CPList, box, triangle, MyPhysicsConfig.CollisionEpsilon);
// adjust the depth
#region delta
Vector3 delta;
Vector3.Subtract(ref boxNewPos, ref boxOldPos, out delta);
#endregion
// report collisions
int numPts = m_CPList.Count;
MySmallCollPointInfo[] collPtArray = MyContactInfoCache.SCPIStackAlloc();
{
if (numPts > 0)
{
if (numPts >= MyPhysicsConfig.MaxContactPoints)
{
numPts = MyPhysicsConfig.MaxContactPoints - 1;
}
// adjust positions
for (i = 0; i < numPts; ++i)
{
collPtArray[i] = new MySmallCollPointInfo(m_CPList[i].m_Position - boxOldPos, m_CPList[i].m_Position - meshPos, GetRigidBody1().LinearVelocity, GetRigidBody2().LinearVelocity, m_CPList[i].m_Normal, m_CPList[i].m_Depth, m_CPList[i].m_Position);
}
MyPhysics.physicsSystem.GetContactConstraintModule().AddContactConstraint(this, collPtArray, numPts);
MyContactInfoCache.FreeStackAlloc(collPtArray);
return(true);
}
else
{
MyContactInfoCache.FreeStackAlloc(collPtArray);
return(false);
}
}
}
private bool DoOverlapBoxTriangleTest(MyBox oldBox, MyBox newBox, MyTriangle_Vertex_Normal triangle, MyPlane plane, float collTolerance,
ref Matrix transformMatrix, ref Vector3 oldBoxPos, ref Vector3 newBoxPos, List <MyCollisionPointStruct> collPoints)
{
Matrix dirs0 = newBox.Orientation;
dirs0.Translation = Vector3.Zero;
#region Triangle
Vector3 triVec0 = triangle.Vertexes.Vertex0;
Vector3 triVec1 = triangle.Vertexes.Vertex1;
Vector3 triVec2 = triangle.Vertexes.Vertex2;
//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);
MyTriangle tri = new MyTriangle(ref triVec0, ref triVec1, ref triVec2);
#endregion
#region triEdge0
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 triEdge1
Vector3 pt2;
tri.GetPoint(2, out pt2);
Vector3 triEdge1;
Vector3.Subtract(ref pt2, ref pt1, out triEdge1);
#endregion
#region triEdge2
Vector3 triEdge2;
Vector3.Subtract(ref pt0, ref pt2, out triEdge2);
#endregion
if (triEdge0.LengthSquared() < MyMwcMathConstants.EPSILON)
{
return(false);
}
if (triEdge1.LengthSquared() < MyMwcMathConstants.EPSILON)
{
return(false);
}
if (triEdge2.LengthSquared() < MyMwcMathConstants.EPSILON)
{
return(false);
}
/*
* triEdge0 = MyMwcUtils.Normalize(triEdge0);
* triEdge1 = MyMwcUtils.Normalize(triEdge1);
* triEdge2 = MyMwcUtils.Normalize(triEdge2);
*/
triEdge0.Normalize();
triEdge1.Normalize();
triEdge2.Normalize();
//Vector3 triNormal = triangle.Plane.Normal;
Vector3 triNormal = plane.Normal;
Vector3.TransformNormal(ref triNormal, ref transformMatrix, out triNormal);
// the 15 potential separating axes (comment by Marek Rosa: note says 15 but code uses 13... I don't know why, mistake in the note??)
const int NUM_AXES = 13;
MyVector3Array13 axes = new MyVector3Array13();
axes[0] = triNormal;
axes[1] = dirs0.Right;
axes[2] = dirs0.Up;
axes[3] = dirs0.Backward;
axes[4] = Vector3.Cross(axes[1], triEdge0);
axes[5] = Vector3.Cross(axes[1], triEdge1);
axes[6] = Vector3.Cross(axes[1], triEdge2);
axes[7] = Vector3.Cross(axes[2], triEdge0);
axes[8] = Vector3.Cross(axes[2], triEdge1);
axes[9] = Vector3.Cross(axes[2], triEdge2);
axes[10] = Vector3.Cross(axes[3], triEdge0);
axes[11] = Vector3.Cross(axes[3], triEdge1);
axes[12] = Vector3.Cross(axes[3], triEdge2);
// the overlap depths along each axis
MyFloatArray13 overlapDepths = new MyFloatArray13();
// 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 < NUM_AXES; ++i)
{
overlapDepths[i] = 1.0f;
bool b;
overlapDepths[i] = Disjoint(out b, axes[i], newBox, tri, collTolerance);
if (b)
{
return(false);
}
}
// The box overlap, find the separation depth closest to 0.
float minDepth = float.MaxValue;
int minAxis = -1;
for (i = 0; i < NUM_AXES; ++i)
{
// If we can't normalise the axis, skip it
float l2 = axes[i].LengthSquared();
if (l2 < MyPhysicsConfig.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 = oldBoxPos; //(info.Skin0.Owner != null) ? info.Skin0.Owner.OldPosition : Vector3.Zero;
Vector3 boxNewPos = newBoxPos; // (info.Skin0.Owner != null) ? info.Skin0.Owner.Position : Vector3.Zero;
Vector3 meshPos = transformMatrix.Translation; // (info.Skin1.Owner != null) ? info.Skin1.Owner.OldPosition : Vector3.Zero;
m_pts.Clear();
const float combinationDist = 0.05f;
GetBoxTriangleIntersectionPoints(m_pts, newBox, tri, depth + combinationDist);
// adjust the depth
#region delta
Vector3 delta;
Vector3.Subtract(ref boxNewPos, ref boxOldPos, out delta);
#endregion
#region oldDepth
float oldDepth;
Vector3.Dot(ref delta, ref N, out oldDepth);
oldDepth += depth;
#endregion
// report collisions
int numPts = m_pts.Count;
if (numPts > 0)
{
for (i = 0; i < numPts; ++i)
{
collPoints.Add(new MyCollisionPointStruct(-oldDepth, new MySmallCollPointInfo(m_pts[i] - boxNewPos, m_pts[i] - meshPos, GetRigidBody1().LinearVelocity, GetRigidBody2().LinearVelocity, N, -oldDepth, m_pts[i])));
}
return(true);
}
else
{
return(false);
}
}
