///<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.
 }
Exemple #3
0
        /// <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);
        }
Exemple #4
0
 /// <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);
 }
Exemple #5
0
 /// <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();
 }
Exemple #6
0
 ///<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);
 }
Exemple #7
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;
        }
Exemple #8
0
        /// <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.");
            }
        }
Exemple #9
0
        /// <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;
        }
Exemple #10
0
        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.");

            }
        }
Exemple #11
0
        /// <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.");

            }
        }
Exemple #12
0
        ///<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);
            }
        }