///<summary> /// Initializes the pair tester. ///</summary> ///<param name="shapeA">First shape in the pair.</param> ///<param name="shapeB">Second shape in the pair.</param> public void Initialize(Collidable shapeA, Collidable shapeB) { collidableA = (ConvexCollidable)shapeA; collidableB = (ConvexCollidable)shapeB; cachedSimplex = new CachedSimplex { State = SimplexState.Point }; // new CachedSimplex(informationA.Shape, informationB.Shape, ref informationA.worldTransform, ref informationB.worldTransform); }
///<summary> /// Cleans up the pair tester. ///</summary> public void CleanUp() { state = CollisionState.Separated; previousState = CollisionState.Separated; cachedSimplex = new CachedSimplex(); localSeparatingAxis = new Vector3(); collidableA = null; collidableB = null; }
///<summary> /// Gets the closest points between the shapes. ///</summary> ///<param name="shapeA">First shape of the pair.</param> ///<param name="shapeB">Second shape of the pair.</param> ///<param name="transformA">Transform to apply to the first shape.</param> ///<param name="transformB">Transform to apply to the second shape.</param> /// <param name="cachedSimplex">Simplex from a previous updated used to warmstart the current attempt. Updated after each run.</param> ///<param name="closestPointA">Closest point on the first shape to the second shape.</param> ///<param name="closestPointB">Closest point on the second shape to the first shape.</param> ///<returns>Whether or not the objects were intersecting. If they are intersecting, then the closest points cannot be identified.</returns> public static bool GetClosestPoints(ConvexShape shapeA, ConvexShape shapeB, ref RigidTransform transformA, ref RigidTransform transformB, ref CachedSimplex cachedSimplex, out Vector3 closestPointA, out Vector3 closestPointB) { RigidTransform localtransformB; MinkowskiToolbox.GetLocalTransform(ref transformA, ref transformB, out localtransformB); bool toReturn = GetClosestPoints(shapeA, shapeB, ref localtransformB, ref cachedSimplex, out closestPointA, out closestPointB); RigidTransform.Transform(ref closestPointA, ref transformA, out closestPointA); RigidTransform.Transform(ref closestPointB, ref transformA, out closestPointB); return(toReturn); }
///<summary> /// Gets the closest points between the shapes. ///</summary> ///<param name="shapeA">First shape of the pair.</param> ///<param name="shapeB">Second shape of the pair.</param> ///<param name="transformA">Transform to apply to the first shape.</param> ///<param name="transformB">Transform to apply to the second shape.</param> ///<param name="closestPointA">Closest point on the first shape to the second shape.</param> ///<param name="closestPointB">Closest point on the second shape to the first shape.</param> ///<returns>Whether or not the objects were intersecting. If they are intersecting, then the closest points cannot be identified.</returns> public static bool GetClosestPoints(ConvexShape shapeA, ConvexShape shapeB, ref RigidTransform transformA, ref RigidTransform transformB, out Vector3 closestPointA, out Vector3 closestPointB) { //The cached simplex stores locations that are local to the shapes. A fairly decent initial state is between the centroids of the objects. //In local space, the centroids are at the origins. RigidTransform localtransformB; MinkowskiToolbox.GetLocalTransform(ref transformA, ref transformB, out localtransformB); var simplex = new CachedSimplex { State = SimplexState.Point }; // new CachedSimplex(shapeA, shapeB, ref localtransformB); bool toReturn = GetClosestPoints(shapeA, shapeB, ref localtransformB, ref simplex, out closestPointA, out closestPointB); RigidTransform.Transform(ref closestPointA, ref transformA, out closestPointA); RigidTransform.Transform(ref closestPointB, ref transformA, out closestPointB); return(toReturn); }
private static bool GetClosestPoints(ConvexShape shapeA, ConvexShape shapeB, ref RigidTransform localTransformB, ref CachedSimplex cachedSimplex, out Vector3 localClosestPointA, out Vector3 localClosestPointB) { var simplex = new PairSimplex(ref cachedSimplex, ref localTransformB); Vector3 closestPoint; int count = 0; while (true) { if (simplex.GetPointClosestToOrigin(out closestPoint) || //Also reduces the simplex and computes barycentric coordinates if necessary. closestPoint.LengthSquared() <= Toolbox.Epsilon * simplex.errorTolerance) { //Intersecting. localClosestPointA = Toolbox.ZeroVector; localClosestPointB = Toolbox.ZeroVector; simplex.UpdateCachedSimplex(ref cachedSimplex); return(true); } if (++count > MaximumGJKIterations) { break; //Must break BEFORE a new vertex is added if we're over the iteration limit. This guarantees final simplex is not a tetrahedron. } if (simplex.GetNewSimplexPoint(shapeA, shapeB, count, ref closestPoint)) { //No progress towards origin, not intersecting. break; } } //Compute closest points from the contributing simplexes and barycentric coordinates simplex.GetClosestPoints(out localClosestPointA, out localClosestPointB); //simplex.VerifyContributions(); //if (Vector3.Distance(localClosestPointA - localClosestPointB, closestPoint) > .00001f) // Debug.WriteLine("break."); simplex.UpdateCachedSimplex(ref cachedSimplex); return(false); }
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); CachedSimplex 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. ContactData 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); }
private bool DoShallowContact(out ContactData contact) { Vector3 closestA, closestB; 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); }