コード例 #1
0
 public override void UpdateTimeOfImpact(Collidable requester, double dt)
 {
     //Notice that we don't test for convex entity null explicitly.  The convex.IsActive property does that for us.
     if (convex.IsActive && convex.Entity.PositionUpdateMode == PositionUpdateMode.Continuous)
     {
         //Only perform the test if the minimum radii are small enough relative to the size of the velocity.
         Vector3 velocity        = convex.Entity.LinearVelocity * dt;
         double  velocitySquared = velocity.LengthSquared();
         double  minimumRadius   = convex.Shape.MinimumRadius * MotionSettings.CoreShapeScaling;
         timeOfImpact = 1;
         if (minimumRadius * minimumRadius < velocitySquared)
         {
             for (int i = 0; i < contactManifold.ActivePairs.Count; i++)
             {
                 GeneralConvexPairTester pair = contactManifold.ActivePairs.Values[i];
                 ReusableGenericCollidable <ConvexShape> boxCollidable = (ReusableGenericCollidable <ConvexShape>)pair.CollidableB;
                 RayHit         rayHit;
                 RigidTransform worldTransform = boxCollidable.WorldTransform;
                 if (GJKToolbox.CCDSphereCast(new Ray(convex.WorldTransform.Position, velocity), minimumRadius, boxCollidable.Shape, ref worldTransform, timeOfImpact, out rayHit) &&
                     rayHit.T > Toolbox.BigEpsilon)
                 {
                     timeOfImpact = rayHit.T;
                 }
             }
         }
     }
 }
コード例 #2
0
ファイル: GJKToolbox.cs プロジェクト: robert0329/mmo-server
        ///<summary>
        /// Casts a fat (sphere expanded) ray against the shape.  If the raycast appears to be stuck in the shape, the cast will be attempted
        /// with a smaller ray (scaled by the MotionSettings.CoreShapeScaling each time).
        ///</summary>
        ///<param name="ray">Ray to test against the shape.</param>
        ///<param name="radius">Radius of the ray.</param>
        ///<param name="target">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 CCDSphereCast(Ray ray, float radius, ConvexShape target, ref RigidTransform shapeTransform, float maximumLength,
                                         out RayHit hit)
        {
            int iterations = 0;

            while (true)
            {
                if (GJKToolbox.SphereCast(ray, radius, target, ref shapeTransform, maximumLength, out hit) &&
                    hit.T > 0)
                {
                    //The ray cast isn't embedded in the shape, and it's less than maximum length away!
                    return(true);
                }
                if (hit.T > maximumLength || hit.T < 0)
                {
                    return(false); //Failure showed it was too far, or behind.
                }
                radius *= MotionSettings.CoreShapeScaling;
                iterations++;
                if (iterations > 3) //Limit could be configurable.
                {
                    //It's iterated too much, let's just do a last ditch attempt using a raycast and hope that can help.
                    return(GJKToolbox.RayCast(ray, target, ref shapeTransform, maximumLength, out hit) && hit.T > 0);
                }
            }
        }
コード例 #3
0
ファイル: StaticMeshPairHandler.cs プロジェクト: zhuowp/ge
        ///<summary>
        /// Updates the time of impact for the pair.
        ///</summary>
        ///<param name="requester">Collidable requesting the update.</param>
        ///<param name="dt">Timestep duration.</param>
        public override void UpdateTimeOfImpact(Collidable requester, float dt)
        {
            //Notice that we don't test for convex entity null explicitly.  The convex.IsActive property does that for us.
            if (convex.IsActive && convex.entity.PositionUpdateMode == PositionUpdateMode.Continuous)
            {
                //TODO: This system could be made more robust by using a similar region-based rejection of edges.
                //CCD events are awfully rare under normal circumstances, so this isn't usually an issue.

                //Only perform the test if the minimum radii are small enough relative to the size of the velocity.
                System.Numerics.Vector3 velocity;
                Vector3Ex.Multiply(ref convex.entity.linearVelocity, dt, out velocity);
                float velocitySquared = velocity.LengthSquared();

                var minimumRadius = convex.Shape.MinimumRadius * MotionSettings.CoreShapeScaling;
                timeOfImpact = 1;
                if (minimumRadius * minimumRadius < velocitySquared)
                {
                    var triangle = PhysicsThreadResources.GetTriangle();
                    triangle.collisionMargin = 0;
                    //Spherecast against all triangles to find the earliest time.
                    for (int i = 0; i < MeshManifold.overlappedTriangles.Count; i++)
                    {
                        mesh.Shape.TriangleMeshData.GetTriangle(MeshManifold.overlappedTriangles.Elements[i], out triangle.vA, out triangle.vB, out triangle.vC);
                        //Put the triangle into 'localish' space of the convex.
                        Vector3Ex.Subtract(ref triangle.vA, ref convex.worldTransform.Position, out triangle.vA);
                        Vector3Ex.Subtract(ref triangle.vB, ref convex.worldTransform.Position, out triangle.vB);
                        Vector3Ex.Subtract(ref triangle.vC, ref convex.worldTransform.Position, out triangle.vC);

                        RayHit rayHit;
                        if (GJKToolbox.CCDSphereCast(new Ray(Toolbox.ZeroVector, velocity), minimumRadius, triangle, ref Toolbox.RigidIdentity, timeOfImpact, out rayHit) &&
                            rayHit.T > Toolbox.BigEpsilon)
                        {
                            if (mesh.sidedness != TriangleSidedness.DoubleSided)
                            {
                                System.Numerics.Vector3 AB, AC;
                                Vector3Ex.Subtract(ref triangle.vB, ref triangle.vA, out AB);
                                Vector3Ex.Subtract(ref triangle.vC, ref triangle.vA, out AC);
                                System.Numerics.Vector3 normal;
                                Vector3Ex.Cross(ref AB, ref AC, out normal);
                                float dot;
                                Vector3Ex.Dot(ref normal, ref rayHit.Normal, out dot);
                                //Only perform sweep if the object is in danger of hitting the object.
                                //Triangles can be one sided, so check the impact normal against the triangle normal.
                                if (mesh.sidedness == TriangleSidedness.Counterclockwise && dot < 0 ||
                                    mesh.sidedness == TriangleSidedness.Clockwise && dot > 0)
                                {
                                    timeOfImpact = rayHit.T;
                                }
                            }
                            else
                            {
                                timeOfImpact = rayHit.T;
                            }
                        }
                    }
                    PhysicsThreadResources.GiveBack(triangle);
                }
            }
        }
