private bool DoExternalNear(TriangleShape triangle, out TinyStructList <ContactData> contactList) { Vector3 closestA, closestB; //Don't bother trying to do any clever caching. The continually transforming simplex makes it very rarely useful. //TODO: Initialize the simplex of the GJK method using the 'true' center of the triangle. //If left unmodified, the simplex that is used in GJK will just be a point at 0,0,0, which of course is at the origin. //This causes an instant-out, always. Not good! //By giving the contributing simplex the average centroid, it has a better guess. Vector3 triangleCentroid; Vector3.Add(ref triangle.vA, ref triangle.vB, out triangleCentroid); Vector3.Add(ref triangleCentroid, ref triangle.vC, out triangleCentroid); Vector3.Multiply(ref triangleCentroid, .33333333f, out triangleCentroid); var initialSimplex = new CachedSimplex { State = SimplexState.Point, LocalSimplexB = { A = triangleCentroid } }; if (GJKToolbox.GetClosestPoints(convex, triangle, ref Toolbox.RigidIdentity, ref Toolbox.RigidIdentity, ref initialSimplex, out closestA, out closestB)) { state = CollisionState.Deep; return(DoDeepContact(triangle, out contactList)); } Vector3 displacement; Vector3.Subtract(ref closestB, ref closestA, out displacement); float distanceSquared = displacement.LengthSquared(); float margin = convex.collisionMargin + triangle.collisionMargin; contactList = new TinyStructList <ContactData>(); if (distanceSquared < margin * margin) { //Try to generate a contact. var contact = new ContactData(); //Determine if the normal points in the appropriate direction given the sidedness of the triangle. if (triangle.sidedness != TriangleSidedness.DoubleSided) { 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 triangleNormal, ref displacement, out dot); if (triangle.sidedness == TriangleSidedness.Clockwise && dot > 0) { return(false); } if (triangle.sidedness == TriangleSidedness.Counterclockwise && dot < 0) { return(false); } } //Displacement is from A to B. point = A + t * AB, where t = marginA / margin. if (margin > Toolbox.Epsilon) //This can be zero! It would cause a NaN if unprotected. { Vector3.Multiply(ref displacement, convex.collisionMargin / margin, out contact.Position); //t * AB } else { contact.Position = new Vector3(); } Vector3.Add(ref closestA, ref contact.Position, out contact.Position); //A + t * AB. contact.Normal = displacement; float distance = (float)Math.Sqrt(distanceSquared); Vector3.Divide(ref contact.Normal, distance, out contact.Normal); contact.PenetrationDepth = margin - distance; contactList.Add(ref contact); TryToEscape(triangle, ref contact.Position); return(true); } //Too far to make a contact- move back to separation. state = CollisionState.ExternalSeparated; return(false); }
private bool DoShallowContact(out ContactData contact) { Vector3 closestA, closestB; //RigidTransform transform = RigidTransform.Identity; //Vector3 closestAnew, closestBnew; //CachedSimplex cachedTest = cachedSimplex; //bool intersecting = GJKToolbox.GetClosestPoints(informationA.Shape, informationB.Shape, ref informationA.worldTransform, ref informationB.worldTransform, ref cachedTest, out closestAnew, out closestBnew); ////bool otherIntersecting = OldGJKVerifier.GetClosestPointsBetweenObjects(informationA.Shape, informationB.Shape, ref informationA.worldTransform, ref informationB.worldTransform, 0, 0, out closestA, out closestB); //bool otherIntersecting = GJKToolbox.GetClosestPoints(informationA.Shape, informationB.Shape, ref informationA.worldTransform, ref informationB.worldTransform, out closestA, out closestB); //Vector3 closestAold, closestBold; //bool oldIntersecting = OldGJKVerifier.GetClosestPointsBetweenObjects(informationA.Shape, informationB.Shape, ref informationA.worldTransform, ref informationB.worldTransform, 0, 0, out closestAold, out closestBold); //if (otherIntersecting != intersecting || (!otherIntersecting && !intersecting && // Vector3.DistanceSquared(closestAnew, closestBnew) - Vector3.DistanceSquared(closestA, closestB) > .0001f && // (Vector3.DistanceSquared(closestA, closestAnew) > .0001f || // Vector3.DistanceSquared(closestB, closestBnew) > .0001f)))// || // //Math.Abs(Vector3.Dot(closestB - closestA, closestBnew - closestAnew) - Vector3.Dot(closestB - closestA, closestB - closestA)) > Toolbox.Epsilon))) // Debug.WriteLine("Break."); //Vector3 sub; //Vector3.Subtract(ref closestA, ref closestB, out sub); //if (sub.LengthSquared() < Toolbox.Epsilon) if (UseSimplexCaching) { GJKToolbox.GetClosestPoints(collidableA.Shape, collidableB.Shape, ref collidableA.worldTransform, ref collidableB.worldTransform, ref cachedSimplex, out closestA, out closestB); } else { //The initialization of the pair creates a pretty decent simplex to start from. //Just don't try to update it. CachedSimplex preInitializedSimplex = cachedSimplex; GJKToolbox.GetClosestPoints(collidableA.Shape, collidableB.Shape, ref collidableA.worldTransform, ref collidableB.worldTransform, ref preInitializedSimplex, out closestA, out closestB); } Vector3 displacement; Vector3.Subtract(ref closestB, ref closestA, out displacement); float distanceSquared = displacement.LengthSquared(); if (distanceSquared < Toolbox.Epsilon) { state = CollisionState.DeepContact; return(DoDeepContact(out contact)); } localDirection = displacement; //Use this as the direction for future deep contacts. float margin = collidableA.Shape.collisionMargin + collidableB.Shape.collisionMargin; if (distanceSquared < margin * margin) { //Generate a contact. contact = new ContactData(); //Displacement is from A to B. point = A + t * AB, where t = marginA / margin. if (margin > Toolbox.Epsilon) //Avoid a NaN! { Vector3.Multiply(ref displacement, collidableA.Shape.collisionMargin / margin, out contact.Position); //t * AB } else { contact.Position = new Vector3(); } Vector3.Add(ref closestA, ref contact.Position, out contact.Position); //A + t * AB. contact.Normal = displacement; float distance = (float)Math.Sqrt(distanceSquared); Vector3.Divide(ref contact.Normal, distance, out contact.Normal); contact.PenetrationDepth = margin - distance; return(true); } //Too shallow to make a contact- move back to separation. state = CollisionState.Separated; contact = new ContactData(); return(false); }