コード例 #1
0
        ///<summary>
        /// Gets the closest points between the shapes.
        ///</summary>
        ///<param name="shapeA">First shape of the pair.</param>
        ///<param name="shapeB">Second shape of the pair.</param>
        ///<param name="transformA">Transform to apply to the first shape.</param>
        ///<param name="transformB">Transform to apply to the second shape.</param>
        /// <param name="cachedSimplex">Simplex from a previous updated used to warmstart the current attempt.  Updated after each run.</param>
        ///<param name="closestPointA">Closest point on the first shape to the second shape.</param>
        ///<param name="closestPointB">Closest point on the second shape to the first shape.</param>
        ///<returns>Whether or not the objects were intersecting.  If they are intersecting, then the closest points cannot be identified.</returns>
        public static bool GetClosestPoints(ConvexShape shapeA, ConvexShape shapeB, ref RigidTransform transformA, ref RigidTransform transformB,
                                            ref CachedSimplex cachedSimplex, out Vector3 closestPointA, out Vector3 closestPointB)
        {
            RigidTransform localtransformB;

            MinkowskiToolbox.GetLocalTransform(ref transformA, ref transformB, out localtransformB);

            bool toReturn = GetClosestPoints(shapeA, shapeB, ref localtransformB, ref cachedSimplex, out closestPointA, out closestPointB);

            RigidTransform.Transform(ref closestPointA, ref transformA, out closestPointA);
            RigidTransform.Transform(ref closestPointB, ref transformA, out closestPointB);
            return(toReturn);
        }
コード例 #2
0
        ///<summary>
        /// Tests if the pair is intersecting.
        ///</summary>
        ///<param name="shapeA">First shape of the pair.</param>
        ///<param name="shapeB">Second shape of the pair.</param>
        ///<param name="transformA">Transform to apply to the first shape.</param>
        ///<param name="transformB">Transform to apply to the second shape.</param>
        ///<param name="localSeparatingAxis">Warmstartable separating axis used by the method to quickly early-out if possible.  Updated to the latest separating axis after each run.</param>
        ///<returns>Whether or not the objects were intersecting.</returns>
        public static bool AreShapesIntersecting(ConvexShape shapeA, ConvexShape shapeB, ref RigidTransform transformA,
                                                 ref RigidTransform transformB,
                                                 ref Vector3 localSeparatingAxis)
        {
            RigidTransform localtransformB;

            MinkowskiToolbox.GetLocalTransform(ref transformA, ref transformB, out localtransformB);

            //Warm start the simplex.
            SimpleSimplex simplex = new SimpleSimplex();
            Vector3       extremePoint;

            MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shapeA, shapeB, ref localSeparatingAxis, ref localtransformB,
                                                           out extremePoint);
            simplex.AddNewSimplexPoint(ref extremePoint);

            Vector3 closestPoint;
            int     count = 0;

            while (count++ < MaximumGJKIterations)
            {
                if (simplex.GetPointClosestToOrigin(out closestPoint) || //Also reduces the simplex.
                    closestPoint.LengthSquared() <= simplex.GetErrorTolerance() * Toolbox.BigEpsilon)
                //Intersecting, or so close to it that it will be difficult/expensive to figure out the separation.
                {
                    return(true);
                }

                //Use the closest point as a direction.
                Vector3 direction;
                Vector3.Negate(ref closestPoint, out direction);
                MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shapeA, shapeB, ref direction, ref localtransformB,
                                                               out extremePoint);
                //Since this is a boolean test, we don't need to refine the simplex if it becomes apparent that we cannot reach the origin.
                //If the most extreme point at any given time does not go past the origin, then we can quit immediately.
                float dot;
                Vector3.Dot(ref extremePoint, ref closestPoint,
                            out dot); //extreme point dotted against the direction pointing backwards towards the CSO.
                if (dot > 0)
                {
                    // If it's positive, that means that the direction pointing towards the origin produced an extreme point 'in front of' the origin, eliminating the possibility of any intersection.
                    localSeparatingAxis = direction;
                    return(false);
                }

                simplex.AddNewSimplexPoint(ref extremePoint);
            }

            return(false);
        }
