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 has velocity.
var entityCollidable = supportData.SupportObject as EntityCollidable;
if (entityCollidable != null)
{
Vector3 entityVelocity = Toolbox.GetVelocityOfPoint(supportData.Position, entityCollidable.Entity);
Vector3.Subtract(ref relativeVelocity, ref entityVelocity, out relativeVelocity);
}
}
}
/// <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;
entityCollidable.Entity.LinearMomentum += change * -Body.Mass;
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 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();
}
}
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);
}
}
void IBeforeSolverUpdateable.Update(float dt)
{
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);
Vector3 horizontalVelocity = relativeVelocity - supportData.Normal * verticalVelocity;
//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();
}
//If we can compute that we're separating faster than we can handle, take off.
if (SupportFinder.HasTraction && verticalVelocity < -VerticalMotionConstraint.MaximumGlueForce * dt / VerticalMotionConstraint.EffectiveMass)
{
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);
}
//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.
ConstraintAccessLocker.Enter();
HorizontalMotionConstraint.SupportData = supportData;
VerticalMotionConstraint.SupportData = verticalSupportData;
ConstraintAccessLocker.Exit();
}