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);
}
}
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);
}