コード例 #4
0
 /// <summary>
 /// Gets the intersection between the convex shape and the ray.
 /// </summary>
 /// <param name="ray">Ray to test.</param>
 /// <param name="transform">Transform of the convex shape.</param>
 /// <param name="maximumLength">Maximum distance to travel in units of the ray direction's length.</param>
 /// <param name="hit">Ray hit data, if any.</param>
 /// <returns>Whether or not the ray hit the target.</returns>
 public virtual bool RayTest(ref Ray ray, ref RigidTransform transform, float maximumLength, out RayHit hit)
 {
     //TODO:
     //RayHit newHit;
     //bool newBool = GJKToolbox.RayCast(ray, this, ref transform, maximumLength, out newHit);
     //bool oldBool = OldGJKVerifier.RayCastGJK(ray.Position, ray.Direction, maximumLength, this, transform, out hit.Location, out hit.Normal, out hit.T);
     //if (newBool != oldBool || ((newBool && oldBool) && Vector3.DistanceSquared(newHit.Location, hit.Location) > .01f))
     //    Debug.WriteLine("break.");
     //return oldBool;
     return(GJKToolbox.RayCast(ray, this, ref transform, maximumLength, out hit));
 }
コード例 #5
0
 private bool DoExternalSeparated(TriangleShape triangle, out TinyStructList <ContactData> contactList)
 {
     if (GJKToolbox.AreShapesIntersecting(convex, triangle, ref Toolbox.RigidIdentity, ref Toolbox.RigidIdentity, ref localSeparatingAxis))
     {
         state = CollisionState.ExternalNear;
         return(DoExternalNear(triangle, out contactList));
     }
     TryToEscape();
     contactList = new TinyStructList <ContactData>();
     return(false);
 }
コード例 #6
0
        ///<summary>
        /// Generates a contact between the objects, if possible.
        ///</summary>
        ///<param name="contact">Contact created between the pair, if possible.</param>
        ///<returns>Whether or not the objects were colliding.</returns>
        public bool GenerateContactCandidate(out ContactData contact)
        {
            //Generate contacts.  This will just find one closest point using general supportmapping based systems like MPR and GJK.

            //The collision system moves through a state machine depending on the latest collision generation result.
            //At first, assume that the pair is completely separating.  This is almost always the correct guess for new pairs.
            //An extremely fast, warm-startable boolean GJK test can be performed.  If it returns with nonintersection, we can quit and do nothing.
            //If the initial boolean GJK test finds intersection, move onto a shallow contact test.
            //The shallow contact test is a different kind of GJK test that finds the closest points between the shape pair.  It's not as speedy as the boolean version.
            //The algorithm is run between the marginless versions of the shapes, so that the closest points will form a contact somewhere in the space separating the cores.
            //If the closest point system finds no intersection and returns the closest points, the state is changed to ShallowContact.
            //If the closest point system finds intersection of the core shapes, then the state is changed to DeepContact, and MPR is run to determine contact information.
            //The system tries to escape from deep contact to shallow contact, and from shallow contact to separated whenever possible.

            //Here's the state flow:
            //On Separated: BooleanGJK
            //  -Intersecting -> Go to ShallowContact.
            //  -Nonintersecting -> Do nothing.
            //On ShallowContact: ClosestPointsGJK
            //  -Intersecting -> Go to DeepContact.
            //  -Nonintersecting: Go to Separated (without test) if squared distance > margin squared, otherwise use closest points to make contact.
            //On DeepContact: MPR
            //  -Intersecting -> Go to ShallowContact if penetration depth < margin
            //  -Nonintersecting -> This case is rare, but not impossible.  Go to Separated (without test).

            previousState = state;
            switch (state)
            {
            case CollisionState.Separated:
                if (GJKToolbox.AreShapesIntersecting(collidableA.Shape, collidableB.Shape,
                                                     ref collidableA.worldTransform, ref collidableB.worldTransform, ref localSeparatingAxis))
                {
                    state = CollisionState.ShallowContact;
                    return(DoShallowContact(out contact));
                }

                contact = new ContactData();
                return(false);

            case CollisionState.ShallowContact:
                return(DoShallowContact(out contact));

            case CollisionState.DeepContact:
                return(DoDeepContact(out contact));
            }

            contact = new ContactData();
            return(false);
        }
