CharacterContactPositionState TrySupportLocation(ConvexCollidable <CylinderShape> queryObject, ref Vector3 position, out float hintOffset,
ref QuickList <CharacterContact> tractionContacts, ref QuickList <CharacterContact> supportContacts, ref QuickList <CharacterContact> sideContacts, ref QuickList <CharacterContact> headContacts)
{
hintOffset = 0;
PrepareQueryObject(queryObject, ref position);
QueryManager.QueryContacts(queryObject, ref tractionContacts, ref supportContacts, ref sideContacts, ref headContacts, true);
bool obstructed = IsObstructed(ref sideContacts, ref headContacts);
if (obstructed)
{
return(CharacterContactPositionState.Obstructed);
}
if (supportContacts.Count > 0)
{
CharacterContactPositionState supportState;
CharacterContact supportContact;
QueryManager.AnalyzeSupportState(ref tractionContacts, ref supportContacts, out supportState, out supportContact);
var down = characterBody.orientationMatrix.Down;
//Note that traction is not tested for; it isn't important for the stance manager.
if (supportState == CharacterContactPositionState.Accepted)
{
//We're done! The guess found a good spot to stand on.
//We need to have fairly good contacts after this process, so only push it up a bit.
hintOffset = Math.Min(0, Vector3.Dot(supportContact.Contact.Normal, down) * (CollisionDetectionSettings.AllowedPenetration * .5f - supportContact.Contact.PenetrationDepth));
return(CharacterContactPositionState.Accepted);
}
else if (supportState == CharacterContactPositionState.TooDeep)
{
//Looks like we have to keep trying, but at least we found a good hint.
hintOffset = Math.Min(0, Vector3.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 - Vector3.Dot(supportContact.Contact.Normal, down) * supportContact.Contact.PenetrationDepth;
return(CharacterContactPositionState.NoHit);
}
}
else //Not obstructed, but no support.
{
return(CharacterContactPositionState.NoHit);
}
}
CharacterContactPositionState TryDownStepPosition(ref Vector3 position, ref 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 (IsDownStepObstructed(ref sideContacts))
{
return(CharacterContactPositionState.Obstructed);
}
//Note the use of traction contacts. We only want to step down if there are traction contacts to support us.
if (tractionContacts.Count > 0)
{
CharacterContactPositionState supportState;
CharacterContact supportContact;
QueryManager.AnalyzeSupportState(ref tractionContacts, ref supportContacts, out supportState, out supportContact);
if (supportState == CharacterContactPositionState.Accepted)
{
//We're done! The guess found a good spot to stand on.
//The final state doesn't need to actually create contacts, so shove it up
//just barely to the surface.
hintOffset = -Vector3.Dot(supportContact.Contact.Normal, down) * supportContact.Contact.PenetrationDepth;
return(CharacterContactPositionState.Accepted);
}
else if (supportState == CharacterContactPositionState.TooDeep)
{
//Looks like we have to keep trying, but at least we found a good hint.
hintOffset = Math.Min(0, .001f - Vector3.Dot(supportContact.Contact.Normal, down) * 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 - Vector3.Dot(supportContact.Contact.Normal, down) * supportContact.Contact.PenetrationDepth;
return(CharacterContactPositionState.NoHit);
}
}
else
{
return(CharacterContactPositionState.NoHit);
}
}
/// <summary>
/// Checks if a transition from the current stance to the target stance is possible given the current environment.
/// </summary>
/// <param name="targetStance">Stance to check for transition safety.</param>
/// <param name="newHeight">If the transition is safe, the new height of the character. Zero otherwise.</param>
/// <param name="newPosition">If the transition is safe, the new location of the character body if the transition occurred. Zero vector otherwise.</param>
/// <returns>True if the target stance is different than the current stance and the transition is valid, false otherwise.</returns>
public bool CheckTransition(Stance targetStance, out float newHeight, out Vector3 newPosition)
{
var currentPosition = characterBody.position;
var down = characterBody.orientationMatrix.Down;
newPosition = new Vector3();
newHeight = 0;
if (CurrentStance != targetStance)
{
float currentHeight;
switch (CurrentStance)
{
case Stance.Prone:
currentHeight = proneHeight;
break;
case Stance.Crouching:
currentHeight = crouchingHeight;
break;
default:
currentHeight = standingHeight;
break;
}
float targetHeight;
switch (targetStance)
{
case Stance.Prone:
targetHeight = proneHeight;
break;
case Stance.Crouching:
targetHeight = crouchingHeight;
break;
default:
targetHeight = standingHeight;
break;
}
if (currentHeight >= targetHeight)
{
//The character is getting smaller, so no validation queries are required.
if (SupportFinder.HasSupport)
{
//On the ground, so need to move the position down.
newPosition = currentPosition + down * ((currentHeight - targetHeight) * 0.5f);
}
else
{
//We're in the air, so we don't have to change the position at all- just change the height.
newPosition = currentPosition;
}
newHeight = targetHeight;
return(true);
}
//The character is getting bigger, so validation is required.
ConvexCollidable <CylinderShape> queryObject;
switch (targetStance)
{
case Stance.Prone:
queryObject = proneQueryObject;
break;
case Stance.Crouching:
queryObject = crouchingQueryObject;
break;
default:
queryObject = standingQueryObject;
break;
}
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
{
if (SupportFinder.HasSupport)
{
//Increasing in size requires a query to verify that the new state is safe.
//TODO: State queries can be expensive if the character is crouching beneath something really detailed.
//There are some situations where you may want to do an upwards-pointing ray cast first. If it hits something,
//there's no need to do the full query.
newPosition = currentPosition - down * ((targetHeight - currentHeight) * .5f);
PrepareQueryObject(queryObject, ref newPosition);
QueryManager.QueryContacts(queryObject, ref tractionContacts, ref supportContacts, ref sideContacts, ref headContacts, true);
if (IsObstructed(ref supportContacts, ref sideContacts, ref headContacts))
{
//Can't stand up if something is in the way!
return(false);
}
newHeight = targetHeight;
return(true);
}
else
{
//This is a complicated case. We must perform a semi-downstep query.
//It's different than a downstep because the head may be obstructed as well. Also, in downstepping, the character always goes *down*.
//In this, while the bottom of the character is extending downward, the character position actually either stays the same or goes up.
//(We arbitrarily ignore the case where the character could push off a ceiling.)
//The goal is to put the feet of the character on any support that can be found, and then verify that the rest of its body fits in that location.
float lowestBound = 0;
float originalHighestBound = (targetHeight - currentHeight) * -.5f;
float highestBound = originalHighestBound;
float currentOffset = 0;
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.
var lastState = CharacterContactPositionState.Accepted;
while (attempts++ < 5 && lowestBound - highestBound > Toolbox.BigEpsilon)
{
Vector3 candidatePosition = currentPosition + currentOffset * down;
float hintOffset;
switch (lastState = TrySupportLocation(queryObject, ref candidatePosition, out hintOffset, ref tractionContacts, ref supportContacts, ref sideContacts, ref headContacts))
{
case CharacterContactPositionState.Accepted:
currentOffset += hintOffset;
//Only use the new position location if the movement distance was the right size.
if (currentOffset <= 0 && currentOffset >= originalHighestBound)
{
newPosition = currentPosition + currentOffset * down;
newHeight = targetHeight;
return(true);
}
else
{
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.TooDeep:
currentOffset += hintOffset;
lowestBound = currentOffset;
break;
}
}
//If it terminated in a safe state, it can increase in size.
//TODO: You could handle this more efficiently by early terminating the above loop once obstruction is clearly unavoidable.
//(Not messing with it right now because this is likely going to change completely.)
if (lastState == CharacterContactPositionState.Accepted || lastState == CharacterContactPositionState.NoHit)
{
newPosition = currentPosition;
newHeight = targetHeight;
return(true);
}
return(false);
}
}
finally
{
tractionContacts.Dispose();
supportContacts.Dispose();
sideContacts.Dispose();
headContacts.Dispose();
}
}
return(false);
}
CharacterContactPositionState TryUpStepPosition(ref Vector3 sideNormal, ref Vector3 position, ref 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;
Vector3.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++)
{
Vector3.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, Vector3.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 = Vector3.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();
Vector3.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, Vector3.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();
Vector3.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, Vector3.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 - Vector3.Dot(supportContact.Contact.Normal, down) * supportContact.Contact.PenetrationDepth;
return(CharacterContactPositionState.NoHit);
}
}
else if (obstructed)
{
return(CharacterContactPositionState.Obstructed);
}
else
{
return(CharacterContactPositionState.NoHit);
}
}