/// <summary>
/// Constructs a new demo.
/// </summary>
/// <param name="game">Game owning this demo.</param>
public MPRTestDemo(DemosGame game)
: base(game)
{
var shapeA = new BoxShape(1, 1, 1);
shapeA.CollisionMargin = 0;
var shapeB = new BoxShape(1, 1, 1);
shapeB.CollisionMargin = 0;
var transformA = new RigidTransform(new Vector3(0, 0, 0));
var transformB = new RigidTransform(new Vector3(.5m, .5m, 0));
Vector3 overlap;
bool overlapped = MPRToolbox.GetLocalOverlapPosition(shapeA, shapeB, ref transformB, out overlap);
Vector3 normal;
Fix64 depth;
Vector3 direction = new Vector3(0, -1, 0);
MPRToolbox.LocalSurfaceCast(shapeA, shapeB, ref transformB, ref direction, out depth, out normal);
ContactData contactData;
//bool overlappedOld = MPRToolboxOld.AreObjectsColliding(shapeA, shapeB, ref transformA, ref transformB, out contactData);
//Random rand = new Random(0);
//for (int i = 0; i < 10000000; i++)
//{
// transformA = new RigidTransform(new Vector3((Fix64)rand.NextDouble() * 10 - 5, (Fix64)rand.NextDouble() * 10 - 5, (Fix64)rand.NextDouble() * 10 - 5),
// Quaternion.CreateFromYawPitchRoll((Fix64)rand.NextDouble() * 1000, (Fix64)rand.NextDouble() * 1000, (Fix64)rand.NextDouble() * 1000));
// transformB = new RigidTransform(new Vector3((Fix64)rand.NextDouble() * 10 - 5, (Fix64)rand.NextDouble() * 10 - 5, (Fix64)rand.NextDouble() * 10 - 5),
// Quaternion.CreateFromYawPitchRoll((Fix64)rand.NextDouble() * 1000, (Fix64)rand.NextDouble() * 1000, (Fix64)rand.NextDouble() * 1000));
// overlapped = MPRTesting.GetOverlapPosition(shapeA, shapeB, ref transformA, ref transformB, out overlap);
// overlappedOld = MPRToolbox.AreObjectsColliding(shapeA, shapeB, ref transformA, ref transformB, out contactData);
// if (overlapped && !overlappedOld &&
// (!MPRToolbox.IsPointInsideShape(ref overlap, shapeA, ref transformA) ||
// !MPRToolbox.IsPointInsideShape(ref overlap, shapeB, ref transformB)))
// Debug.WriteLine("Break.");
// if (overlappedOld && !overlapped &&
// (!MPRToolbox.IsPointInsideShape(ref contactData.Position, shapeA, ref transformA) ||
// !MPRToolbox.IsPointInsideShape(ref contactData.Position, shapeB, ref transformB)))
// Debug.WriteLine("Break.");
// if (overlapped && overlappedOld &&
// (!MPRToolbox.IsPointInsideShape(ref overlap, shapeA, ref transformA) ||
// !MPRToolbox.IsPointInsideShape(ref overlap, shapeB, ref transformB) ||
// !MPRToolbox.IsPointInsideShape(ref contactData.Position, shapeA, ref transformA) ||
// !MPRToolbox.IsPointInsideShape(ref contactData.Position, shapeB, ref transformB)))
// Debug.WriteLine("Break.");
//}
//Do these tests with rotationally immobile objects.
CollisionDetectionSettings.DefaultMargin = 0;
groundWidth = 10;
groundHeight = .1m;
groundLength = 10;
//a = new Box(new Vector3(0, -5, 0), groundWidth, groundHeight, groundLength, 1);
//a = new TransformableEntity(new Vector3(0,0,0), new TriangleShape(new Vector3(-5, -5, -5), new Vector3(5, -5, -5), new Vector3(-5, -5, 5)), Matrix3x3.Identity);
a = new Triangle(new Vector3(0, -5, 0), new Vector3(5, -5, 0), new Vector3(5, -5, 5), 1);
Space.Add(a);
Space.ForceUpdater.Gravity = new Vector3();
boxWidth = .25m;
boxHeight = .05m;
boxLength = 1;
b = new TransformableEntity(new Vector3(0, 2, 0), new BoxShape(boxWidth, boxHeight, boxLength), Matrix3x3.Identity, 1);
//b = new Cone(new Vector3(0, 2, 0), .2m, .1m, 1);
//b = new Capsule(new Vector3(0, 2, 0), 1, .5m, 1);
//b = new Capsule(new Vector3(0, 2, 0), 1, .5m, 1);
b.LocalInertiaTensorInverse = new Matrix3x3();
CollisionRules.AddRule(b, a, CollisionRule.NoSolver);
b.ActivityInformation.IsAlwaysActive = true;
Space.Add(b);
//Space.Add(new TransformableEntity(new Vector3(0, 4, 0), new BoxShape(1, 1, 1), Matrix3x3.Identity, 1));
//Space.Add( new TransformableEntity(new Vector3(0, 6, 0), new BoxShape(1, 1, 1), Matrix3x3.Identity, 1));
//Vector3[] vertices = new Vector3[] { new Vector3(0, -5, 0), new Vector3(5, -5, 0), new Vector3(5, -5, 5), new Vector3(0, -60, 5) };
//int[] indices = new int[] { 0, 1, 2 , 0, 2, 3 };
//StaticMesh mesh = new StaticMesh(vertices, indices);
//Space.Add(mesh);
//mesh.ImproveBoundaryBehavior = true;
//mesh.Sidedness = TriangleSidedness.Counterclockwise;
//game.ModelDrawer.Add(mesh);
//mesh.CollisionRules.Personal = CollisionRule.NoSolver;
}
public override void Update(Fix64 dt)
{
if (Game.KeyboardInput.IsKeyDown(Keys.Left))
{
rayCastDirection = Matrix3x3.Transform(rayCastDirection, Matrix3x3.CreateFromAxisAngle(Vector3.Forward, .01m));
}
if (Game.KeyboardInput.IsKeyDown(Keys.Right))
{
rayCastDirection = Matrix3x3.Transform(rayCastDirection, Matrix3x3.CreateFromAxisAngle(Vector3.Forward, -.01m));
}
if (Game.KeyboardInput.IsKeyDown(Keys.Down))
{
rayCastDirection = Matrix3x3.Transform(rayCastDirection, Matrix3x3.CreateFromAxisAngle(Vector3.Right, .01m));
}
if (Game.KeyboardInput.IsKeyDown(Keys.Up))
{
rayCastDirection = Matrix3x3.Transform(rayCastDirection, Matrix3x3.CreateFromAxisAngle(Vector3.Right, -.01m));
}
if (Game.KeyboardInput.IsKeyDown(Keys.P))
{
Debug.WriteLine("Break.");
}
base.Update(dt);
RigidTransform localTransformB;
RigidTransform aTransform = a.CollisionInformation.WorldTransform, bTransform = b.CollisionInformation.WorldTransform;
MinkowskiToolbox.GetLocalTransform(ref aTransform, ref bTransform, out localTransformB);
Vector3 position;
if (MPRToolbox.GetLocalOverlapPosition((a.CollisionInformation.Shape as ConvexShape), (b.CollisionInformation.Shape as ConvexShape), ref localTransformB, out position))
{
//Vector3 rayCastDirection = new Vector3(1,0,0);// (Vector3.Normalize(localDirection) + Vector3.Normalize(collidableB.worldTransform.Position - collidableA.worldTransform.Position)) / 2;
Fix64 previousT;
Vector3 previousNormal;
Fix64 t;
Vector3 normal;
rayCastDirection = localTransformB.Position;
MPRToolbox.LocalSurfaceCast((a.CollisionInformation.Shape as ConvexShape), (b.CollisionInformation.Shape as ConvexShape), ref localTransformB, ref rayCastDirection, out previousT, out previousNormal);
//Vector3 secondDirection = Vector3.Cross(rayCastDirection, Vector3.Up);
//MPRTesting.LocalSurfaceCast((a.CollisionInformation.Shape as ConvexShape), (b.CollisionInformation.Shape as ConvexShape), ref localTransformB, ref secondDirection, out t, out normal);
//if (t < previousT)
//{
// previousNormal = normal;
// previousT = t;
//}
//Vector3 thirdDirection = Vector3.Cross(secondDirection, rayCastDirection);
//MPRTesting.LocalSurfaceCast((a.CollisionInformation.Shape as ConvexShape), (b.CollisionInformation.Shape as ConvexShape), ref localTransformB, ref thirdDirection, out t, out normal);
//if (t < previousT)
//{
// previousNormal = normal;
// previousT = t;
//}
//Vector3 fourthDirection = -secondDirection;
//MPRTesting.LocalSurfaceCast((a.CollisionInformation.Shape as ConvexShape), (b.CollisionInformation.Shape as ConvexShape), ref localTransformB, ref fourthDirection, out t, out normal);
//if (t < previousT)
//{
// previousNormal = normal;
// previousT = t;
//}
//Vector3 fifthDirection = -thirdDirection;
//MPRTesting.LocalSurfaceCast((a.CollisionInformation.Shape as ConvexShape), (b.CollisionInformation.Shape as ConvexShape), ref localTransformB, ref fifthDirection, out t, out normal);
//if (t < previousT)
//{
// previousNormal = normal;
// previousT = t;
//}
//Correct the penetration depth.
MPRToolbox.LocalSurfaceCast((a.CollisionInformation.Shape as ConvexShape), (b.CollisionInformation.Shape as ConvexShape), ref localTransformB, ref previousNormal, out t, out normal);
contactDepth = t;
contactNormal = previousNormal;
////Converge to local minimum.
//while (true)
//{
// MPRTesting.LocalSurfaceCast((a.CollisionInformation.Shape as ConvexShape), (b.CollisionInformation.Shape as ConvexShape), ref localTransformB, ref previousNormal, out t, out normal);
// if (previousT - t <= Toolbox.BigEpsilon)
// break;
// previousT = t;
// previousNormal = normal;
//}
}
#region Box Box minkowski sum
////Construct explicit minkowski sum.
//Vector3[] aLines = new Vector3[8];
//aLines[0] = new Vector3(-boxWidth / 2, -boxHeight / 2, -boxLength / 2);
//aLines[1] = new Vector3(-boxWidth / 2, -boxHeight / 2, boxLength / 2);
//aLines[2] = new Vector3(-boxWidth / 2, boxHeight / 2, -boxLength / 2);
//aLines[3] = new Vector3(-boxWidth / 2, boxHeight / 2, boxLength / 2);
//aLines[4] = new Vector3(boxWidth / 2, -boxHeight / 2, -boxLength / 2);
//aLines[5] = new Vector3(boxWidth / 2, -boxHeight / 2, boxLength / 2);
//aLines[6] = new Vector3(boxWidth / 2, boxHeight / 2, -boxLength / 2);
//aLines[7] = new Vector3(boxWidth / 2, boxHeight / 2, boxLength / 2);
//Vector3[] bLines = new Vector3[8];
//bLines[0] = new Vector3(-groundWidth / 2, -groundHeight / 2, -groundLength / 2);
//bLines[1] = new Vector3(-groundWidth / 2, -groundHeight / 2, groundLength / 2);
//bLines[2] = new Vector3(-groundWidth / 2, groundHeight / 2, -groundLength / 2);
//bLines[3] = new Vector3(-groundWidth / 2, groundHeight / 2, groundLength / 2);
//bLines[4] = new Vector3(groundWidth / 2, -groundHeight / 2, -groundLength / 2);
//bLines[5] = new Vector3(groundWidth / 2, -groundHeight / 2, groundLength / 2);
//bLines[6] = new Vector3(groundWidth / 2, groundHeight / 2, -groundLength / 2);
//bLines[7] = new Vector3(groundWidth / 2, groundHeight / 2, groundLength / 2);
//for (int i = 0; i < 8; i++)
// aLines[i] = Vector3.Transform(aLines[i], localTransformB.Matrix);
//List<Vector3> vertices = new List<Vector3>();
//for (int i = 0; i < 8; i++)
//{
// for (int j = 0; j < 8; j++)
// {
// if (b.CollisionInformation.Pairs.Count > 0)
// {
// if (b.CollisionInformation.Pairs[0].BroadPhaseOverlap.EntryA == b.CollisionInformation)
// vertices.Add(aLines[i] - bLines[j]);
// else
// vertices.Add(bLines[i] - aLines[j]);
// }
// else
// {
// vertices.Add(bLines[i] - aLines[j]);
// }
// }
//}
//var indices = new List<int>();
//Toolbox.GetConvexHull(vertices, indices);
#endregion
#region Arbitrary minkowski sum
var vertices = new List <Vector3>();
Vector3 max;
var direction = new Vector3();
int NumSamples = 16;
Fix64 angleChange = MathHelper.TwoPi / NumSamples;
for (int i = 1; i < NumSamples / 2 - 1; i++)
{
Fix64 phi = MathHelper.PiOver2 - i * angleChange;
var sinPhi = Fix64.Sin(phi);
var cosPhi = Fix64.Cos(phi);
for (int j = 0; j < NumSamples; j++)
{
Fix64 theta = j * angleChange;
direction.X = Fix64.Cos(theta) * cosPhi;
direction.Y = sinPhi;
direction.Z = Fix64.Sin(theta) * cosPhi;
MinkowskiToolbox.GetLocalMinkowskiExtremePoint(a.CollisionInformation.Shape as ConvexShape, b.CollisionInformation.Shape as ConvexShape, ref direction, ref localTransformB, out max);
vertices.Add(max);
}
}
MinkowskiToolbox.GetLocalMinkowskiExtremePoint(a.CollisionInformation.Shape as ConvexShape, b.CollisionInformation.Shape as ConvexShape, ref Toolbox.UpVector, ref localTransformB, out max);
vertices.Add(max);
MinkowskiToolbox.GetLocalMinkowskiExtremePoint(a.CollisionInformation.Shape as ConvexShape, b.CollisionInformation.Shape as ConvexShape, ref Toolbox.DownVector, ref localTransformB, out max);
vertices.Add(max);
var indices = new List <int>();
ConvexHullHelper.GetConvexHull(vertices, indices);
#endregion
minkowskiLines.Clear();
for (int i = 0; i < indices.Count; i += 3)
{
minkowskiLines.Add(new VertexPositionColor(MathConverter.Convert(vertices[indices[i]]), Microsoft.Xna.Framework.Color.Blue));
minkowskiLines.Add(new VertexPositionColor(MathConverter.Convert(vertices[indices[i + 1]]), Microsoft.Xna.Framework.Color.Blue));
minkowskiLines.Add(new VertexPositionColor(MathConverter.Convert(vertices[indices[i + 1]]), Microsoft.Xna.Framework.Color.Blue));
minkowskiLines.Add(new VertexPositionColor(MathConverter.Convert(vertices[indices[i + 2]]), Microsoft.Xna.Framework.Color.Blue));
minkowskiLines.Add(new VertexPositionColor(MathConverter.Convert(vertices[indices[i + 2]]), Microsoft.Xna.Framework.Color.Blue));
minkowskiLines.Add(new VertexPositionColor(MathConverter.Convert(vertices[indices[i]]), Microsoft.Xna.Framework.Color.Blue));
}
}
private bool DoDeepContact(out TinyStructList <ContactData> contactList)
{
//Find the origin to triangle center offset.
Vector3 center;
Vector3.Add(ref triangle.vA, ref triangle.vB, out center);
Vector3.Add(ref center, ref triangle.vC, out center);
Vector3.Multiply(ref center, 1f / 3f, out center);
ContactData contact;
contactList = new TinyStructList <ContactData>();
if (MPRToolbox.AreLocalShapesOverlapping(convex, triangle, ref center, ref Toolbox.RigidIdentity))
{
float dot;
Vector3 triangleNormal, ab, ac;
Vector3.Subtract(ref triangle.vB, ref triangle.vA, out ab);
Vector3.Subtract(ref triangle.vC, ref triangle.vA, out ac);
Vector3.Cross(ref ab, ref ac, out triangleNormal);
float lengthSquared = triangleNormal.LengthSquared();
if (lengthSquared < Toolbox.Epsilon * .01f)
{
//Degenerate triangle! That's no good.
//Just use the direction pointing from A to B, "B" being the triangle. That direction is center - origin, or just center.
MPRToolbox.LocalSurfaceCast(convex, triangle, ref Toolbox.RigidIdentity, ref center, out contact.PenetrationDepth, out contact.Normal, out contact.Position);
}
else
{
//Normalize the normal.
Vector3.Divide(ref triangleNormal, (float)Math.Sqrt(lengthSquared), out triangleNormal);
////The first direction to check is one of the triangle's edge normals. Choose the one that is most aligned with the offset from A to B.
////Project the direction onto the triangle plane.
//Vector3.Dot(ref triangleNormal, ref center, out dot);
//Vector3 trianglePlaneDirection;
//Vector3.Multiply(ref triangleNormal, dot, out trianglePlaneDirection);
//Vector3.Subtract(ref trianglePlaneDirection, ref center, out trianglePlaneDirection);
////To find out which edge to use, compute which region the direction is in.
////This is done by constructing three planes which segment the triangle into three sub-triangles.
////These planes are defined by A, origin, center; B, origin, center; C, origin, center.
////The plane tests against the direction can be reordered to:
////(center x direction) * A
////(center x direction) * B
////(center x direction) * C
//Vector3 OxD;
//Vector3.Cross(ref trianglePlaneDirection, ref center, out OxD);
//Vector3 p;
//float dotA, dotB, dotC;
//Vector3.Dot(ref triangle.vA, ref OxD, out dotA);
//Vector3.Dot(ref triangle.vB, ref OxD, out dotB);
//Vector3.Dot(ref triangle.vC, ref OxD, out dotC);
//if (dotA >= 0 && dotB <= 0)
//{
// //Direction is in the AB edge zone.
// //Compute the edge normal using AB x (AO x AB).
// Vector3 AB, AO;
// Vector3.Subtract(ref triangle.vB, ref triangle.vA, out AB);
// Vector3.Subtract(ref center, ref triangle.vA, out AO);
// Vector3.Cross(ref AO, ref AB, out p);
// Vector3.Cross(ref AB, ref p, out trianglePlaneDirection);
//}
//else if (dotB >= 0 && dotC <= 0)
//{
// //Direction is in the BC edge zone.
// //Compute the edge normal using BC x (BO x BC).
// Vector3 BC, BO;
// Vector3.Subtract(ref triangle.vC, ref triangle.vB, out BC);
// Vector3.Subtract(ref center, ref triangle.vB, out BO);
// Vector3.Cross(ref BO, ref BC, out p);
// Vector3.Cross(ref BC, ref p, out trianglePlaneDirection);
//}
//else // dotC > 0 && dotA < 0
//{
// //Direction is in the CA edge zone.
// //Compute the edge normal using CA x (CO x CA).
// Vector3 CA, CO;
// Vector3.Subtract(ref triangle.vA, ref triangle.vC, out CA);
// Vector3.Subtract(ref center, ref triangle.vC, out CO);
// Vector3.Cross(ref CO, ref CA, out p);
// Vector3.Cross(ref CA, ref p, out trianglePlaneDirection);
//}
//dot = trianglePlaneDirection.LengthSquared();
//if (dot > Toolbox.Epsilon)
//{
// Vector3.Divide(ref trianglePlaneDirection, (float)Math.Sqrt(dot), out trianglePlaneDirection);
// MPRTesting.LocalSurfaceCast(convex, triangle, ref Toolbox.RigidIdentity, ref trianglePlaneDirection, out contact.PenetrationDepth, out contact.Normal);
// //Check to see if the normal is facing in the proper direction, considering that this may not be a two-sided triangle.
// Vector3.Dot(ref triangleNormal, ref contact.Normal, out dot);
// if ((triangle.sidedness == TriangleSidedness.Clockwise && dot > 0) || (triangle.sidedness == TriangleSidedness.Counterclockwise && dot < 0))
// {
// //Normal was facing the wrong way.
// //Instead of ignoring it entirely, correct the direction to as close as it can get by removing any component parallel to the triangle normal.
// Vector3 previousNormal = contact.Normal;
// Vector3.Dot(ref contact.Normal, ref triangleNormal, out dot);
// Vector3.Multiply(ref contact.Normal, dot, out p);
// Vector3.Subtract(ref contact.Normal, ref p, out contact.Normal);
// float length = contact.Normal.LengthSquared();
// if (length > Toolbox.Epsilon)
// {
// //Renormalize the corrected normal.
// Vector3.Divide(ref contact.Normal, (float)Math.Sqrt(length), out contact.Normal);
// Vector3.Dot(ref contact.Normal, ref previousNormal, out dot);
// contact.PenetrationDepth *= dot;
// }
// else
// {
// contact.PenetrationDepth = float.MaxValue;
// contact.Normal = new Vector3();
// }
// }
//}
//else
//{
// contact.PenetrationDepth = float.MaxValue;
// contact.Normal = new Vector3();
//}
//TODO: This tests all three edge axes with a full MPR raycast. That's not really necessary; the correct edge normal should be discoverable, resulting in a single MPR raycast.
//Find the edge directions that will be tested with MPR.
Vector3 AO, BO, CO;
Vector3 AB, BC, CA;
Vector3.Subtract(ref center, ref triangle.vA, out AO);
Vector3.Subtract(ref center, ref triangle.vB, out BO);
Vector3.Subtract(ref center, ref triangle.vC, out CO);
Vector3.Subtract(ref triangle.vB, ref triangle.vA, out AB);
Vector3.Subtract(ref triangle.vC, ref triangle.vB, out BC);
Vector3.Subtract(ref triangle.vA, ref triangle.vC, out CA);
//We don't have to worry about degenerate triangles here because we've already handled that possibility above.
Vector3 ABnormal, BCnormal, CAnormal;
//Project the center onto the edge to find the direction from the center to the edge AB.
Vector3.Dot(ref AO, ref AB, out dot);
Vector3.Multiply(ref AB, dot / AB.LengthSquared(), out ABnormal);
Vector3.Subtract(ref AO, ref ABnormal, out ABnormal);
ABnormal.Normalize();
//Project the center onto the edge to find the direction from the center to the edge BC.
Vector3.Dot(ref BO, ref BC, out dot);
Vector3.Multiply(ref BC, dot / BC.LengthSquared(), out BCnormal);
Vector3.Subtract(ref BO, ref BCnormal, out BCnormal);
BCnormal.Normalize();
//Project the center onto the edge to find the direction from the center to the edge BC.
Vector3.Dot(ref CO, ref CA, out dot);
Vector3.Multiply(ref CA, dot / CA.LengthSquared(), out CAnormal);
Vector3.Subtract(ref CO, ref CAnormal, out CAnormal);
CAnormal.Normalize();
MPRToolbox.LocalSurfaceCast(convex, triangle, ref Toolbox.RigidIdentity, ref ABnormal, out contact.PenetrationDepth, out contact.Normal);
//Check to see if the normal is facing in the proper direction, considering that this may not be a two-sided triangle.
Vector3.Dot(ref triangleNormal, ref contact.Normal, out dot);
if ((triangle.sidedness == TriangleSidedness.Clockwise && dot > 0) || (triangle.sidedness == TriangleSidedness.Counterclockwise && dot < 0))
{
//Normal was facing the wrong way.
//Instead of ignoring it entirely, correct the direction to as close as it can get by removing any component parallel to the triangle normal.
Vector3 previousNormal = contact.Normal;
Vector3.Dot(ref contact.Normal, ref triangleNormal, out dot);
Vector3 p;
Vector3.Multiply(ref contact.Normal, dot, out p);
Vector3.Subtract(ref contact.Normal, ref p, out contact.Normal);
float length = contact.Normal.LengthSquared();
if (length > Toolbox.Epsilon)
{
//Renormalize the corrected normal.
Vector3.Divide(ref contact.Normal, (float)Math.Sqrt(length), out contact.Normal);
Vector3.Dot(ref contact.Normal, ref previousNormal, out dot);
contact.PenetrationDepth *= dot;
}
else
{
contact.PenetrationDepth = float.MaxValue;
contact.Normal = new Vector3();
}
}
Vector3 candidateNormal;
float candidateDepth;
MPRToolbox.LocalSurfaceCast(convex, triangle, ref Toolbox.RigidIdentity, ref BCnormal, out candidateDepth, out candidateNormal);
//Check to see if the normal is facing in the proper direction, considering that this may not be a two-sided triangle.
Vector3.Dot(ref triangleNormal, ref candidateNormal, out dot);
if ((triangle.sidedness == TriangleSidedness.Clockwise && dot > 0) || (triangle.sidedness == TriangleSidedness.Counterclockwise && dot < 0))
{
//Normal was facing the wrong way.
//Instead of ignoring it entirely, correct the direction to as close as it can get by removing any component parallel to the triangle normal.
Vector3 previousNormal = candidateNormal;
Vector3.Dot(ref candidateNormal, ref triangleNormal, out dot);
Vector3 p;
Vector3.Multiply(ref candidateNormal, dot, out p);
Vector3.Subtract(ref candidateNormal, ref p, out candidateNormal);
float length = candidateNormal.LengthSquared();
if (length > Toolbox.Epsilon)
{
//Renormalize the corrected normal.
Vector3.Divide(ref candidateNormal, (float)Math.Sqrt(length), out candidateNormal);
Vector3.Dot(ref candidateNormal, ref previousNormal, out dot);
candidateDepth *= dot;
}
else
{
contact.PenetrationDepth = float.MaxValue;
contact.Normal = new Vector3();
}
}
if (candidateDepth < contact.PenetrationDepth)
{
contact.Normal = candidateNormal;
contact.PenetrationDepth = candidateDepth;
}
MPRToolbox.LocalSurfaceCast(convex, triangle, ref Toolbox.RigidIdentity, ref CAnormal, out candidateDepth, out candidateNormal);
//Check to see if the normal is facing in the proper direction, considering that this may not be a two-sided triangle.
Vector3.Dot(ref triangleNormal, ref candidateNormal, out dot);
if ((triangle.sidedness == TriangleSidedness.Clockwise && dot > 0) || (triangle.sidedness == TriangleSidedness.Counterclockwise && dot < 0))
{
//Normal was facing the wrong way.
//Instead of ignoring it entirely, correct the direction to as close as it can get by removing any component parallel to the triangle normal.
Vector3 previousNormal = candidateNormal;
Vector3.Dot(ref candidateNormal, ref triangleNormal, out dot);
Vector3 p;
Vector3.Multiply(ref candidateNormal, dot, out p);
Vector3.Subtract(ref candidateNormal, ref p, out candidateNormal);
float length = candidateNormal.LengthSquared();
if (length > Toolbox.Epsilon)
{
//Renormalize the corrected normal.
Vector3.Divide(ref candidateNormal, (float)Math.Sqrt(length), out candidateNormal);
Vector3.Dot(ref candidateNormal, ref previousNormal, out dot);
candidateDepth *= dot;
}
else
{
contact.PenetrationDepth = float.MaxValue;
contact.Normal = new Vector3();
}
}
if (candidateDepth < contact.PenetrationDepth)
{
contact.Normal = candidateNormal;
contact.PenetrationDepth = candidateDepth;
}
//Try the depth along the positive triangle normal.
//If it's clockwise, this direction is unnecessary (the resulting normal would be invalidated by the onesidedness of the triangle).
if (triangle.sidedness != TriangleSidedness.Clockwise)
{
MPRToolbox.LocalSurfaceCast(convex, triangle, ref Toolbox.RigidIdentity, ref triangleNormal, out candidateDepth, out candidateNormal);
if (candidateDepth < contact.PenetrationDepth)
{
contact.Normal = candidateNormal;
contact.PenetrationDepth = candidateDepth;
}
}
//Try the depth along the negative triangle normal.
//If it's counterclockwise, this direction is unnecessary (the resulting normal would be invalidated by the onesidedness of the triangle).
if (triangle.sidedness != TriangleSidedness.Counterclockwise)
{
Vector3.Negate(ref triangleNormal, out triangleNormal);
MPRToolbox.LocalSurfaceCast(convex, triangle, ref Toolbox.RigidIdentity, ref triangleNormal, out candidateDepth, out candidateNormal);
if (candidateDepth < contact.PenetrationDepth)
{
contact.Normal = candidateNormal;
contact.PenetrationDepth = candidateDepth;
}
}
}
MPRToolbox.RefinePenetration(convex, triangle, ref Toolbox.RigidIdentity, contact.PenetrationDepth, ref contact.Normal, out contact.PenetrationDepth, out contact.Normal, out contact.Position);
//It's possible for the normal to still face the 'wrong' direction according to one sided triangles.
if (triangle.sidedness != TriangleSidedness.DoubleSided)
{
Vector3.Dot(ref triangleNormal, ref contact.Normal, out dot);
if (dot < 0)
{
return(false);
}
}
////The local casting can optionally continue. Eventually, it will converge to the local minimum.
//int optimizingCount = 0;
//while (true)
//{
// MPRTesting.LocalSurfaceCast(convex, triangle, ref Toolbox.RigidIdentity, ref contact.Normal, out candidateDepth, out candidateNormal);
// if (contact.PenetrationDepth - candidateDepth <= Toolbox.BigEpsilon ||
// ++optimizingCount < 4)
// {
// //If we've reached the end due to convergence, the normal will be extremely close to correct (if not 100% correct).
// //The candidateDepth computed is the previous contact normal's depth.
// //The reason why the previous normal is kept is that the last raycast computed the depth for that normal, not the new normal.
// contact.PenetrationDepth = candidateDepth;
// break;
// }
// contact.PenetrationDepth = candidateDepth;
// contact.Normal = candidateNormal;
//}
//Correct the penetration depth.
//MPRTesting.LocalSurfaceCast(convex, triangle, ref Toolbox.RigidIdentity, ref contact.Normal, out contact.PenetrationDepth, out center); //Center is just a trash variable now.
contact.Id = -1;
if (contact.PenetrationDepth < convex.collisionMargin + triangle.collisionMargin)
{
state = CollisionState.ExternalNear; //If it's emerged from the deep contact, we can go back to using the preferred GJK method.
}
contactList.Add(ref contact);
}
if (TryInnerSphereContact(out contact))
{
contactList.Add(ref contact);
}
if (contactList.count > 0)
{
return(true);
}
state = CollisionState.ExternalSeparated;
return(false);
}
private bool DoDeepContact(TriangleShape triangle, out TinyStructList <ContactData> contactList)
{
//Find the origin to triangle center offset.
Vector3 center;
Vector3.Add(ref triangle.vA, ref triangle.vB, out center);
Vector3.Add(ref center, ref triangle.vC, out center);
Vector3.Multiply(ref center, 1f / 3f, out center);
ContactData contact;
contactList = new TinyStructList <ContactData>();
if (MPRToolbox.AreLocalShapesOverlapping(convex, triangle, ref center, ref Toolbox.RigidIdentity))
{
float dot;
Vector3 triangleNormal, ab, ac;
Vector3.Subtract(ref triangle.vB, ref triangle.vA, out ab);
Vector3.Subtract(ref triangle.vC, ref triangle.vA, out ac);
Vector3.Cross(ref ab, ref ac, out triangleNormal);
float lengthSquared = triangleNormal.LengthSquared();
if (lengthSquared < Toolbox.Epsilon * .01f)
{
//Degenerate triangle! That's no good.
//Just use the direction pointing from A to B, "B" being the triangle. That direction is center - origin, or just center.
MPRToolbox.LocalSurfaceCast(convex, triangle, ref Toolbox.RigidIdentity, ref center,
out contact.PenetrationDepth, out contact.Normal, out contact.Position);
}
else
{
//Normalize the normal.
Vector3.Divide(ref triangleNormal, (float)Math.Sqrt(lengthSquared), out triangleNormal);
//TODO: This tests all three edge axes with a full MPR raycast. That's not really necessary; the correct edge normal should be discoverable, resulting in a single MPR raycast.
//Find the edge directions that will be tested with MPR.
Vector3 AO, BO, CO;
Vector3 AB, BC, CA;
Vector3.Subtract(ref center, ref triangle.vA, out AO);
Vector3.Subtract(ref center, ref triangle.vB, out BO);
Vector3.Subtract(ref center, ref triangle.vC, out CO);
Vector3.Subtract(ref triangle.vB, ref triangle.vA, out AB);
Vector3.Subtract(ref triangle.vC, ref triangle.vB, out BC);
Vector3.Subtract(ref triangle.vA, ref triangle.vC, out CA);
//We don't have to worry about degenerate triangles here because we've already handled that possibility above.
Vector3 ABnormal, BCnormal, CAnormal;
//Project the center onto the edge to find the direction from the center to the edge AB.
Vector3.Dot(ref AO, ref AB, out dot);
Vector3.Multiply(ref AB, dot / AB.LengthSquared(), out ABnormal);
Vector3.Subtract(ref AO, ref ABnormal, out ABnormal);
ABnormal.Normalize();
//Project the center onto the edge to find the direction from the center to the edge BC.
Vector3.Dot(ref BO, ref BC, out dot);
Vector3.Multiply(ref BC, dot / BC.LengthSquared(), out BCnormal);
Vector3.Subtract(ref BO, ref BCnormal, out BCnormal);
BCnormal.Normalize();
//Project the center onto the edge to find the direction from the center to the edge BC.
Vector3.Dot(ref CO, ref CA, out dot);
Vector3.Multiply(ref CA, dot / CA.LengthSquared(), out CAnormal);
Vector3.Subtract(ref CO, ref CAnormal, out CAnormal);
CAnormal.Normalize();
MPRToolbox.LocalSurfaceCast(convex, triangle, ref Toolbox.RigidIdentity, ref ABnormal,
out contact.PenetrationDepth, out contact.Normal);
//Check to see if the normal is facing in the proper direction, considering that this may not be a two-sided triangle.
Vector3.Dot(ref triangleNormal, ref contact.Normal, out dot);
if (triangle.sidedness == TriangleSidedness.Clockwise && dot > 0 ||
triangle.sidedness == TriangleSidedness.Counterclockwise && dot < 0)
{
//Normal was facing the wrong way.
//Instead of ignoring it entirely, correct the direction to as close as it can get by removing any component parallel to the triangle normal.
Vector3 previousNormal = contact.Normal;
Vector3.Dot(ref contact.Normal, ref triangleNormal, out dot);
Vector3 p;
Vector3.Multiply(ref contact.Normal, dot, out p);
Vector3.Subtract(ref contact.Normal, ref p, out contact.Normal);
float length = contact.Normal.LengthSquared();
if (length > Toolbox.Epsilon)
{
//Renormalize the corrected normal.
Vector3.Divide(ref contact.Normal, (float)Math.Sqrt(length), out contact.Normal);
Vector3.Dot(ref contact.Normal, ref previousNormal, out dot);
contact.PenetrationDepth *= dot;
}
else
{
contact.PenetrationDepth = float.MaxValue;
contact.Normal = new Vector3();
}
}
Vector3 candidateNormal;
float candidateDepth;
MPRToolbox.LocalSurfaceCast(convex, triangle, ref Toolbox.RigidIdentity, ref BCnormal,
out candidateDepth, out candidateNormal);
//Check to see if the normal is facing in the proper direction, considering that this may not be a two-sided triangle.
Vector3.Dot(ref triangleNormal, ref candidateNormal, out dot);
if (triangle.sidedness == TriangleSidedness.Clockwise && dot > 0 ||
triangle.sidedness == TriangleSidedness.Counterclockwise && dot < 0)
{
//Normal was facing the wrong way.
//Instead of ignoring it entirely, correct the direction to as close as it can get by removing any component parallel to the triangle normal.
Vector3 previousNormal = candidateNormal;
Vector3.Dot(ref candidateNormal, ref triangleNormal, out dot);
Vector3 p;
Vector3.Multiply(ref candidateNormal, dot, out p);
Vector3.Subtract(ref candidateNormal, ref p, out candidateNormal);
float length = candidateNormal.LengthSquared();
if (length > Toolbox.Epsilon)
{
//Renormalize the corrected normal.
Vector3.Divide(ref candidateNormal, (float)Math.Sqrt(length), out candidateNormal);
Vector3.Dot(ref candidateNormal, ref previousNormal, out dot);
candidateDepth *= dot;
}
else
{
contact.PenetrationDepth = float.MaxValue;
contact.Normal = new Vector3();
}
}
if (candidateDepth < contact.PenetrationDepth)
{
contact.Normal = candidateNormal;
contact.PenetrationDepth = candidateDepth;
}
MPRToolbox.LocalSurfaceCast(convex, triangle, ref Toolbox.RigidIdentity, ref CAnormal,
out candidateDepth, out candidateNormal);
//Check to see if the normal is facing in the proper direction, considering that this may not be a two-sided triangle.
Vector3.Dot(ref triangleNormal, ref candidateNormal, out dot);
if (triangle.sidedness == TriangleSidedness.Clockwise && dot > 0 ||
triangle.sidedness == TriangleSidedness.Counterclockwise && dot < 0)
{
//Normal was facing the wrong way.
//Instead of ignoring it entirely, correct the direction to as close as it can get by removing any component parallel to the triangle normal.
Vector3 previousNormal = candidateNormal;
Vector3.Dot(ref candidateNormal, ref triangleNormal, out dot);
Vector3 p;
Vector3.Multiply(ref candidateNormal, dot, out p);
Vector3.Subtract(ref candidateNormal, ref p, out candidateNormal);
float length = candidateNormal.LengthSquared();
if (length > Toolbox.Epsilon)
{
//Renormalize the corrected normal.
Vector3.Divide(ref candidateNormal, (float)Math.Sqrt(length), out candidateNormal);
Vector3.Dot(ref candidateNormal, ref previousNormal, out dot);
candidateDepth *= dot;
}
else
{
contact.PenetrationDepth = float.MaxValue;
contact.Normal = new Vector3();
}
}
if (candidateDepth < contact.PenetrationDepth)
{
contact.Normal = candidateNormal;
contact.PenetrationDepth = candidateDepth;
}
//Try the depth along the positive triangle normal.
//If it's clockwise, this direction is unnecessary (the resulting normal would be invalidated by the onesidedness of the triangle).
if (triangle.sidedness != TriangleSidedness.Clockwise)
{
MPRToolbox.LocalSurfaceCast(convex, triangle, ref Toolbox.RigidIdentity, ref triangleNormal,
out candidateDepth, out candidateNormal);
if (candidateDepth < contact.PenetrationDepth)
{
contact.Normal = candidateNormal;
contact.PenetrationDepth = candidateDepth;
}
}
//Try the depth along the negative triangle normal.
//If it's counterclockwise, this direction is unnecessary (the resulting normal would be invalidated by the onesidedness of the triangle).
if (triangle.sidedness != TriangleSidedness.Counterclockwise)
{
Vector3.Negate(ref triangleNormal, out triangleNormal);
MPRToolbox.LocalSurfaceCast(convex, triangle, ref Toolbox.RigidIdentity, ref triangleNormal,
out candidateDepth, out candidateNormal);
if (candidateDepth < contact.PenetrationDepth)
{
contact.Normal = candidateNormal;
contact.PenetrationDepth = candidateDepth;
}
}
}
MPRToolbox.RefinePenetration(convex, triangle, ref Toolbox.RigidIdentity, contact.PenetrationDepth,
ref contact.Normal, out contact.PenetrationDepth, out contact.Normal, out contact.Position);
//It's possible for the normal to still face the 'wrong' direction according to one sided triangles.
if (triangle.sidedness != TriangleSidedness.DoubleSided)
{
Vector3.Dot(ref triangleNormal, ref contact.Normal, out dot);
if (dot < 0)
//Skip the add process.
{
goto InnerSphere;
}
}
contact.Id = -1;
if (contact.PenetrationDepth < convex.collisionMargin + triangle.collisionMargin)
{
state = CollisionState
.ExternalNear; //If it's emerged from the deep contact, we can go back to using the preferred GJK method.
}
contactList.Add(ref contact);
}
InnerSphere:
if (TryInnerSphereContact(triangle, out contact))
{
contactList.Add(ref contact);
}
if (contactList.Count > 0)
{
return(true);
}
state = CollisionState.ExternalSeparated;
return(false);
}
private bool TryInnerSphereContact(out ContactData contact)
{
Vector3 closestPoint;
Toolbox.GetClosestPointOnTriangleToPoint(ref triangle.vA, ref triangle.vB, ref triangle.vC, ref Toolbox.ZeroVector, out closestPoint);
float length = closestPoint.LengthSquared();
float minimumRadius = convex.minimumRadius * (MotionSettings.CoreShapeScaling + .01f);
if (length < minimumRadius * minimumRadius)
{
Vector3 triangleNormal, ab, ac;
Vector3.Subtract(ref triangle.vB, ref triangle.vA, out ab);
Vector3.Subtract(ref triangle.vC, ref triangle.vA, out ac);
Vector3.Cross(ref ab, ref ac, out triangleNormal);
float dot;
Vector3.Dot(ref closestPoint, ref triangleNormal, out dot);
if ((triangle.sidedness == TriangleSidedness.Clockwise && dot > 0) || (triangle.sidedness == TriangleSidedness.Counterclockwise && dot < 0))
{
//Normal was facing the wrong way.
contact = new ContactData();
return(false);
}
length = (float)Math.Sqrt(length);
contact.Position = closestPoint;
if (length > Toolbox.Epsilon) //Watch out for NaN's!
{
Vector3.Divide(ref closestPoint, length, out contact.Normal);
}
else
{
//The direction is undefined. Use the triangle's normal.
//One sided triangles can only face in the appropriate direction.
float normalLength = triangleNormal.LengthSquared();
if (triangleNormal.LengthSquared() > Toolbox.Epsilon)
{
Vector3.Divide(ref triangleNormal, (float)Math.Sqrt(normalLength), out triangleNormal);
if (triangle.sidedness == TriangleSidedness.Clockwise)
{
contact.Normal = triangleNormal;
}
else
{
Vector3.Negate(ref triangleNormal, out contact.Normal);
}
}
else
{
//Degenerate triangle!
contact = new ContactData();
return(false);
}
}
//Compute the actual depth of the contact.
MPRToolbox.LocalSurfaceCast(convex, triangle, ref Toolbox.RigidIdentity, ref contact.Normal, out contact.PenetrationDepth, out triangleNormal); //Trash the 'corrected' normal. We want to use the spherical normal.
contact.Id = -1;
return(true);
}
contact = new ContactData();
return(false);
}