コード例 #7
0
        /// <summary>
        /// Performs a convex cast to determine if a step of a given height is valid.
        /// This means that stepping up onto the new support wouldn't shove the character's head into a ceiling or other obstacle.
        /// </summary>
        /// <param name="hitDistance">Vertical distance from the convex cast start to the hit location.</param>
        /// <returns>Whether or not the step is safe.</returns>
        private bool IsStepSafe(float hitDistance)
        {
            float   stepHeight       = maximumStepHeight - hitDistance;
            var     sweep            = new Vector3(0, stepHeight + Body.CollisionInformation.Shape.CollisionMargin, 0);
            Vector3 startingLocation = headBlockageFinderOffset + Body.Position;

            foreach (Entity candidate in headCollisionPairCollector.CollisionInformation.OverlappedEntities)
            {
                //foreach (CollisionPair pair in headCollisionPairCollector.CollisionPairs)
                //{
                //    //Determine which member of the collision pair is the possible blockage.
                //    //The comparisons are all kept on a "parent" as opposed to "collider" level so that interaction with compound shapes is simpler.

                //    Entity candidate = pair.ParentA == headCollisionPairCollector ? pair.ParentB : pair.ParentA;
                //Ensure that the candidate is a valid blocking entity.
                if (candidate.CollisionInformation.CollisionRules.Personal > CollisionRule.Normal)
                {
                    continue; //It is invalid!
                }
                //Fire a convex cast at the candidate and determine some details!
                ConvexShape targetShape = candidate.CollisionInformation.Shape as ConvexShape;
                if (targetShape != null)
                {
                    RigidTransform sweepTransform  = new RigidTransform(startingLocation);
                    RigidTransform targetTransform = new RigidTransform(candidate.Position, candidate.Orientation);
                    RayHit         rayHit;
                    if (GJKToolbox.ConvexCast(castingShape, targetShape, ref sweep,
                                              ref sweepTransform, ref targetTransform,
                                              out rayHit))
                    {
                        return(false);
                    }
                }
            }
            return(true);
        }
コード例 #8
0
        ///<summary>
        /// Updates the time of impact for the pair.
        ///</summary>
        ///<param name="requester">Collidable requesting the update.</param>
        ///<param name="dt">Timestep duration.</param>
        public override void UpdateTimeOfImpact(Collidable requester, float dt)
        {
            BroadPhaseOverlap  overlap      = BroadPhaseOverlap;
            PositionUpdateMode triangleMode = triangle.entity == null
                ? PositionUpdateMode.Discrete
                : triangle.entity.PositionUpdateMode;
            PositionUpdateMode convexMode =
                convex.entity == null ? PositionUpdateMode.Discrete : convex.entity.PositionUpdateMode;

            if (
                (overlap.entryA.IsActive || overlap.entryB.IsActive) && //At least one has to be active.
                (
                    convexMode == PositionUpdateMode.Continuous &&      //If both are continuous, only do the process for A.
                    triangleMode == PositionUpdateMode.Continuous &&
                    overlap.entryA == requester ||
                    (convexMode == PositionUpdateMode.Continuous) ^ //If only one is continuous, then we must do it.
                    (triangleMode == PositionUpdateMode.Continuous)
                )
                )
            {
                //Only perform the test if the minimum radii are small enough relative to the size of the velocity.
                Vector3 velocity;
                if (convexMode == PositionUpdateMode.Discrete)
                //Triangle is static for the purposes of this continuous test.
                {
                    velocity = triangle.entity.linearVelocity;
                }
                else if (triangleMode == PositionUpdateMode.Discrete)
                //Convex is static for the purposes of this continuous test.
                {
                    Vector3.Negate(ref convex.entity.linearVelocity, out velocity);
                }
                else
                //Both objects are moving.
                {
                    Vector3.Subtract(ref triangle.entity.linearVelocity, ref convex.entity.linearVelocity,
                                     out velocity);
                }

                Vector3.Multiply(ref velocity, dt, out velocity);
                float velocitySquared = velocity.LengthSquared();

                float minimumRadiusA = convex.Shape.MinimumRadius * MotionSettings.CoreShapeScaling;
                timeOfImpact = 1;
                if (minimumRadiusA * minimumRadiusA < velocitySquared)
                {
                    //Spherecast A against B.
                    RayHit rayHit;
                    if (GJKToolbox.CCDSphereCast(new Ray(convex.worldTransform.Position, -velocity), minimumRadiusA,
                                                 triangle.Shape, ref triangle.worldTransform, timeOfImpact, out rayHit))
                    {
                        if (triangle.Shape.sidedness != TriangleSidedness.DoubleSided)
                        {
                            //Only perform sweep if the object is in danger of hitting the object.
                            //Triangles can be one sided, so check the impact normal against the triangle normal.
                            Vector3 AB, AC;
                            Vector3.Subtract(ref triangle.Shape.vB, ref triangle.Shape.vA, out AB);
                            Vector3.Subtract(ref triangle.Shape.vC, ref triangle.Shape.vA, out AC);
                            Vector3 normal;
                            Vector3.Cross(ref AB, ref AC, out normal);

                            float dot;
                            Vector3.Dot(ref rayHit.Normal, ref normal, out dot);
                            if (triangle.Shape.sidedness == TriangleSidedness.Counterclockwise && dot < 0 ||
                                triangle.Shape.sidedness == TriangleSidedness.Clockwise && dot > 0)
                            {
                                timeOfImpact = rayHit.T;
                            }
                        }
                        else
                        {
                            timeOfImpact = rayHit.T;
                        }
                    }
                }

                //TECHNICALLY, the triangle should be casted too.  But, given the way triangles are usually used and their tiny minimum radius, ignoring it is usually just fine.
                //var minimumRadiusB = triangle.minimumRadius * MotionSettings.CoreShapeScaling;
                //if (minimumRadiusB * minimumRadiusB < velocitySquared)
                //{
                //    //Spherecast B against A.
                //    RayHit rayHit;
                //    if (GJKToolbox.SphereCast(new Ray(triangle.entity.position, velocity), minimumRadiusB, convex.Shape, ref convex.worldTransform, 1, out rayHit) &&
                //        rayHit.T < timeOfImpact)
                //    {
                //        if (triangle.Shape.sidedness != TriangleSidedness.DoubleSided)
                //        {
                //            float dot;
                //            Vector3.Dot(ref rayHit.Normal, ref normal, out dot);
                //            if (dot > 0)
                //            {
                //                timeOfImpact = rayHit.T;
                //            }
                //        }
                //        else
                //        {
                //            timeOfImpact = rayHit.T;
                //        }
                //    }
                //}

                //If it's intersecting, throw our hands into the air and give up.
                //This is generally a perfectly acceptable thing to do, since it's either sitting
                //inside another object (no ccd makes sense) or we're still in an intersecting case
                //from a previous frame where CCD took place and a contact should have been created
                //to deal with interpenetrating velocity.  Sometimes that contact isn't sufficient,
                //but it's good enough.
                if (timeOfImpact == 0)
                {
                    timeOfImpact = 1;
                }
            }
        }
