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);
}
public static void Test()
{
var f0 = BuildHull();
f0.CollisionMargin = 0;
//Generate spheres all around the central froxel in such a way that we know that they're not colliding.
var froxelSphereSurface = new BoundingBox(new Vector3(-1.51f, -1.51f, -1.51f), new Vector3(1.51f, 1.51f, 1.51f));
int testIterations = 1000;
int innerIterations = 1000;
Random random = new Random(5);
long sphereFroxelSeparatedTicks = 0;
SphereShape sphere = new SphereShape(1);
for (int i = 0; i < testIterations; ++i)
{
var ray = GetRandomRay(ref froxelSphereSurface, random);
float t;
ray.Intersects(ref froxelSphereSurface, out t);
var sphereTransform = new RigidTransform {
Position = ray.Position + ray.Direction * t, Orientation = Quaternion.Identity
};
var start = Stopwatch.GetTimestamp();
for (int j = 0; j < innerIterations; ++j)
{
if (MPRToolbox.AreLocalShapesOverlapping(f0, sphere, ref sphereTransform))
{
Trace.Fail("By construction there can be no intersection!");
}
}
var end = Stopwatch.GetTimestamp();
sphereFroxelSeparatedTicks += (end - start);
}
Console.WriteLine($"Sphere-froxel separated: {(1e6 * sphereFroxelSeparatedTicks) / (testIterations * innerIterations * Stopwatch.Frequency)}");
//Do the same kind of test, but now with intersection.
froxelSphereSurface = new BoundingBox(new Vector3(-0.5f, -0.5f, -0.5f), new Vector3(0.5f, 0.5f, 0.5f));
long sphereFroxelIntersectingTicks = 0;
for (int i = 0; i < testIterations; ++i)
{
var ray = GetRandomRay(ref froxelSphereSurface, random);
float t;
ray.Intersects(ref froxelSphereSurface, out t);
var sphereTransform = new RigidTransform {
Position = ray.Position + ray.Direction * (t - 0.99f), Orientation = Quaternion.Identity
};
var start = Stopwatch.GetTimestamp();
for (int j = 0; j < innerIterations; ++j)
{
if (!MPRToolbox.AreLocalShapesOverlapping(f0, sphere, ref sphereTransform))
{
Trace.Fail("By construction there can be no separation!");
}
}
var end = Stopwatch.GetTimestamp();
sphereFroxelIntersectingTicks += (end - start);
}
Console.WriteLine($"Sphere-froxel intersecting: {(1e6 * sphereFroxelIntersectingTicks) / (testIterations * innerIterations * Stopwatch.Frequency)}");
//Create a surface for the rays to hit such that every query froxel will be just outside of the central froxel.
var froxelFroxelSurface = new BoundingBox(new Vector3(-1.01f, -1.01f, -1.01f), new Vector3(1.01f, 1.01f, 1.01f));
var queryHull = BuildHull();
queryHull.CollisionMargin = 0;
long froxelFroxelSeparatedTicks = 0;
for (int i = 0; i < testIterations; ++i)
{
var ray = GetRandomRay(ref froxelFroxelSurface, random);
float t;
ray.Intersects(ref froxelFroxelSurface, out t);
var queryTransform = new RigidTransform(ray.Position + ray.Direction * t);
var start = Stopwatch.GetTimestamp();
for (int j = 0; j < innerIterations; ++j)
{
if (MPRToolbox.AreLocalShapesOverlapping(f0, queryHull, ref queryTransform))
{
Trace.Fail("By construction there can be no intersection!");
}
}
var end = Stopwatch.GetTimestamp();
froxelFroxelSeparatedTicks += (end - start);
}
Console.WriteLine($"Froxel-froxel separated: {(1e6 * froxelFroxelSeparatedTicks) / (testIterations * innerIterations * Stopwatch.Frequency)}");
//Same thing as above, but now with slight intersection.
froxelFroxelSurface = new BoundingBox(new Vector3(-.99f, -.99f, -.99f), new Vector3(0.99f, 0.99f, 0.99f));
long froxelFroxelIntersectingTicks = 0;
for (int i = 0; i < testIterations; ++i)
{
var ray = GetRandomRay(ref froxelFroxelSurface, random);
float t;
ray.Intersects(ref froxelFroxelSurface, out t);
var queryTransform = new RigidTransform(ray.Position + ray.Direction * t);
var start = Stopwatch.GetTimestamp();
for (int j = 0; j < innerIterations; ++j)
{
if (!MPRToolbox.AreLocalShapesOverlapping(f0, queryHull, ref queryTransform))
{
Trace.Fail("By construction there can be no separation!");
}
}
var end = Stopwatch.GetTimestamp();
froxelFroxelIntersectingTicks += (end - start);
}
Console.WriteLine($"Froxel-froxel intersecting: {(1e6 * froxelFroxelIntersectingTicks) / (testIterations * innerIterations * Stopwatch.Frequency)}");
}