Exemplo n.º 1
0
        CharacterContactPositionState TryUpStepPosition(ref System.Numerics.Vector3 sideNormal, ref System.Numerics.Vector3 position, ref System.Numerics.Vector3 down,
                                                        ref QuickList <CharacterContact> tractionContacts, ref QuickList <CharacterContact> supportContacts, ref QuickList <CharacterContact> sideContacts, ref QuickList <CharacterContact> headContacts,
                                                        out float hintOffset)
        {
            hintOffset = 0;
            PrepareQueryObject(ref position);
            QueryManager.QueryContacts(currentQueryObject, ref tractionContacts, ref supportContacts, ref sideContacts, ref headContacts);
            if (headContacts.Count > 0)
            {
                //The head is obstructed.  This will define a maximum bound.
                //Find the deepest contact on the head and use it to provide a hint.
                float dot;
                Vector3Ex.Dot(ref down, ref headContacts.Elements[0].Contact.Normal, out dot);
                hintOffset = -dot * headContacts.Elements[0].Contact.PenetrationDepth;
                for (int i = 1; i < headContacts.Count; i++)
                {
                    Vector3Ex.Dot(ref down, ref headContacts.Elements[i].Contact.Normal, out dot);
                    dot *= -headContacts.Elements[i].Contact.PenetrationDepth;
                    if (dot > hintOffset)
                    {
                        hintOffset = dot;
                    }
                }
                return(CharacterContactPositionState.HeadObstructed);
            }
            bool obstructed = IsUpStepObstructedBySideContacts(ref sideNormal, ref sideContacts);

            if (!obstructed && supportContacts.Count > 0)
            {
                CharacterContactPositionState supportState;
                CharacterContact supportContact;
                QueryManager.AnalyzeSupportState(ref tractionContacts, ref supportContacts, out supportState, out supportContact);
                if (supportState == CharacterContactPositionState.Accepted)
                {
                    if (tractionContacts.Count > 0)
                    {
                        //We're done! The guess found a good spot to stand on.
                        //Unlike down stepping, upstepping DOES need good contacts in the final state.
                        //Push it up if necessary, but don't push it too far.
                        //Putting it into the middle of the allowed penetration makes it very likely that it will properly generate contacts.
                        //Choosing something smaller than allowed penetration ensures that the search makes meaningful progress forward when the sizes get really tiny;
                        //we wouldn't want it edging every closer to AllowedPenetration and then exit because too many queries were made.
                        hintOffset = Math.Min(0, Vector3Ex.Dot(supportContact.Contact.Normal, down) * (CollisionDetectionSettings.AllowedPenetration * .5f - supportContact.Contact.PenetrationDepth));
                        return(CharacterContactPositionState.Accepted);
                    }
                    else
                    {
                        //No traction... Before we give up and reject the step altogether, let's try one last thing.  It's possible that the character is trying to step up onto the side of a ramp or something.
                        //In this scenario, the top-down ray cast detects a perfectly walkable slope.  However, the contact queries will find a contact with a normal necessarily
                        //steeper than the one found by the ray cast because it is an edge contact.  Not being able to step up in this scenario doesn't feel natural to the player
                        //even if it is technically consistent.

                        //So, let's try to ray cast down to the a point just barely beyond the contact (to ensure we don't land right on the edge, which would invite numerical issues).
                        //Note that this is NOT equivalent to the ray cast we performed earlier to test for an initial step height and surface normal.
                        //This one is based on the QUERY state and the QUERY's contact position.

                        //Find the down test ray's position.
                        Ray downRay;
                        downRay.Position = supportContact.Contact.Position + sideNormal * .001f;
                        float verticalOffset = Vector3Ex.Dot(downRay.Position - position, down);
                        verticalOffset    = characterBody.Height * .5f + verticalOffset;
                        downRay.Position -= verticalOffset * down;
                        downRay.Direction = down;

                        //First, we must ensure that the ray cast test origin is not obstructed.  Starting very close to the very top of the character is safe because the process has already validated
                        //this location as accepted, just without traction.
                        Ray obstructionTestRay;
                        obstructionTestRay.Position  = position - down * (characterBody.Height * .5f);
                        obstructionTestRay.Direction = downRay.Position - obstructionTestRay.Position;

                        if (!QueryManager.RayCastHitAnything(obstructionTestRay, 1))
                        {
                            //Okay! it's safe to cast down, then.
                            RayHit hit;
                            if (QueryManager.RayCast(downRay, characterBody.Height, out hit))
                            {
                                //Got a hit!
                                if (characterBody.Height - maximumStepHeight < hit.T)
                                {
                                    //It's in range!
                                    float dot;
                                    hit.Normal.Normalize();
                                    Vector3Ex.Dot(ref hit.Normal, ref down, out dot);
                                    if (Math.Abs(dot) > ContactCategorizer.TractionThreshold)
                                    {
                                        //Slope is shallow enough to stand on!
                                        hintOffset = Math.Min(0, Vector3Ex.Dot(supportContact.Contact.Normal, down) * (CollisionDetectionSettings.AllowedPenetration * .5f - supportContact.Contact.PenetrationDepth));
                                        //ONE MORE thing to check.  The new position of the center ray must be able to touch the ground!
                                        downRay.Position = position;
                                        if (QueryManager.RayCast(downRay, characterBody.Height * .5f + maximumStepHeight, out hit))
                                        {
                                            //It hit.. almost there!
                                            hit.Normal.Normalize();
                                            Vector3Ex.Dot(ref hit.Normal, ref down, out dot);
                                            if (Math.Abs(dot) > ContactCategorizer.TractionThreshold)
                                            {
                                                //It has traction! We can step!
                                                return(CharacterContactPositionState.Accepted);
                                            }
                                        }
                                    }
                                }
                            }
                        }

                        //If it didn't have traction, and this was the most valid location we could find, then there is no support.
                        return(CharacterContactPositionState.Rejected);
                    }
                }
                else if (supportState == CharacterContactPositionState.TooDeep)
                {
                    //Looks like we have to keep trying, but at least we found a good hint.
                    hintOffset = Math.Min(0, Vector3Ex.Dot(supportContact.Contact.Normal, down) * (CollisionDetectionSettings.AllowedPenetration * .5f - supportContact.Contact.PenetrationDepth));
                    return(CharacterContactPositionState.TooDeep);
                }
                else //if (supportState == SupportState.Separated)
                {
                    //It's not obstructed, but the support isn't quite right.
                    //It's got a negative penetration depth.
                    //We can use that as a hint.
                    hintOffset = -.001f - Vector3Ex.Dot(supportContact.Contact.Normal, down) * supportContact.Contact.PenetrationDepth;
                    return(CharacterContactPositionState.NoHit);
                }
            }
            else if (obstructed)
            {
                return(CharacterContactPositionState.Obstructed);
            }
            else
            {
                return(CharacterContactPositionState.NoHit);
            }
        }