コード例 #9
0
        ///<summary>
        /// Updates the time of impact for the pair.
        ///</summary>
        ///<param name="requester">Collidable requesting the update.</param>
        ///<param name="dt">Timestep duration.</param>
        public override void UpdateTimeOfImpact(Collidable requester, float dt)
        {
            //TODO: This conditional early outing stuff could be pulled up into a common system, along with most of the pair handler.
            if (convex.IsActive && convex.entity.PositionUpdateMode == PositionUpdateMode.Continuous)
            {
                //TODO: This system could be made more robust by using a similar region-based rejection of edges.
                //CCD events are awfully rare under normal circumstances, so this isn't usually an issue.

                //Only perform the test if the minimum radii are small enough relative to the size of the velocity.
                Vector3 velocity;
                Vector3.Multiply(ref convex.entity.linearVelocity, dt, out velocity);
                float velocitySquared = velocity.LengthSquared();

                var minimumRadius = convex.Shape.minimumRadius * MotionSettings.CoreShapeScaling;
                timeOfImpact = 1;
                if (minimumRadius * minimumRadius < velocitySquared)
                {
                    var triangle = Resources.GetTriangle();
                    triangle.collisionMargin = 0;
                    Vector3 terrainUp = new Vector3(terrain.worldTransform.LinearTransform.M21, terrain.worldTransform.LinearTransform.M22, terrain.worldTransform.LinearTransform.M23);
                    //Spherecast against all triangles to find the earliest time.
                    for (int i = 0; i < TerrainManifold.overlappedTriangles.count; i++)
                    {
                        terrain.Shape.GetTriangle(ref TerrainManifold.overlappedTriangles.Elements[i], ref terrain.worldTransform, out triangle.vA, out triangle.vB, out triangle.vC);
                        //Put the triangle into 'localish' space of the convex.
                        Vector3.Subtract(ref triangle.vA, ref convex.worldTransform.Position, out triangle.vA);
                        Vector3.Subtract(ref triangle.vB, ref convex.worldTransform.Position, out triangle.vB);
                        Vector3.Subtract(ref triangle.vC, ref convex.worldTransform.Position, out triangle.vC);

                        RayHit rayHit;
                        if (GJKToolbox.CCDSphereCast(new Ray(Toolbox.ZeroVector, velocity), minimumRadius, triangle, ref Toolbox.RigidIdentity, timeOfImpact, out rayHit) &&
                            rayHit.T > Toolbox.BigEpsilon)
                        {
                            Vector3 AB, AC;
                            Vector3.Subtract(ref triangle.vB, ref triangle.vA, out AB);
                            Vector3.Subtract(ref triangle.vC, ref triangle.vA, out AC);
                            Vector3 normal;
                            Vector3.Cross(ref AC, ref AB, out normal);
                            float dot;
                            Vector3.Dot(ref normal, ref terrainUp, out dot);
                            if (dot < 0)
                            {
                                Vector3.Dot(ref normal, ref rayHit.Normal, out dot);
                            }
                            else
                            {
                                Vector3.Dot(ref normal, ref rayHit.Normal, out dot);
                                dot = -dot;
                            }
                            //Only perform sweep if the object is in danger of hitting the object.
                            //Triangles can be one sided, so check the impact normal against the triangle normal.
                            if (dot < 0)
                            {
                                timeOfImpact = rayHit.T;
                            }
                        }
                    }
                    Resources.GiveBack(triangle);
                }
            }
        }