コード例 #3
0
        ///<summary>
        /// Gets the closest points between the shapes.
        ///</summary>
        ///<param name="shapeA">First shape of the pair.</param>
        ///<param name="shapeB">Second shape of the pair.</param>
        ///<param name="transformA">Transform to apply to the first shape.</param>
        ///<param name="transformB">Transform to apply to the second shape.</param>
        ///<param name="closestPointA">Closest point on the first shape to the second shape.</param>
        ///<param name="closestPointB">Closest point on the second shape to the first shape.</param>
        ///<returns>Whether or not the objects were intersecting.  If they are intersecting, then the closest points cannot be identified.</returns>
        public static bool GetClosestPoints(ConvexShape shapeA, ConvexShape shapeB, ref RigidTransform transformA, ref RigidTransform transformB,
                                            out Vector3 closestPointA, out Vector3 closestPointB)
        {
            //The cached simplex stores locations that are local to the shapes.  A fairly decent initial state is between the centroids of the objects.
            //In local space, the centroids are at the origins.

            RigidTransform localtransformB;

            MinkowskiToolbox.GetLocalTransform(ref transformA, ref transformB, out localtransformB);

            var simplex = new CachedSimplex();// new CachedSimplex(shapeA, shapeB, ref localtransformB);

            simplex.State = SimplexState.Point;
            bool toReturn = GetClosestPoints(shapeA, shapeB, ref localtransformB, ref simplex, out closestPointA, out closestPointB);

            RigidTransform.Transform(ref closestPointA, ref transformA, out closestPointA);
            RigidTransform.Transform(ref closestPointB, ref transformA, out closestPointB);
            return(toReturn);
        }
コード例 #4
0
        ///<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);
        }
コード例 #5
0
        ///<summary>
        /// Sweeps two shapes against another.
        ///</summary>
        ///<param name="shapeA">First shape being swept.</param>
        ///<param name="shapeB">Second shape being swept.</param>
        ///<param name="sweepA">Sweep vector for the first shape.</param>
        ///<param name="sweepB">Sweep vector for the second shape.</param>
        ///<param name="transformA">Transform to apply to the first shape.</param>
        ///<param name="transformB">Transform to apply to the second shape.</param>
        ///<param name="hit">Hit data of the sweep test, if any.</param>
        ///<returns>Whether or not the swept shapes hit each other..</returns>
        public static bool ConvexCast(ConvexShape shapeA, ConvexShape shapeB, ref Vector3 sweepA, ref Vector3 sweepB,
                                      ref RigidTransform transformA, ref RigidTransform transformB,
                                      out RayHit hit)
        {
            //Put the velocity into shapeA's local space.
            Vector3 velocityWorld;

            Vector3.Subtract(ref sweepB, ref sweepA, out velocityWorld);
            Quaternion conjugateOrientationA;

            Quaternion.Conjugate(ref transformA.Orientation, out conjugateOrientationA);
            Vector3 rayDirection;

            Quaternion.Transform(ref velocityWorld, ref conjugateOrientationA, out rayDirection);
            //Transform b into a's local space.
            RigidTransform localTransformB;

            Quaternion.Concatenate(ref transformB.Orientation, ref conjugateOrientationA,
                                   out localTransformB.Orientation);
            Vector3.Subtract(ref transformB.Position, ref transformA.Position, out localTransformB.Position);
            Quaternion.Transform(ref localTransformB.Position, ref conjugateOrientationA, out localTransformB.Position);


            Vector3 w, p;

            hit.T        = 0;
            hit.Location = Vector3.Zero; //The ray starts at the origin.
            hit.Normal   = Toolbox.ZeroVector;
            Vector3 v = hit.Location;

            RaySimplex simplex = new RaySimplex();


            float vw, vdir;
            int   count = 0;

            do
            {
                if (++count > MaximumGJKIterations)
                {
                    //It's taken too long to find a hit.  Numerical problems are probable; quit.
                    hit = new RayHit();
                    return(false);
                }

                MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shapeA, shapeB, ref v, ref localTransformB, 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 rayDirection, out vdir);
                    if (vdir >= 0)
                    {
                        hit = new RayHit();
                        return(false);
                    }

                    hit.T = hit.T - vw / vdir;
                    if (hit.T > 1)
                    {
                        //If we've gone beyond where the ray can reach, there's obviously no hit.
                        hit = new RayHit();
                        return(false);
                    }

                    //Shift the ray up.
                    Vector3.Multiply(ref rayDirection, hit.T, out hit.Location);
                    //The ray origin is the origin!  Don't need to add any ray position.
                    hit.Normal = v;
                }

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

                shiftedSimplex.GetPointClosestToOrigin(ref simplex, out v);

                //Could measure the progress of the ray.  If it's too little, could early out.
                //Not used by default since it's biased towards precision over performance.
            } while (v.LengthSquared() >= Toolbox.Epsilon * simplex.GetErrorTolerance(ref Toolbox.ZeroVector));

            //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.
            //Transform the hit data into world space.
            Quaternion.Transform(ref hit.Normal, ref transformA.Orientation, out hit.Normal);
            Vector3.Multiply(ref velocityWorld, hit.T, out hit.Location);
            Vector3.Add(ref hit.Location, ref transformA.Position, out hit.Location);
            return(true);
        }
