C# ConvexShape.GetLocalExtremePointWithoutMargin方法代码示例

        ///<summary>
        /// Gets the extreme point of the minkowski difference of shapeA and shapeB in the local space of shapeA.
        ///</summary>
        ///<param name="shapeA">First shape.</param>
        ///<param name="shapeB">Second shape.</param>
        ///<param name="direction">Extreme point direction in local space.</param>
        ///<param name="localTransformB">Transform of shapeB in the local space of A.</param>
        ///<param name="extremePoint">The extreme point in the local space of A.</param>
        public static void GetLocalMinkowskiExtremePoint(ConvexShape shapeA, ConvexShape shapeB, ref Vector3 direction, ref RigidTransform localTransformB, out Vector3 extremePoint)
        {
            //Extreme point of A-B along D = (extreme point of A along D) - (extreme point of B along -D)
            shapeA.GetLocalExtremePointWithoutMargin(ref direction, out extremePoint);
            Vector3 v;
            Vector3 negativeN;
            Vector3.Negate(ref direction, out negativeN);
            shapeB.GetExtremePointWithoutMargin(negativeN, ref localTransformB, out v);
            Vector3.Subtract(ref extremePoint, ref v, out extremePoint);

            ExpandMinkowskiSum(shapeA.collisionMargin, shapeB.collisionMargin, ref direction, out v);
            Vector3.Add(ref extremePoint, ref v, out extremePoint);
        }

        ///<summary>
        /// Casts a fat (sphere expanded) ray against the shape.
        ///</summary>
        ///<param name="ray">Ray to test against the shape.</param>
        ///<param name="radius">Radius of the ray.</param>
        ///<param name="shape">Shape to test against.</param>
        ///<param name="shapeTransform">Transform to apply to the shape for the test.</param>
        ///<param name="maximumLength">Maximum length of the ray in units of the ray direction's length.</param>
        ///<param name="hit">Hit data of the sphere cast, if any.</param>
        ///<returns>Whether or not the sphere cast hit the shape.</returns>
        public static bool SphereCast(Ray ray, float radius, ConvexShape shape, ref RigidTransform shapeTransform, float maximumLength,
                                   out RayHit hit)
        {
            //Transform the ray into the object's local space.
            Vector3.Subtract(ref ray.Position, ref shapeTransform.Position, out ray.Position);
            Quaternion conjugate;
            Quaternion.Conjugate(ref shapeTransform.Orientation, out conjugate);
            Quaternion.Transform(ref ray.Position, ref conjugate, out ray.Position);
            Quaternion.Transform(ref ray.Direction, ref conjugate, out ray.Direction);

            Vector3 w, p;
            hit.T = 0;
            hit.Location = ray.Position;
            hit.Normal = Toolbox.ZeroVector;
            Vector3 v = hit.Location;

            RaySimplex simplex = new RaySimplex();

            float vw, vdir;
            int count = 0;

            //This epsilon has a significant impact on performance and accuracy.  Changing it to use BigEpsilon instead increases speed by around 30-40% usually, but jigging is more evident.
            while (v.LengthSquared() >= Toolbox.Epsilon * simplex.GetErrorTolerance(ref ray.Position))
            {
                if (++count > MaximumGJKIterations)
                {
                    //It's taken too long to find a hit.  Numerical problems are probable; quit.
                    hit = new RayHit();
                    return false;
                }

                shape.GetLocalExtremePointWithoutMargin(ref v, out p);
                Vector3 contribution;
                MinkowskiToolbox.ExpandMinkowskiSum(shape.collisionMargin, radius, ref v, out contribution);
                Vector3.Add(ref p, ref contribution, out p);

                Vector3.Subtract(ref hit.Location, ref p, out w);
                Vector3.Dot(ref v, ref w, out vw);
                if (vw > 0)
                {
                    Vector3.Dot(ref v, ref ray.Direction, out vdir);
                    hit.T = hit.T - vw / vdir;
                    if (vdir >= 0)
                    {
                        //We would have to back up!
                        return false;
                    }
                    if (hit.T > maximumLength)
                    {
                        //If we've gone beyond where the ray can reach, there's obviously no hit.
                        return false;
                    }
                    //Shift the ray up.
                    Vector3.Multiply(ref ray.Direction, hit.T, out hit.Location);
                    Vector3.Add(ref hit.Location, ref ray.Position, out hit.Location);
                    hit.Normal = v;
                }

                RaySimplex shiftedSimplex;
                simplex.AddNewSimplexPoint(ref p, ref hit.Location, out shiftedSimplex);

                shiftedSimplex.GetPointClosestToOrigin(ref simplex, out v);

            }
            //Transform the hit data into world space.
            Quaternion.Transform(ref hit.Normal, ref shapeTransform.Orientation, out hit.Normal);
            Quaternion.Transform(ref hit.Location, ref shapeTransform.Orientation, out hit.Location);
            Vector3.Add(ref hit.Location, ref shapeTransform.Position, out hit.Location);

            return true;
        }

        ///<summary>
        /// Adds a new point to the simplex.
        ///</summary>
        ///<param name="shapeA">First shape in the pair.</param>
        ///<param name="shapeB">Second shape in the pair.</param>
        ///<param name="iterationCount">Current iteration count.</param>
        ///<param name="closestPoint">Current point on simplex closest to origin.</param>
        ///<returns>Whether or not GJK should exit due to a lack of progression.</returns>
        public bool GetNewSimplexPoint(ConvexShape shapeA, ConvexShape shapeB, int iterationCount, ref Vector3 closestPoint)
        {
            Vector3 negativeDirection;
            Vector3.Negate(ref closestPoint, out negativeDirection);
            Vector3 sa, sb;
            shapeA.GetLocalExtremePointWithoutMargin(ref negativeDirection, out sa);
            shapeB.GetExtremePointWithoutMargin(closestPoint, ref LocalTransformB, out sb);
            Vector3 S;
            Vector3.Subtract(ref sa, ref sb, out S);
            //If S is not further towards the origin along negativeDirection than closestPoint, then we're done.
            float dotS;
            Vector3.Dot(ref S, ref negativeDirection, out dotS); //-P * S
            float distanceToClosest = closestPoint.LengthSquared();

            float progression = dotS + distanceToClosest;
            //It's likely that the system is oscillating between two or more states, usually because of a degenerate simplex.
            //Rather than detect specific problem cases, this approach just lets it run and catches whatever falls through.
            //During oscillation, one of the states is usually just BARELY outside of the numerical tolerance.
            //After a bunch of iterations, the system lets it pick the 'better' one.
            if (iterationCount > GJKToolbox.HighGJKIterations && distanceToClosest - previousDistanceToClosest < DistanceConvergenceEpsilon * errorTolerance)
                return true;
            if (distanceToClosest < previousDistanceToClosest)
                previousDistanceToClosest = distanceToClosest;

            //If "A" is the new point always, then the switch statement can be removed
            //in favor of just pushing three points up.
            switch (State)
            {
                case SimplexState.Point:
                    if (progression <= (errorTolerance = MathHelper.Max(A.LengthSquared(), S.LengthSquared())) * ProgressionEpsilon)
                        return true;

                    State = SimplexState.Segment;
                    B = S;
                    SimplexA.B = sa;
                    SimplexB.B = sb;
                    return false;
                case SimplexState.Segment:
                    if (progression <= (errorTolerance = MathHelper.Max(MathHelper.Max(A.LengthSquared(), B.LengthSquared()), S.LengthSquared())) * ProgressionEpsilon)
                        return true;

                    State = SimplexState.Triangle;
                    C = S;
                    SimplexA.C = sa;
                    SimplexB.C = sb;
                    return false;
                case SimplexState.Triangle:
                    if (progression <= (errorTolerance = MathHelper.Max(MathHelper.Max(A.LengthSquared(), B.LengthSquared()), MathHelper.Max(C.LengthSquared(), S.LengthSquared()))) * ProgressionEpsilon)
                        return true;

                    State = SimplexState.Tetrahedron;
                    D = S;
                    SimplexA.D = sa;
                    SimplexB.D = sb;
                    return false;
            }
            return false;
        }

        ///<summary>
        /// Gets the extreme point of the minkowski difference of shapeA and shapeB in the local space of shapeA.
        ///</summary>
        ///<param name="shapeA">First shape.</param>
        ///<param name="shapeB">Second shape.</param>
        ///<param name="direction">Extreme point direction in local space.</param>
        ///<param name="localTransformB">Transform of shapeB in the local space of A.</param>
        /// <param name="extremePointA">The extreme point on shapeA.</param>
        ///<param name="extremePoint">The extreme point in the local space of A.</param>
        public static void GetLocalMinkowskiExtremePoint(ConvexShape shapeA, ConvexShape shapeB, ref System.Numerics.Vector3 direction, ref RigidTransform localTransformB,
                                                 out System.Numerics.Vector3 extremePointA, out System.Numerics.Vector3 extremePoint)
        {
            //Extreme point of A-B along D = (extreme point of A along D) - (extreme point of B along -D)
            shapeA.GetLocalExtremePointWithoutMargin(ref direction, out extremePointA);
            System.Numerics.Vector3 v;
            Vector3Ex.Negate(ref direction, out v);
            System.Numerics.Vector3 extremePointB;
            shapeB.GetExtremePointWithoutMargin(v, ref localTransformB, out extremePointB);

            ExpandMinkowskiSum(shapeA.collisionMargin, shapeB.collisionMargin, direction, ref extremePointA, ref extremePointB);
            Vector3Ex.Subtract(ref extremePointA, ref extremePointB, out extremePoint);
        }

 ///<summary>
 /// Gets the extreme point of the minkowski difference of shapeA and shapeB in the local space of shapeA, without a margin.
 ///</summary>
 ///<param name="shapeA">First shape.</param>
 ///<param name="shapeB">Second shape.</param>
 ///<param name="direction">Extreme point direction in local space.</param>
 ///<param name="localTransformB">Transform of shapeB in the local space of A.</param>
 ///<param name="extremePoint">The extreme point in the local space of A.</param>
 public static void GetLocalMinkowskiExtremePointWithoutMargin(ConvexShape shapeA, ConvexShape shapeB, ref System.Numerics.Vector3 direction, ref RigidTransform localTransformB, out System.Numerics.Vector3 extremePoint)
 {
     //Extreme point of A-B along D = (extreme point of A along D) - (extreme point of B along -D)
     shapeA.GetLocalExtremePointWithoutMargin(ref direction, out extremePoint);
     System.Numerics.Vector3 extremePointB;
     System.Numerics.Vector3 negativeN;
     Vector3Ex.Negate(ref direction, out negativeN);
     shapeB.GetExtremePointWithoutMargin(negativeN, ref localTransformB, out extremePointB);
     Vector3Ex.Subtract(ref extremePoint, ref extremePointB, out extremePoint);
 }