コード例 #10
0
        ///<summary>
        /// Updates the time of impact for the pair.
        ///</summary>
        ///<param name="requester">Collidable requesting the update.</param>
        ///<param name="dt">Timestep duration.</param>
        public override void UpdateTimeOfImpact(Collidable requester, float dt)
        {
            var collidableA = CollidableA as ConvexCollidable;
            var collidableB = CollidableB as ConvexCollidable;
            var modeA       = collidableA.entity == null ? PositionUpdateMode.Discrete : collidableA.entity.PositionUpdateMode;
            var modeB       = collidableB.entity == null ? PositionUpdateMode.Discrete : collidableB.entity.PositionUpdateMode;

            var overlap = BroadPhaseOverlap;

            if (
                (overlap.entryA.IsActive || overlap.entryB.IsActive) &&     //At least one has to be active.
                (
                    (
                        modeA == PositionUpdateMode.Continuous &&       //If both are continuous, only do the process for A.
                        modeB == PositionUpdateMode.Continuous &&
                        overlap.entryA == requester
                    ) ||
                    (
                        modeA == PositionUpdateMode.Continuous ^       //If only one is continuous, then we must do it.
                        modeB == PositionUpdateMode.Continuous
                    )

                )
                )
            {
                //Only perform the test if the minimum radii are small enough relative to the size of the velocity.
                //Discrete objects have already had their linear motion integrated, so don't use their velocity.
                Vector3 velocity;
                if (modeA == PositionUpdateMode.Discrete)
                {
                    //CollidableA is static for the purposes of this continuous test.
                    velocity = collidableB.entity.linearVelocity;
                }
                else if (modeB == PositionUpdateMode.Discrete)
                {
                    //CollidableB is static for the purposes of this continuous test.
                    Vector3.Negate(ref collidableA.entity.linearVelocity, out velocity);
                }
                else
                {
                    //Both objects are moving.
                    Vector3.Subtract(ref collidableB.entity.linearVelocity, ref collidableA.entity.linearVelocity, out velocity);
                }
                Vector3.Multiply(ref velocity, dt, out velocity);
                float velocitySquared = velocity.LengthSquared();

                var minimumRadiusA = collidableA.Shape.MinimumRadius * MotionSettings.CoreShapeScaling;
                timeOfImpact = 1;
                if (minimumRadiusA * minimumRadiusA < velocitySquared)
                {
                    //Spherecast A against B.
                    RayHit rayHit;
                    if (GJKToolbox.CCDSphereCast(new Ray(collidableA.worldTransform.Position, -velocity), minimumRadiusA, collidableB.Shape, ref collidableB.worldTransform, timeOfImpact, out rayHit))
                    {
                        timeOfImpact = rayHit.T;
                    }
                }

                var minimumRadiusB = collidableB.Shape.MinimumRadius * MotionSettings.CoreShapeScaling;
                if (minimumRadiusB * minimumRadiusB < velocitySquared)
                {
                    //Spherecast B against A.
                    RayHit rayHit;
                    if (GJKToolbox.CCDSphereCast(new Ray(collidableB.worldTransform.Position, velocity), minimumRadiusB, collidableA.Shape, ref collidableA.worldTransform, timeOfImpact, out rayHit))
                    {
                        timeOfImpact = rayHit.T;
                    }
                }

                //If it's intersecting, throw our hands into the air and give up.
                //This is generally a perfectly acceptable thing to do, since it's either sitting
                //inside another object (no ccd makes sense) or we're still in an intersecting case
                //from a previous frame where CCD took place and a contact should have been created
                //to deal with interpenetrating velocity.  Sometimes that contact isn't sufficient,
                //but it's good enough.
                if (timeOfImpact == 0)
                {
                    timeOfImpact = 1;
                }
            }
        }
コード例 #11
0
        ///<summary>
        /// Updates the time of impact for the pair.
        ///</summary>
        ///<param name="requester">Collidable requesting the update.</param>
        ///<param name="dt">Timestep duration.</param>
        public override void UpdateTimeOfImpact(Collidable requester, float dt)
        {
            //TODO: This conditional early outing stuff could be pulled up into a common system, along with most of the pair handler.
            var overlap    = BroadPhaseOverlap;
            var convexMode = convex.entity == null ? PositionUpdateMode.Discrete : convex.entity.PositionUpdateMode;

            if (
                (mobileMesh.IsActive || (convex.entity == null ? false : convex.entity.activityInformation.IsActive)) &&     //At least one has to be active.
                (
                    (
                        convexMode == PositionUpdateMode.Continuous &&       //If both are continuous, only do the process for A.
                        mobileMesh.entity.PositionUpdateMode == PositionUpdateMode.Continuous &&
                        overlap.entryA == requester
                    ) ||
                    (
                        convexMode == PositionUpdateMode.Continuous ^       //If only one is continuous, then we must do it.
                        mobileMesh.entity.PositionUpdateMode == PositionUpdateMode.Continuous
                    )
                )
                )
            {
                //TODO: This system could be made more robust by using a similar region-based rejection of edges.
                //CCD events are awfully rare under normal circumstances, so this isn't usually an issue.

                //Only perform the test if the minimum radii are small enough relative to the size of the velocity.
                Vector3 velocity;
                if (convex.entity != null)
                {
                    Vector3.Subtract(ref convex.entity.linearVelocity, ref mobileMesh.entity.linearVelocity, out velocity);
                }
                else
                {
                    Vector3.Negate(ref mobileMesh.entity.linearVelocity, out velocity);
                }
                Vector3.Multiply(ref velocity, dt, out velocity);
                float velocitySquared = velocity.LengthSquared();

                var minimumRadius = convex.Shape.minimumRadius * MotionSettings.CoreShapeScaling;
                timeOfImpact = 1;
                if (minimumRadius * minimumRadius < velocitySquared)
                {
                    TriangleSidedness sidedness = mobileMesh.Shape.Sidedness;
                    Matrix3X3         orientation;
                    Matrix3X3.CreateFromQuaternion(ref mobileMesh.worldTransform.Orientation, out orientation);
                    var triangle = Resources.GetTriangle();
                    triangle.collisionMargin = 0;
                    //Spherecast against all triangles to find the earliest time.
                    for (int i = 0; i < MeshManifold.overlappedTriangles.count; i++)
                    {
                        MeshBoundingBoxTreeData data = mobileMesh.Shape.TriangleMesh.Data;
                        int triangleIndex            = MeshManifold.overlappedTriangles.Elements[i];
                        data.GetTriangle(triangleIndex, out triangle.vA, out triangle.vB, out triangle.vC);
                        Matrix3X3.Transform(ref triangle.vA, ref orientation, out triangle.vA);
                        Matrix3X3.Transform(ref triangle.vB, ref orientation, out triangle.vB);
                        Matrix3X3.Transform(ref triangle.vC, ref orientation, out triangle.vC);
                        Vector3.Add(ref triangle.vA, ref mobileMesh.worldTransform.Position, out triangle.vA);
                        Vector3.Add(ref triangle.vB, ref mobileMesh.worldTransform.Position, out triangle.vB);
                        Vector3.Add(ref triangle.vC, ref mobileMesh.worldTransform.Position, out triangle.vC);
                        //Put the triangle into 'localish' space of the convex.
                        Vector3.Subtract(ref triangle.vA, ref convex.worldTransform.Position, out triangle.vA);
                        Vector3.Subtract(ref triangle.vB, ref convex.worldTransform.Position, out triangle.vB);
                        Vector3.Subtract(ref triangle.vC, ref convex.worldTransform.Position, out triangle.vC);

                        RayHit rayHit;
                        if (GJKToolbox.CCDSphereCast(new Ray(Toolbox.ZeroVector, velocity), minimumRadius, triangle, ref Toolbox.RigidIdentity, timeOfImpact, out rayHit) &&
                            rayHit.T > Toolbox.BigEpsilon)
                        {
                            if (sidedness != TriangleSidedness.DoubleSided)
                            {
                                Vector3 AB, AC;
                                Vector3.Subtract(ref triangle.vB, ref triangle.vA, out AB);
                                Vector3.Subtract(ref triangle.vC, ref triangle.vA, out AC);
                                Vector3 normal;
                                Vector3.Cross(ref AB, ref AC, out normal);
                                float dot;
                                Vector3.Dot(ref normal, ref rayHit.Normal, out dot);
                                //Only perform sweep if the object is in danger of hitting the object.
                                //Triangles can be one sided, so check the impact normal against the triangle normal.
                                if (sidedness == TriangleSidedness.Counterclockwise && dot < 0 ||
                                    sidedness == TriangleSidedness.Clockwise && dot > 0)
                                {
                                    timeOfImpact = rayHit.T;
                                }
                            }
                            else
                            {
                                timeOfImpact = rayHit.T;
                            }
                        }
                    }
                    Resources.GiveBack(triangle);
                }
            }
        }
