///<summary> /// Computes the expansion of the minkowski sum due to margins in a given direction. ///</summary> ///<param name="marginA">First margin.</param> ///<param name="marginB">Second margin.</param> ///<param name="direction">Extreme point direction.</param> ///<param name="contribution">Margin contribution to the extreme point.</param> public static void ExpandMinkowskiSum(float marginA, float marginB, ref System.Numerics.Vector3 direction, out System.Numerics.Vector3 contribution) { float lengthSquared = direction.LengthSquared(); if (lengthSquared > Toolbox.Epsilon) { //The contribution to the minkowski sum by the margin is: //direction * marginA - (-direction) * marginB. Vector3Ex.Multiply(ref direction, (marginA + marginB) / (float)Math.Sqrt(lengthSquared), out contribution); } else { contribution = new System.Numerics.Vector3(); } }
///<summary> /// Computes the expansion of the minkowski sum due to margins in a given direction. ///</summary> ///<param name="marginA">First margin.</param> ///<param name="marginB">Second margin.</param> ///<param name="direction">Extreme point direction.</param> ///<param name="toExpandA">Margin contribution to the shapeA.</param> ///<param name="toExpandB">Margin contribution to the shapeB.</param> public static void ExpandMinkowskiSum(float marginA, float marginB, System.Numerics.Vector3 direction, ref System.Numerics.Vector3 toExpandA, ref System.Numerics.Vector3 toExpandB) { float lengthSquared = direction.LengthSquared(); if (lengthSquared > Toolbox.Epsilon) { lengthSquared = 1 / (float)Math.Sqrt(lengthSquared); //The contribution to the minkowski sum by the margin is: //direction * marginA - (-direction) * marginB. System.Numerics.Vector3 contribution; Vector3Ex.Multiply(ref direction, marginA * lengthSquared, out contribution); Vector3Ex.Add(ref toExpandA, ref contribution, out toExpandA); Vector3Ex.Multiply(ref direction, marginB * lengthSquared, out contribution); Vector3Ex.Subtract(ref toExpandB, ref contribution, out toExpandB); } //If the direction is too small, then the expansion values are left unchanged. }
/// <summary> /// Updates the collection of supporting contacts. /// </summary> public void UpdateSupports(ref System.Numerics.Vector3 movementDirection) { bool hadTraction = HasTraction; //Reset traction/support. HasTraction = false; HasSupport = false; System.Numerics.Vector3 downDirection = characterBody.orientationMatrix.Down; System.Numerics.Vector3 bodyPosition = characterBody.position; //Compute the character's radius, minus a little margin. We want the rays to originate safely within the character's body. //Assume vertical rotational invariance. Spheres, cylinders, and capsules don't have varying horizontal radii. System.Numerics.Vector3 extremePoint; var convexShape = characterBody.CollisionInformation.Shape as ConvexShape; Debug.Assert(convexShape != null, "Character bodies must be convex."); //Find the lowest point on the collision shape. convexShape.GetLocalExtremePointWithoutMargin(ref Toolbox.DownVector, out extremePoint); BottomDistance = -extremePoint.Y + convexShape.collisionMargin; convexShape.GetLocalExtremePointWithoutMargin(ref Toolbox.RightVector, out extremePoint); float rayCastInnerRadius = Math.Max((extremePoint.X + convexShape.collisionMargin) * 0.8f, extremePoint.X); //Vertically, the rays will start at the same height as the character's center. //While they could be started lower on a cylinder, that wouldn't always work for a sphere or capsule: the origin might end up outside of the shape! tractionContacts.Clear(); supportContacts.Clear(); sideContacts.Clear(); headContacts.Clear(); foreach (var pair in characterBody.CollisionInformation.Pairs) { //Don't stand on things that aren't really colliding fully. if (pair.CollisionRule != CollisionRule.Normal) continue; ContactCategorizer.CategorizeContacts(pair, characterBody.CollisionInformation, ref downDirection, ref tractionContacts, ref supportContacts, ref sideContacts, ref headContacts); } HasSupport = supportContacts.Count > 0; HasTraction = tractionContacts.Count > 0; //Only perform ray casts if the character has fully left the surface, and only if the previous frame had traction. //(If ray casts are allowed when support contacts still exist, the door is opened for climbing surfaces which should not be climbable. //Consider a steep slope. If the character runs at it, the character will likely be wedged off of the ground, making it lose traction while still having a support contact with the slope. //If ray tests are allowed when support contacts exist, the character will maintain traction despite climbing the wall. //The VerticalMotionConstraint can stop the character from climbing in many cases, but it's nice not to have to rely on it. //Disallowing ray tests when supports exist does have a cost, though. For example, consider rounded steps. //If the character walks off a step such that it is still in contact with the step but is far enough down that the slope is too steep for traction, //the ray test won't recover traction. This situation just isn't very common.) if (!HasSupport && hadTraction) { float supportRayLength = maximumAssistedDownStepHeight + BottomDistance; SupportRayData = null; //If the contacts aren't available to support the character, raycast down to find the ground. if (!HasTraction) { //TODO: could also require that the character has a nonzero movement direction in order to use a ray cast. Questionable- would complicate the behavior on edges. Ray ray = new Ray(bodyPosition, downDirection); bool hasTraction; SupportRayData data; if (TryDownCast(ref ray, supportRayLength, out hasTraction, out data)) { SupportRayData = data; HasTraction = data.HasTraction; HasSupport = true; } } //If contacts and the center ray cast failed, try a ray offset in the movement direction. bool tryingToMove = movementDirection.LengthSquared() > 0; if (!HasTraction && tryingToMove) { Ray ray = new Ray( characterBody.Position + movementDirection * rayCastInnerRadius, downDirection); //Have to test to make sure the ray doesn't get obstructed. This could happen if the character is deeply embedded in a wall; we wouldn't want it detecting things inside the wall as a support! Ray obstructionRay; obstructionRay.Position = characterBody.Position; obstructionRay.Direction = ray.Position - obstructionRay.Position; if (!QueryManager.RayCastHitAnything(obstructionRay, 1)) { //The origin isn't obstructed, so now ray cast down. bool hasTraction; SupportRayData data; if (TryDownCast(ref ray, supportRayLength, out hasTraction, out data)) { if (SupportRayData == null || data.HitData.T < SupportRayData.Value.HitData.T) { //Only replace the previous support ray if we now have traction or we didn't have a support ray at all before, //or this hit is a better (sooner) hit. if (hasTraction) { SupportRayData = data; HasTraction = true; } else if (SupportRayData == null) SupportRayData = data; HasSupport = true; } } } } //If contacts, center ray, AND forward ray failed to find traction, try a side ray created from down x forward. if (!HasTraction && tryingToMove) { //Compute the horizontal offset direction. Down direction and the movement direction are normalized and perpendicular, so the result is too. System.Numerics.Vector3 horizontalOffset; Vector3Ex.Cross(ref movementDirection, ref downDirection, out horizontalOffset); Vector3Ex.Multiply(ref horizontalOffset, rayCastInnerRadius, out horizontalOffset); Ray ray = new Ray(bodyPosition + horizontalOffset, downDirection); //Have to test to make sure the ray doesn't get obstructed. This could happen if the character is deeply embedded in a wall; we wouldn't want it detecting things inside the wall as a support! Ray obstructionRay; obstructionRay.Position = bodyPosition; obstructionRay.Direction = ray.Position - obstructionRay.Position; if (!QueryManager.RayCastHitAnything(obstructionRay, 1)) { //The origin isn't obstructed, so now ray cast down. bool hasTraction; SupportRayData data; if (TryDownCast(ref ray, supportRayLength, out hasTraction, out data)) { if (SupportRayData == null || data.HitData.T < SupportRayData.Value.HitData.T) { //Only replace the previous support ray if we now have traction or we didn't have a support ray at all before, //or this hit is a better (sooner) hit. if (hasTraction) { SupportRayData = data; HasTraction = true; } else if (SupportRayData == null) SupportRayData = data; HasSupport = true; } } } } //If contacts, center ray, forward ray, AND the first side ray failed to find traction, try a side ray created from forward x down. if (!HasTraction && tryingToMove) { //Compute the horizontal offset direction. Down direction and the movement direction are normalized and perpendicular, so the result is too. System.Numerics.Vector3 horizontalOffset; Vector3Ex.Cross(ref downDirection, ref movementDirection, out horizontalOffset); Vector3Ex.Multiply(ref horizontalOffset, rayCastInnerRadius, out horizontalOffset); Ray ray = new Ray(bodyPosition + horizontalOffset, downDirection); //Have to test to make sure the ray doesn't get obstructed. This could happen if the character is deeply embedded in a wall; we wouldn't want it detecting things inside the wall as a support! Ray obstructionRay; obstructionRay.Position = bodyPosition; obstructionRay.Direction = ray.Position - obstructionRay.Position; if (!QueryManager.RayCastHitAnything(obstructionRay, 1)) { //The origin isn't obstructed, so now ray cast down. bool hasTraction; SupportRayData data; if (TryDownCast(ref ray, supportRayLength, out hasTraction, out data)) { if (SupportRayData == null || data.HitData.T < SupportRayData.Value.HitData.T) { //Only replace the previous support ray if we now have traction or we didn't have a support ray at all before, //or this hit is a better (sooner) hit. if (hasTraction) { SupportRayData = data; HasTraction = true; } else if (SupportRayData == null) SupportRayData = data; HasSupport = true; } } } } } UpdateSupportData(ref downDirection); UpdateVerticalSupportData(ref downDirection, ref movementDirection); }
/// <summary> /// Determines the location of the point when projected onto the plane defined by the normal and a point on the plane. /// </summary> /// <param name="point">Point to project onto plane.</param> /// <param name="normal">Normal of the plane.</param> /// <param name="pointOnPlane">Point located on the plane.</param> /// <param name="projectedPoint">Projected location of point onto plane.</param> public static void GetPointProjectedOnPlane(ref System.Numerics.Vector3 point, ref System.Numerics.Vector3 normal, ref System.Numerics.Vector3 pointOnPlane, out System.Numerics.Vector3 projectedPoint) { float dot; Vector3Ex.Dot(ref normal, ref point, out dot); float dot2; Vector3Ex.Dot(ref pointOnPlane, ref normal, out dot2); float t = (dot - dot2) / normal.LengthSquared(); System.Numerics.Vector3 multiply; Vector3Ex.Multiply(ref normal, t, out multiply); Vector3Ex.Subtract(ref point, ref multiply, out projectedPoint); }
/// <summary> /// Determines the distance between a point and a plane.. /// </summary> /// <param name="point">Point to project onto plane.</param> /// <param name="normal">Normal of the plane.</param> /// <param name="pointOnPlane">Point located on the plane.</param> /// <returns>Distance from the point to the plane.</returns> public static float GetDistancePointToPlane(ref System.Numerics.Vector3 point, ref System.Numerics.Vector3 normal, ref System.Numerics.Vector3 pointOnPlane) { System.Numerics.Vector3 offset; Vector3Ex.Subtract(ref point, ref pointOnPlane, out offset); float dot; Vector3Ex.Dot(ref normal, ref offset, out dot); return dot / normal.LengthSquared(); }
///<summary> /// Gets the extreme point of the shape in local space in a given direction. ///</summary> ///<param name="direction">Direction to find the extreme point in.</param> ///<param name="extremePoint">Extreme point on the shape.</param> public override void GetLocalExtremePointWithoutMargin(ref System.Numerics.Vector3 direction, out System.Numerics.Vector3 extremePoint) { //Is it the tip of the cone? float sinThetaSquared = radius * radius / (radius * radius + height * height); //If d.Y * d.Y / d.LengthSquared >= sinthetaSquared if (direction.Y > 0 && direction.Y * direction.Y >= direction.LengthSquared() * sinThetaSquared) { extremePoint = new System.Numerics.Vector3(0, .75f * height, 0); return; } //Is it a bottom edge of the cone? float horizontalLengthSquared = direction.X * direction.X + direction.Z * direction.Z; if (horizontalLengthSquared > Toolbox.Epsilon) { var radOverSigma = radius / Math.Sqrt(horizontalLengthSquared); extremePoint = new System.Numerics.Vector3((float)(radOverSigma * direction.X), -.25f * height, (float)(radOverSigma * direction.Z)); } else // It's pointing almost straight down... extremePoint = new System.Numerics.Vector3(0, -.25f * height, 0); }
///<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 System.Numerics.Vector3 closestPoint) { System.Numerics.Vector3 negativeDirection; Vector3Ex.Negate(ref closestPoint, out negativeDirection); System.Numerics.Vector3 sa, sb; shapeA.GetLocalExtremePointWithoutMargin(ref negativeDirection, out sa); shapeB.GetExtremePointWithoutMargin(closestPoint, ref LocalTransformB, out sb); System.Numerics.Vector3 S; Vector3Ex.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; Vector3Ex.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; }
/// <summary> /// Casts a ray from the origin in the given direction at the surface of the minkowski difference. /// Assumes that the origin is within the minkowski difference. /// </summary> /// <param name="shapeA">First shape in the pair.</param> /// <param name="shapeB">Second shape in the pair.</param> /// <param name="localTransformB">Transformation of shape B relative to shape A.</param> /// <param name="direction">Direction to cast the ray.</param> /// <param name="t">Length along the direction vector that the impact was found.</param> /// <param name="normal">Normal of the impact at the surface of the convex.</param> /// <param name="position">Location of the ray cast hit on the surface of A.</param> public static void LocalSurfaceCast(ConvexShape shapeA, ConvexShape shapeB, ref RigidTransform localTransformB, ref System.Numerics.Vector3 direction, out float t, out System.Numerics.Vector3 normal, out System.Numerics.Vector3 position) { // Local surface cast is very similar to regular MPR. However, instead of starting at an interior point and targeting the origin, // the ray starts at the origin (a point known to be in both shape and shapeB), and just goes towards the direction until the surface // is found. The portal (v1, v2, v3) at termination defines the surface normal, and the distance from the origin to the portal along the direction is used as the 't' result. //'v0' is no longer explicitly tracked since it is simply the origin. //Now that the origin ray is known, create a portal through which the ray passes. //To do this, first guess a portal. //This implementation is similar to that of the original XenoCollide. //'n' will be the direction used to find supports throughout the algorithm. System.Numerics.Vector3 n = direction; System.Numerics.Vector3 v1, v1A; MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shapeA, shapeB, ref n, ref localTransformB, out v1A, out v1); //v1 could be zero in some degenerate cases. //if (v1.LengthSquared() < Toolbox.Epsilon) //{ // t = 0; // normal = n; // return; //} //Find another extreme point in a direction perpendicular to the previous. System.Numerics.Vector3 v2, v2A; Vector3Ex.Cross(ref direction, ref v1, out n); if (n.LengthSquared() < Toolbox.Epsilon) { //v1 and v0 could be parallel. //This isn't a bad thing- it means the direction is exactly aligned with the extreme point offset. //In other words, if the raycast is followed out to the surface, it will arrive at the extreme point! float rayLengthSquared = direction.LengthSquared(); if (rayLengthSquared > Toolbox.Epsilon * .01f) Vector3Ex.Divide(ref direction, (float)Math.Sqrt(rayLengthSquared), out normal); else normal = new System.Numerics.Vector3(); float rate; Vector3Ex.Dot(ref normal, ref direction, out rate); float distance; Vector3Ex.Dot(ref normal, ref v1, out distance); if (rate > 0) t = distance / rate; else t = 0; position = v1A; return; } MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shapeA, shapeB, ref n, ref localTransformB, out v2A, out v2); System.Numerics.Vector3 temp1, temp2; //Set n for the first iteration. Vector3Ex.Cross(ref v1, ref v2, out n); //It's possible that v1 and v2 were constructed in such a way that 'n' is not properly calibrated //relative to the direction vector. float dot; Vector3Ex.Dot(ref n, ref direction, out dot); if (dot > 0) { //It's not properly calibrated. Flip the winding (and the previously calculated normal). Vector3Ex.Negate(ref n, out n); temp1 = v1; v1 = v2; v2 = temp1; temp1 = v1A; v1A = v2A; v2A = temp1; } System.Numerics.Vector3 v3, v3A; int count = 0; while (true) { //Find a final extreme point using the normal of the plane defined by v0, v1, v2. MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shapeA, shapeB, ref n, ref localTransformB, out v3A, out v3); if (count > MPRToolbox.OuterIterationLimit) { //Can't enclose the origin! That's a bit odd; something is wrong. t = float.MaxValue; normal = Toolbox.UpVector; position = new System.Numerics.Vector3(); return; } count++; //By now, the simplex is a tetrahedron, but it is not known whether or not the ray actually passes through the portal //defined by v1, v2, v3. // If the direction is outside the plane defined by v1,v0,v3, then the portal is invalid. Vector3Ex.Cross(ref v1, ref v3, out temp1); Vector3Ex.Dot(ref temp1, ref direction, out dot); if (dot < 0) { //Replace the point that was on the inside of the plane (v2) with the new extreme point. v2 = v3; v2A = v3A; // Calculate the normal of the plane that will be used to find a new extreme point. Vector3Ex.Cross(ref v1, ref v3, out n); continue; } // If the direction is outside the plane defined by v3,v0,v2, then the portal is invalid. Vector3Ex.Cross(ref v3, ref v2, out temp1); Vector3Ex.Dot(ref temp1, ref direction, out dot); if (dot < 0) { //Replace the point that was on the inside of the plane (v1) with the new extreme point. v1 = v3; v1A = v3A; // Calculate the normal of the plane that will be used to find a new extreme point. Vector3Ex.Cross(ref v2, ref v3, out n); continue; } break; } //if (!VerifySimplex(ref Toolbox.ZeroVector, ref v1, ref v2, ref v3, ref direction)) // Debug.WriteLine("Break."); // Refine the portal. count = 0; while (true) { //Compute the outward facing normal. Vector3Ex.Subtract(ref v1, ref v2, out temp1); Vector3Ex.Subtract(ref v3, ref v2, out temp2); Vector3Ex.Cross(ref temp1, ref temp2, out n); //Keep working towards the surface. Find the next extreme point. System.Numerics.Vector3 v4, v4A; MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shapeA, shapeB, ref n, ref localTransformB, out v4A, out v4); //If the plane which generated the normal is very close to the extreme point, then we're at the surface. Vector3Ex.Dot(ref n, ref v1, out dot); float supportDot; Vector3Ex.Dot(ref v4, ref n, out supportDot); if (supportDot - dot < surfaceEpsilon || count > MPRToolbox.InnerIterationLimit) // TODO: Could use a dynamic epsilon for possibly better behavior. { //normal = n; //float normalLengthInverse = 1 / normal.Length(); //Vector3Ex.Multiply(ref normal, normalLengthInverse, out normal); ////Find the distance from the origin to the plane. //t = dot * normalLengthInverse; float lengthSquared = n.LengthSquared(); if (lengthSquared > Toolbox.Epsilon * .01f) { Vector3Ex.Divide(ref n, (float)Math.Sqrt(lengthSquared), out normal); //The plane is very close to the surface, and the ray is known to pass through it. //dot is the rate. Vector3Ex.Dot(ref normal, ref direction, out dot); //supportDot is the distance to the plane. Vector3Ex.Dot(ref normal, ref v1, out supportDot); if (dot > 0) t = supportDot / dot; else t = 0; } else { normal = Vector3Ex.Up; t = 0; } float v1Weight, v2Weight, v3Weight; Vector3Ex.Multiply(ref direction, t, out position); Toolbox.GetBarycentricCoordinates(ref position, ref v1, ref v2, ref v3, out v1Weight, out v2Weight, out v3Weight); Vector3Ex.Multiply(ref v1A, v1Weight, out position); System.Numerics.Vector3 temp; Vector3Ex.Multiply(ref v2A, v2Weight, out temp); Vector3Ex.Add(ref temp, ref position, out position); Vector3Ex.Multiply(ref v3A, v3Weight, out temp); Vector3Ex.Add(ref temp, ref position, out position); ////DEBUG STUFF: //DEBUGlastRayT = t; //DEBUGlastRayDirection = direction; //DEBUGlastDepth = t; //DEBUGlastNormal = normal; //DEBUGlastV1 = v1; //DEBUGlastV2 = v2; //DEBUGlastV3 = v3; return; } //Still haven't exited, so refine the portal. //Test direction against the three planes that separate the new portal candidates: (v1,v4,v0) (v2,v4,v0) (v3,v4,v0) //This may look a little weird at first. //'inside' here means 'on the positive side of the plane.' //There are three total planes being tested, one for each of v1, v2, and v3. //The planes are created from consistently wound vertices, so it's possible to determine //where the ray passes through the portal based upon its relationship to two of the three planes. //The third vertex which is found to be opposite the face which contains the ray is replaced with the extreme point. //This v4 x direction is just a minor reordering of a scalar triple product: (v1 x v4) * direction. //It eliminates the need for extra cross products for the inner if. Vector3Ex.Cross(ref v4, ref direction, out temp1); Vector3Ex.Dot(ref v1, ref temp1, out dot); if (dot >= 0) { Vector3Ex.Dot(ref v2, ref temp1, out dot); if (dot >= 0) { v1 = v4; // Inside v1 & inside v2 ==> eliminate v1 v1A = v4A; } else { v3 = v4; // Inside v1 & outside v2 ==> eliminate v3 v3A = v4A; } } else { Vector3Ex.Dot(ref v3, ref temp1, out dot); if (dot >= 0) { v2 = v4; // Outside v1 & inside v3 ==> eliminate v2 v2A = v4A; } else { v1 = v4; // Outside v1 & outside v3 ==> eliminate v1 v1A = v4A; } } count++; //Here's an unoptimized equivalent without the scalar triple product reorder. #region Equivalent refinement //Vector3Ex.Cross(ref v1, ref v4, out temp1); //Vector3Ex.Dot(ref temp1, ref direction, out dot); //if (dot > 0) //{ // Vector3Ex.Cross(ref v2, ref v4, out temp2); // Vector3Ex.Dot(ref temp2, ref direction, out dot); // if (dot > 0) // { // //Inside v1, v4, v0 and inside v2, v4, v0 // v1 = v4; // } // else // { // //Inside v1, v4, v0 and outside v2, v4, v0 // v3 = v4; // } //} //else //{ // Vector3Ex.Cross(ref v3, ref v4, out temp2); // Vector3Ex.Dot(ref temp2, ref direction, out dot); // if (dot > 0) // { // //Outside v1, v4, v0 and inside v3, v4, v0 // v2 = v4; // } // else // { // //Outside v1, v4, v0 and outside v3, v4, v0 // v1 = v4; // } //} #endregion //if (!VerifySimplex(ref Toolbox.ZeroVector, ref v1, ref v2, ref v3, ref direction)) // Debug.WriteLine("Break."); } }
/// <summary> /// Gets a contact point between two convex shapes. /// </summary> /// <param name="shapeA">First shape in the pair.</param> /// <param name="shapeB">Second shape in the pair.</param> /// <param name="transformA">Transformation to apply to the first shape.</param> /// <param name="transformB">Transformation to apply to the second shape.</param> /// <param name="penetrationAxis">Axis along which to first test the penetration depth.</param> /// <param name="contact">Contact data between the two shapes, if any.</param> /// <returns>Whether or not the shapes overlap.</returns> public static bool GetContact(ConvexShape shapeA, ConvexShape shapeB, ref RigidTransform transformA, ref RigidTransform transformB, ref System.Numerics.Vector3 penetrationAxis, out ContactData contact) { RigidTransform localTransformB; MinkowskiToolbox.GetLocalTransform(ref transformA, ref transformB, out localTransformB); if (MPRToolbox.AreLocalShapesOverlapping(shapeA, shapeB, ref localTransformB)) { //First, try to use the heuristically found direction. This comes from either the GJK shallow contact separating axis or from the relative velocity. System.Numerics.Vector3 rayCastDirection; float lengthSquared = penetrationAxis.LengthSquared(); if (lengthSquared > Toolbox.Epsilon) { Vector3Ex.Divide(ref penetrationAxis, (float)Math.Sqrt(lengthSquared), out rayCastDirection);// (System.Numerics.Vector3.Normalize(localDirection) + System.Numerics.Vector3.Normalize(collidableB.worldTransform.Position - collidableA.worldTransform.Position)) / 2; MPRToolbox.LocalSurfaceCast(shapeA, shapeB, ref localTransformB, ref rayCastDirection, out contact.PenetrationDepth, out contact.Normal); } else { contact.PenetrationDepth = float.MaxValue; contact.Normal = Toolbox.UpVector; } //Try the offset between the origins as a second option. Sometimes this is a better choice than the relative velocity. //TODO: Could use the position-finding MPR iteration to find the A-B direction hit by continuing even after the origin has been found (optimization). System.Numerics.Vector3 normalCandidate; float depthCandidate; lengthSquared = localTransformB.Position.LengthSquared(); if (lengthSquared > Toolbox.Epsilon) { Vector3Ex.Divide(ref localTransformB.Position, (float)Math.Sqrt(lengthSquared), out rayCastDirection); MPRToolbox.LocalSurfaceCast(shapeA, shapeB, ref localTransformB, ref rayCastDirection, out depthCandidate, out normalCandidate); if (depthCandidate < contact.PenetrationDepth) { contact.Normal = normalCandidate; contact.PenetrationDepth = depthCandidate; } } //if (contact.PenetrationDepth > 1) // Debug.WriteLine("Break."); //Correct the penetration depth. RefinePenetration(shapeA, shapeB, ref localTransformB, contact.PenetrationDepth, ref contact.Normal, out contact.PenetrationDepth, out contact.Normal, out contact.Position); ////Correct the penetration depth. //MPRTesting.LocalSurfaceCast(shape, shapeB, ref localPoint, ref contact.Normal, out contact.PenetrationDepth, out rayCastDirection); ////The local casting can optionally continue. Eventually, it will converge to the local minimum. //while (true) //{ // MPRTesting.LocalSurfaceCast(collidableA.Shape, collidableB.Shape, ref localPoint, ref contact.Normal, out depthCandidate, out normalCandidate); // if (contact.PenetrationDepth - depthCandidate <= Toolbox.BigEpsilon) // break; // contact.PenetrationDepth = depthCandidate; // contact.Normal = normalCandidate; //} contact.Id = -1; //we're still in local space! transform it all back. Matrix3x3 orientation; Matrix3x3.CreateFromQuaternion(ref transformA.Orientation, out orientation); Matrix3x3.Transform(ref contact.Normal, ref orientation, out contact.Normal); //Vector3Ex.Negate(ref contact.Normal, out contact.Normal); Matrix3x3.Transform(ref contact.Position, ref orientation, out contact.Position); Vector3Ex.Add(ref contact.Position, ref transformA.Position, out contact.Position); return true; } contact = new ContactData(); return false; }
internal static bool GetLocalOverlapPosition(ConvexShape shapeA, ConvexShape shapeB, ref System.Numerics.Vector3 originRay, ref RigidTransform localTransformB, out System.Numerics.Vector3 position) { //Compute the origin ray. This points from a point known to be inside the minkowski sum to the origin. //The centers of the shapes are used to create the interior point. //It's possible that the two objects' centers are overlapping, or very very close to it. In this case, //they are obviously colliding and we can immediately exit. if (originRay.LengthSquared() < Toolbox.Epsilon) { position = new System.Numerics.Vector3(); //DEBUGlastPosition = position; return true; } System.Numerics.Vector3 v0; Vector3Ex.Negate(ref originRay, out v0); //Since we're in A's local space, A-B is just -B. //Now that the origin ray is known, create a portal through which the ray passes. //To do this, first guess a portal. //This implementation is similar to that of the original XenoCollide. //'n' will be the direction used to find supports throughout the algorithm. System.Numerics.Vector3 n = originRay; System.Numerics.Vector3 v1; System.Numerics.Vector3 v1A, v1B; //extreme point contributions from each shape. Used later to compute contact position; could be used to cache simplex too. MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shapeA, shapeB, ref n, ref localTransformB, out v1A, out v1B, out v1); //Find another extreme point in a direction perpendicular to the previous. System.Numerics.Vector3 v2; System.Numerics.Vector3 v2A, v2B; Vector3Ex.Cross(ref v1, ref v0, out n); if (n.LengthSquared() < Toolbox.Epsilon) { //v1 and v0 could be parallel. //This isn't a bad thing- it means the direction is exactly aligned with the extreme point offset. //In other words, if the raycast is followed out to the surface, it will arrive at the extreme point! //If the origin is further along this direction than the extreme point, then there is no intersection. //If the origin is within this extreme point, then there is an intersection. float dot; Vector3Ex.Dot(ref v1, ref originRay, out dot); if (dot < 0) { //Origin is outside. position = new System.Numerics.Vector3(); return false; } //Origin is inside. //Compute barycentric coordinates along simplex (segment). float dotv0; //Dot > 0, so dotv0 starts out negative. Vector3Ex.Dot(ref v0, ref originRay, out dotv0); float barycentricCoordinate = -dotv0 / (dot - dotv0); //Vector3Ex.Subtract(ref v1A, ref v0A, out offset); //'v0a' is just the zero vector, so there's no need to calculate the offset. Vector3Ex.Multiply(ref v1A, barycentricCoordinate, out position); return true; } MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shapeA, shapeB, ref n, ref localTransformB, out v2A, out v2B, out v2); System.Numerics.Vector3 temp1, temp2; //Set n for the first iteration. Vector3Ex.Subtract(ref v1, ref v0, out temp1); Vector3Ex.Subtract(ref v2, ref v0, out temp2); Vector3Ex.Cross(ref temp1, ref temp2, out n); System.Numerics.Vector3 v3A, v3B, v3; int count = 0; while (true) { //Find a final extreme point using the normal of the plane defined by v0, v1, v2. MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shapeA, shapeB, ref n, ref localTransformB, out v3A, out v3B, out v3); if (count > MPRToolbox.OuterIterationLimit) break; count++; //By now, the simplex is a tetrahedron, but it is not known whether or not the origin ray found earlier actually passes through the portal //defined by v1, v2, v3. // If the origin is outside the plane defined by v1,v0,v3, then the portal is invalid. Vector3Ex.Cross(ref v1, ref v3, out temp1); float dot; Vector3Ex.Dot(ref temp1, ref v0, out dot); if (dot < 0) { //Replace the point that was on the inside of the plane (v2) with the new extreme point. v2 = v3; v2A = v3A; v2B = v3B; // Calculate the normal of the plane that will be used to find a new extreme point. Vector3Ex.Subtract(ref v1, ref v0, out temp1); Vector3Ex.Subtract(ref v3, ref v0, out temp2); Vector3Ex.Cross(ref temp1, ref temp2, out n); continue; } // If the origin is outside the plane defined by v3,v0,v2, then the portal is invalid. Vector3Ex.Cross(ref v3, ref v2, out temp1); Vector3Ex.Dot(ref temp1, ref v0, out dot); if (dot < 0) { //Replace the point that was on the inside of the plane (v1) with the new extreme point. v1 = v3; v1A = v3A; v1B = v3B; // Calculate the normal of the plane that will be used to find a new extreme point. Vector3Ex.Subtract(ref v2, ref v0, out temp1); Vector3Ex.Subtract(ref v3, ref v0, out temp2); Vector3Ex.Cross(ref temp1, ref temp2, out n); continue; } break; } //if (!VerifySimplex(ref v0, ref v1, ref v2, ref v3, ref localPoint.Position)) // Debug.WriteLine("Break."); // Refine the portal. while (true) { //Test the origin against the plane defined by v1, v2, v3. If it's inside, we're done. //Compute the outward facing normal. Vector3Ex.Subtract(ref v3, ref v2, out temp1); Vector3Ex.Subtract(ref v1, ref v2, out temp2); Vector3Ex.Cross(ref temp1, ref temp2, out n); float dot; Vector3Ex.Dot(ref n, ref v1, out dot); if (dot >= 0) { System.Numerics.Vector3 temp3; //Compute the barycentric coordinates of the origin. //This is done by computing the scaled volume (parallelepiped) of the tetrahedra //formed by each triangle of the v0v1v2v3 tetrahedron and the origin. //TODO: consider a different approach using T parameter or something. Vector3Ex.Subtract(ref v1, ref v0, out temp1); Vector3Ex.Subtract(ref v2, ref v0, out temp2); Vector3Ex.Subtract(ref v3, ref v0, out temp3); System.Numerics.Vector3 cross; Vector3Ex.Cross(ref temp1, ref temp2, out cross); float v0v1v2v3volume; Vector3Ex.Dot(ref cross, ref temp3, out v0v1v2v3volume); Vector3Ex.Cross(ref v1, ref v2, out cross); float ov1v2v3volume; Vector3Ex.Dot(ref cross, ref v3, out ov1v2v3volume); Vector3Ex.Cross(ref originRay, ref temp2, out cross); float v0ov2v3volume; Vector3Ex.Dot(ref cross, ref temp3, out v0ov2v3volume); Vector3Ex.Cross(ref temp1, ref originRay, out cross); float v0v1ov3volume; Vector3Ex.Dot(ref cross, ref temp3, out v0v1ov3volume); if (v0v1v2v3volume > Toolbox.Epsilon * .01f) { float inverseTotalVolume = 1 / v0v1v2v3volume; float v0Weight = ov1v2v3volume * inverseTotalVolume; float v1Weight = v0ov2v3volume * inverseTotalVolume; float v2Weight = v0v1ov3volume * inverseTotalVolume; float v3Weight = 1 - v0Weight - v1Weight - v2Weight; position = v1Weight * v1A + v2Weight * v2A + v3Weight * v3A; } else { position = new System.Numerics.Vector3(); } //DEBUGlastPosition = position; return true; } //We haven't yet found the origin. Find the support point in the portal's outward facing direction. System.Numerics.Vector3 v4, v4A, v4B; MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shapeA, shapeB, ref n, ref localTransformB, out v4A, out v4B, out v4); //If the origin is further along the direction than the extreme point, it's not inside the shape. float dot2; Vector3Ex.Dot(ref v4, ref n, out dot2); if (dot2 < 0) { //The origin is outside! position = new System.Numerics.Vector3(); return false; } //If the plane which generated the normal is very close to the extreme point, then we're at the surface //and we have not found the origin; it's either just BARELY inside, or it is outside. Assume it's outside. if (dot2 - dot < surfaceEpsilon || count > MPRToolbox.InnerIterationLimit) // TODO: Could use a dynamic epsilon for possibly better behavior. { position = new System.Numerics.Vector3(); //DEBUGlastPosition = position; return false; } count++; //Still haven't exited, so refine the portal. //Test origin against the three planes that separate the new portal candidates: (v1,v4,v0) (v2,v4,v0) (v3,v4,v0) Vector3Ex.Cross(ref v4, ref v0, out temp1); Vector3Ex.Dot(ref v1, ref temp1, out dot); if (dot >= 0) { Vector3Ex.Dot(ref v2, ref temp1, out dot); if (dot >= 0) { v1 = v4; // Inside v1 & inside v2 ==> eliminate v1 v1A = v4A; v1B = v4B; } else { v3 = v4; // Inside v1 & outside v2 ==> eliminate v3 v3A = v4A; v3B = v4B; } } else { Vector3Ex.Dot(ref v3, ref temp1, out dot); if (dot >= 0) { v2 = v4; // Outside v1 & inside v3 ==> eliminate v2 v2A = v4A; v2B = v4B; } else { v1 = v4; // Outside v1 & outside v3 ==> eliminate v1 v1A = v4A; v1B = v4B; } } //if (!VerifySimplex(ref v0, ref v1, ref v2, ref v3, ref localPoint.Position)) // Debug.WriteLine("Break."); } }
/// <summary> /// Determines if two shapes are colliding. Shape B is positioned relative to shape A. /// </summary> /// <param name="shapeA">First shape in the pair.</param> /// <param name="shapeB">Second shape of the pair.</param> /// <param name="originRay">Direction in which to cast the overlap ray. Necessary when an object's origin is not contained in its geometry.</param> /// <param name="localTransformB">Relative transform of shape B to shape A.</param> /// <returns>Whether or not the shapes are overlapping.</returns> internal static bool AreLocalShapesOverlapping(ConvexShape shapeA, ConvexShape shapeB, ref System.Numerics.Vector3 originRay, ref RigidTransform localTransformB) { //Compute the origin ray. This points from a point known to be inside the minkowski sum to the origin. //The centers of the shapes are used to create the interior point. //It's possible that the two objects' centers are overlapping, or very very close to it. In this case, //they are obviously colliding and we can immediately exit. if (originRay.LengthSquared() < Toolbox.Epsilon) { return true; } System.Numerics.Vector3 v0; Vector3Ex.Negate(ref originRay, out v0); //Since we're in A's local space, A-B is just -B. //Now that the origin ray is known, create a portal through which the ray passes. //To do this, first guess a portal. //This implementation is similar to that of the original XenoCollide. //'n' will be the direction used to find supports throughout the algorithm. System.Numerics.Vector3 n = originRay; System.Numerics.Vector3 v1; MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shapeA, shapeB, ref n, ref localTransformB, out v1); //Find another extreme point in a direction perpendicular to the previous. System.Numerics.Vector3 v2; Vector3Ex.Cross(ref v1, ref v0, out n); if (n.LengthSquared() < Toolbox.Epsilon) { //v1 and v0 could be parallel. //This isn't a bad thing- it means the direction is exactly aligned with the extreme point offset. //In other words, if the raycast is followed out to the surface, it will arrive at the extreme point! //If the origin is further along this direction than the extreme point, then there is no intersection. //If the origin is within this extreme point, then there is an intersection. float dot; Vector3Ex.Dot(ref v1, ref originRay, out dot); if (dot < 0) { //Origin is outside. return false; } //Origin is inside. return true; } MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shapeA, shapeB, ref n, ref localTransformB, out v2); System.Numerics.Vector3 temp1, temp2; //Set n for the first iteration. Vector3Ex.Subtract(ref v1, ref v0, out temp1); Vector3Ex.Subtract(ref v2, ref v0, out temp2); Vector3Ex.Cross(ref temp1, ref temp2, out n); System.Numerics.Vector3 v3; int count = 0; while (true) { //Find a final extreme point using the normal of the plane defined by v0, v1, v2. MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shapeA, shapeB, ref n, ref localTransformB, out v3); if (count > MPRToolbox.OuterIterationLimit) break; count++; //By now, the simplex is a tetrahedron, but it is not known whether or not the origin ray found earlier actually passes through the portal //defined by v1, v2, v3. // If the origin is outside the plane defined by v1,v0,v3, then the portal is invalid. Vector3Ex.Cross(ref v1, ref v3, out temp1); float dot; Vector3Ex.Dot(ref temp1, ref v0, out dot); if (dot < 0) { //Replace the point that was on the inside of the plane (v2) with the new extreme point. v2 = v3; // Calculate the normal of the plane that will be used to find a new extreme point. Vector3Ex.Subtract(ref v1, ref v0, out temp1); Vector3Ex.Subtract(ref v3, ref v0, out temp2); Vector3Ex.Cross(ref temp1, ref temp2, out n); continue; } // If the origin is outside the plane defined by v3,v0,v2, then the portal is invalid. Vector3Ex.Cross(ref v3, ref v2, out temp1); Vector3Ex.Dot(ref temp1, ref v0, out dot); if (dot < 0) { //Replace the point that was on the inside of the plane (v1) with the new extreme point. v1 = v3; // Calculate the normal of the plane that will be used to find a new extreme point. Vector3Ex.Subtract(ref v2, ref v0, out temp1); Vector3Ex.Subtract(ref v3, ref v0, out temp2); Vector3Ex.Cross(ref temp1, ref temp2, out n); continue; } break; } //if (!VerifySimplex(ref v0, ref v1, ref v2, ref v3, ref localPoint.Position)) // Debug.WriteLine("Break."); // Refine the portal. while (true) { //Test the origin against the plane defined by v1, v2, v3. If it's inside, we're done. //Compute the outward facing normal. Vector3Ex.Subtract(ref v3, ref v2, out temp1); Vector3Ex.Subtract(ref v1, ref v2, out temp2); Vector3Ex.Cross(ref temp1, ref temp2, out n); float dot; Vector3Ex.Dot(ref n, ref v1, out dot); if (dot >= 0) { return true; } //We haven't yet found the origin. Find the support point in the portal's outward facing direction. System.Numerics.Vector3 v4; MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shapeA, shapeB, ref n, ref localTransformB, out v4); //If the origin is further along the direction than the extreme point, it's not inside the shape. float dot2; Vector3Ex.Dot(ref v4, ref n, out dot2); if (dot2 < 0) { //The origin is outside! return false; } //If the plane which generated the normal is very close to the extreme point, then we're at the surface //and we have not found the origin; it's either just BARELY inside, or it is outside. Assume it's outside. if (dot2 - dot < surfaceEpsilon || count > MPRToolbox.InnerIterationLimit) // TODO: Could use a dynamic epsilon for possibly better behavior. { return false; } count++; //Still haven't exited, so refine the portal. //Test origin against the three planes that separate the new portal candidates: (v1,v4,v0) (v2,v4,v0) (v3,v4,v0) Vector3Ex.Cross(ref v4, ref v0, out temp1); Vector3Ex.Dot(ref v1, ref temp1, out dot); if (dot >= 0) { Vector3Ex.Dot(ref v2, ref temp1, out dot); if (dot >= 0) { v1 = v4; // Inside v1 & inside v2 ==> eliminate v1 } else { v3 = v4; // Inside v1 & outside v2 ==> eliminate v3 } } else { Vector3Ex.Dot(ref v3, ref temp1, out dot); if (dot >= 0) { v2 = v4; // Outside v1 & inside v3 ==> eliminate v2 } else { v1 = v4; // Outside v1 & outside v3 ==> eliminate v1 } } //if (!VerifySimplex(ref v0, ref v1, ref v2, ref v3, ref localPoint.Position)) // Debug.WriteLine("Break."); } }
///<summary> /// Gets the extreme point of the shape in local space in a given direction with margin expansion. ///</summary> ///<param name="direction">Direction to find the extreme point in.</param> ///<param name="extremePoint">Extreme point on the shape.</param> public void GetLocalExtremePoint(System.Numerics.Vector3 direction, out System.Numerics.Vector3 extremePoint) { GetLocalExtremePointWithoutMargin(ref direction, out extremePoint); float directionLength = direction.LengthSquared(); if (directionLength > Toolbox.Epsilon) { Vector3Ex.Multiply(ref direction, collisionMargin / (float)Math.Sqrt(directionLength), out direction); Vector3Ex.Add(ref extremePoint, ref direction, out extremePoint); } }