/// <summary>
/// Changes the relative velocity between the character and its support.
/// </summary>
/// <param name="supportData">Support data to use to jump.</param>
/// <param name="velocityChange">Change to apply to the character and support relative velocity.</param>
/// <param name="relativeVelocity">Relative velocity to update.</param>
void ApplyJumpVelocity(ref SupportData supportData, Vector3 velocityChange, ref Vector3 relativeVelocity)
{
Body.LinearVelocity += velocityChange;
var entityCollidable = supportData.SupportObject as EntityCollidable;
if (entityCollidable != null)
{
if (entityCollidable.Entity.IsDynamic)
{
Vector3 change = velocityChange * jumpForceFactor;
//Multiple characters cannot attempt to modify another entity's velocity at the same time.
entityCollidable.Entity.Locker.Enter();
try
{
entityCollidable.Entity.LinearMomentum += change * -Body.Mass;
}
finally
{
entityCollidable.Entity.Locker.Exit();
}
velocityChange += change;
}
}
//Update the relative velocity as well. It's a ref parameter, so this update will be reflected in the calling scope.
Vector3.Add(ref relativeVelocity, ref velocityChange, out relativeVelocity);
}
void ComputeRelativeVelocity(ref SupportData supportData, out Vector3 relativeVelocity)
{
//Compute the relative velocity between the body and its support, if any.
//The relative velocity will be updated as impulses are applied.
relativeVelocity = Body.LinearVelocity;
if (SupportFinder.HasSupport)
{
//Only entities have velocity.
var entityCollidable = supportData.SupportObject as EntityCollidable;
if (entityCollidable != null)
{
//It's possible for the support's velocity to change due to another character jumping if the support is dynamic.
//Don't let that happen while the character is computing a relative velocity!
Vector3 entityVelocity;
bool locked = entityCollidable.Entity.IsDynamic;
if (locked)
{
entityCollidable.Entity.Locker.Enter();
}
try
{
entityVelocity = Toolbox.GetVelocityOfPoint(supportData.Position, entityCollidable.Entity.Position, entityCollidable.Entity.LinearVelocity, entityCollidable.Entity.AngularVelocity);
}
finally
{
if (locked)
{
entityCollidable.Entity.Locker.Exit();
}
}
Vector3.Subtract(ref relativeVelocity, ref entityVelocity, out relativeVelocity);
}
}
}
/// <summary>
/// Updates the movement basis of the horizontal motion constraint and updates the horizontal motion constraint's support data.
/// Should be updated automatically by the character on each time step; other code should not need to call this.
/// </summary>
public void UpdateSupportData()
{
//Check if the support has changed, and perform the necessary bookkeeping to keep the connections up to date.
var oldSupport = supportData.SupportObject;
supportData = supportFinder.VerticalSupportData;
if (oldSupport != supportData.SupportObject)
{
OnInvolvedEntitiesChanged();
}
}
/// <summary>
/// Computes a traction contact using a movement direction. This is helpful for the vertical motion constraint.
/// By biasing the search in the movement direction, contacts on the character's butt (which might otherwise hold the character back via the vertical motion constraint) are ignored.
/// </summary>
/// <param name="down">Down direction of the character.</param>
/// <param name="movementDirection">Movement direction of the character.</param>
private void UpdateVerticalSupportData(ref Vector3 down, ref Vector3 movementDirection)
{
if (HasTraction)
{
if (tractionContacts.Count > 0)
{
//Find the traction-providing contact which is furthest in the direction of the movement direction.
int greatestIndex = -1;
float greatestDot = -float.MaxValue;
for (int i = 0; i < tractionContacts.Count; i++)
{
float dot;
Vector3.Dot(ref movementDirection, ref tractionContacts.Elements[i].Contact.Normal, out dot);
if (dot > greatestDot)
{
greatestDot = dot;
greatestIndex = i;
}
}
verticalSupportData.Position = tractionContacts.Elements[greatestIndex].Contact.Position;
verticalSupportData.Normal = tractionContacts.Elements[greatestIndex].Contact.Normal;
verticalSupportData.SupportObject = tractionContacts.Elements[greatestIndex].Collidable;
//Project all other contact depths onto the chosen normal, keeping the largest one.
//This lets the vertical motion constraint relax when objects are penetrating deeply.
float depth = -float.MaxValue;
for (int i = 0; i < tractionContacts.Count; i++)
{
float dot;
Vector3.Dot(ref tractionContacts.Elements[i].Contact.Normal, ref verticalSupportData.Normal, out dot);
dot = dot * tractionContacts.Elements[i].Contact.PenetrationDepth;
if (dot > depth)
{
depth = dot;
}
}
verticalSupportData.Depth = depth;
return;
}
//There were no traction providing contacts, so check the support ray.
Debug.Assert(SupportRayData != null, "If the character has traction but there are no contacts, there must be a ray cast with traction.");
verticalSupportData.Position = SupportRayData.Value.HitData.Location;
verticalSupportData.Normal = SupportRayData.Value.HitData.Normal;
verticalSupportData.Depth = Vector3.Dot(down, SupportRayData.Value.HitData.Normal) * (BottomDistance - SupportRayData.Value.HitData.T);
verticalSupportData.SupportObject = SupportRayData.Value.HitObject;
return;
}
verticalSupportData = new SupportData();
}
/// <summary>
/// Updates the constraint's view of the character's support data.
/// </summary>
public void UpdateSupportData()
{
//Check if the support has changed, and perform the necessary bookkeeping to keep the connections up to date.
var oldSupport = supportData.SupportObject;
supportData = supportFinder.SupportData;
if (oldSupport != supportData.SupportObject)
{
OnInvolvedEntitiesChanged();
var supportEntityCollidable = supportData.SupportObject as EntityCollidable;
if (supportEntityCollidable != null)
{
supportEntity = supportEntityCollidable.Entity;
}
else
{
//We aren't on an entity, so clear out the support entity.
supportEntity = null;
}
}
}
void CollectSupportData()
{
//Identify supports.
SupportFinder.UpdateSupports();
//Collect the support data from the support, if any.
if (SupportFinder.HasSupport)
{
if (SupportFinder.HasTraction)
{
supportData = SupportFinder.TractionData.Value;
}
else
{
supportData = SupportFinder.SupportData.Value;
}
}
else
{
supportData = new SupportData();
}
}
/// <summary>
/// Updates the movement basis of the horizontal motion constraint and updates the horizontal motion constraint's support data.
/// Should be updated automatically by the character on each time step; other code should not need to call this.
/// </summary>
public void UpdateSupportData()
{
//Check if the support has changed, and perform the necessary bookkeeping to keep the connections up to date.
var oldSupport = supportData.SupportObject;
supportData = supportFinder.VerticalSupportData;
if (oldSupport != supportData.SupportObject)
{
OnInvolvedEntitiesChanged();
}
}
/// <summary>
/// Computes a traction contact using a movement direction. This is helpful for the vertical motion constraint.
/// By biasing the search in the movement direction, contacts on the character's butt (which might otherwise hold the character back via the vertical motion constraint) are ignored.
/// </summary>
/// <param name="down">Down direction of the character.</param>
/// <param name="movementDirection">Movement direction of the character.</param>
private void UpdateVerticalSupportData(ref System.Numerics.Vector3 down, ref System.Numerics.Vector3 movementDirection)
{
if (HasTraction)
{
if (tractionContacts.Count > 0)
{
//Find the traction-providing contact which is furthest in the direction of the movement direction.
int greatestIndex = -1;
float greatestDot = -float.MaxValue;
for (int i = 0; i < tractionContacts.Count; i++)
{
float dot;
Vector3Ex.Dot(ref movementDirection, ref tractionContacts.Elements[i].Contact.Normal, out dot);
if (dot > greatestDot)
{
greatestDot = dot;
greatestIndex = i;
}
}
verticalSupportData.Position = tractionContacts.Elements[greatestIndex].Contact.Position;
verticalSupportData.Normal = tractionContacts.Elements[greatestIndex].Contact.Normal;
verticalSupportData.SupportObject = tractionContacts.Elements[greatestIndex].Collidable;
//Project all other contact depths onto the chosen normal, keeping the largest one.
//This lets the vertical motion constraint relax when objects are penetrating deeply.
float depth = -float.MaxValue;
for (int i = 0; i < tractionContacts.Count; i++)
{
float dot;
Vector3Ex.Dot(ref tractionContacts.Elements[i].Contact.Normal, ref verticalSupportData.Normal, out dot);
dot = dot * tractionContacts.Elements[i].Contact.PenetrationDepth;
if (dot > depth)
{
depth = dot;
}
}
verticalSupportData.Depth = depth;
return;
}
//There were no traction providing contacts, so check the support ray.
Debug.Assert(SupportRayData != null, "If the character has traction but there are no contacts, there must be a ray cast with traction.");
verticalSupportData.Position = SupportRayData.Value.HitData.Location;
verticalSupportData.Normal = SupportRayData.Value.HitData.Normal;
verticalSupportData.Depth = Vector3Ex.Dot(down, SupportRayData.Value.HitData.Normal) * (BottomDistance - SupportRayData.Value.HitData.T);
verticalSupportData.SupportObject = SupportRayData.Value.HitObject;
return;
}
verticalSupportData = new SupportData();
}
void IBeforeSolverUpdateable.Update(float dt)
{
//Someone may want to use the Body.CollisionInformation.Tag for their own purposes.
//That could screw up the locking mechanism above and would be tricky to track down.
//Consider using the making the custom tag implement ICharacterTag, modifying LockCharacterPairs to analyze the different Tag type, or using the Entity.Tag for the custom data instead.
Debug.Assert(Body.CollisionInformation.Tag is ICharacterTag, "The character.Body.CollisionInformation.Tag must implement ICharacterTag to link the CharacterController and its body together for character-related locking to work in multithreaded simulations.");
//We can't let multiple characters manage the same pairs simultaneously. Lock it up!
LockCharacterPairs();
try
{
CorrectContacts();
bool hadTraction = SupportFinder.HasTraction;
CollectSupportData();
//Compute the initial velocities relative to the support.
Vector3 relativeVelocity;
ComputeRelativeVelocity(ref supportData, out relativeVelocity);
float verticalVelocity = Vector3.Dot(supportData.Normal, relativeVelocity);
//Don't attempt to use an object as support if we are flying away from it (and we were never standing on it to begin with).
if (SupportFinder.HasTraction && !hadTraction && verticalVelocity < 0)
{
SupportFinder.ClearSupportData();
supportData = new SupportData();
}
//Attempt to jump.
if (tryToJump && StanceManager.CurrentStance != Stance.Crouching) //Jumping while crouching would be a bit silly.
{
//In the following, note that the jumping velocity changes are computed such that the separating velocity is specifically achieved,
//rather than just adding some speed along an arbitrary direction. This avoids some cases where the character could otherwise increase
//the jump speed, which may not be desired.
if (SupportFinder.HasTraction)
{
//The character has traction, so jump straight up.
float currentUpVelocity = Vector3.Dot(Body.OrientationMatrix.Up, relativeVelocity);
//Target velocity is JumpSpeed.
float velocityChange = Math.Max(jumpSpeed - currentUpVelocity, 0);
ApplyJumpVelocity(ref supportData, Body.OrientationMatrix.Up * velocityChange, ref relativeVelocity);
//Prevent any old contacts from hanging around and coming back with a negative depth.
foreach (var pair in Body.CollisionInformation.Pairs)
{
pair.ClearContacts();
}
SupportFinder.ClearSupportData();
supportData = new SupportData();
}
else if (SupportFinder.HasSupport)
{
//The character does not have traction, so jump along the surface normal instead.
float currentNormalVelocity = Vector3.Dot(supportData.Normal, relativeVelocity);
//Target velocity is JumpSpeed.
float velocityChange = Math.Max(slidingJumpSpeed - currentNormalVelocity, 0);
ApplyJumpVelocity(ref supportData, supportData.Normal * -velocityChange, ref relativeVelocity);
//Prevent any old contacts from hanging around and coming back with a negative depth.
foreach (var pair in Body.CollisionInformation.Pairs)
{
pair.ClearContacts();
}
SupportFinder.ClearSupportData();
supportData = new SupportData();
}
}
tryToJump = false;
//Try to step!
Vector3 newPosition;
if (StepManager.TryToStepDown(out newPosition) ||
StepManager.TryToStepUp(out newPosition))
{
TeleportToPosition(newPosition, dt);
}
if (StanceManager.UpdateStance(out newPosition))
{
TeleportToPosition(newPosition, dt);
}
}
finally
{
UnlockCharacterPairs();
}
//if (SupportFinder.HasTraction && SupportFinder.Supports.Count == 0)
//{
//There's another way to step down that is a lot cheaper, but less robust.
//This modifies the velocity of the character to make it fall faster.
//Impacts with the ground will be harder, so it will apply superfluous force to supports.
//Additionally, it will not be consistent with instant up-stepping.
//However, because it does not do any expensive queries, it is very fast!
////We are being supported by a ray cast, but we're floating.
////Let's try to get to the ground faster.
////How fast? Try picking an arbitrary velocity and setting our relative vertical velocity to that value.
////Don't go farther than the maximum distance, though.
//float maxVelocity = (SupportFinder.SupportRayData.Value.HitData.T - SupportFinder.RayLengthToBottom);
//if (maxVelocity > 0)
//{
// maxVelocity = (maxVelocity + .01f) / dt;
// float targetVerticalVelocity = -3;
// verticalVelocity = Vector3.Dot(Body.OrientationMatrix.Up, relativeVelocity);
// float change = MathHelper.Clamp(targetVerticalVelocity - verticalVelocity, -maxVelocity, 0);
// ChangeVelocityUnilaterally(Body.OrientationMatrix.Up * change, ref relativeVelocity);
//}
//}
//Vertical support data is different because it has the capacity to stop the character from moving unless
//contacts are pruned appropriately.
SupportData verticalSupportData;
Vector3 movement3d = new Vector3(HorizontalMotionConstraint.MovementDirection.X, 0, HorizontalMotionConstraint.MovementDirection.Y);
SupportFinder.GetTractionInDirection(ref movement3d, out verticalSupportData);
//Warning:
//Changing a constraint's support data is not thread safe; it modifies simulation islands!
//If something other than a CharacterController can modify simulation islands is running
//simultaneously (in the IBeforeSolverUpdateable.Update stage), it will need to be synchronized.
//We don't need to synchronize this all the time- only when the support object changes.
bool needToLock = HorizontalMotionConstraint.SupportData.SupportObject != supportData.SupportObject ||
VerticalMotionConstraint.SupportData.SupportObject != verticalSupportData.SupportObject;
if (needToLock)
{
CharacterSynchronizer.ConstraintAccessLocker.Enter();
}
HorizontalMotionConstraint.SupportData = supportData;
VerticalMotionConstraint.SupportData = verticalSupportData;
if (needToLock)
{
CharacterSynchronizer.ConstraintAccessLocker.Exit();
}
}
/// <summary>
/// Computes representative support information based on the character's current traction contacts, support contacts, and ray contacts.
/// </summary>
/// <param name="down">Down direction of the character.</param>
private void UpdateSupportData(ref System.Numerics.Vector3 down)
{
//Choose which set of contacts to use.
RawList<CharacterContact> contacts;
if (tractionContacts.Count > 0)
{
contacts = tractionContacts;
}
else if (supportContacts.Count > 0)
{
contacts = supportContacts;
}
else
{
//No contacts provide support!
//Fall back to the ray cast result.
if (SupportRayData != null)
{
supportData = new SupportData
{
Position = SupportRayData.Value.HitData.Location,
Normal = SupportRayData.Value.HitData.Normal,
Depth = Vector3Ex.Dot(down, SupportRayData.Value.HitData.Normal) * (BottomDistance - SupportRayData.Value.HitData.T),
SupportObject = SupportRayData.Value.HitObject
};
}
else
{
supportData = new SupportData();
}
return;
}
//Compute a representative support from the set of contacts.
supportData.Position = contacts.Elements[0].Contact.Position;
supportData.Normal = contacts.Elements[0].Contact.Normal;
for (int i = 1; i < contacts.Count; i++)
{
Vector3Ex.Add(ref supportData.Position, ref contacts.Elements[i].Contact.Position, out supportData.Position);
Vector3Ex.Add(ref supportData.Normal, ref contacts.Elements[i].Contact.Normal, out supportData.Normal);
}
if (contacts.Count > 1)
{
Vector3Ex.Divide(ref supportData.Position, contacts.Count, out supportData.Position);
float length = supportData.Normal.LengthSquared();
if (length < Toolbox.Epsilon)
{
//It's possible that the normals have cancelled each other out- that would be bad!
//Just use an arbitrary support's normal in that case.
supportData.Normal = contacts.Elements[0].Contact.Normal;
}
else
{
Vector3Ex.Multiply(ref supportData.Normal, 1 / (float)Math.Sqrt(length), out supportData.Normal);
}
}
//Now that we have the normal, cycle through all the contacts again and find the deepest projected depth.
//Use that object as our support too.
float depth = -float.MaxValue;
Collidable supportObject = null;
for (int i = 0; i < contacts.Count; i++)
{
float dot;
Vector3Ex.Dot(ref contacts.Elements[i].Contact.Normal, ref supportData.Normal, out dot);
dot = dot * contacts.Elements[i].Contact.PenetrationDepth;
if (dot > depth)
{
depth = dot;
supportObject = contacts.Elements[i].Collidable;
}
}
supportData.Depth = depth;
supportData.SupportObject = supportObject;
}
void IBeforeSolverUpdateable.Update(float dt)
{
//Someone may want to use the Body.CollisionInformation.Tag for their own purposes.
//That could screw up the locking mechanism above and would be tricky to track down.
//Consider using the making the custom tag implement ICharacterTag, modifying LockCharacterPairs to analyze the different Tag type, or using the Entity.Tag for the custom data instead.
Debug.Assert(Body.CollisionInformation.Tag is ICharacterTag, "The character.Body.CollisionInformation.Tag must implement ICharacterTag to link the SphereCharacterController and its body together for character-related locking to work in multithreaded simulations.");
SupportData supportData;
HorizontalMotionConstraint.UpdateMovementBasis(ref viewDirection);
//We can't let multiple characters manage the same pairs simultaneously. Lock it up!
PairLocker.LockCharacterPairs();
try
{
bool hadSupport = SupportFinder.HasSupport;
SupportFinder.UpdateSupports(ref HorizontalMotionConstraint.movementDirection3d);
supportData = SupportFinder.SupportData;
//Compute the initial velocities relative to the support.
Vector3 relativeVelocity;
ComputeRelativeVelocity(ref supportData, out relativeVelocity);
float verticalVelocity = Vector3.Dot(supportData.Normal, relativeVelocity);
//Don't attempt to use an object as support if we are flying away from it (and we were never standing on it to begin with).
if (SupportFinder.HasSupport && !hadSupport && verticalVelocity < 0)
{
SupportFinder.ClearSupportData();
supportData = new SupportData();
}
//Attempt to jump.
if (tryToJump) //Jumping while crouching would be a bit silly.
{
//In the following, note that the jumping velocity changes are computed such that the separating velocity is specifically achieved,
//rather than just adding some speed along an arbitrary direction. This avoids some cases where the character could otherwise increase
//the jump speed, which may not be desired.
if (SupportFinder.HasTraction)
{
//The character has traction, so jump straight up.
float currentDownVelocity;
Vector3.Dot(ref down, ref relativeVelocity, out currentDownVelocity);
//Target velocity is JumpSpeed.
float velocityChange = Math.Max(jumpSpeed + currentDownVelocity, 0);
ApplyJumpVelocity(ref supportData, down * -velocityChange, ref relativeVelocity);
//Prevent any old contacts from hanging around and coming back with a negative depth.
foreach (var pair in Body.CollisionInformation.Pairs)
{
pair.ClearContacts();
}
SupportFinder.ClearSupportData();
supportData = new SupportData();
}
else if (SupportFinder.HasSupport)
{
//The character does not have traction, so jump along the surface normal instead.
float currentNormalVelocity = Vector3.Dot(supportData.Normal, relativeVelocity);
//Target velocity is JumpSpeed.
float velocityChange = Math.Max(slidingJumpSpeed - currentNormalVelocity, 0);
ApplyJumpVelocity(ref supportData, supportData.Normal * -velocityChange, ref relativeVelocity);
//Prevent any old contacts from hanging around and coming back with a negative depth.
foreach (var pair in Body.CollisionInformation.Pairs)
{
pair.ClearContacts();
}
SupportFinder.ClearSupportData();
supportData = new SupportData();
}
}
tryToJump = false;
}
finally
{
PairLocker.UnlockCharacterPairs();
}
//Tell the constraints to get ready to solve.
HorizontalMotionConstraint.UpdateSupportData();
VerticalMotionConstraint.UpdateSupportData();
//Update the horizontal motion constraint's state.
if (supportData.SupportObject != null)
{
if (SupportFinder.HasTraction)
{
HorizontalMotionConstraint.MovementMode = MovementMode.Traction;
HorizontalMotionConstraint.TargetSpeed = speed;
HorizontalMotionConstraint.MaximumForce = tractionForce;
}
else
{
HorizontalMotionConstraint.MovementMode = MovementMode.Sliding;
HorizontalMotionConstraint.TargetSpeed = slidingSpeed;
HorizontalMotionConstraint.MaximumForce = slidingForce;
}
}
else
{
HorizontalMotionConstraint.MovementMode = MovementMode.Floating;
HorizontalMotionConstraint.TargetSpeed = airSpeed;
HorizontalMotionConstraint.MaximumForce = airForce;
}
HorizontalMotionConstraint.TargetSpeed *= SpeedScale;
}
/// <summary>
/// Updates the constraint's view of the character's support data.
/// </summary>
public void UpdateSupportData()
{
//Check if the support has changed, and perform the necessary bookkeeping to keep the connections up to date.
var oldSupport = supportData.SupportObject;
supportData = supportFinder.SupportData;
if (oldSupport != supportData.SupportObject)
{
OnInvolvedEntitiesChanged();
var supportEntityCollidable = supportData.SupportObject as EntityCollidable;
if (supportEntityCollidable != null)
{
supportEntity = supportEntityCollidable.Entity;
}
else
{
//We aren't on an entity, so clear out the support entity.
supportEntity = null;
}
}
}
/// <summary>
/// Changes the relative velocity between the character and its support.
/// </summary>
/// <param name="supportData">Support data to use to jump.</param>
/// <param name="velocityChange">Change to apply to the character and support relative velocity.</param>
/// <param name="relativeVelocity">Relative velocity to update.</param>
void ApplyJumpVelocity(ref SupportData supportData, Vector3 velocityChange, ref Vector3 relativeVelocity)
{
Body.LinearVelocity += velocityChange;
var entityCollidable = supportData.SupportObject as EntityCollidable;
if (entityCollidable != null)
{
if (entityCollidable.Entity.IsDynamic)
{
Vector3 change = velocityChange * jumpForceFactor;
//Multiple characters cannot attempt to modify another entity's velocity at the same time.
entityCollidable.Entity.Locker.Enter();
try
{
entityCollidable.Entity.LinearMomentum += change * -Body.Mass;
}
finally
{
entityCollidable.Entity.Locker.Exit();
}
velocityChange += change;
}
}
//Update the relative velocity as well. It's a ref parameter, so this update will be reflected in the calling scope.
Vector3.Add(ref relativeVelocity, ref velocityChange, out relativeVelocity);
}
/// <summary>
/// Computes representative support information based on the character's current traction contacts, support contacts, and ray contacts.
/// </summary>
/// <param name="down">Down direction of the character.</param>
private void UpdateSupportData(ref Vector3 down)
{
//Choose which set of contacts to use.
RawList <CharacterContact> contacts;
if (tractionContacts.Count > 0)
{
contacts = tractionContacts;
}
else if (supportContacts.Count > 0)
{
contacts = supportContacts;
}
else
{
//No contacts provide support!
//Fall back to the ray cast result.
if (SupportRayData != null)
{
supportData = new SupportData
{
Position = SupportRayData.Value.HitData.Location,
Normal = SupportRayData.Value.HitData.Normal,
Depth = Vector3.Dot(down, SupportRayData.Value.HitData.Normal) * (BottomDistance - SupportRayData.Value.HitData.T),
SupportObject = SupportRayData.Value.HitObject
};
}
else
{
supportData = new SupportData();
}
return;
}
//Compute a representative support from the set of contacts.
supportData.Position = contacts.Elements[0].Contact.Position;
supportData.Normal = contacts.Elements[0].Contact.Normal;
for (int i = 1; i < contacts.Count; i++)
{
Vector3.Add(ref supportData.Position, ref contacts.Elements[i].Contact.Position, out supportData.Position);
Vector3.Add(ref supportData.Normal, ref contacts.Elements[i].Contact.Normal, out supportData.Normal);
}
if (contacts.Count > 1)
{
Vector3.Divide(ref supportData.Position, contacts.Count, out supportData.Position);
float length = supportData.Normal.LengthSquared();
if (length < Toolbox.Epsilon)
{
//It's possible that the normals have cancelled each other out- that would be bad!
//Just use an arbitrary support's normal in that case.
supportData.Normal = contacts.Elements[0].Contact.Normal;
}
else
{
Vector3.Multiply(ref supportData.Normal, 1 / (float)Math.Sqrt(length), out supportData.Normal);
}
}
//Now that we have the normal, cycle through all the contacts again and find the deepest projected depth.
//Use that object as our support too.
float depth = -float.MaxValue;
Collidable supportObject = null;
for (int i = 0; i < contacts.Count; i++)
{
float dot;
Vector3.Dot(ref contacts.Elements[i].Contact.Normal, ref supportData.Normal, out dot);
dot = dot * contacts.Elements[i].Contact.PenetrationDepth;
if (dot > depth)
{
depth = dot;
supportObject = contacts.Elements[i].Collidable;
}
}
supportData.Depth = depth;
supportData.SupportObject = supportObject;
}
/// <summary>
/// Changes the relative velocity between the character and its support.
/// </summary>
/// <param name="supportData">Support data to use to jump.</param>
/// <param name="velocityChange">Change to apply to the character and support relative velocity.</param>
/// <param name="delta">multiplier of velocity change</param>
/// <param name="relativeVelocity">Relative velocity to update.</param>
void ApplyJumpVelocity( ref SupportData supportData, ref Vector3 velocityChange, float delta, ref Vector3 relativeVelocity )
{
Vector3 del_velocityChange; velocityChange.Mult( delta, out del_velocityChange );
Vector3 tmp;
Body.LinearVelocity.Add( ref del_velocityChange, out tmp );
Body.LinearVelocity = tmp;
var entityCollidable = supportData.SupportObject as EntityCollidable;
if( entityCollidable != null )
{
if( entityCollidable.Entity.IsDynamic )
{
Vector3 change; velocityChange.Mult( delta * jumpForceFactor, out change );
//Multiple characters cannot attempt to modify another entity's velocity at the same time.
entityCollidable.Entity.Locker.Enter();
try
{
change.Mult( -Body.Mass, out tmp );
entityCollidable.Entity.LinearMomentum.Add( ref tmp, out tmp );
entityCollidable.Entity.LinearMomentum = tmp;
}
finally
{
entityCollidable.Entity.Locker.Exit();
}
change.Add( ref del_velocityChange, out del_velocityChange );
}
}
//Update the relative velocity as well. It's a ref parameter, so this update will be reflected in the calling scope.
Vector3.Add( ref relativeVelocity, ref del_velocityChange, out relativeVelocity );
}
void IBeforeSolverUpdateable.Update(float dt)
{
//Someone may want to use the Body.CollisionInformation.Tag for their own purposes.
//That could screw up the locking mechanism above and would be tricky to track down.
//Consider using the making the custom tag implement ICharacterTag, modifying LockCharacterPairs to analyze the different Tag type, or using the Entity.Tag for the custom data instead.
Debug.Assert(Body.CollisionInformation.Tag is ICharacterTag, "The character.Body.CollisionInformation.Tag must implement ICharacterTag to link the SphereCharacterController and its body together for character-related locking to work in multithreaded simulations.");
SupportData supportData;
HorizontalMotionConstraint.UpdateMovementBasis(ref viewDirection);
//We can't let multiple characters manage the same pairs simultaneously. Lock it up!
PairLocker.LockCharacterPairs();
try
{
bool hadSupport = SupportFinder.HasSupport;
SupportFinder.UpdateSupports(ref HorizontalMotionConstraint.movementDirection3d);
supportData = SupportFinder.SupportData;
//Compute the initial velocities relative to the support.
Vector3 relativeVelocity;
ComputeRelativeVelocity(ref supportData, out relativeVelocity);
float verticalVelocity = Vector3.Dot(ref supportData.Normal, ref relativeVelocity);
//Don't attempt to use an object as support if we are flying away from it (and we were never standing on it to begin with).
if (SupportFinder.HasSupport && !hadSupport && verticalVelocity < 0)
{
SupportFinder.ClearSupportData();
supportData = new SupportData();
}
//Attempt to jump.
if (tryToJump) //Jumping while crouching would be a bit silly.
{
//In the following, note that the jumping velocity changes are computed such that the separating velocity is specifically achieved,
//rather than just adding some speed along an arbitrary direction. This avoids some cases where the character could otherwise increase
//the jump speed, which may not be desired.
if (SupportFinder.HasTraction)
{
//The character has traction, so jump straight up.
float currentDownVelocity;
Vector3.Dot(ref down, ref relativeVelocity, out currentDownVelocity);
//Target velocity is JumpSpeed.
float velocityChange = Math.Max(jumpSpeed + currentDownVelocity, 0);
ApplyJumpVelocity(ref supportData, ref down, -velocityChange, ref relativeVelocity);
//Prevent any old contacts from hanging around and coming back with a negative depth.
foreach (var pair in Body.CollisionInformation.Pairs)
pair.ClearContacts();
SupportFinder.ClearSupportData();
supportData = new SupportData();
}
else if (SupportFinder.HasSupport)
{
//The character does not have traction, so jump along the surface normal instead.
float currentNormalVelocity = Vector3.Dot(ref supportData.Normal, ref relativeVelocity);
//Target velocity is JumpSpeed.
float velocityChange = Math.Max(slidingJumpSpeed - currentNormalVelocity, 0);
ApplyJumpVelocity(ref supportData, ref supportData.Normal, -velocityChange, ref relativeVelocity);
//Prevent any old contacts from hanging around and coming back with a negative depth.
foreach (var pair in Body.CollisionInformation.Pairs)
pair.ClearContacts();
SupportFinder.ClearSupportData();
supportData = new SupportData();
}
}
tryToJump = false;
}
finally
{
PairLocker.UnlockCharacterPairs();
}
//Tell the constraints to get ready to solve.
HorizontalMotionConstraint.UpdateSupportData();
VerticalMotionConstraint.UpdateSupportData();
//Update the horizontal motion constraint's state.
if (supportData.SupportObject != null)
{
if (SupportFinder.HasTraction)
{
HorizontalMotionConstraint.MovementMode = MovementMode.Traction;
HorizontalMotionConstraint.TargetSpeed = speed;
HorizontalMotionConstraint.MaximumForce = tractionForce;
}
else
{
HorizontalMotionConstraint.MovementMode = MovementMode.Sliding;
HorizontalMotionConstraint.TargetSpeed = slidingSpeed;
HorizontalMotionConstraint.MaximumForce = slidingForce;
}
}
else
{
HorizontalMotionConstraint.MovementMode = MovementMode.Floating;
HorizontalMotionConstraint.TargetSpeed = airSpeed;
HorizontalMotionConstraint.MaximumForce = airForce;
}
HorizontalMotionConstraint.TargetSpeed *= SpeedScale;
}
/// <summary>
/// Changes the relative velocity between the character and its support.
/// </summary>
/// <param name="supportData">Support data to use to jump.</param>
/// <param name="velocityChange">Change to apply to the character and support relative velocity.</param>
///
/// <param name="relativeVelocity">Relative velocity to update.</param>
void ApplyJumpVelocity( ref SupportData supportData, ref Vector3 velocityChange, float delta, ref Vector3 relativeVelocity )
{
Vector3 tmp;
velocityChange.Mult( delta, out tmp );
ApplyJumpVelocity( ref supportData, ref tmp, ref relativeVelocity );
}
void IBeforeSolverUpdateable.Update(float dt)
{
//Someone may want to use the Body.CollisionInformation.Tag for their own purposes.
//That could screw up the locking mechanism above and would be tricky to track down.
//Consider using the making the custom tag implement ICharacterTag, modifying LockCharacterPairs to analyze the different Tag type, or using the Entity.Tag for the custom data instead.
Debug.Assert(Body.CollisionInformation.Tag is ICharacterTag, "The character.Body.CollisionInformation.Tag must implement ICharacterTag to link the CharacterController and its body together for character-related locking to work in multithreaded simulations.");
SupportData supportData;
HorizontalMotionConstraint.UpdateMovementBasis(ref viewDirection);
//We can't let multiple characters manage the same pairs simultaneously. Lock it up!
PairLocker.LockCharacterPairs();
try
{
CorrectContacts();
bool hadSupport = SupportFinder.HasSupport;
SupportFinder.UpdateSupports(ref HorizontalMotionConstraint.movementDirection3d);
supportData = SupportFinder.SupportData;
//Compute the initial velocities relative to the support.
Vector3 relativeVelocity;
ComputeRelativeVelocity(ref supportData, out relativeVelocity);
float verticalVelocity = Vector3.Dot(supportData.Normal, relativeVelocity);
//Don't attempt to use an object as support if we are flying away from it (and we were never standing on it to begin with).
if (SupportFinder.HasSupport && !hadSupport && verticalVelocity < 0)
{
SupportFinder.ClearSupportData();
supportData = new SupportData();
}
//Attempt to jump.
if (tryToJump)
{
//In the following, note that the jumping velocity changes are computed such that the separating velocity is specifically achieved,
//rather than just adding some speed along an arbitrary direction. This avoids some cases where the character could otherwise increase
//the jump speed, which may not be desired.
if (SupportFinder.HasTraction)
{
//The character has traction, so jump straight up.
float currentDownVelocity = Vector3.Dot(Down, relativeVelocity);
//Target velocity is JumpSpeed.
float velocityChange = Math.Max(jumpSpeed + currentDownVelocity, 0);
ApplyJumpVelocity(ref supportData, Down * -velocityChange, ref relativeVelocity);
//Prevent any old contacts from hanging around and coming back with a negative depth.
foreach (var pair in Body.CollisionInformation.Pairs)
pair.ClearContacts();
SupportFinder.ClearSupportData();
supportData = new SupportData();
}
else if (SupportFinder.HasSupport)
{
//The character does not have traction, so jump along the surface normal instead.
float currentNormalVelocity = Vector3.Dot(supportData.Normal, relativeVelocity);
//Target velocity is JumpSpeed.
float velocityChange = Math.Max(slidingJumpSpeed - currentNormalVelocity, 0);
ApplyJumpVelocity(ref supportData, supportData.Normal * -velocityChange, ref relativeVelocity);
//Prevent any old contacts from hanging around and coming back with a negative depth.
foreach (var pair in Body.CollisionInformation.Pairs)
pair.ClearContacts();
SupportFinder.ClearSupportData();
supportData = new SupportData();
}
}
tryToJump = false;
//Try to step!
Vector3 newPosition;
//Note: downstepping is often not required.
//It's only really there for games that expect to be able to run down stairs at 40 miles an hour without zipping off into the void.
//Most of the time, you can just comment out downstepping, and so long as the character is running at a reasonable speed,
//gravity will do the work.
//If your game would work without teleportation-based downstepping, it's probably a good idea to comment it out.
//Downstepping can be fairly expensive.
//You can also avoid doing upstepping by fattening up the character's margin, turning it into more of a capsule.
//Instead of teleporting up steps, it would slide up.
//Without teleportation-based upstepping, steps usually need to be quite a bit smaller (i.e. fairly normal sized, instead of 2 feet tall).
if (StepManager.TryToStepDown(out newPosition) ||
StepManager.TryToStepUp(out newPosition))
{
supportData = TeleportToPosition(newPosition, dt);
}
if (StanceManager.UpdateStance(out newPosition))
{
supportData = TeleportToPosition(newPosition, dt);
}
}
finally
{
PairLocker.UnlockCharacterPairs();
}
//Tell the constraints to get ready to solve.
HorizontalMotionConstraint.UpdateSupportData();
VerticalMotionConstraint.UpdateSupportData();
//Update the horizontal motion constraint's state.
if (supportData.SupportObject != null)
{
float speed;
switch (StanceManager.CurrentStance)
{
case Stance.Prone:
speed = proneSpeed;
break;
case Stance.Crouching:
speed = crouchingSpeed;
break;
default:
speed = standingSpeed;
break;
}
if (SupportFinder.HasTraction)
{
HorizontalMotionConstraint.MovementMode = MovementMode.Traction;
HorizontalMotionConstraint.TargetSpeed = speed;
HorizontalMotionConstraint.MaximumForce = tractionForce;
}
else
{
HorizontalMotionConstraint.MovementMode = MovementMode.Sliding;
HorizontalMotionConstraint.TargetSpeed = Math.Min(speed, slidingSpeed);
HorizontalMotionConstraint.MaximumForce = Math.Min(tractionForce, slidingForce);
}
}
else
{
HorizontalMotionConstraint.MovementMode = MovementMode.Floating;
HorizontalMotionConstraint.TargetSpeed = airSpeed;
HorizontalMotionConstraint.MaximumForce = airForce;
}
HorizontalMotionConstraint.TargetSpeed *= SpeedScale;
}
void IBeforeSolverUpdateable.Update(float dt)
{
//Someone may want to use the Body.CollisionInformation.Tag for their own purposes.
//That could screw up the locking mechanism above and would be tricky to track down.
//Consider using the making the custom tag implement ICharacterTag, modifying LockCharacterPairs to analyze the different Tag type, or using the Entity.Tag for the custom data instead.
Debug.Assert(Body.CollisionInformation.Tag is ICharacterTag, "The character.Body.CollisionInformation.Tag must implement ICharacterTag to link the CharacterController and its body together for character-related locking to work in multithreaded simulations.");
SupportData supportData;
HorizontalMotionConstraint.UpdateMovementBasis(ref viewDirection);
//We can't let multiple characters manage the same pairs simultaneously. Lock it up!
PairLocker.LockCharacterPairs();
try
{
CorrectContacts();
bool hadSupport = SupportFinder.HasSupport;
SupportFinder.UpdateSupports(ref HorizontalMotionConstraint.movementDirection3d);
supportData = SupportFinder.SupportData;
//Compute the initial velocities relative to the support.
Vector3 relativeVelocity;
ComputeRelativeVelocity(ref supportData, out relativeVelocity);
float verticalVelocity = Vector3.Dot(supportData.Normal, relativeVelocity);
//Don't attempt to use an object as support if we are flying away from it (and we were never standing on it to begin with).
if (SupportFinder.HasSupport && !hadSupport && verticalVelocity < 0)
{
SupportFinder.ClearSupportData();
supportData = new SupportData();
}
//Attempt to jump.
if (TryToJump && StanceManager.CurrentStance != Stance.Crouching) //Jumping while crouching would be a bit silly.
{
//In the following, note that the jumping velocity changes are computed such that the separating velocity is specifically achieved,
//rather than just adding some speed along an arbitrary direction. This avoids some cases where the character could otherwise increase
//the jump speed, which may not be desired.
if (SupportFinder.HasTraction)
{
//The character has traction, so jump straight up.
float currentDownVelocity = Vector3.Dot(Down, relativeVelocity);
//Target velocity is JumpSpeed.
float velocityChange = Math.Max(jumpSpeed + currentDownVelocity, 0);
ApplyJumpVelocity(ref supportData, Down * -velocityChange, ref relativeVelocity);
//Prevent any old contacts from hanging around and coming back with a negative depth.
foreach (var pair in Body.CollisionInformation.Pairs)
{
pair.ClearContacts();
}
SupportFinder.ClearSupportData();
supportData = new SupportData();
}
else if (SupportFinder.HasSupport)
{
//The character does not have traction, so jump along the surface normal instead.
float currentNormalVelocity = Vector3.Dot(supportData.Normal, relativeVelocity);
//Target velocity is JumpSpeed.
float velocityChange = Math.Max(slidingJumpSpeed - currentNormalVelocity, 0);
ApplyJumpVelocity(ref supportData, supportData.Normal * -velocityChange, ref relativeVelocity);
//Prevent any old contacts from hanging around and coming back with a negative depth.
foreach (var pair in Body.CollisionInformation.Pairs)
{
pair.ClearContacts();
}
SupportFinder.ClearSupportData();
supportData = new SupportData();
}
}
TryToJump = false;
//Try to step!
Vector3 newPosition;
//Note: downstepping is often not required.
//It's only really there for games that expect to be able to run down stairs at 40 miles an hour without zipping off into the void.
//Most of the time, you can just comment out downstepping, and so long as the character is running at a reasonable speed,
//gravity will do the work.
//If your game would work without teleportation-based downstepping, it's probably a good idea to comment it out.
//Downstepping can be fairly expensive.
//You can also avoid doing upstepping by fattening up the character's margin, turning it into more of a capsule.
//Instead of teleporting up steps, it would slide up.
//Without teleportation-based upstepping, steps usually need to be quite a bit smaller (i.e. fairly normal sized, instead of 2 feet tall).
if (StepManager.TryToStepDown(out newPosition) ||
StepManager.TryToStepUp(out newPosition))
{
supportData = TeleportToPosition(newPosition, dt);
}
if (StanceManager.UpdateStance(out newPosition))
{
supportData = TeleportToPosition(newPosition, dt);
}
}
finally
{
PairLocker.UnlockCharacterPairs();
}
//Tell the constraints to get ready to solve.
HorizontalMotionConstraint.UpdateSupportData();
VerticalMotionConstraint.UpdateSupportData();
//Update the horizontal motion constraint's state.
if (supportData.SupportObject != null)
{
if (SupportFinder.HasTraction)
{
HorizontalMotionConstraint.MovementMode = MovementMode.Traction;
HorizontalMotionConstraint.TargetSpeed = StanceManager.CurrentStance == Stance.Standing ? standingSpeed : crouchingSpeed;
HorizontalMotionConstraint.MaximumForce = tractionForce;
}
else
{
HorizontalMotionConstraint.MovementMode = MovementMode.Sliding;
if (StanceManager.CurrentStance == Stance.Standing)
{
HorizontalMotionConstraint.TargetSpeed = Math.Min(standingSpeed, slidingSpeed);
HorizontalMotionConstraint.MaximumForce = Math.Min(tractionForce, slidingForce);
}
else
{
HorizontalMotionConstraint.TargetSpeed = Math.Min(crouchingSpeed, slidingSpeed);
HorizontalMotionConstraint.MaximumForce = Math.Min(tractionForce, slidingForce);
}
}
}
else
{
HorizontalMotionConstraint.MovementMode = MovementMode.Floating;
HorizontalMotionConstraint.TargetSpeed = airSpeed;
HorizontalMotionConstraint.MaximumForce = airForce;
}
HorizontalMotionConstraint.TargetSpeed *= SpeedScale;
}
void ComputeRelativeVelocity(ref SupportData supportData, out Vector3 relativeVelocity)
{
//Compute the relative velocity between the body and its support, if any.
//The relative velocity will be updated as impulses are applied.
relativeVelocity = Body.LinearVelocity;
if (SupportFinder.HasSupport)
{
//Only entities have velocity.
var entityCollidable = supportData.SupportObject as EntityCollidable;
if (entityCollidable != null)
{
//It's possible for the support's velocity to change due to another character jumping if the support is dynamic.
//Don't let that happen while the character is computing a relative velocity!
Vector3 entityVelocity;
bool locked = entityCollidable.Entity.IsDynamic;
if (locked)
entityCollidable.Entity.Locker.Enter();
try
{
entityVelocity = Toolbox.GetVelocityOfPoint(supportData.Position, entityCollidable.Entity.Position, entityCollidable.Entity.LinearVelocity, entityCollidable.Entity.AngularVelocity);
}
finally
{
if (locked)
entityCollidable.Entity.Locker.Exit();
}
Vector3.Subtract(ref relativeVelocity, ref entityVelocity, out relativeVelocity);
}
}
}
public bool GetTractionInDirection(ref Vector3 movementDirection, out SupportData supportData)
{
if (HasTraction)
{
int greatestIndex = -1;
float greatestDot = -float.MaxValue;
for (int i = 0; i < supports.Count; i++)
{
if (supports.Elements[i].HasTraction)
{
float dot;
Vector3.Dot(ref movementDirection, ref supports.Elements[i].Contact.Normal, out dot);
if (dot > greatestDot)
{
greatestDot = dot;
greatestIndex = i;
}
}
}
if (greatestIndex != -1)
{
supportData.Position = supports.Elements[greatestIndex].Contact.Position;
supportData.Normal = supports.Elements[greatestIndex].Contact.Normal;
supportData.SupportObject = supports.Elements[greatestIndex].Support;
supportData.HasTraction = true;
float depth = -float.MaxValue;
for (int i = 0; i < supports.Count; i++)
{
if (supports.Elements[i].HasTraction)
{
float dot;
Vector3.Dot(ref supports.Elements[i].Contact.Normal, ref supportData.Normal, out dot);
dot = dot * supports.Elements[i].Contact.PenetrationDepth;
if (dot > depth)
{
depth = dot;
}
}
}
supportData.Depth = depth;
return(true);
}
//Okay, try the ray cast result then.
if (SupportRayData != null && SupportRayData.Value.HasTraction)
{
supportData.Position = SupportRayData.Value.HitData.Location;
supportData.Normal = SupportRayData.Value.HitData.Normal;
supportData.Depth = Vector3.Dot(character.Body.OrientationMatrix.Down, SupportRayData.Value.HitData.Normal) * (bottomHeight - SupportRayData.Value.HitData.T);
supportData.SupportObject = SupportRayData.Value.HitObject;
supportData.HasTraction = true;
return(true);
}
//Well that's strange!
supportData = new SupportData();
return(false);
}
else
{
supportData = new SupportData();
return(false);
}
}
