/// <summary> /// Computes a convex shape description for a TransformableShape and applies it. /// </summary> public void UpdateConvexShapeInfo() { //Compute the volume distribution. var samples = CommonResources.GetVectorList(); if (samples.Capacity < InertiaHelper.SampleDirections.Length) { samples.Capacity = InertiaHelper.SampleDirections.Length; } samples.Count = InertiaHelper.SampleDirections.Length; for (int i = 0; i < InertiaHelper.SampleDirections.Length; ++i) { shape.GetLocalExtremePointWithoutMargin(ref InertiaHelper.SampleDirections[i], out samples.Elements[i]); } var triangles = CommonResources.GetIntList(); ConvexHullHelper.GetConvexHull(samples, triangles); float volume; InertiaHelper.ComputeShapeDistribution(samples, triangles, out volume, out volumeDistribution); Volume = volume; //Estimate the minimum radius based on the surface mesh. MinimumRadius = InertiaHelper.ComputeMinimumRadius(samples, triangles, ref Toolbox.ZeroVector) + collisionMargin; MaximumRadius = ComputeMaximumRadius(); CommonResources.GiveBack(samples); CommonResources.GiveBack(triangles); }
///<summary> /// Gets the extreme point of the minkowski difference of shapeA and shapeB in the local space of shapeA, without a margin. ///</summary> ///<param name="shapeA">First shape.</param> ///<param name="shapeB">Second shape.</param> ///<param name="direction">Extreme point direction in local space.</param> ///<param name="localTransformB">Transform of shapeB in the local space of A.</param> ///<param name="extremePoint">The extreme point in the local space of A.</param> public static void GetLocalMinkowskiExtremePointWithoutMargin(ConvexShape shapeA, ConvexShape shapeB, ref Vector3 direction, ref RigidTransform localTransformB, out Vector3 extremePoint) { //Extreme point of A-B along D = (extreme point of A along D) - (extreme point of B along -D) shapeA.GetLocalExtremePointWithoutMargin(ref direction, out extremePoint); Vector3 extremePointB; Vector3 negativeN; Vector3.Negate(ref direction, out negativeN); shapeB.GetExtremePointWithoutMargin(negativeN, ref localTransformB, out extremePointB); Vector3.Subtract(ref extremePoint, ref extremePointB, out extremePoint); }
///<summary> /// Gets the extreme point of the minkowski difference of shapeA and shapeB in the local space of shapeA. ///</summary> ///<param name="shapeA">First shape.</param> ///<param name="shapeB">Second shape.</param> ///<param name="direction">Extreme point direction in local space.</param> ///<param name="localTransformB">Transform of shapeB in the local space of A.</param> /// <param name="extremePointA">The extreme point on shapeA.</param> /// <param name="extremePointB">The extreme point on shapeB.</param> ///<param name="extremePoint">The extreme point in the local space of A.</param> public static void GetLocalMinkowskiExtremePoint(ConvexShape shapeA, ConvexShape shapeB, ref Vector3 direction, ref RigidTransform localTransformB, out Vector3 extremePointA, out Vector3 extremePointB, out Vector3 extremePoint) { //Extreme point of A-B along D = (extreme point of A along D) - (extreme point of B along -D) shapeA.GetLocalExtremePointWithoutMargin(ref direction, out extremePointA); Vector3 v; Vector3.Negate(ref direction, out v); shapeB.GetExtremePointWithoutMargin(v, ref localTransformB, out extremePointB); ExpandMinkowskiSum(shapeA.collisionMargin, shapeB.collisionMargin, direction, ref extremePointA, ref extremePointB); Vector3.Subtract(ref extremePointA, ref extremePointB, out extremePoint); }
///<summary> /// Adds a new point to the simplex. ///</summary> ///<param name="shapeA">First shape in the pair.</param> ///<param name="shapeB">Second shape in the pair.</param> ///<param name="iterationCount">Current iteration count.</param> ///<param name="closestPoint">Current point on simplex closest to origin.</param> ///<returns>Whether or not GJK should exit due to a lack of progression.</returns> public bool GetNewSimplexPoint(ConvexShape shapeA, ConvexShape shapeB, int iterationCount, ref Vector3 closestPoint) { Vector3 negativeDirection; Vector3.Negate(ref closestPoint, out negativeDirection); Vector3 sa, sb; shapeA.GetLocalExtremePointWithoutMargin(ref negativeDirection, out sa); shapeB.GetExtremePointWithoutMargin(closestPoint, ref LocalTransformB, out sb); Vector3 S; Vector3.Subtract(ref sa, ref sb, out S); //If S is not further towards the origin along negativeDirection than closestPoint, then we're done. float dotS; Vector3.Dot(ref S, ref negativeDirection, out dotS); //-P * S float distanceToClosest = closestPoint.LengthSquared(); float progression = dotS + distanceToClosest; //It's likely that the system is oscillating between two or more states, usually because of a degenerate simplex. //Rather than detect specific problem cases, this approach just lets it run and catches whatever falls through. //During oscillation, one of the states is usually just BARELY outside of the numerical tolerance. //After a bunch of iterations, the system lets it pick the 'better' one. if (iterationCount > GJKToolbox.HighGJKIterations && distanceToClosest - previousDistanceToClosest < DistanceConvergenceEpsilon * errorTolerance) { return(true); } if (distanceToClosest < previousDistanceToClosest) { previousDistanceToClosest = distanceToClosest; } //If "A" is the new point always, then the switch statement can be removed //in favor of just pushing three points up. switch (State) { case SimplexState.Point: if (progression <= (errorTolerance = MathHelper.Max(A.LengthSquared(), S.LengthSquared())) * ProgressionEpsilon) { return(true); } State = SimplexState.Segment; B = S; SimplexA.B = sa; SimplexB.B = sb; return(false); case SimplexState.Segment: if (progression <= (errorTolerance = MathHelper.Max(MathHelper.Max(A.LengthSquared(), B.LengthSquared()), S.LengthSquared())) * ProgressionEpsilon) { return(true); } State = SimplexState.Triangle; C = S; SimplexA.C = sa; SimplexB.C = sb; return(false); case SimplexState.Triangle: if (progression <= (errorTolerance = MathHelper.Max(MathHelper.Max(A.LengthSquared(), B.LengthSquared()), MathHelper.Max(C.LengthSquared(), S.LengthSquared()))) * ProgressionEpsilon) { return(true); } State = SimplexState.Tetrahedron; D = S; SimplexA.D = sa; SimplexB.D = sb; return(false); } return(false); }
private bool DoPlaneTest(TriangleShape triangle, out TinyStructList <ContactData> contactList) { //Find closest point between object and plane. Vector3 reverseNormal; Vector3 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 ac, ref ab, out reverseNormal); //Convex position dot normal is ALWAYS zero. The thing to look at is the plane's 'd'. //If the distance along the normal is positive, then the convex is 'behind' that normal. float dotA; Vector3.Dot(ref triangle.vA, ref reverseNormal, out dotA); contactList = new TinyStructList <ContactData>(); switch (triangle.sidedness) { case TriangleSidedness.DoubleSided: if (dotA < 0) { //The reverse normal is pointing towards the convex. //It needs to point away from the convex so that the direction //will get the proper extreme point. Vector3.Negate(ref reverseNormal, out reverseNormal); dotA = -dotA; } break; case TriangleSidedness.Clockwise: //if (dotA < 0) //{ // //The reverse normal is pointing towards the convex. // return false; //} break; case TriangleSidedness.Counterclockwise: //if (dotA > 0) //{ // //The reverse normal is pointing away from the convex. // return false; //} //The reverse normal is pointing towards the convex. //It needs to point away from the convex so that the direction //will get the proper extreme point. Vector3.Negate(ref reverseNormal, out reverseNormal); dotA = -dotA; break; } Vector3 extremePoint; convex.GetLocalExtremePointWithoutMargin(ref reverseNormal, out extremePoint); //See if the extreme point is within the face or not. //It might seem like the easy "depth" test should come first, since a barycentric //calculation takes a bit more time. However, transferring from plane to depth is 'rare' //(like all transitions), and putting this test here is logically closer to its requirements' //computation. if (GetVoronoiRegion(triangle, ref extremePoint) != VoronoiRegion.ABC) { state = CollisionState.ExternalSeparated; return(DoExternalSeparated(triangle, out contactList)); } float dotE; Vector3.Dot(ref extremePoint, ref reverseNormal, out dotE); float t = (dotA - dotE) / reverseNormal.LengthSquared(); Vector3 offset; Vector3.Multiply(ref reverseNormal, t, out offset); //Compare the distance from the plane to the convex object. float distanceSquared = offset.LengthSquared(); float marginSum = triangle.collisionMargin + convex.collisionMargin; //TODO: Could just normalize early and avoid computing point plane before it's necessary. //Exposes a sqrt but... if (t <= 0 || distanceSquared < marginSum * marginSum) { //The convex object is in the margin of the plane. //All that's left is to create the contact. var contact = new ContactData(); //Displacement is from A to B. point = A + t * AB, where t = marginA / margin. if (marginSum > Toolbox.Epsilon) //This can be zero! It would cause a NaN is unprotected. { Vector3.Multiply(ref offset, convex.collisionMargin / marginSum, out contact.Position); //t * AB } else { contact.Position = new Vector3(); } Vector3.Add(ref extremePoint, ref contact.Position, out contact.Position); //A + t * AB. float normalLength = reverseNormal.Length(); Vector3.Divide(ref reverseNormal, normalLength, out contact.Normal); float distance = normalLength * t; contact.PenetrationDepth = marginSum - distance; if (contact.PenetrationDepth > marginSum) { //Check to see if the inner sphere is touching the plane. //This does not override other tests; there can be more than one contact from a single triangle. ContactData alternateContact; if (TryInnerSphereContact(triangle, out alternateContact))// && alternateContact.PenetrationDepth > contact.PenetrationDepth) { contactList.Add(ref alternateContact); } //The convex object is stuck deep in the plane! //The most problematic case for this is when //an object is right on top of a cliff. //The lower, vertical triangle may occasionally detect //a contact with the object, but would compute an extremely //deep depth if the normal plane test was used. //Verify that the depth is correct by trying another approach. CollisionState previousState = state; state = CollisionState.ExternalNear; TinyStructList <ContactData> alternateContacts; if (DoExternalNear(triangle, out alternateContacts)) { alternateContacts.Get(0, out alternateContact); if (alternateContact.PenetrationDepth + .01f < contact.PenetrationDepth) //Bias against the subtest's result, since the plane version will probably have a better position. { //It WAS a bad contact. contactList.Add(ref alternateContact); //DoDeepContact (which can be called from within DoExternalNear) can generate two contacts, but the second contact would just be an inner sphere (which we already generated). //DoExternalNear can only generate one contact. So we only need the first contact! //TODO: This is a fairly fragile connection between the two stages. Consider robustifying. (Also, the TryInnerSphereContact is done twice! This process is very rare for marginful pairs, though) } else { //Well, it really is just that deep. contactList.Add(ref contact); state = previousState; } } else { //If the external near test finds that there was no collision at all, //just return to plane testing. If the point turns up outside the face region //next time, the system will adapt. state = previousState; return(false); } } else { contactList.Add(ref contact); } return(true); } return(false); }