Exemplo n.º 2
0
        bool TryToStepUsingContact(ref ContactData contact, ref System.Numerics.Vector3 down, out System.Numerics.Vector3 newPosition)
        {
            System.Numerics.Vector3 position = characterBody.Position;
            //The normal of the contact may not be facing perfectly out to the side.
            //The detection process allows a bit of slop.
            //Correct it by removing any component of the normal along the local up vector.
            System.Numerics.Vector3 normal = contact.Normal;
            float dot;

            Vector3Ex.Dot(ref normal, ref down, out dot);
            System.Numerics.Vector3 error;
            Vector3Ex.Multiply(ref down, dot, out error);
            Vector3Ex.Subtract(ref normal, ref error, out normal);
            normal.Normalize();

            //Now we need to ray cast out from the center of the character in the direction of this normal to check for obstructions.
            //Compute the ray origin location.  Fire it out of the top of the character; if we're stepping, this must be a valid location.
            //Putting it as high as possible helps to reject more invalid step geometry.
            Ray   ray;
            float downRayLength = characterBody.Height;// MaximumStepHeight + upStepMargin;

            Vector3Ex.Multiply(ref down, characterBody.Height * .5f - downRayLength, out ray.Position);
            Vector3Ex.Add(ref ray.Position, ref position, out ray.Position);
            ray.Direction = normal;
            //Include a little margin in the length.
            //Technically, the character only needs to teleport horizontally by the complicated commented expression.
            //That puts it just far enough to have traction on the new surface.
            //In practice, the current contact refreshing approach used for many pair types causes contacts to persist horizontally a bit,
            //which can cause side effects for the character.
            float horizontalOffsetAmount = characterBody.CollisionInformation.Shape.CollisionMargin; // (float)((1 - character.SupportFinder.sinMaximumSlope) * character.Body.CollisionInformation.Shape.CollisionMargin + 0);
            float length = characterBody.Radius + horizontalOffsetAmount;                            // -contact.PenetrationDepth;


            if (QueryManager.RayCastHitAnything(ray, length))
            {
                //The step is obstructed!
                newPosition = new System.Numerics.Vector3();
                return(false);
            }

            //The down-cast ray origin has been verified by the previous ray cast.
            //Let's look for a support!
            System.Numerics.Vector3 horizontalOffset;
            Vector3Ex.Multiply(ref normal, length, out horizontalOffset);
            Vector3Ex.Add(ref ray.Position, ref horizontalOffset, out ray.Position);
            ray.Direction = down;

            //Find the earliest hit, if any.
            RayHit earliestHit;

            if (!QueryManager.RayCast(ray, downRayLength, out earliestHit) ||      //Can't do anything if it didn't hit.
                earliestHit.T <= 0 ||                                              //Can't do anything if the hit was invalid.
                earliestHit.T - downRayLength > -minimumUpStepHeight ||            //Don't bother doing anything if the step is too small.
                earliestHit.T - downRayLength < -maximumStepHeight - upStepMargin) //Can't do anything if the step is too tall.
            {
                //No valid hit was detected.
                newPosition = new System.Numerics.Vector3();
                return(false);
            }

            //Ensure the candidate surface supports traction.
            System.Numerics.Vector3 supportNormal;
            Vector3Ex.Normalize(ref earliestHit.Normal, out supportNormal);
            //Calibrate the normal to face in the same direction as the down vector for consistency.
            Vector3Ex.Dot(ref supportNormal, ref down, out dot);
            if (dot < 0)
            {
                Vector3Ex.Negate(ref supportNormal, out supportNormal);
                dot = -dot;
            }

            //If the new surface does not have traction, do not attempt to step up.
            if (dot < ContactCategorizer.TractionThreshold)
            {
                newPosition = new System.Numerics.Vector3();
                return(false);
            }

            //Since contact queries are frequently expensive compared to ray cast tests,
            //do one more ray cast test.  This time, starting from the same position, cast upwards.
            //In order to step up, the previous down-ray hit must be at least a character height away from the result of the up-ray.
            Vector3Ex.Negate(ref down, out ray.Direction);
            //Find the earliest hit, if any.
            //RayHit earliestHitUp = new RayHit();
            //earliestHitUp.T = float.MaxValue;
            float upLength = characterBody.Height - earliestHit.T;

            //If the sum of the up and down distances is less than the height, the character can't fit.
            if (QueryManager.RayCastHitAnything(ray, upLength))
            {
                newPosition = new System.Numerics.Vector3();
                return(false);
            }

            //By now, a valid ray hit has been found.  Now we need to validate it using contact queries.
            //This process is very similar in concept to the down step verification, but it has some extra
            //requirements.

            //Predict a hit location based on the time of impact and the normal at the intersection.
            //Take into account the radius of the character (don't forget the collision margin!)



            RigidTransform transform = characterBody.CollisionInformation.WorldTransform;

            //The transform must be modified to position the query body at the right location.
            //The horizontal offset of the queries ensures that a tractionable part of the character will be put onto the new support.
            Vector3Ex.Multiply(ref normal, horizontalOffsetAmount, out horizontalOffset);
            Vector3Ex.Add(ref transform.Position, ref horizontalOffset, out transform.Position);
            System.Numerics.Vector3 verticalOffset;
            Vector3Ex.Multiply(ref down, -downRayLength, out verticalOffset);
            Vector3Ex.Add(ref transform.Position, ref verticalOffset, out transform.Position);

            //We know that the closest point to the plane will be the extreme point in the plane's direction.
            //Use it as the ray origin.
            Ray downRay;

            characterBody.CollisionInformation.Shape.GetExtremePoint(supportNormal, ref transform, out downRay.Position);
            downRay.Direction = down;

            //Intersect the ray against the plane defined by the support hit.
            System.Numerics.Vector3 intersection;
            Vector3Ex.Dot(ref earliestHit.Location, ref supportNormal, out dot);
            Plane plane = new Plane(supportNormal, dot);

            System.Numerics.Vector3 candidatePosition;

            //Define the interval bounds to be used later.

            //The words 'highest' and 'lowest' here refer to the position relative to the character's body.
            //The ray cast points downward relative to the character's body.
            float highestBound  = -maximumStepHeight;
            float lowestBound   = characterBody.CollisionInformation.Shape.CollisionMargin - downRayLength + earliestHit.T;
            float currentOffset = lowestBound;
            float hintOffset;

            var tractionContacts = new QuickList <CharacterContact>(BufferPools <CharacterContact> .Thread);
            var supportContacts  = new QuickList <CharacterContact>(BufferPools <CharacterContact> .Thread);
            var sideContacts     = new QuickList <CharacterContact>(BufferPools <CharacterContact> .Thread);
            var headContacts     = new QuickList <CharacterContact>(BufferPools <CharacterContact> .Thread);

            try
            {
                //This guess may either win immediately, or at least give us a better idea of where to search.
                float hitT;
                if (Toolbox.GetRayPlaneIntersection(ref downRay, ref plane, out hitT, out intersection))
                {
                    hitT = -downRayLength + hitT + CollisionDetectionSettings.AllowedPenetration;
                    if (hitT < highestBound)
                    {
                        //Don't try a location known to be too high.
                        hitT = highestBound;
                    }
                    currentOffset = hitT;
                    if (currentOffset > lowestBound)
                    {
                        lowestBound = currentOffset;
                    }
                    candidatePosition = characterBody.Position + down * currentOffset + horizontalOffset;
                    switch (TryUpStepPosition(ref normal, ref candidatePosition, ref down,
                                              ref tractionContacts, ref supportContacts, ref sideContacts, ref headContacts,
                                              out hintOffset))
                    {
                    case CharacterContactPositionState.Accepted:
                        currentOffset += hintOffset;
                        //Only use the new position location if the movement distance was the right size.
                        if (currentOffset < 0 && currentOffset > -maximumStepHeight - CollisionDetectionSettings.AllowedPenetration)
                        {
                            //It's possible that we let a just-barely-too-high step occur, limited by the allowed penetration.
                            //Just clamp the overall motion and let it penetrate a bit.
                            newPosition = characterBody.Position + Math.Max(-maximumStepHeight, currentOffset) * down + horizontalOffset;
                            return(true);
                        }
                        else
                        {
                            newPosition = new System.Numerics.Vector3();
                            return(false);
                        }

                    case CharacterContactPositionState.Rejected:
                        newPosition = new System.Numerics.Vector3();
                        return(false);

                    case CharacterContactPositionState.NoHit:
                        highestBound  = currentOffset + hintOffset;
                        currentOffset = (lowestBound + currentOffset) * .5f;
                        break;

                    case CharacterContactPositionState.Obstructed:
                        lowestBound   = currentOffset;
                        currentOffset = (highestBound + currentOffset) * .5f;
                        break;

                    case CharacterContactPositionState.HeadObstructed:
                        highestBound  = currentOffset + hintOffset;
                        currentOffset = (lowestBound + currentOffset) * .5f;
                        break;

                    case CharacterContactPositionState.TooDeep:
                        currentOffset += hintOffset;
                        lowestBound    = currentOffset;
                        break;
                    }
                } //TODO: If the ray cast doesn't hit, that could be used to early out...  Then again, it pretty much can't happen.

                //Our guesses failed.
                //Begin the regular process.  Start at the time of impact of the ray itself.
                //How about trying the time of impact of the ray itself?

                //Since we wouldn't be here unless there were no contacts at the body's current position,
                //testing the ray cast location gives us the second bound we need to do an informed binary search.



                int attempts = 0;
                //Don't keep querying indefinitely.  If we fail to reach it in a few informed steps, it's probably not worth continuing.
                //The bound size check prevents the system from continuing to search a meaninglessly tiny interval.
                while (attempts++ < 5 && lowestBound - highestBound > Toolbox.BigEpsilon)
                {
                    candidatePosition = characterBody.Position + currentOffset * down + horizontalOffset;
                    switch (TryUpStepPosition(ref normal, ref candidatePosition, ref down,
                                              ref tractionContacts, ref supportContacts, ref sideContacts, ref headContacts,
                                              out hintOffset))
                    {
                    case CharacterContactPositionState.Accepted:
                        currentOffset += hintOffset;
                        //Only use the new position location if the movement distance was the right size.
                        if (currentOffset < 0 && currentOffset > -maximumStepHeight - CollisionDetectionSettings.AllowedPenetration)
                        {
                            //It's possible that we let a just-barely-too-high step occur, limited by the allowed penetration.
                            //Just clamp the overall motion and let it penetrate a bit.
                            newPosition = characterBody.Position + Math.Max(-maximumStepHeight, currentOffset) * down + horizontalOffset;
                            return(true);
                        }
                        else
                        {
                            newPosition = new System.Numerics.Vector3();
                            return(false);
                        }

                    case CharacterContactPositionState.Rejected:
                        newPosition = new System.Numerics.Vector3();
                        return(false);

                    case CharacterContactPositionState.NoHit:
                        highestBound  = currentOffset + hintOffset;
                        currentOffset = (lowestBound + highestBound) * .5f;
                        break;

                    case CharacterContactPositionState.Obstructed:
                        lowestBound   = currentOffset;
                        currentOffset = (highestBound + lowestBound) * .5f;
                        break;

                    case CharacterContactPositionState.HeadObstructed:
                        highestBound  = currentOffset + hintOffset;
                        currentOffset = (lowestBound + currentOffset) * .5f;
                        break;

                    case CharacterContactPositionState.TooDeep:
                        currentOffset += hintOffset;
                        lowestBound    = currentOffset;
                        break;
                    }
                }
            }
            finally
            {
                tractionContacts.Dispose();
                supportContacts.Dispose();
                sideContacts.Dispose();
                headContacts.Dispose();
            }
            //Couldn't find a candidate.
            newPosition = new System.Numerics.Vector3();
            return(false);
        }