コード例 #6
0
        public override void Update(Fix64 dt)
        {
            if (Game.KeyboardInput.IsKeyDown(Keys.Left))
            {
                rayCastDirection = Matrix3x3.Transform(rayCastDirection, Matrix3x3.CreateFromAxisAngle(Vector3.Forward, .01m));
            }
            if (Game.KeyboardInput.IsKeyDown(Keys.Right))
            {
                rayCastDirection = Matrix3x3.Transform(rayCastDirection, Matrix3x3.CreateFromAxisAngle(Vector3.Forward, -.01m));
            }
            if (Game.KeyboardInput.IsKeyDown(Keys.Down))
            {
                rayCastDirection = Matrix3x3.Transform(rayCastDirection, Matrix3x3.CreateFromAxisAngle(Vector3.Right, .01m));
            }
            if (Game.KeyboardInput.IsKeyDown(Keys.Up))
            {
                rayCastDirection = Matrix3x3.Transform(rayCastDirection, Matrix3x3.CreateFromAxisAngle(Vector3.Right, -.01m));
            }


            if (Game.KeyboardInput.IsKeyDown(Keys.P))
            {
                Debug.WriteLine("Break.");
            }

            base.Update(dt);

            RigidTransform localTransformB;
            RigidTransform aTransform = a.CollisionInformation.WorldTransform, bTransform = b.CollisionInformation.WorldTransform;

            MinkowskiToolbox.GetLocalTransform(ref aTransform, ref bTransform, out localTransformB);

            Vector3 position;

            if (MPRToolbox.GetLocalOverlapPosition((a.CollisionInformation.Shape as ConvexShape), (b.CollisionInformation.Shape as ConvexShape), ref localTransformB, out position))
            {
                //Vector3 rayCastDirection = new Vector3(1,0,0);// (Vector3.Normalize(localDirection) + Vector3.Normalize(collidableB.worldTransform.Position - collidableA.worldTransform.Position)) / 2;
                Fix64   previousT;
                Vector3 previousNormal;
                Fix64   t;
                Vector3 normal;

                rayCastDirection = localTransformB.Position;
                MPRToolbox.LocalSurfaceCast((a.CollisionInformation.Shape as ConvexShape), (b.CollisionInformation.Shape as ConvexShape), ref localTransformB, ref rayCastDirection, out previousT, out previousNormal);
                //Vector3 secondDirection = Vector3.Cross(rayCastDirection, Vector3.Up);
                //MPRTesting.LocalSurfaceCast((a.CollisionInformation.Shape as ConvexShape), (b.CollisionInformation.Shape as ConvexShape), ref localTransformB, ref secondDirection, out t, out normal);
                //if (t < previousT)
                //{
                //    previousNormal = normal;
                //    previousT = t;
                //}
                //Vector3 thirdDirection = Vector3.Cross(secondDirection, rayCastDirection);
                //MPRTesting.LocalSurfaceCast((a.CollisionInformation.Shape as ConvexShape), (b.CollisionInformation.Shape as ConvexShape), ref localTransformB, ref thirdDirection, out t, out normal);
                //if (t < previousT)
                //{
                //    previousNormal = normal;
                //    previousT = t;
                //}
                //Vector3 fourthDirection = -secondDirection;
                //MPRTesting.LocalSurfaceCast((a.CollisionInformation.Shape as ConvexShape), (b.CollisionInformation.Shape as ConvexShape), ref localTransformB, ref fourthDirection, out t, out normal);
                //if (t < previousT)
                //{
                //    previousNormal = normal;
                //    previousT = t;
                //}
                //Vector3 fifthDirection = -thirdDirection;
                //MPRTesting.LocalSurfaceCast((a.CollisionInformation.Shape as ConvexShape), (b.CollisionInformation.Shape as ConvexShape), ref localTransformB, ref fifthDirection, out t, out normal);
                //if (t < previousT)
                //{
                //    previousNormal = normal;
                //    previousT = t;
                //}

                //Correct the penetration depth.

                MPRToolbox.LocalSurfaceCast((a.CollisionInformation.Shape as ConvexShape), (b.CollisionInformation.Shape as ConvexShape), ref localTransformB, ref previousNormal, out t, out normal);
                contactDepth  = t;
                contactNormal = previousNormal;

                ////Converge to local minimum.
                //while (true)
                //{
                //    MPRTesting.LocalSurfaceCast((a.CollisionInformation.Shape as ConvexShape), (b.CollisionInformation.Shape as ConvexShape), ref localTransformB, ref previousNormal, out t, out normal);
                //    if (previousT - t <= Toolbox.BigEpsilon)
                //        break;

                //    previousT = t;
                //    previousNormal = normal;
                //}
            }

            #region Box Box minkowski sum
            ////Construct explicit minkowski sum.
            //Vector3[] aLines = new Vector3[8];
            //aLines[0] = new Vector3(-boxWidth / 2, -boxHeight / 2, -boxLength / 2);
            //aLines[1] = new Vector3(-boxWidth / 2, -boxHeight / 2, boxLength / 2);
            //aLines[2] = new Vector3(-boxWidth / 2, boxHeight / 2, -boxLength / 2);
            //aLines[3] = new Vector3(-boxWidth / 2, boxHeight / 2, boxLength / 2);
            //aLines[4] = new Vector3(boxWidth / 2, -boxHeight / 2, -boxLength / 2);
            //aLines[5] = new Vector3(boxWidth / 2, -boxHeight / 2, boxLength / 2);
            //aLines[6] = new Vector3(boxWidth / 2, boxHeight / 2, -boxLength / 2);
            //aLines[7] = new Vector3(boxWidth / 2, boxHeight / 2, boxLength / 2);

            //Vector3[] bLines = new Vector3[8];
            //bLines[0] = new Vector3(-groundWidth / 2, -groundHeight / 2, -groundLength / 2);
            //bLines[1] = new Vector3(-groundWidth / 2, -groundHeight / 2, groundLength / 2);
            //bLines[2] = new Vector3(-groundWidth / 2, groundHeight / 2, -groundLength / 2);
            //bLines[3] = new Vector3(-groundWidth / 2, groundHeight / 2, groundLength / 2);
            //bLines[4] = new Vector3(groundWidth / 2, -groundHeight / 2, -groundLength / 2);
            //bLines[5] = new Vector3(groundWidth / 2, -groundHeight / 2, groundLength / 2);
            //bLines[6] = new Vector3(groundWidth / 2, groundHeight / 2, -groundLength / 2);
            //bLines[7] = new Vector3(groundWidth / 2, groundHeight / 2, groundLength / 2);

            //for (int i = 0; i < 8; i++)
            //    aLines[i] = Vector3.Transform(aLines[i], localTransformB.Matrix);

            //List<Vector3> vertices = new List<Vector3>();
            //for (int i = 0; i < 8; i++)
            //{
            //    for (int j = 0; j < 8; j++)
            //    {

            //        if (b.CollisionInformation.Pairs.Count > 0)
            //        {
            //            if (b.CollisionInformation.Pairs[0].BroadPhaseOverlap.EntryA == b.CollisionInformation)
            //                vertices.Add(aLines[i] - bLines[j]);
            //            else
            //                vertices.Add(bLines[i] - aLines[j]);
            //        }
            //        else
            //        {
            //            vertices.Add(bLines[i] - aLines[j]);
            //        }
            //    }
            //}

            //var indices = new List<int>();
            //Toolbox.GetConvexHull(vertices, indices);
            #endregion

            #region Arbitrary minkowski sum
            var     vertices = new List <Vector3>();
            Vector3 max;
            var     direction   = new Vector3();
            int     NumSamples  = 16;
            Fix64   angleChange = MathHelper.TwoPi / NumSamples;

            for (int i = 1; i < NumSamples / 2 - 1; i++)
            {
                Fix64 phi    = MathHelper.PiOver2 - i * angleChange;
                var   sinPhi = Fix64.Sin(phi);
                var   cosPhi = Fix64.Cos(phi);
                for (int j = 0; j < NumSamples; j++)
                {
                    Fix64 theta = j * angleChange;
                    direction.X = Fix64.Cos(theta) * cosPhi;
                    direction.Y = sinPhi;
                    direction.Z = Fix64.Sin(theta) * cosPhi;


                    MinkowskiToolbox.GetLocalMinkowskiExtremePoint(a.CollisionInformation.Shape as ConvexShape, b.CollisionInformation.Shape as ConvexShape, ref direction, ref localTransformB, out max);

                    vertices.Add(max);
                }
            }


            MinkowskiToolbox.GetLocalMinkowskiExtremePoint(a.CollisionInformation.Shape as ConvexShape, b.CollisionInformation.Shape as ConvexShape, ref Toolbox.UpVector, ref localTransformB, out max);
            vertices.Add(max);
            MinkowskiToolbox.GetLocalMinkowskiExtremePoint(a.CollisionInformation.Shape as ConvexShape, b.CollisionInformation.Shape as ConvexShape, ref Toolbox.DownVector, ref localTransformB, out max);
            vertices.Add(max);



            var indices = new List <int>();
            ConvexHullHelper.GetConvexHull(vertices, indices);
            #endregion

            minkowskiLines.Clear();
            for (int i = 0; i < indices.Count; i += 3)
            {
                minkowskiLines.Add(new VertexPositionColor(MathConverter.Convert(vertices[indices[i]]), Microsoft.Xna.Framework.Color.Blue));
                minkowskiLines.Add(new VertexPositionColor(MathConverter.Convert(vertices[indices[i + 1]]), Microsoft.Xna.Framework.Color.Blue));

                minkowskiLines.Add(new VertexPositionColor(MathConverter.Convert(vertices[indices[i + 1]]), Microsoft.Xna.Framework.Color.Blue));
                minkowskiLines.Add(new VertexPositionColor(MathConverter.Convert(vertices[indices[i + 2]]), Microsoft.Xna.Framework.Color.Blue));

                minkowskiLines.Add(new VertexPositionColor(MathConverter.Convert(vertices[indices[i + 2]]), Microsoft.Xna.Framework.Color.Blue));
                minkowskiLines.Add(new VertexPositionColor(MathConverter.Convert(vertices[indices[i]]), Microsoft.Xna.Framework.Color.Blue));
            }
        }