コード例 #12
0
        private bool DoExternalNear(TriangleShape triangle, out TinyStructList <ContactData> contactList)
        {
            Vector3 closestA, closestB;


            //Don't bother trying to do any clever caching.  The continually transforming simplex makes it very rarely useful.
            //TODO: Initialize the simplex of the GJK method using the 'true' center of the triangle.
            //If left unmodified, the simplex that is used in GJK will just be a point at 0,0,0, which of course is at the origin.
            //This causes an instant-out, always.  Not good!
            //By giving the contributing simplex the average centroid, it has a better guess.
            Vector3 triangleCentroid;

            Vector3.Add(ref triangle.vA, ref triangle.vB, out triangleCentroid);
            Vector3.Add(ref triangleCentroid, ref triangle.vC, out triangleCentroid);
            Vector3.Multiply(ref triangleCentroid, .33333333f, out triangleCentroid);

            CachedSimplex initialSimplex = new CachedSimplex
            {
                State = SimplexState.Point, LocalSimplexB = { A = triangleCentroid }
            };

            if (GJKToolbox.GetClosestPoints(convex, triangle, ref Toolbox.RigidIdentity, ref Toolbox.RigidIdentity,
                                            ref initialSimplex, out closestA, out closestB))
            {
                state = CollisionState.Deep;
                return(DoDeepContact(triangle, out contactList));
            }

            Vector3 displacement;

            Vector3.Subtract(ref closestB, ref closestA, out displacement);
            float distanceSquared = displacement.LengthSquared();
            float margin          = convex.collisionMargin + triangle.collisionMargin;

            contactList = new TinyStructList <ContactData>();
            if (distanceSquared < margin * margin)
            {
                //Try to generate a contact.
                ContactData contact = new ContactData();

                //Determine if the normal points in the appropriate direction given the sidedness of the triangle.
                if (triangle.sidedness != TriangleSidedness.DoubleSided)
                {
                    Vector3 triangleNormal, ab, ac;
                    Vector3.Subtract(ref triangle.vB, ref triangle.vA, out ab);
                    Vector3.Subtract(ref triangle.vC, ref triangle.vA, out ac);
                    Vector3.Cross(ref ab, ref ac, out triangleNormal);
                    float dot;
                    Vector3.Dot(ref triangleNormal, ref displacement, out dot);
                    if (triangle.sidedness == TriangleSidedness.Clockwise && dot > 0)
                    {
                        return(false);
                    }

                    if (triangle.sidedness == TriangleSidedness.Counterclockwise && dot < 0)
                    {
                        return(false);
                    }
                }


                //Displacement is from A to B.  point = A + t * AB, where t = marginA / margin.
                if (margin > Toolbox.Epsilon)                                                                  //This can be zero! It would cause a NaN if unprotected.
                {
                    Vector3.Multiply(ref displacement, convex.collisionMargin / margin, out contact.Position); //t * AB
                }
                else
                {
                    contact.Position = new Vector3();
                }

                Vector3.Add(ref closestA, ref contact.Position, out contact.Position); //A + t * AB.


                contact.Normal = displacement;
                float distance = (float)Math.Sqrt(distanceSquared);
                Vector3.Divide(ref contact.Normal, distance, out contact.Normal);
                contact.PenetrationDepth = margin - distance;


                contactList.Add(ref contact);
                TryToEscape(triangle, ref contact.Position);
                return(true);
            }

            //Too far to make a contact- move back to separation.
            state = CollisionState.ExternalSeparated;
            return(false);
        }
