///<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> /// Updates the cached simplex with the latest run's results. ///</summary> ///<param name="simplex">Simplex to update.</param> public void UpdateCachedSimplex(ref CachedSimplex simplex) { simplex.LocalSimplexA = SimplexA; switch (State) { case SimplexState.Point: Vector3.Subtract(ref SimplexB.A, ref LocalTransformB.Position, out simplex.LocalSimplexB.A); Quaternion conjugate; Quaternion.Conjugate(ref LocalTransformB.Orientation, out conjugate); Quaternion.Transform(ref simplex.LocalSimplexB.A, ref conjugate, out simplex.LocalSimplexB.A); break; case SimplexState.Segment: Vector3.Subtract(ref SimplexB.A, ref LocalTransformB.Position, out simplex.LocalSimplexB.A); Vector3.Subtract(ref SimplexB.B, ref LocalTransformB.Position, out simplex.LocalSimplexB.B); Matrix3x3 transform; Matrix3x3.CreateFromQuaternion(ref LocalTransformB.Orientation, out transform); Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.A, ref transform, out simplex.LocalSimplexB.A); Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.B, ref transform, out simplex.LocalSimplexB.B); break; case SimplexState.Triangle: Vector3.Subtract(ref SimplexB.A, ref LocalTransformB.Position, out simplex.LocalSimplexB.A); Vector3.Subtract(ref SimplexB.B, ref LocalTransformB.Position, out simplex.LocalSimplexB.B); Vector3.Subtract(ref SimplexB.C, ref LocalTransformB.Position, out simplex.LocalSimplexB.C); Matrix3x3.CreateFromQuaternion(ref LocalTransformB.Orientation, out transform); Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.A, ref transform, out simplex.LocalSimplexB.A); Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.B, ref transform, out simplex.LocalSimplexB.B); Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.C, ref transform, out simplex.LocalSimplexB.C); break; case SimplexState.Tetrahedron: Vector3.Subtract(ref SimplexB.A, ref LocalTransformB.Position, out simplex.LocalSimplexB.A); Vector3.Subtract(ref SimplexB.B, ref LocalTransformB.Position, out simplex.LocalSimplexB.B); Vector3.Subtract(ref SimplexB.C, ref LocalTransformB.Position, out simplex.LocalSimplexB.C); Vector3.Subtract(ref SimplexB.D, ref LocalTransformB.Position, out simplex.LocalSimplexB.D); Matrix3x3.CreateFromQuaternion(ref LocalTransformB.Orientation, out transform); Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.A, ref transform, out simplex.LocalSimplexB.A); Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.B, ref transform, out simplex.LocalSimplexB.B); Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.C, ref transform, out simplex.LocalSimplexB.C); Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.D, ref transform, out simplex.LocalSimplexB.D); break; } simplex.State = State; }
///<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();// new CachedSimplex(shapeA, shapeB, ref localtransformB); simplex.State = SimplexState.Point; 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 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; }
///<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; }
///<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); }
private bool DoExternalNear(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(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(ref contact.Position); return true; } //Too far to make a contact- move back to separation. state = CollisionState.ExternalSeparated; return false; }
///<summary> /// Constructs a new pair simplex. ///</summary> ///<param name="cachedSimplex">Cached simplex to use to warmstart the simplex.</param> ///<param name="localTransformB">Transform of shape B in the local space of A.</param> public PairSimplex(ref CachedSimplex cachedSimplex, ref RigidTransform localTransformB) { //NOTE: //USING A CACHED SIMPLEX INVALIDATES ASSUMPTIONS THAT ALLOW SIMPLEX CASES TO BE IGNORED! //To get those assumptions back, either DO NOT USE CACHED SIMPLEXES, or //VERIFY THE SIMPLEXES. //-A point requires no verification. //-A segment needs verification that the origin is in front of A in the direction of B. //-A triangle needs verification that the origin is within the edge planes and in the direction of C. //-A tetrahedron needs verification that the origin is within the edge planes of triangle ABC and is in the direction of D. //This simplex implementation will not ignore any cases, so we can warm start safely with one problem. //Due to relative movement, the simplex may become degenerate. Edges could become points, etc. //Some protections are built into the simplex cases, but keep an eye out for issues. //Most dangerous degeneracy seen so far is tetrahedron. It fails to find any points on opposing sides due to numerical problems and returns intersection. previousDistanceToClosest = Fix64.MaxValue; errorTolerance = F64.C0; LocalTransformB = localTransformB; //Transform the SimplexB into the working space of the simplex and compute the working space simplex. State = cachedSimplex.State; SimplexA = cachedSimplex.LocalSimplexA; SimplexB = new ContributingShapeSimplex(); U = F64.C0; V = F64.C0; W = F64.C0; switch (State) { case SimplexState.Point: Quaternion.Transform(ref cachedSimplex.LocalSimplexB.A, ref LocalTransformB.Orientation, out SimplexB.A); Vector3.Add(ref SimplexB.A, ref LocalTransformB.Position, out SimplexB.A); Vector3.Subtract(ref SimplexA.A, ref SimplexB.A, out A); B = new Vector3(); C = new Vector3(); D = new Vector3(); break; case SimplexState.Segment: Matrix3x3 transform; Matrix3x3.CreateFromQuaternion(ref localTransformB.Orientation, out transform); Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.A, ref transform, out SimplexB.A); Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.B, ref transform, out SimplexB.B); Vector3.Add(ref SimplexB.A, ref LocalTransformB.Position, out SimplexB.A); Vector3.Add(ref SimplexB.B, ref LocalTransformB.Position, out SimplexB.B); Vector3.Subtract(ref SimplexA.A, ref SimplexB.A, out A); Vector3.Subtract(ref SimplexA.B, ref SimplexB.B, out B); C = new Vector3(); D = new Vector3(); ////Test for degeneracy. //Fix64 edgeLengthAB; //Vector3.DistanceSquared(ref A, ref B, out edgeLengthAB); //if (edgeLengthAB < Toolbox.Epsilon) // State = SimplexState.Point; break; case SimplexState.Triangle: Matrix3x3.CreateFromQuaternion(ref localTransformB.Orientation, out transform); Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.A, ref transform, out SimplexB.A); Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.B, ref transform, out SimplexB.B); Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.C, ref transform, out SimplexB.C); Vector3.Add(ref SimplexB.A, ref LocalTransformB.Position, out SimplexB.A); Vector3.Add(ref SimplexB.B, ref LocalTransformB.Position, out SimplexB.B); Vector3.Add(ref SimplexB.C, ref LocalTransformB.Position, out SimplexB.C); Vector3.Subtract(ref SimplexA.A, ref SimplexB.A, out A); Vector3.Subtract(ref SimplexA.B, ref SimplexB.B, out B); Vector3.Subtract(ref SimplexA.C, ref SimplexB.C, out C); D = new Vector3(); ////Test for degeneracy. //Vector3 AB, AC; //Vector3.Subtract(ref B, ref A, out AB); //Vector3.Subtract(ref C, ref A, out AC); //Vector3 cross; //Vector3.Cross(ref AB, ref AC, out cross); ////If the area is small compared to a tolerance (adjusted by the partial perimeter), it's degenerate. //if (cross.LengthSquared() < Toolbox.BigEpsilon * (AB.LengthSquared() + AC.LengthSquared())) // State = SimplexState.Point; break; case SimplexState.Tetrahedron: Matrix3x3.CreateFromQuaternion(ref localTransformB.Orientation, out transform); Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.A, ref transform, out SimplexB.A); Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.B, ref transform, out SimplexB.B); Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.C, ref transform, out SimplexB.C); Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.D, ref transform, out SimplexB.D); Vector3.Add(ref SimplexB.A, ref LocalTransformB.Position, out SimplexB.A); Vector3.Add(ref SimplexB.B, ref LocalTransformB.Position, out SimplexB.B); Vector3.Add(ref SimplexB.C, ref LocalTransformB.Position, out SimplexB.C); Vector3.Add(ref SimplexB.D, ref LocalTransformB.Position, out SimplexB.D); Vector3.Subtract(ref SimplexA.A, ref SimplexB.A, out A); Vector3.Subtract(ref SimplexA.B, ref SimplexB.B, out B); Vector3.Subtract(ref SimplexA.C, ref SimplexB.C, out C); Vector3.Subtract(ref SimplexA.D, ref SimplexB.D, out D); ////Test for degeneracy. //Vector3 AD; //Vector3.Subtract(ref B, ref A, out AB); //Vector3.Subtract(ref C, ref A, out AC); //Vector3.Subtract(ref D, ref A, out AD); //Vector3.Cross(ref AB, ref AC, out cross); //Fix64 volume; //Vector3.Dot(ref cross, ref AD, out volume); ////Volume is small compared to partial 'perimeter.' //if (volume < Toolbox.BigEpsilon * (AB.LengthSquared() + AC.LengthSquared() + AD.LengthSquared())) // State = SimplexState.Point; break; default: A = new Vector3(); B = new Vector3(); C = new Vector3(); D = new Vector3(); break; } }
///<summary> /// Updates the cached simplex with the latest run's results. ///</summary> ///<param name="simplex">Simplex to update.</param> public void UpdateCachedSimplex(ref CachedSimplex simplex) { simplex.LocalSimplexA = SimplexA; switch (State) { case SimplexState.Point: Vector3.Subtract(ref SimplexB.A, ref LocalTransformB.Position, out simplex.LocalSimplexB.A); Quaternion conjugate; Quaternion.Conjugate(ref LocalTransformB.Orientation, out conjugate); Vector3.Transform(ref simplex.LocalSimplexB.A, ref conjugate, out simplex.LocalSimplexB.A); break; case SimplexState.Segment: Vector3.Subtract(ref SimplexB.A, ref LocalTransformB.Position, out simplex.LocalSimplexB.A); Vector3.Subtract(ref SimplexB.B, ref LocalTransformB.Position, out simplex.LocalSimplexB.B); Matrix3x3 transform; Matrix3x3.CreateFromQuaternion(ref LocalTransformB.Orientation, out transform); Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.A, ref transform, out simplex.LocalSimplexB.A); Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.B, ref transform, out simplex.LocalSimplexB.B); break; case SimplexState.Triangle: Vector3.Subtract(ref SimplexB.A, ref LocalTransformB.Position, out simplex.LocalSimplexB.A); Vector3.Subtract(ref SimplexB.B, ref LocalTransformB.Position, out simplex.LocalSimplexB.B); Vector3.Subtract(ref SimplexB.C, ref LocalTransformB.Position, out simplex.LocalSimplexB.C); Matrix3x3.CreateFromQuaternion(ref LocalTransformB.Orientation, out transform); Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.A, ref transform, out simplex.LocalSimplexB.A); Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.B, ref transform, out simplex.LocalSimplexB.B); Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.C, ref transform, out simplex.LocalSimplexB.C); break; case SimplexState.Tetrahedron: Vector3.Subtract(ref SimplexB.A, ref LocalTransformB.Position, out simplex.LocalSimplexB.A); Vector3.Subtract(ref SimplexB.B, ref LocalTransformB.Position, out simplex.LocalSimplexB.B); Vector3.Subtract(ref SimplexB.C, ref LocalTransformB.Position, out simplex.LocalSimplexB.C); Vector3.Subtract(ref SimplexB.D, ref LocalTransformB.Position, out simplex.LocalSimplexB.D); Matrix3x3.CreateFromQuaternion(ref LocalTransformB.Orientation, out transform); Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.A, ref transform, out simplex.LocalSimplexB.A); Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.B, ref transform, out simplex.LocalSimplexB.B); Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.C, ref transform, out simplex.LocalSimplexB.C); Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.D, ref transform, out simplex.LocalSimplexB.D); break; } simplex.State = State; }
///<summary> /// Constructs a new pair simplex. ///</summary> ///<param name="cachedSimplex">Cached simplex to use to warmstart the simplex.</param> ///<param name="localTransformB">Transform of shape B in the local space of A.</param> public PairSimplex(ref CachedSimplex cachedSimplex, ref RigidTransform localTransformB) { //NOTE: //USING A CACHED SIMPLEX INVALIDATES ASSUMPTIONS THAT ALLOW SIMPLEX CASES TO BE IGNORED! //To get those assumptions back, either DO NOT USE CACHED SIMPLEXES, or //VERIFY THE SIMPLEXES. //-A point requires no verification. //-A segment needs verification that the origin is in front of A in the direction of B. //-A triangle needs verification that the origin is within the edge planes and in the direction of C. //-A tetrahedron needs verification that the origin is within the edge planes of triangle ABC and is in the direction of D. //This simplex implementation will not ignore any cases, so we can warm start safely with one problem. //Due to relative movement, the simplex may become degenerate. Edges could become points, etc. //Some protections are built into the simplex cases, but keep an eye out for issues. //Most dangerous degeneracy seen so far is tetrahedron. It fails to find any points on opposing sides due to numerical problems and returns intersection. previousDistanceToClosest = float.MaxValue; errorTolerance = 0; LocalTransformB = localTransformB; //Transform the SimplexB into the working space of the simplex and compute the working space simplex. State = cachedSimplex.State; SimplexA = cachedSimplex.LocalSimplexA; SimplexB = new ContributingShapeSimplex(); U = 0; V = 0; W = 0; switch (State) { case SimplexState.Point: Vector3.Transform(ref cachedSimplex.LocalSimplexB.A, ref LocalTransformB.Orientation, out SimplexB.A); Vector3.Add(ref SimplexB.A, ref LocalTransformB.Position, out SimplexB.A); Vector3.Subtract(ref SimplexA.A, ref SimplexB.A, out A); B = new Vector3(); C = new Vector3(); D = new Vector3(); break; case SimplexState.Segment: Matrix3x3 transform; Matrix3x3.CreateFromQuaternion(ref localTransformB.Orientation, out transform); Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.A, ref transform, out SimplexB.A); Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.B, ref transform, out SimplexB.B); Vector3.Add(ref SimplexB.A, ref LocalTransformB.Position, out SimplexB.A); Vector3.Add(ref SimplexB.B, ref LocalTransformB.Position, out SimplexB.B); Vector3.Subtract(ref SimplexA.A, ref SimplexB.A, out A); Vector3.Subtract(ref SimplexA.B, ref SimplexB.B, out B); C = new Vector3(); D = new Vector3(); ////Test for degeneracy. //float edgeLengthAB; //Vector3.DistanceSquared(ref A, ref B, out edgeLengthAB); //if (edgeLengthAB < Toolbox.Epsilon) // State = SimplexState.Point; break; case SimplexState.Triangle: Matrix3x3.CreateFromQuaternion(ref localTransformB.Orientation, out transform); Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.A, ref transform, out SimplexB.A); Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.B, ref transform, out SimplexB.B); Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.C, ref transform, out SimplexB.C); Vector3.Add(ref SimplexB.A, ref LocalTransformB.Position, out SimplexB.A); Vector3.Add(ref SimplexB.B, ref LocalTransformB.Position, out SimplexB.B); Vector3.Add(ref SimplexB.C, ref LocalTransformB.Position, out SimplexB.C); Vector3.Subtract(ref SimplexA.A, ref SimplexB.A, out A); Vector3.Subtract(ref SimplexA.B, ref SimplexB.B, out B); Vector3.Subtract(ref SimplexA.C, ref SimplexB.C, out C); D = new Vector3(); ////Test for degeneracy. //Vector3 AB, AC; //Vector3.Subtract(ref B, ref A, out AB); //Vector3.Subtract(ref C, ref A, out AC); //Vector3 cross; //Vector3.Cross(ref AB, ref AC, out cross); ////If the area is small compared to a tolerance (adjusted by the partial perimeter), it's degenerate. //if (cross.LengthSquared() < Toolbox.BigEpsilon * (AB.LengthSquared() + AC.LengthSquared())) // State = SimplexState.Point; break; case SimplexState.Tetrahedron: Matrix3x3.CreateFromQuaternion(ref localTransformB.Orientation, out transform); Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.A, ref transform, out SimplexB.A); Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.B, ref transform, out SimplexB.B); Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.C, ref transform, out SimplexB.C); Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.D, ref transform, out SimplexB.D); Vector3.Add(ref SimplexB.A, ref LocalTransformB.Position, out SimplexB.A); Vector3.Add(ref SimplexB.B, ref LocalTransformB.Position, out SimplexB.B); Vector3.Add(ref SimplexB.C, ref LocalTransformB.Position, out SimplexB.C); Vector3.Add(ref SimplexB.D, ref LocalTransformB.Position, out SimplexB.D); Vector3.Subtract(ref SimplexA.A, ref SimplexB.A, out A); Vector3.Subtract(ref SimplexA.B, ref SimplexB.B, out B); Vector3.Subtract(ref SimplexA.C, ref SimplexB.C, out C); Vector3.Subtract(ref SimplexA.D, ref SimplexB.D, out D); ////Test for degeneracy. //Vector3 AD; //Vector3.Subtract(ref B, ref A, out AB); //Vector3.Subtract(ref C, ref A, out AC); //Vector3.Subtract(ref D, ref A, out AD); //Vector3.Cross(ref AB, ref AC, out cross); //float volume; //Vector3.Dot(ref cross, ref AD, out volume); ////Volume is small compared to partial 'perimeter.' //if (volume < Toolbox.BigEpsilon * (AB.LengthSquared() + AC.LengthSquared() + AD.LengthSquared())) // State = SimplexState.Point; break; default: A = new Vector3(); B = new Vector3(); C = new Vector3(); D = new Vector3(); break; } }