Пример #1
0
 private PairSimplex(ref RigidTransform localTransformB)
 {
     //This isn't a very good approach since the transform position is not guaranteed to be within the object.  Would have to use the GetNewSimplexPoint to make it valid.
     previousDistanceToClosest = float.MaxValue;
     errorTolerance            = 0;
     LocalTransformB           = localTransformB;
     //Warm up the simplex using the centroids.
     //Could also use the GetNewSimplexPoint if it had a Empty case, but test before choosing.
     State    = SimplexState.Point;
     SimplexA = new ContributingShapeSimplex();
     SimplexB = new ContributingShapeSimplex {
         A = localTransformB.Position
     };
     //minkowski space support = shapeA-shapeB = 0,0,0 - positionB
     Vector3.Negate(ref localTransformB.Position, out A);
     B = new Vector3();
     C = new Vector3();
     D = new Vector3();
     U = 0;
     V = 0;
     W = 0;
 }
Пример #2
0
        ///<summary>
        /// Constructs a new pair simplex.
        ///</summary>
        ///<param name="cachedSimplex">Cached simplex to use to warmstart the simplex.</param>
        ///<param name="localTransformB">Transform of shape B in the local space of A.</param>
        public PairSimplex(ref CachedSimplex cachedSimplex, ref RigidTransform localTransformB)
        {
            //NOTE:
            //USING A CACHED SIMPLEX INVALIDATES ASSUMPTIONS THAT ALLOW SIMPLEX CASES TO BE IGNORED!
            //To get those assumptions back, either DO NOT USE CACHED SIMPLEXES, or
            //VERIFY THE SIMPLEXES.
            //-A point requires no verification.
            //-A segment needs verification that the origin is in front of A in the direction of B.
            //-A triangle needs verification that the origin is within the edge planes and in the direction of C.
            //-A tetrahedron needs verification that the origin is within the edge planes of triangle ABC and is in the direction of D.

            //This simplex implementation will not ignore any cases, so we can warm start safely with one problem.
            //Due to relative movement, the simplex may become degenerate.  Edges could become points, etc.
            //Some protections are built into the simplex cases, but keep an eye out for issues.
            //Most dangerous degeneracy seen so far is tetrahedron.  It fails to find any points on opposing sides due to numerical problems and returns intersection.


            previousDistanceToClosest = float.MaxValue;
            errorTolerance            = 0;
            LocalTransformB           = localTransformB;

            //Transform the SimplexB into the working space of the simplex and compute the working space simplex.
            State    = cachedSimplex.State;
            SimplexA = cachedSimplex.LocalSimplexA;
            SimplexB = new ContributingShapeSimplex();
            U        = 0;
            V        = 0;
            W        = 0;
            switch (State)
            {
            case SimplexState.Point:
                Quaternion.Transform(ref cachedSimplex.LocalSimplexB.A, ref LocalTransformB.Orientation, out SimplexB.A);
                Vector3.Add(ref SimplexB.A, ref LocalTransformB.Position, out SimplexB.A);

                Vector3.Subtract(ref SimplexA.A, ref SimplexB.A, out A);
                B = new Vector3();
                C = new Vector3();
                D = new Vector3();
                break;

            case SimplexState.Segment:
                Matrix3x3 transform;
                Matrix3x3.CreateFromQuaternion(ref localTransformB.Orientation, out transform);
                Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.A, ref transform, out SimplexB.A);
                Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.B, ref transform, out SimplexB.B);
                Vector3.Add(ref SimplexB.A, ref LocalTransformB.Position, out SimplexB.A);
                Vector3.Add(ref SimplexB.B, ref LocalTransformB.Position, out SimplexB.B);

                Vector3.Subtract(ref SimplexA.A, ref SimplexB.A, out A);
                Vector3.Subtract(ref SimplexA.B, ref SimplexB.B, out B);
                C = new Vector3();
                D = new Vector3();

                ////Test for degeneracy.
                //float edgeLengthAB;
                //Vector3.DistanceSquared(ref A, ref B, out edgeLengthAB);
                //if (edgeLengthAB < Toolbox.Epsilon)
                //    State = SimplexState.Point;

                break;

            case SimplexState.Triangle:
                Matrix3x3.CreateFromQuaternion(ref localTransformB.Orientation, out transform);
                Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.A, ref transform, out SimplexB.A);
                Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.B, ref transform, out SimplexB.B);
                Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.C, ref transform, out SimplexB.C);
                Vector3.Add(ref SimplexB.A, ref LocalTransformB.Position, out SimplexB.A);
                Vector3.Add(ref SimplexB.B, ref LocalTransformB.Position, out SimplexB.B);
                Vector3.Add(ref SimplexB.C, ref LocalTransformB.Position, out SimplexB.C);

                Vector3.Subtract(ref SimplexA.A, ref SimplexB.A, out A);
                Vector3.Subtract(ref SimplexA.B, ref SimplexB.B, out B);
                Vector3.Subtract(ref SimplexA.C, ref SimplexB.C, out C);
                D = new Vector3();

                ////Test for degeneracy.
                //Vector3 AB, AC;
                //Vector3.Subtract(ref B, ref A, out AB);
                //Vector3.Subtract(ref C, ref A, out AC);
                //Vector3 cross;
                //Vector3.Cross(ref AB, ref AC, out cross);
                ////If the area is small compared to a tolerance (adjusted by the partial perimeter), it's degenerate.
                //if (cross.LengthSquared() < Toolbox.BigEpsilon * (AB.LengthSquared() + AC.LengthSquared()))
                //    State = SimplexState.Point;


                break;

            case SimplexState.Tetrahedron:
                Matrix3x3.CreateFromQuaternion(ref localTransformB.Orientation, out transform);
                Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.A, ref transform, out SimplexB.A);
                Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.B, ref transform, out SimplexB.B);
                Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.C, ref transform, out SimplexB.C);
                Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.D, ref transform, out SimplexB.D);
                Vector3.Add(ref SimplexB.A, ref LocalTransformB.Position, out SimplexB.A);
                Vector3.Add(ref SimplexB.B, ref LocalTransformB.Position, out SimplexB.B);
                Vector3.Add(ref SimplexB.C, ref LocalTransformB.Position, out SimplexB.C);
                Vector3.Add(ref SimplexB.D, ref LocalTransformB.Position, out SimplexB.D);

                Vector3.Subtract(ref SimplexA.A, ref SimplexB.A, out A);
                Vector3.Subtract(ref SimplexA.B, ref SimplexB.B, out B);
                Vector3.Subtract(ref SimplexA.C, ref SimplexB.C, out C);
                Vector3.Subtract(ref SimplexA.D, ref SimplexB.D, out D);

                ////Test for degeneracy.
                //Vector3 AD;
                //Vector3.Subtract(ref B, ref A, out AB);
                //Vector3.Subtract(ref C, ref A, out AC);
                //Vector3.Subtract(ref D, ref A, out AD);
                //Vector3.Cross(ref AB, ref AC, out cross);
                //float volume;
                //Vector3.Dot(ref cross, ref AD, out volume);

                ////Volume is small compared to partial 'perimeter.'
                //if (volume < Toolbox.BigEpsilon * (AB.LengthSquared() + AC.LengthSquared() + AD.LengthSquared()))
                //    State = SimplexState.Point;
                break;

            default:
                A = new Vector3();
                B = new Vector3();
                C = new Vector3();
                D = new Vector3();
                break;
            }
        }