コード例 #13
0
        private bool DoShallowContact(out ContactData contact)
        {
            Vector3 closestA, closestB;

            //RigidTransform transform = RigidTransform.Identity;
            //Vector3 closestAnew, closestBnew;
            //CachedSimplex cachedTest = cachedSimplex;
            //bool intersecting = GJKToolbox.GetClosestPoints(informationA.Shape, informationB.Shape, ref informationA.worldTransform, ref informationB.worldTransform, ref cachedTest, out closestAnew, out closestBnew);

            ////bool otherIntersecting = OldGJKVerifier.GetClosestPointsBetweenObjects(informationA.Shape, informationB.Shape, ref informationA.worldTransform, ref informationB.worldTransform, 0, 0, out closestA, out closestB);
            //bool otherIntersecting = GJKToolbox.GetClosestPoints(informationA.Shape, informationB.Shape, ref informationA.worldTransform, ref informationB.worldTransform, out closestA, out closestB);

            //Vector3 closestAold, closestBold;
            //bool oldIntersecting = OldGJKVerifier.GetClosestPointsBetweenObjects(informationA.Shape, informationB.Shape, ref informationA.worldTransform, ref informationB.worldTransform, 0, 0, out closestAold, out closestBold);

            //if (otherIntersecting != intersecting || (!otherIntersecting && !intersecting &&
            //    Vector3.DistanceSquared(closestAnew, closestBnew) - Vector3.DistanceSquared(closestA, closestB) > .0001f &&
            //    (Vector3.DistanceSquared(closestA, closestAnew) > .0001f ||
            //    Vector3.DistanceSquared(closestB, closestBnew) > .0001f)))// ||
            //    //Math.Abs(Vector3.Dot(closestB - closestA, closestBnew - closestAnew) - Vector3.Dot(closestB - closestA, closestB - closestA)) > Toolbox.Epsilon)))
            //    Debug.WriteLine("Break.");

            //Vector3 sub;
            //Vector3.Subtract(ref closestA, ref closestB, out sub);
            //if (sub.LengthSquared() < Toolbox.Epsilon)

            if (UseSimplexCaching)
            {
                GJKToolbox.GetClosestPoints(collidableA.Shape, collidableB.Shape, ref collidableA.worldTransform, ref collidableB.worldTransform, ref cachedSimplex, out closestA, out closestB);
            }
            else
            {
                //The initialization of the pair creates a pretty decent simplex to start from.
                //Just don't try to update it.
                CachedSimplex preInitializedSimplex = cachedSimplex;
                GJKToolbox.GetClosestPoints(collidableA.Shape, collidableB.Shape, ref collidableA.worldTransform, ref collidableB.worldTransform, ref preInitializedSimplex, out closestA, out closestB);
            }

            Vector3 displacement;

            Vector3.Subtract(ref closestB, ref closestA, out displacement);
            float distanceSquared = displacement.LengthSquared();

            if (distanceSquared < Toolbox.Epsilon)
            {
                state = CollisionState.DeepContact;
                return(DoDeepContact(out contact));
            }

            localDirection = displacement; //Use this as the direction for future deep contacts.
            float margin = collidableA.Shape.collisionMargin + collidableB.Shape.collisionMargin;


            if (distanceSquared < margin * margin)
            {
                //Generate a contact.
                contact = new ContactData();
                //Displacement is from A to B.  point = A + t * AB, where t = marginA / margin.
                if (margin > Toolbox.Epsilon)                                                                             //Avoid a NaN!
                {
                    Vector3.Multiply(ref displacement, collidableA.Shape.collisionMargin / margin, out contact.Position); //t * AB
                }
                else
                {
                    contact.Position = new Vector3();
                }

                Vector3.Add(ref closestA, ref contact.Position, out contact.Position); //A + t * AB.

                contact.Normal = displacement;
                float distance = (float)Math.Sqrt(distanceSquared);
                Vector3.Divide(ref contact.Normal, distance, out contact.Normal);
                contact.PenetrationDepth = margin - distance;
                return(true);
            }
            //Too shallow to make a contact- move back to separation.
            state   = CollisionState.Separated;
            contact = new ContactData();
            return(false);
        }
        /// <summary>
        /// Constructs a new demo.
        /// </summary>
        /// <param name="game">Game owning this demo.</param>
        public BooleanConvexTestDemo(DemosGame game)
            : base(game)
        {
            var random = new Random();

            int numberOfConfigurations        = 1000;
            int numberOfTestsPerConfiguration = 10000;


            float size            = 2;
            var   aPositionBounds = new BoundingBox(new Vector3(-size, -size, -size), new Vector3(size, size, size));
            var   bPositionBounds = new BoundingBox(new Vector3(-size, -size, -size), new Vector3(size, size, size));

            size = 1;
            var aShapeBounds = new BoundingBox(new Vector3(-size, -size, -size), new Vector3(size, size, size));
            var bShapeBounds = new BoundingBox(new Vector3(-size, -size, -size), new Vector3(size, size, size));
            int pointsInA    = 10;
            int pointsInB    = 10;



            RawList <Vector3> points = new RawList <Vector3>();

            long accumulatedMPR = 0;
            long accumulatedGJK = 0;
            long accumulatedGJKSeparatingAxis = 0;


            for (int i = 0; i < numberOfConfigurations; i++)
            {
                //Create two convex hull shapes.
                for (int j = 0; j < pointsInA; j++)
                {
                    Vector3 point;
                    GetRandomPointInBoundingBox(random, ref aShapeBounds, out point);
                    points.Add(point);
                }
                var a = new ConvexHullShape(points);
                points.Clear();
                for (int j = 0; j < pointsInB; j++)
                {
                    Vector3 point;
                    GetRandomPointInBoundingBox(random, ref bShapeBounds, out point);
                    points.Add(point);
                }
                var b = new ConvexHullShape(points);
                points.Clear();

                //Generate some random tranforms for the shapes.
                RigidTransform aTransform;
                var            axis  = Vector3.Normalize(new Vector3((float)((random.NextDouble() - .5f) * 2), (float)((random.NextDouble() - .5f) * 2), (float)((random.NextDouble() - .5f) * 2)));
                var            angle = (float)random.NextDouble() * MathHelper.TwoPi;
                Quaternion.CreateFromAxisAngle(ref axis, angle, out aTransform.Orientation);
                GetRandomPointInBoundingBox(random, ref aPositionBounds, out aTransform.Position);

                RigidTransform bTransform;
                axis  = Vector3.Normalize(new Vector3((float)((random.NextDouble() - .5f) * 2), (float)((random.NextDouble() - .5f) * 2), (float)((random.NextDouble() - .5f) * 2)));
                angle = (float)random.NextDouble() * MathHelper.TwoPi;
                Quaternion.CreateFromAxisAngle(ref axis, angle, out bTransform.Orientation);
                GetRandomPointInBoundingBox(random, ref bPositionBounds, out bTransform.Position);

                //Perform MPR tests.
                //Warm up the cache a bit.
                MPRToolbox.AreShapesOverlapping(a, b, ref aTransform, ref bTransform);
                long start = Stopwatch.GetTimestamp();
                for (int j = 0; j < numberOfTestsPerConfiguration; j++)
                {
                    if (MPRToolbox.AreShapesOverlapping(a, b, ref aTransform, ref bTransform))
                    {
                        overlapsMPR++;
                    }
                }
                long end = Stopwatch.GetTimestamp();
                accumulatedMPR += end - start;

                //Perform GJK tests.
                //Warm up the cache a bit.
                GJKToolbox.AreShapesIntersecting(a, b, ref aTransform, ref bTransform);
                start = Stopwatch.GetTimestamp();
                for (int j = 0; j < numberOfTestsPerConfiguration; j++)
                {
                    if (GJKToolbox.AreShapesIntersecting(a, b, ref aTransform, ref bTransform))
                    {
                        overlapsGJK++;
                    }
                }
                end             = Stopwatch.GetTimestamp();
                accumulatedGJK += end - start;

                //Perform GJK Separating Axis tests.
                //Warm up the cache a bit.
                Vector3 localSeparatingAxis = Vector3.Up;
                GJKToolbox.AreShapesIntersecting(a, b, ref aTransform, ref bTransform, ref localSeparatingAxis);
                start = Stopwatch.GetTimestamp();
                for (int j = 0; j < numberOfTestsPerConfiguration; j++)
                {
                    if (GJKToolbox.AreShapesIntersecting(a, b, ref aTransform, ref bTransform, ref localSeparatingAxis))
                    {
                        overlapsGJKSeparatingAxis++;
                    }
                }
                end = Stopwatch.GetTimestamp();
                accumulatedGJKSeparatingAxis += end - start;
            }

            //Compute the actual time per test.
            long denominator = Stopwatch.Frequency * numberOfConfigurations * numberOfTestsPerConfiguration;

            timeMPR = (double)accumulatedMPR / denominator;
            timeGJK = (double)accumulatedGJK / denominator;
            timeGJKSeparatingAxis = (double)accumulatedGJKSeparatingAxis / denominator;
        }
