/// <summary>
/// Initializes a new instance of <see cref="Triangle"/> from a <see cref="TriangleShape"/>.
/// </summary>
/// <param name="triangleShape">
/// The <see cref="TriangleShape"/> from which vertices are copied.
/// </param>
/// <exception cref="ArgumentNullException">
/// <paramref name="triangleShape"/> is <see langword="null"/>.
/// </exception>
public Triangle(TriangleShape triangleShape)
{
if (triangleShape == null)
{
throw new ArgumentNullException("triangleShape");
}
Vertex0 = triangleShape.Vertex0;
Vertex1 = triangleShape.Vertex1;
Vertex2 = triangleShape.Vertex2;
}
// Creates a lot of random objects.
private void CreateRandomObjects()
{
var random = new Random();
var isFirstHeightField = true;
int currentShape = 0;
int numberOfObjects = 0;
while (true)
{
numberOfObjects++;
if (numberOfObjects > ObjectsPerType)
{
currentShape++;
numberOfObjects = 0;
}
Shape shape;
switch (currentShape)
{
case 0:
// Box
shape = new BoxShape(ObjectSize, ObjectSize * 2, ObjectSize * 3);
break;
case 1:
// Capsule
shape = new CapsuleShape(0.3f * ObjectSize, 2 * ObjectSize);
break;
case 2:
// Cone
shape = new ConeShape(1 * ObjectSize, 2 * ObjectSize);
break;
case 3:
// Cylinder
shape = new CylinderShape(0.4f * ObjectSize, 2 * ObjectSize);
break;
case 4:
// Sphere
shape = new SphereShape(ObjectSize);
break;
case 5:
// Convex hull of several points.
ConvexHullOfPoints hull = new ConvexHullOfPoints();
hull.Points.Add(new Vector3F(-1 * ObjectSize, -2 * ObjectSize, -1 * ObjectSize));
hull.Points.Add(new Vector3F(2 * ObjectSize, -1 * ObjectSize, -0.5f * ObjectSize));
hull.Points.Add(new Vector3F(1 * ObjectSize, 2 * ObjectSize, 1 * ObjectSize));
hull.Points.Add(new Vector3F(-1 * ObjectSize, 2 * ObjectSize, 1 * ObjectSize));
hull.Points.Add(new Vector3F(-1 * ObjectSize, 0.7f * ObjectSize, -0.6f * ObjectSize));
shape = hull;
break;
case 6:
// A composite shape: two boxes that form a "T" shape.
var composite = new CompositeShape();
composite.Children.Add(
new GeometricObject(
new BoxShape(ObjectSize, 3 * ObjectSize, ObjectSize),
new Pose(new Vector3F(0, 0, 0))));
composite.Children.Add(
new GeometricObject(
new BoxShape(2 * ObjectSize, ObjectSize, ObjectSize),
new Pose(new Vector3F(0, 2 * ObjectSize, 0))));
shape = composite;
break;
case 7:
shape = new CircleShape(ObjectSize);
break;
case 8:
{
var compBvh = new CompositeShape();
compBvh.Children.Add(new GeometricObject(new BoxShape(0.5f, 1, 0.5f), new Pose(new Vector3F(0, 0.5f, 0), Matrix33F.Identity)));
compBvh.Children.Add(new GeometricObject(new BoxShape(0.8f, 0.5f, 0.5f), new Pose(new Vector3F(0.5f, 0.7f, 0), Matrix33F.CreateRotationZ(-MathHelper.ToRadians(15)))));
compBvh.Children.Add(new GeometricObject(new SphereShape(0.3f), new Pose(new Vector3F(0, 1.15f, 0), Matrix33F.Identity)));
compBvh.Children.Add(new GeometricObject(new CapsuleShape(0.2f, 1), new Pose(new Vector3F(0.6f, 1.15f, 0), Matrix33F.CreateRotationX(0.3f))));
compBvh.Partition = new AabbTree<int>();
shape = compBvh;
break;
}
case 9:
CompositeShape comp = new CompositeShape();
comp.Children.Add(new GeometricObject(new BoxShape(0.5f * ObjectSize, 1 * ObjectSize, 0.5f * ObjectSize), new Pose(new Vector3F(0, 0.5f * ObjectSize, 0), QuaternionF.Identity)));
comp.Children.Add(new GeometricObject(new BoxShape(0.8f * ObjectSize, 0.5f * ObjectSize, 0.5f * ObjectSize), new Pose(new Vector3F(0.3f * ObjectSize, 0.7f * ObjectSize, 0), QuaternionF.CreateRotationZ(-MathHelper.ToRadians(45)))));
comp.Children.Add(new GeometricObject(new SphereShape(0.3f * ObjectSize), new Pose(new Vector3F(0, 1.15f * ObjectSize, 0), QuaternionF.Identity)));
shape = comp;
break;
case 10:
shape = new ConvexHullOfPoints(new[]
{
new Vector3F(-1 * ObjectSize, -2 * ObjectSize, -1 * ObjectSize),
new Vector3F(2 * ObjectSize, -1 * ObjectSize, -0.5f * ObjectSize),
new Vector3F(1 * ObjectSize, 2 * ObjectSize, 1 * ObjectSize),
new Vector3F(-1 * ObjectSize, 2 * ObjectSize, 1 * ObjectSize),
new Vector3F(-1 * ObjectSize, 0.7f * ObjectSize, -0.6f * ObjectSize)
});
break;
case 11:
ConvexHullOfShapes shapeHull = new ConvexHullOfShapes();
shapeHull.Children.Add(new GeometricObject(new SphereShape(0.3f * ObjectSize), new Pose(new Vector3F(0, 2 * ObjectSize, 0), Matrix33F.Identity)));
shapeHull.Children.Add(new GeometricObject(new BoxShape(1 * ObjectSize, 2 * ObjectSize, 3 * ObjectSize), Pose.Identity));
shape = shapeHull;
break;
case 12:
shape = Shape.Empty;
break;
case 13:
var numberOfSamplesX = 10;
var numberOfSamplesZ = 10;
var samples = new float[numberOfSamplesX * numberOfSamplesZ];
for (int z = 0; z < numberOfSamplesZ; z++)
for (int x = 0; x < numberOfSamplesX; x++)
samples[z * numberOfSamplesX + x] = (float)(Math.Cos(z / 3f) * Math.Sin(x / 2f) * BoxSize / 6);
HeightField heightField = new HeightField(0, 0, 2 * BoxSize, 2 * BoxSize, samples, numberOfSamplesX, numberOfSamplesZ);
shape = heightField;
break;
//case 14:
//shape = new LineShape(new Vector3F(0.1f, 0.2f, 0.3f), new Vector3F(0.1f, 0.2f, -0.3f).Normalized);
//break;
case 15:
shape = new LineSegmentShape(
new Vector3F(0.1f, 0.2f, 0.3f), new Vector3F(0.1f, 0.2f, 0.3f) + 3 * ObjectSize * new Vector3F(0.1f, 0.2f, -0.3f));
break;
case 16:
shape = new MinkowskiDifferenceShape
{
ObjectA = new GeometricObject(new SphereShape(0.1f * ObjectSize)),
ObjectB = new GeometricObject(new BoxShape(1 * ObjectSize, 2 * ObjectSize, 3 * ObjectSize))
};
break;
case 17:
shape = new MinkowskiSumShape
{
ObjectA = new GeometricObject(new SphereShape(0.1f * ObjectSize)),
ObjectB = new GeometricObject(new BoxShape(1 * ObjectSize, 2 * ObjectSize, 3 * ObjectSize)),
};
break;
case 18:
shape = new OrthographicViewVolume(0, ObjectSize, 0, ObjectSize, ObjectSize / 2, ObjectSize * 2);
break;
case 19:
shape = new PerspectiveViewVolume(MathHelper.ToRadians(60f), 16f / 10, ObjectSize / 2, ObjectSize * 3);
break;
case 20:
shape = new PointShape(0.1f, 0.3f, 0.2f);
break;
case 21:
shape = new RayShape(new Vector3F(0.2f, 0, -0.12f), new Vector3F(1, 2, 3).Normalized, ObjectSize * 2);
break;
case 22:
shape = new RayShape(new Vector3F(0.2f, 0, -0.12f), new Vector3F(1, 2, 3).Normalized, ObjectSize * 2)
{
StopsAtFirstHit = true
};
break;
case 23:
shape = new RectangleShape(ObjectSize, ObjectSize * 2);
break;
case 24:
shape = new TransformedShape(
new GeometricObject(
new BoxShape(1 * ObjectSize, 2 * ObjectSize, 3 * ObjectSize),
new Pose(new Vector3F(0.1f, 1, -0.2f))));
break;
case 25:
shape = new TriangleShape(
new Vector3F(ObjectSize, 0, 0), new Vector3F(0, ObjectSize, 0), new Vector3F(ObjectSize, ObjectSize, ObjectSize));
break;
//case 26:
// {
// // Create a composite object from which we get the mesh.
// CompositeShape compBvh = new CompositeShape();
// compBvh.Children.Add(new GeometricObject(new BoxShape(0.5f, 1, 0.5f), new Pose(new Vector3F(0, 0.5f, 0), Matrix33F.Identity)));
// compBvh.Children.Add(
// new GeometricObject(
// new BoxShape(0.8f, 0.5f, 0.5f),
// new Pose(new Vector3F(0.5f, 0.7f, 0), Matrix33F.CreateRotationZ(-(float)MathHelper.ToRadians(15)))));
// compBvh.Children.Add(new GeometricObject(new SphereShape(0.3f), new Pose(new Vector3F(0, 1.15f, 0), Matrix33F.Identity)));
// compBvh.Children.Add(
// new GeometricObject(new CapsuleShape(0.2f, 1), new Pose(new Vector3F(0.6f, 1.15f, 0), Matrix33F.CreateRotationX(0.3f))));
// TriangleMeshShape meshBvhShape = new TriangleMeshShape { Mesh = compBvh.GetMesh(0.01f, 3) };
// meshBvhShape.Partition = new AabbTree<int>();
// shape = meshBvhShape;
// break;
// }
//case 27:
// {
// // Create a composite object from which we get the mesh.
// CompositeShape compBvh = new CompositeShape();
// compBvh.Children.Add(new GeometricObject(new BoxShape(0.5f, 1, 0.5f), new Pose(new Vector3F(0, 0.5f, 0), QuaternionF.Identity)));
// compBvh.Children.Add(
// new GeometricObject(
// new BoxShape(0.8f, 0.5f, 0.5f),
// new Pose(new Vector3F(0.5f, 0.7f, 0), QuaternionF.CreateRotationZ(-(float)MathHelper.ToRadians(15)))));
// compBvh.Children.Add(new GeometricObject(new SphereShape(0.3f), new Pose(new Vector3F(0, 1.15f, 0), QuaternionF.Identity)));
// compBvh.Children.Add(
// new GeometricObject(new CapsuleShape(0.2f, 1), new Pose(new Vector3F(0.6f, 1.15f, 0), QuaternionF.CreateRotationX(0.3f))));
// TriangleMeshShape meshBvhShape = new TriangleMeshShape { Mesh = compBvh.GetMesh(0.01f, 3) };
// meshBvhShape.Partition = new AabbTree<int>();
// shape = meshBvhShape;
// break;
// }
case 28:
shape = new ConvexPolyhedron(new[]
{
new Vector3F(-1 * ObjectSize, -2 * ObjectSize, -1 * ObjectSize),
new Vector3F(2 * ObjectSize, -1 * ObjectSize, -0.5f * ObjectSize),
new Vector3F(1 * ObjectSize, 2 * ObjectSize, 1 * ObjectSize),
new Vector3F(-1 * ObjectSize, 2 * ObjectSize, 1 * ObjectSize),
new Vector3F(-1 * ObjectSize, 0.7f * ObjectSize, -0.6f * ObjectSize)
});
break;
case 29:
return;
default:
currentShape++;
continue;
}
// Create an object with the random shape, pose, color and velocity.
Pose randomPose = new Pose(
random.NextVector3F(-BoxSize + ObjectSize * 2, BoxSize - ObjectSize * 2),
random.NextQuaternionF());
var newObject = new MovingGeometricObject
{
Pose = randomPose,
Shape = shape,
LinearVelocity = random.NextQuaternionF().Rotate(new Vector3F(MaxLinearVelocity, 0, 0)),
AngularVelocity = random.NextQuaternionF().Rotate(Vector3F.Forward)
* RandomHelper.Random.NextFloat(0, MaxAngularVelocity),
};
if (RandomHelper.Random.NextBool())
newObject.LinearVelocity = Vector3F.Zero;
if (RandomHelper.Random.NextBool())
newObject.AngularVelocity = Vector3F.Zero;
if (shape is LineShape || shape is HeightField)
{
// Do not move lines or the height field.
newObject.LinearVelocity = Vector3F.Zero;
newObject.AngularVelocity = Vector3F.Zero;
}
// Create only 1 heightField!
if (shape is HeightField)
{
if (isFirstHeightField)
{
isFirstHeightField = true;
newObject.Pose = new Pose(new Vector3F(-BoxSize, -BoxSize, -BoxSize));
}
else
{
currentShape++;
numberOfObjects = 0;
continue;
}
}
// Add collision object to collision domain.
_domain.CollisionObjects.Add(new CollisionObject(newObject));
//co.Type = CollisionObjectType.Trigger;
//co.Name = "Object" + shape.GetType().Name + "_" + i;
}
}
// The parameters 'testXxx' are initialized objects which are re-used to avoid a lot of GC garbage.
private void AddTriangleTriangleContacts(
ContactSet contactSet, int triangleIndexA, int triangleIndexB, CollisionQueryType type,
ContactSet testContactSet, CollisionObject testCollisionObjectA, TestGeometricObject testGeometricObjectA,
TriangleShape testTriangleA, CollisionObject testCollisionObjectB, TestGeometricObject testGeometricObjectB,
TriangleShape testTriangleB)
{
CollisionObject collisionObjectA = contactSet.ObjectA;
CollisionObject collisionObjectB = contactSet.ObjectB;
IGeometricObject geometricObjectA = collisionObjectA.GeometricObject;
IGeometricObject geometricObjectB = collisionObjectB.GeometricObject;
TriangleMeshShape triangleMeshShapeA = (TriangleMeshShape)geometricObjectA.Shape;
Triangle triangleA = triangleMeshShapeA.Mesh.GetTriangle(triangleIndexA);
TriangleMeshShape triangleMeshShapeB = (TriangleMeshShape)geometricObjectB.Shape;
Triangle triangleB = triangleMeshShapeB.Mesh.GetTriangle(triangleIndexB);
Pose poseA = geometricObjectA.Pose;
Pose poseB = geometricObjectB.Pose;
Vector3F scaleA = geometricObjectA.Scale;
Vector3F scaleB = geometricObjectB.Scale;
// Apply SRT.
Triangle transformedTriangleA;
transformedTriangleA.Vertex0 = poseA.ToWorldPosition(triangleA.Vertex0 * scaleA);
transformedTriangleA.Vertex1 = poseA.ToWorldPosition(triangleA.Vertex1 * scaleA);
transformedTriangleA.Vertex2 = poseA.ToWorldPosition(triangleA.Vertex2 * scaleA);
Triangle transformedTriangleB;
transformedTriangleB.Vertex0 = poseB.ToWorldPosition(triangleB.Vertex0 * scaleB);
transformedTriangleB.Vertex1 = poseB.ToWorldPosition(triangleB.Vertex1 * scaleB);
transformedTriangleB.Vertex2 = poseB.ToWorldPosition(triangleB.Vertex2 * scaleB);
// Make super-fast boolean check first. This is redundant if we have to compute
// a contact with SAT below. But in stochastic benchmarks it seems to be 10% faster.
bool haveContact = GeometryHelper.HaveContact(ref transformedTriangleA, ref transformedTriangleB);
if (type == CollisionQueryType.Boolean)
{
contactSet.HaveContact = (contactSet.HaveContact || haveContact);
return;
}
if (haveContact)
{
// Make sure the scaled triangles have the correct normal.
// (A negative scale changes the normal/winding order. See unit test in TriangleTest.cs.)
if (scaleA.X * scaleA.Y * scaleA.Z < 0)
MathHelper.Swap(ref transformedTriangleA.Vertex0, ref transformedTriangleA.Vertex1);
if (scaleB.X * scaleB.Y * scaleB.Z < 0)
MathHelper.Swap(ref transformedTriangleB.Vertex0, ref transformedTriangleB.Vertex1);
// Compute contact.
Vector3F position, normal;
float penetrationDepth;
haveContact = TriangleTriangleAlgorithm.GetContact(
ref transformedTriangleA, ref transformedTriangleB,
!triangleMeshShapeA.IsTwoSided, !triangleMeshShapeB.IsTwoSided,
out position, out normal, out penetrationDepth);
if (haveContact)
{
contactSet.HaveContact = true;
// In deep interpenetrations we might get no contact (penDepth = NaN).
if (!Numeric.IsNaN(penetrationDepth))
{
Contact contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, false);
contact.FeatureA = triangleIndexA;
contact.FeatureB = triangleIndexB;
ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
}
return;
}
// We might come here if the boolean test reports contact but the SAT test
// does not because of numerical errors.
}
Debug.Assert(!haveContact);
if (type == CollisionQueryType.Contacts)
return;
Debug.Assert(type == CollisionQueryType.ClosestPoints);
if (contactSet.HaveContact)
{
// These triangles are separated but other parts of the meshes touches.
// --> Abort.
return;
}
// We do not have a specialized triangle-triangle closest points algorithm.
// Fall back to the default algorithm (GJK).
// Initialize temporary test contact set and test objects.
// Note: We assume the triangle-triangle does not care about front/back faces.
testTriangleA.Vertex0 = transformedTriangleA.Vertex0;
testTriangleA.Vertex1 = transformedTriangleA.Vertex1;
testTriangleA.Vertex2 = transformedTriangleA.Vertex2;
testGeometricObjectA.Shape = testTriangleA;
Debug.Assert(testGeometricObjectA.Scale == Vector3F.One);
Debug.Assert(testGeometricObjectA.Pose == Pose.Identity);
testCollisionObjectA.SetInternal(collisionObjectA, testGeometricObjectA);
testTriangleB.Vertex0 = transformedTriangleB.Vertex0;
testTriangleB.Vertex1 = transformedTriangleB.Vertex1;
testTriangleB.Vertex2 = transformedTriangleB.Vertex2;
testGeometricObjectB.Shape = testTriangleB;
Debug.Assert(testGeometricObjectB.Scale == Vector3F.One);
Debug.Assert(testGeometricObjectB.Pose == Pose.Identity);
testCollisionObjectB.SetInternal(collisionObjectB, testGeometricObjectB);
Debug.Assert(testContactSet.Count == 0, "testContactSet needs to be cleared.");
testContactSet.Reset(testCollisionObjectA, testCollisionObjectB);
testContactSet.IsPerturbationTestAllowed = false;
_triangleTriangleAlgorithm.ComputeCollision(testContactSet, type);
// Note: We expect no contact but because of numerical differences the triangle-triangle
// algorithm could find a shallow surface contact.
contactSet.HaveContact = (contactSet.HaveContact || testContactSet.HaveContact);
#region ----- Merge testContactSet into contactSet -----
if (testContactSet.Count > 0)
{
// Set the shape feature of the new contacts.
int numberOfContacts = testContactSet.Count;
for (int i = 0; i < numberOfContacts; i++)
{
Contact contact = testContactSet[i];
//if (contact.Lifetime.Ticks == 0) // Currently, this check is not necessary because triangleSet does not contain old contacts.
//{
contact.FeatureA = triangleIndexA;
contact.FeatureB = triangleIndexB;
//}
}
// Merge the contact info.
ContactHelper.Merge(contactSet, testContactSet, type, CollisionDetection.ContactPositionTolerance);
}
#endregion
}
private void AddTriangleContacts(ContactSet contactSet,
bool swapped,
int triangleIndex,
CollisionQueryType type,
ContactSet testContactSet,
CollisionObject testCollisionObject,
TestGeometricObject testGeometricObject,
TriangleShape testTriangle)
{
// Object A should be the triangle mesh.
CollisionObject collisionObjectA = (swapped) ? contactSet.ObjectB : contactSet.ObjectA;
CollisionObject collisionObjectB = (swapped) ? contactSet.ObjectA : contactSet.ObjectB;
IGeometricObject geometricObjectA = collisionObjectA.GeometricObject;
var triangleMeshShape = ((TriangleMeshShape)geometricObjectA.Shape);
Triangle triangle = triangleMeshShape.Mesh.GetTriangle(triangleIndex);
Pose poseA = geometricObjectA.Pose;
Vector3F scaleA = geometricObjectA.Scale;
// Find collision algorithm.
CollisionAlgorithm collisionAlgorithm = CollisionDetection.AlgorithmMatrix[typeof(TriangleShape), collisionObjectB.GeometricObject.Shape.GetType()];
// Apply scaling.
testTriangle.Vertex0 = triangle.Vertex0 * scaleA;
testTriangle.Vertex1 = triangle.Vertex1 * scaleA;
testTriangle.Vertex2 = triangle.Vertex2 * scaleA;
// Set the shape temporarily to the current triangles.
testGeometricObject.Shape = testTriangle;
testGeometricObject.Scale = Vector3F.One;
testGeometricObject.Pose = poseA;
testCollisionObject.SetInternal(collisionObjectA, testGeometricObject);
// Make a temporary contact set.
// (Object A and object B should have the same order as in contactSet; otherwise we couldn't
// simply merge them.)
Debug.Assert(testContactSet.Count == 0, "testContactSet needs to be cleared.");
if (swapped)
testContactSet.Reset(collisionObjectB, testCollisionObject);
else
testContactSet.Reset(testCollisionObject, collisionObjectB);
if (type == CollisionQueryType.Boolean)
{
collisionAlgorithm.ComputeCollision(testContactSet, CollisionQueryType.Boolean);
contactSet.HaveContact = contactSet.HaveContact || testContactSet.HaveContact;
}
else
{
// No perturbation test. Most triangle mesh shapes are either complex and automatically
// have more contacts. Or they are complex and will not be used for stacking
// where full contact sets would be needed.
testContactSet.IsPerturbationTestAllowed = false;
// TODO: Copy separating axis info and similar things into triangleContactSet.
// But currently this info is not used in the queries.
// For closest points: If we know that we have a contact, then we can make a
// faster contact query instead of a closest-point query.
CollisionQueryType queryType = (contactSet.HaveContact) ? CollisionQueryType.Contacts : type;
collisionAlgorithm.ComputeCollision(testContactSet, queryType);
contactSet.HaveContact = contactSet.HaveContact || testContactSet.HaveContact;
if (testContactSet.HaveContact && testContactSet.Count > 0 && !triangleMeshShape.IsTwoSided)
{
// To compute the triangle normal in world space we take the normal of the unscaled
// triangle and transform the normal with: (M^-1)^T = 1 / scale
Vector3F triangleNormalLocal = Vector3F.Cross(triangle.Vertex1 - triangle.Vertex0, triangle.Vertex2 - triangle.Vertex0) / scaleA;
Vector3F triangleNormal = poseA.ToWorldDirection(triangleNormalLocal);
if (triangleNormal.TryNormalize())
{
var preferredNormal = swapped ? -triangleNormal : triangleNormal;
// ----- Remove bad normal.
// Triangles are double sided, but meshes are single sided.
// --> Remove contacts where the contact normal points into the wrong direction.
ContactHelper.RemoveBadContacts(testContactSet, preferredNormal, -Numeric.EpsilonF);
if (testContactSet.Count > 0 && triangleMeshShape.EnableContactWelding)
{
var contactDotTriangle = Vector3F.Dot(testContactSet[0].Normal, preferredNormal);
if (contactDotTriangle < WeldingLimit)
{
// Bad normal. Perform welding.
Vector3F contactPositionOnTriangle = swapped
? testContactSet[0].PositionBLocal / scaleA
: testContactSet[0].PositionALocal / scaleA;
Vector3F neighborNormal;
float triangleDotNeighbor;
GetNeighborNormal(triangleIndex, triangle, contactPositionOnTriangle, triangleNormal, triangleMeshShape, poseA, scaleA, out neighborNormal, out triangleDotNeighbor);
if (triangleDotNeighbor < float.MaxValue && Numeric.IsLess(contactDotTriangle, triangleDotNeighbor))
{
// Normal is not in allowed range.
// Test again in triangle normal direction.
Contact c0 = testContactSet[0];
testContactSet.RemoveAt(0);
testContactSet.Clear();
testContactSet.PreferredNormal = preferredNormal;
collisionAlgorithm.ComputeCollision(testContactSet, queryType);
testContactSet.PreferredNormal = Vector3F.Zero;
if (testContactSet.Count > 0)
{
Contact c1 = testContactSet[0];
float contact1DotTriangle = Vector3F.Dot(c1.Normal, preferredNormal);
// We use c1 instead of c0 if it has lower penetration depth (then it is simply
// better). Or we use c1 if the penetration depth increase is in an allowed range
// and c1 has a normal in the allowed range.
if (c1.PenetrationDepth < c0.PenetrationDepth
|| Numeric.IsGreaterOrEqual(contact1DotTriangle, triangleDotNeighbor)
&& c1.PenetrationDepth < c0.PenetrationDepth + CollisionDetection.ContactPositionTolerance)
{
c0.Recycle();
c0 = c1;
testContactSet.RemoveAt(0);
contactDotTriangle = contact1DotTriangle;
}
}
if (Numeric.IsLess(contactDotTriangle, triangleDotNeighbor))
{
// Clamp contact to allowed normal:
// We keep the contact position on the mesh and the penetration depth. We set
// a new normal and compute the other related values for this normal.
if (!swapped)
{
var positionAWorld = c0.PositionAWorld;
c0.Normal = neighborNormal;
var positionBWorld = positionAWorld - c0.Normal * c0.PenetrationDepth;
c0.Position = (positionAWorld + positionBWorld) / 2;
c0.PositionBLocal = testContactSet.ObjectB.GeometricObject.Pose.ToLocalPosition(positionBWorld);
}
else
{
var positionBWorld = c0.PositionBWorld;
c0.Normal = -neighborNormal;
var positionAWorld = positionBWorld + c0.Normal * c0.PenetrationDepth;
c0.Position = (positionAWorld + positionBWorld) / 2;
c0.PositionALocal = testContactSet.ObjectA.GeometricObject.Pose.ToLocalPosition(positionAWorld);
}
}
c0.Recycle();
}
}
}
}
}
#region ----- Merge testContactSet into contactSet -----
if (testContactSet.Count > 0)
{
// Set the shape feature of the new contacts.
int numberOfContacts = testContactSet.Count;
for (int i = 0; i < numberOfContacts; i++)
{
Contact contact = testContactSet[i];
//if (contact.Lifetime.Ticks == 0) // Currently, this check is not necessary because triangleSet does not contain old contacts.
//{
if (swapped)
contact.FeatureB = triangleIndex;
else
contact.FeatureA = triangleIndex;
//}
}
// Merge the contact info.
ContactHelper.Merge(contactSet, testContactSet, type, CollisionDetection.ContactPositionTolerance);
}
#endregion
}
}