/// <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 } }