コード例 #15
0
        private bool DoShallowContact(out ContactData contact)
        {
            Vector3 closestA, closestB;


            if (UseSimplexCaching)
            {
                GJKToolbox.GetClosestPoints(collidableA.Shape, collidableB.Shape, ref collidableA.worldTransform,
                                            ref collidableB.worldTransform, ref cachedSimplex, out closestA, out closestB);
            }
            else
            {
                //The initialization of the pair creates a pretty decent simplex to start from.
                //Just don't try to update it.
                CachedSimplex preInitializedSimplex = cachedSimplex;
                GJKToolbox.GetClosestPoints(collidableA.Shape, collidableB.Shape, ref collidableA.worldTransform,
                                            ref collidableB.worldTransform, ref preInitializedSimplex, out closestA, out closestB);
            }

            Vector3 displacement;

            Vector3.Subtract(ref closestB, ref closestA, out displacement);
            float distanceSquared = displacement.LengthSquared();

            if (distanceSquared < Toolbox.Epsilon)
            {
                state = CollisionState.DeepContact;
                return(DoDeepContact(out contact));
            }

            localDirection = displacement; //Use this as the direction for future deep contacts.
            float margin = collidableA.Shape.collisionMargin + collidableB.Shape.collisionMargin;


            if (distanceSquared < margin * margin)
            {
                //Generate a contact.
                contact = new ContactData();
                //Displacement is from A to B.  point = A + t * AB, where t = marginA / margin.
                if (margin > Toolbox.Epsilon) //Avoid a NaN!
                {
                    Vector3.Multiply(ref displacement, collidableA.Shape.collisionMargin / margin,
                                     out contact.Position); //t * AB
                }
                else
                {
                    contact.Position = new Vector3();
                }

                Vector3.Add(ref closestA, ref contact.Position, out contact.Position); //A + t * AB.

                contact.Normal = displacement;
                float distance = (float)Math.Sqrt(distanceSquared);
                Vector3.Divide(ref contact.Normal, distance, out contact.Normal);
                contact.PenetrationDepth = margin - distance;
                return(true);
            }

            //Too shallow to make a contact- move back to separation.
            state   = CollisionState.Separated;
            contact = new ContactData();
            return(false);
        }