/// <summary>
/// Constructs a stance manager for a character.
/// </summary>
/// <param name="characterBody">The character's body entity.</param>
/// <param name="crouchingHeight">Crouching height of the character.</param>
/// <param name="proneHeight">Prone height of the character.</param>
/// <param name="queryManager">Provider of queries used by the stance manager to test if it is okay to change stances.</param>
/// <param name="supportFinder">Support finder used by the character.</param>
public StanceManager(Cylinder characterBody, float crouchingHeight, float proneHeight, QueryManager queryManager, SupportFinder supportFinder)
{
this.QueryManager = queryManager;
this.SupportFinder = supportFinder;
this.characterBody = characterBody;
standingHeight = characterBody.Height;
if (crouchingHeight < standingHeight)
this.crouchingHeight = crouchingHeight;
else
throw new ArgumentException("Crouching height must be less than standing height.");
if (proneHeight < crouchingHeight)
this.proneHeight = proneHeight;
else
throw new ArgumentException("Prone height must be less than crouching height.");
//We can share the real shape with the query objects.
currentQueryObject = new ConvexCollidable<CylinderShape>(characterBody.CollisionInformation.Shape);
standingQueryObject = new ConvexCollidable<CylinderShape>(new CylinderShape(StandingHeight, characterBody.Radius) { CollisionMargin = currentQueryObject.Shape.CollisionMargin });
crouchingQueryObject = new ConvexCollidable<CylinderShape>(new CylinderShape(CrouchingHeight, characterBody.Radius) { CollisionMargin = currentQueryObject.Shape.CollisionMargin });
proneQueryObject = new ConvexCollidable<CylinderShape>(new CylinderShape(proneHeight, characterBody.Radius) { CollisionMargin = currentQueryObject.Shape.CollisionMargin });
//Share the collision rules between the main body and its query objects. That way, the character's queries return valid results.
currentQueryObject.CollisionRules = characterBody.CollisionInformation.CollisionRules;
standingQueryObject.CollisionRules = characterBody.CollisionInformation.CollisionRules;
crouchingQueryObject.CollisionRules = characterBody.CollisionInformation.CollisionRules;
proneQueryObject.CollisionRules = characterBody.CollisionInformation.CollisionRules;
}
float upStepMargin = .1f; //There's a little extra space above the maximum step height to start the obstruction and downcast test rays. Helps when a step is very close to the max step height.
#endregion Fields
#region Constructors
/// <summary>
/// Constructs a new step manager for a character.
/// </summary>
/// <param name="characterBody">The character's body.</param>
/// <param name="contactCategorizer">Contact categorizer used by the character.</param>
/// <param name="supportFinder">Support finder used by the character.</param>
/// <param name="queryManager">Query provider to use in checking for obstructions.</param>
/// <param name="horizontalMotionConstraint">Horizontal motion constraint used by the character. Source of 3d movement direction.</param>
public StepManager(Cylinder characterBody, CharacterContactCategorizer contactCategorizer, SupportFinder supportFinder, QueryManager queryManager, HorizontalMotionConstraint horizontalMotionConstraint)
{
this.characterBody = characterBody;
currentQueryObject = new ConvexCollidable<CylinderShape>(characterBody.CollisionInformation.Shape);
ContactCategorizer = contactCategorizer;
SupportFinder = supportFinder;
QueryManager = queryManager;
HorizontalMotionConstraint = horizontalMotionConstraint;
//The minimum step height is just barely above where the character would generally find the ground.
//This helps avoid excess tests.
minimumUpStepHeight = CollisionDetectionSettings.AllowedPenetration * 1.1f;// Math.Max(0, -.01f + character.Body.CollisionInformation.Shape.CollisionMargin * (1 - character.SupportFinder.sinMaximumSlope));
}
/// <summary>
/// Constructs a new character controller.
/// </summary>
/// <param name="position">Initial position of the character.</param>
/// <param name="radius">Radius of the character body.</param>
/// <param name="mass">Mass of the character body.</param>
/// <param name="maximumTractionSlope">Steepest slope, in radians, that the character can maintain traction on.</param>
/// <param name="maximumSupportSlope">Steepest slope, in radians, that the character can consider a support.</param>
/// <param name="speed">Speed at which the character will try to move while crouching with a support that provides traction.
/// Relative velocities with a greater magnitude will be decelerated.</param>
/// <param name="tractionForce">Maximum force that the character can apply while on a support which provides traction.</param>
/// <param name="slidingSpeed">Speed at which the character will try to move while on a support that does not provide traction.
/// Relative velocities with a greater magnitude will be decelerated.</param>
/// <param name="slidingForce">Maximum force that the character can apply while on a support which does not provide traction</param>
/// <param name="airSpeed">Speed at which the character will try to move with no support.
/// The character will not be decelerated while airborne.</param>
/// <param name="airForce">Maximum force that the character can apply with no support.</param>
/// <param name="jumpSpeed">Speed at which the character leaves the ground when it jumps</param>
/// <param name="slidingJumpSpeed">Speed at which the character leaves the ground when it jumps without traction</param>
/// <param name="maximumGlueForce">Maximum force the vertical motion constraint is allowed to apply in an attempt to keep the character on the ground.</param>
public SphereCharacterController(
System.Numerics.Vector3 position = new System.Numerics.Vector3(),
float radius = .85f, float mass = 10f,
float maximumTractionSlope = 0.8f, float maximumSupportSlope = 1.3f,
float speed = 8f, float tractionForce = 1000, float slidingSpeed = 6, float slidingForce = 50, float airSpeed = 1, float airForce = 250,
float jumpSpeed = 4.5f, float slidingJumpSpeed = 3,
float maximumGlueForce = 5000)
{
Body = new Sphere(position, radius, mass);
Body.IgnoreShapeChanges = true; //Wouldn't want inertia tensor recomputations to occur if the shape changes.
//Making the character a continuous object prevents it from flying through walls which would be pretty jarring from a player's perspective.
Body.PositionUpdateMode = PositionUpdateMode.Continuous;
Body.LocalInertiaTensorInverse = new Matrix3x3();
//TODO: In v0.16.2, compound bodies would override the material properties that get set in the CreatingPair event handler.
//In a future version where this is changed, change this to conceptually minimally required CreatingPair.
Body.CollisionInformation.Events.DetectingInitialCollision += RemoveFriction;
Body.LinearDamping = 0;
ContactCategorizer = new CharacterContactCategorizer(maximumTractionSlope, maximumSupportSlope);
QueryManager = new QueryManager(Body, ContactCategorizer);
SupportFinder = new SupportFinder(Body, QueryManager, ContactCategorizer);
HorizontalMotionConstraint = new HorizontalMotionConstraint(Body, SupportFinder);
HorizontalMotionConstraint.PositionAnchorDistanceThreshold = (3f / 17f) * radius;
VerticalMotionConstraint = new VerticalMotionConstraint(Body, SupportFinder, maximumGlueForce);
PairLocker = new CharacterPairLocker(Body);
Speed = speed;
TractionForce = tractionForce;
SlidingSpeed = slidingSpeed;
SlidingForce = slidingForce;
AirSpeed = airSpeed;
AirForce = airForce;
JumpSpeed = jumpSpeed;
SlidingJumpSpeed = slidingJumpSpeed;
//Enable multithreading for the sphere characters.
//See the bottom of the Update method for more information about using multithreading with this character.
IsUpdatedSequentially = false;
//Link the character body to the character controller so that it can be identified by the locker.
//Any object which replaces this must implement the ICharacterTag for locking to work properly.
Body.CollisionInformation.Tag = new CharacterSynchronizer(Body);
}
/// <summary>
/// Constructs a new character controller.
/// </summary>
/// <param name="position">Initial position of the character.</param>
/// <param name="height">Height of the character body while standing.</param>
/// <param name="crouchingHeight">Height of the character body while crouching.</param>
/// <param name="proneHeight">Height of the character body while prone.</param>
/// <param name="radius">Radius of the character body.</param>
/// <param name="margin">Radius of 'rounding' applied to the cylindrical body. Higher values make the cylinder's edges more rounded.
/// The margin is contained within the cylinder's height and radius, so it must not exceed the radius or height of the cylinder.
/// To change the collision margin later, use the CharacterController.CollisionMargin property.</param>
/// <param name="mass">Mass of the character body.</param>
/// <param name="maximumTractionSlope">Steepest slope, in radians, that the character can maintain traction on.</param>
/// <param name="maximumSupportSlope">Steepest slope, in radians, that the character can consider a support.</param>
/// <param name="standingSpeed">Speed at which the character will try to move while crouching with a support that provides traction.
/// Relative velocities with a greater magnitude will be decelerated.</param>
/// <param name="crouchingSpeed">Speed at which the character will try to move while crouching with a support that provides traction.
/// Relative velocities with a greater magnitude will be decelerated.</param>
/// <param name="proneSpeed">Speed at which the character will try to move while prone with a support that provides traction.
/// Relative velocities with a greater magnitude will be decelerated.</param>
/// <param name="tractionForce">Maximum force that the character can apply while on a support which provides traction.</param>
/// <param name="slidingSpeed">Speed at which the character will try to move while on a support that does not provide traction.
/// Relative velocities with a greater magnitude will be decelerated.</param>
/// <param name="slidingForce">Maximum force that the character can apply while on a support which does not provide traction</param>
/// <param name="airSpeed">Speed at which the character will try to move with no support.
/// The character will not be decelerated while airborne.</param>
/// <param name="airForce">Maximum force that the character can apply with no support.</param>
/// <param name="jumpSpeed">Speed at which the character leaves the ground when it jumps</param>
/// <param name="slidingJumpSpeed">Speed at which the character leaves the ground when it jumps without traction</param>
/// <param name="maximumGlueForce">Maximum force the vertical motion constraint is allowed to apply in an attempt to keep the character on the ground.</param>
public CharacterController(
Vector3 position = new Vector3(),
float height = 1.7f, float crouchingHeight = 1.7f * .7f, float proneHeight = 1.7f * 0.3f, float radius = 0.6f, float margin = 0.1f, float mass = 10f,
float maximumTractionSlope = 0.8f, float maximumSupportSlope = 1.3f,
float standingSpeed = 8f, float crouchingSpeed = 3f, float proneSpeed = 1.5f, float tractionForce = 1000, float slidingSpeed = 6, float slidingForce = 50, float airSpeed = 1, float airForce = 250,
float jumpSpeed = 4.5f, float slidingJumpSpeed = 3,
float maximumGlueForce = 5000
)
{
if (margin > radius || margin > crouchingHeight || margin > height)
throw new ArgumentException("Margin must not be larger than the character's radius or height.");
Body = new Cylinder(position, height, radius, mass);
Body.IgnoreShapeChanges = true; //Wouldn't want inertia tensor recomputations to occur when crouching and such.
Body.CollisionInformation.Shape.CollisionMargin = margin;
//Making the character a continuous object prevents it from flying through walls which would be pretty jarring from a player's perspective.
Body.PositionUpdateMode = PositionUpdateMode.Continuous;
Body.LocalInertiaTensorInverse = new Matrix3x3();
//TODO: In v0.16.2, compound bodies would override the material properties that get set in the CreatingPair event handler.
//In a future version where this is changed, change this to conceptually minimally required CreatingPair.
Body.CollisionInformation.Events.DetectingInitialCollision += RemoveFriction;
Body.LinearDamping = 0;
ContactCategorizer = new CharacterContactCategorizer(maximumTractionSlope, maximumSupportSlope);
QueryManager = new QueryManager(Body, ContactCategorizer);
SupportFinder = new SupportFinder(Body, QueryManager, ContactCategorizer);
HorizontalMotionConstraint = new HorizontalMotionConstraint(Body, SupportFinder);
HorizontalMotionConstraint.PositionAnchorDistanceThreshold = radius * 0.25f;
VerticalMotionConstraint = new VerticalMotionConstraint(Body, SupportFinder, maximumGlueForce);
StepManager = new StepManager(Body, ContactCategorizer, SupportFinder, QueryManager, HorizontalMotionConstraint);
StanceManager = new StanceManager(Body, crouchingHeight, proneHeight, QueryManager, SupportFinder);
PairLocker = new CharacterPairLocker(Body);
StandingSpeed = standingSpeed;
CrouchingSpeed = crouchingSpeed;
ProneSpeed = proneSpeed;
TractionForce = tractionForce;
SlidingSpeed = slidingSpeed;
SlidingForce = slidingForce;
AirSpeed = airSpeed;
AirForce = airForce;
JumpSpeed = jumpSpeed;
SlidingJumpSpeed = slidingJumpSpeed;
//Enable multithreading for the characters.
IsUpdatedSequentially = false;
//Link the character body to the character controller so that it can be identified by the locker.
//Any object which replaces this must implement the ICharacterTag for locking to work properly.
Body.CollisionInformation.Tag = new CharacterSynchronizer(Body);
}
/// <summary>
/// Constructs a new support finder.
/// </summary>
/// <param name="characterBody">Body entity used by the character.</param>
/// <param name="queryManager">Query provider used by the character. Used to perform ray cast tests against the character's near environment.</param>
/// <param name="contactCategorizer">Contact categorizer to use.</param>
public SupportFinder(Entity characterBody, QueryManager queryManager, CharacterContactCategorizer contactCategorizer)
{
this.characterBody = characterBody;
QueryManager = queryManager;
ContactCategorizer = contactCategorizer;
}
/// <summary>
/// Constructs a new support finder.
/// </summary>
/// <param name="characterBody">Body entity used by the character.</param>
/// <param name="queryManager">Query provider used by the character. Used to perform ray cast tests against the character's near environment.</param>
/// <param name="contactCategorizer">Contact categorizer to use.</param>
public SupportFinder(Entity characterBody, QueryManager queryManager, CharacterContactCategorizer contactCategorizer)
{
this.characterBody = characterBody;
QueryManager = queryManager;
ContactCategorizer = contactCategorizer;
}
/// <summary>
/// Constructs a new step manager for a character.
/// </summary>
/// <param name="characterBody">The character's body.</param>
/// <param name="contactCategorizer">Contact categorizer used by the character.</param>
/// <param name="supportFinder">Support finder used by the character.</param>
/// <param name="queryManager">Query provider to use in checking for obstructions.</param>
/// <param name="horizontalMotionConstraint">Horizontal motion constraint used by the character. Source of 3d movement direction.</param>
public StepManager(Cylinder characterBody, CharacterContactCategorizer contactCategorizer, SupportFinder supportFinder, QueryManager queryManager, HorizontalMotionConstraint horizontalMotionConstraint)
{
this.characterBody = characterBody;
currentQueryObject = new ConvexCollidable <CylinderShape>(characterBody.CollisionInformation.Shape);
ContactCategorizer = contactCategorizer;
SupportFinder = supportFinder;
QueryManager = queryManager;
HorizontalMotionConstraint = horizontalMotionConstraint;
//The minimum step height is just barely above where the character would generally find the ground.
//This helps avoid excess tests.
minimumUpStepHeight = CollisionDetectionSettings.AllowedPenetration * 1.1f;// Math.Max(0, -.01f + character.Body.CollisionInformation.Shape.CollisionMargin * (1 - character.SupportFinder.sinMaximumSlope));
}
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);
}
}
bool TryToStepUsingContact(ref ContactData contact, ref Vector3 down, out Vector3 newPosition)
{
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.
Vector3 normal = contact.Normal;
float dot;
Vector3.Dot(ref normal, ref down, out dot);
Vector3 error;
Vector3.Multiply(ref down, dot, out error);
Vector3.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;
Vector3.Multiply(ref down, characterBody.Height * .5f - downRayLength, out ray.Position);
Vector3.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 Vector3();
return(false);
}
//The down-cast ray origin has been verified by the previous ray cast.
//Let's look for a support!
Vector3 horizontalOffset;
Vector3.Multiply(ref normal, length, out horizontalOffset);
Vector3.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 Vector3();
return(false);
}
//Ensure the candidate surface supports traction.
Vector3 supportNormal;
Vector3.Normalize(ref earliestHit.Normal, out supportNormal);
//Calibrate the normal to face in the same direction as the down vector for consistency.
Vector3.Dot(ref supportNormal, ref down, out dot);
if (dot < 0)
{
Vector3.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 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.
Vector3.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 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.
Vector3.Multiply(ref normal, horizontalOffsetAmount, out horizontalOffset);
Vector3.Add(ref transform.Position, ref horizontalOffset, out transform.Position);
Vector3 verticalOffset;
Vector3.Multiply(ref down, -downRayLength, out verticalOffset);
Vector3.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.
Vector3 intersection;
Vector3.Dot(ref earliestHit.Location, ref supportNormal, out dot);
Plane plane = new Plane(supportNormal, dot);
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 Vector3();
return(false);
}
case CharacterContactPositionState.Rejected:
newPosition = new 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 Vector3();
return(false);
}
case CharacterContactPositionState.Rejected:
newPosition = new 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 Vector3();
return(false);
}
/// <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);
if (IsObstructed(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.
float highestBound = 0;
float lowestBound = (targetHeight - currentHeight) * .5f;
float currentOffset = lowestBound;
float maximum = lowestBound;
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)
{
Vector3 candidatePosition = currentPosition + currentOffset * down;
float hintOffset;
switch (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 < maximum)
{
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;
}
}
//Couldn't find a hit. Go ahead and get bigger!
newPosition = currentPosition;
newHeight = targetHeight;
return(true);
}
}
finally
{
tractionContacts.Dispose();
supportContacts.Dispose();
sideContacts.Dispose();
headContacts.Dispose();
}
}
return(false);
}
/// <summary>
/// Constructs a stance manager for a character.
/// </summary>
/// <param name="characterBody">The character's body entity.</param>
/// <param name="crouchingHeight">Crouching height of the character.</param>
/// <param name="proneHeight">Prone height of the character.</param>
/// <param name="queryManager">Provider of queries used by the stance manager to test if it is okay to change stances.</param>
/// <param name="supportFinder">Support finder used by the character.</param>
public StanceManager(Cylinder characterBody, float crouchingHeight, float proneHeight, QueryManager queryManager, SupportFinder supportFinder)
{
this.QueryManager = queryManager;
this.SupportFinder = supportFinder;
this.characterBody = characterBody;
standingHeight = characterBody.Height;
if (crouchingHeight < standingHeight)
{
this.crouchingHeight = crouchingHeight;
}
else
{
throw new ArgumentException("Crouching height must be less than standing height.");
}
if (proneHeight < crouchingHeight)
{
this.proneHeight = proneHeight;
}
else
{
throw new ArgumentException("Prone height must be less than crouching height.");
}
//We can share the real shape with the query objects.
currentQueryObject = new ConvexCollidable <CylinderShape>(characterBody.CollisionInformation.Shape);
standingQueryObject = new ConvexCollidable <CylinderShape>(new CylinderShape(StandingHeight, characterBody.Radius)
{
CollisionMargin = currentQueryObject.Shape.CollisionMargin
});
crouchingQueryObject = new ConvexCollidable <CylinderShape>(new CylinderShape(CrouchingHeight, characterBody.Radius)
{
CollisionMargin = currentQueryObject.Shape.CollisionMargin
});
proneQueryObject = new ConvexCollidable <CylinderShape>(new CylinderShape(proneHeight, characterBody.Radius)
{
CollisionMargin = currentQueryObject.Shape.CollisionMargin
});
//Share the collision rules between the main body and its query objects. That way, the character's queries return valid results.
currentQueryObject.CollisionRules = characterBody.CollisionInformation.CollisionRules;
standingQueryObject.CollisionRules = characterBody.CollisionInformation.CollisionRules;
crouchingQueryObject.CollisionRules = characterBody.CollisionInformation.CollisionRules;
proneQueryObject.CollisionRules = characterBody.CollisionInformation.CollisionRules;
}
