/// <summary>
/// Computes per-frame information necessary for the constraint.
/// </summary>
/// <param name="dt">Time step duration.</param>
public override void Update(float dt)
{
bool isTryingToMove = movementDirection3d.LengthSquared() > 0;
if (!isTryingToMove)
{
TargetSpeed = 0;
}
maxForce = MaximumForce * dt;
//Compute the jacobians. This is basically a PointOnLineJoint with motorized degrees of freedom.
Vector3 downDirection = characterBody.orientationMatrix.Down;
if (MovementMode != MovementMode.Floating)
{
//Compute the linear jacobians first.
if (isTryingToMove)
{
Vector3 velocityDirection;
Vector3 offVelocityDirection;
//Project the movement direction onto the support plane defined by the support normal.
//This projection is NOT along the support normal to the plane; that would cause the character to veer off course when moving on slopes.
//Instead, project along the sweep direction to the plane.
//For a 6DOF character controller, the lineStart would be different; it must be perpendicular to the local up.
Vector3 lineStart = movementDirection3d;
Vector3 lineEnd;
Vector3.Add(ref lineStart, ref downDirection, out lineEnd);
Plane plane = new Plane(supportData.Normal, 0);
float t;
//This method can return false when the line is parallel to the plane, but previous tests and the slope limit guarantee that it won't happen.
Toolbox.GetLinePlaneIntersection(ref lineStart, ref lineEnd, ref plane, out t, out velocityDirection);
//The origin->intersection line direction defines the horizontal velocity direction in 3d space.
velocityDirection.Normalize();
//The normal and velocity direction are perpendicular and normal, so the off velocity direction doesn't need to be normalized.
Vector3.Cross(ref velocityDirection, ref supportData.Normal, out offVelocityDirection);
linearJacobianA1 = velocityDirection;
linearJacobianA2 = offVelocityDirection;
linearJacobianB1 = -velocityDirection;
linearJacobianB2 = -offVelocityDirection;
}
else
{
//If the character isn't trying to move, then the velocity directions are not well defined.
//Instead, pick two arbitrary vectors on the support plane.
//First guess will be based on the previous jacobian.
//Project the old linear jacobian onto the support normal plane.
float dot;
Vector3.Dot(ref linearJacobianA1, ref supportData.Normal, out dot);
Vector3 toRemove;
Vector3.Multiply(ref supportData.Normal, dot, out toRemove);
Vector3.Subtract(ref linearJacobianA1, ref toRemove, out linearJacobianA1);
//Vector3.Cross(ref linearJacobianA2, ref supportData.Normal, out linearJacobianA1);
float length = linearJacobianA1.LengthSquared();
if (length < Toolbox.Epsilon)
{
//First guess failed. Try the right vector.
Vector3.Cross(ref Toolbox.RightVector, ref supportData.Normal, out linearJacobianA1);
length = linearJacobianA1.LengthSquared();
if (length < Toolbox.Epsilon)
{
//Okay that failed too! try the forward vector.
Vector3.Cross(ref Toolbox.ForwardVector, ref supportData.Normal, out linearJacobianA1);
length = linearJacobianA1.LengthSquared();
//Unless something really weird is happening, we do not need to test any more axes.
}
}
Vector3.Divide(ref linearJacobianA1, (float)Math.Sqrt(length), out linearJacobianA1);
//Pick another perpendicular vector. Don't need to normalize it since the normal and A1 are already normalized and perpendicular.
Vector3.Cross(ref linearJacobianA1, ref supportData.Normal, out linearJacobianA2);
//B's linear jacobians are just -A's.
linearJacobianB1 = -linearJacobianA1;
linearJacobianB2 = -linearJacobianA2;
}
if (supportEntity != null)
{
//Compute the angular jacobians.
Vector3 supportToContact = supportData.Position - supportEntity.Position;
//Since we treat the character to have infinite inertia, we're only concerned with the support's angular jacobians.
//Note the order of the cross product- it is reversed to negate the result.
Vector3.Cross(ref linearJacobianA1, ref supportToContact, out angularJacobianB1);
Vector3.Cross(ref linearJacobianA2, ref supportToContact, out angularJacobianB2);
}
else
{
//If we're not standing on an entity, there are no angular jacobians.
angularJacobianB1 = new Vector3();
angularJacobianB2 = new Vector3();
}
}
else
{
//If the character is floating, then the jacobians are simply the 3d movement direction and the perpendicular direction on the character's horizontal plane.
linearJacobianA1 = movementDirection3d;
linearJacobianA2 = Vector3.Cross(linearJacobianA1, characterBody.orientationMatrix.Down);
}
//Compute the target velocity (in constraint space) for this frame. The hard work has already been done.
targetVelocity.X = TargetSpeed;
targetVelocity.Y = 0;
//Compute the effective mass matrix.
if (supportEntity != null && supportEntity.IsDynamic)
{
float m11, m22, m1221 = 0;
float inverseMass;
Vector3 intermediate;
inverseMass = characterBody.InverseMass;
m11 = inverseMass;
m22 = inverseMass;
//Scale the inertia and mass of the support. This will make the solver view the object as 'heavier' with respect to horizontal motion.
Matrix3x3 inertiaInverse = supportEntity.InertiaTensorInverse;
Matrix3x3.Multiply(ref inertiaInverse, supportForceFactor, out inertiaInverse);
float extra;
inverseMass = supportForceFactor * supportEntity.InverseMass;
Matrix3x3.Transform(ref angularJacobianB1, ref inertiaInverse, out intermediate);
Vector3.Dot(ref intermediate, ref angularJacobianB1, out extra);
m11 += inverseMass + extra;
Vector3.Dot(ref intermediate, ref angularJacobianB2, out extra);
m1221 += extra;
Matrix3x3.Transform(ref angularJacobianB2, ref inertiaInverse, out intermediate);
Vector3.Dot(ref intermediate, ref angularJacobianB2, out extra);
m22 += inverseMass + extra;
massMatrix.M11 = m11;
massMatrix.M12 = m1221;
massMatrix.M21 = m1221;
massMatrix.M22 = m22;
Matrix2x2.Invert(ref massMatrix, out massMatrix);
}
else
{
//If we're not standing on a dynamic entity, then the mass matrix is defined entirely by the character.
Matrix2x2.CreateScale(characterBody.Mass, out massMatrix);
}
//If we're trying to stand still on an object that's moving, use a position correction term to keep the character
//from drifting due to accelerations.
//First thing to do is to check to see if we're moving into a traction/trying to stand still state from a
//non-traction || trying to move state. Either that, or we've switched supports and need to update the offset.
if (supportEntity != null && ((wasTryingToMove && !isTryingToMove) || (!hadTraction && supportFinder.HasTraction) || supportEntity != previousSupportEntity))
{
//We're transitioning into a new 'use position correction' state.
//Force a recomputation of the local offset.
//The time since transition is used as a flag.
timeSinceTransition = 0;
}
//The state is now up to date. Compute an error and velocity bias, if needed.
if (!isTryingToMove && MovementMode == MovementMode.Traction && supportEntity != null)
{
var distanceToBottomOfCharacter = supportFinder.BottomDistance;
if (timeSinceTransition >= 0 && timeSinceTransition < timeUntilPositionAnchor)
{
timeSinceTransition += dt;
}
if (timeSinceTransition >= timeUntilPositionAnchor)
{
Vector3.Multiply(ref downDirection, distanceToBottomOfCharacter, out positionLocalOffset);
positionLocalOffset = (positionLocalOffset + characterBody.Position) - supportEntity.Position;
positionLocalOffset = Matrix3x3.TransformTranspose(positionLocalOffset, supportEntity.OrientationMatrix);
timeSinceTransition = -1; //Negative 1 means that the offset has been computed.
}
if (timeSinceTransition < 0)
{
Vector3 targetPosition;
Vector3.Multiply(ref downDirection, distanceToBottomOfCharacter, out targetPosition);
targetPosition += characterBody.Position;
Vector3 worldSupportLocation = Matrix3x3.Transform(positionLocalOffset, supportEntity.OrientationMatrix) + supportEntity.Position;
Vector3 error;
Vector3.Subtract(ref targetPosition, ref worldSupportLocation, out error);
//If the error is too large, then recompute the offset. We don't want the character rubber banding around.
if (error.LengthSquared() > PositionAnchorDistanceThreshold * PositionAnchorDistanceThreshold)
{
Vector3.Multiply(ref downDirection, distanceToBottomOfCharacter, out positionLocalOffset);
positionLocalOffset = (positionLocalOffset + characterBody.Position) - supportEntity.Position;
positionLocalOffset = Matrix3x3.TransformTranspose(positionLocalOffset, supportEntity.OrientationMatrix);
positionCorrectionBias = new Vector2();
}
else
{
//The error in world space is now available. We can't use this error to directly create a velocity bias, though.
//It needs to be transformed into constraint space where the constraint operates.
//Use the jacobians!
Vector3.Dot(ref error, ref linearJacobianA1, out positionCorrectionBias.X);
Vector3.Dot(ref error, ref linearJacobianA2, out positionCorrectionBias.Y);
//Scale the error so that a portion of the error is resolved each frame.
Vector2.Multiply(ref positionCorrectionBias, .2f / dt, out positionCorrectionBias);
}
}
}
else
{
timeSinceTransition = 0;
positionCorrectionBias = new Vector2();
}
wasTryingToMove = isTryingToMove;
hadTraction = supportFinder.HasTraction;
previousSupportEntity = supportEntity;
}
/// <summary>
/// Computes per-frame information necessary for the constraint.
/// </summary>
/// <param name="dt">Time step duration.</param>
public override void Update(float dt)
{
//Collect references, pick the mode, and configure the coefficients to be used by the solver.
Vector3 movementDirectionIn3D;
this.GetMovementDirectionIn3D(out movementDirectionIn3D);
bool isTryingToMove = movementDirectionIn3D.LengthSquared() > 0;
if (this.supportData.SupportObject != null)
{
this.MovementMode = MovementMode.Traction;
this.maxSpeed = this.speed;
this.maxForce = this.maximumForce;
}
else
{
this.MovementMode = MovementMode.Floating;
this.maxSpeed = this.airSpeed;
this.maxForce = this.maximumAirForce;
this.supportEntity = null;
}
if (!isTryingToMove)
{
this.maxSpeed = 0;
}
this.maxForce *= dt;
//Compute the jacobians. This is basically a PointOnLineJoint with motorized degrees of freedom.
Vector3 downDirection = this.character.Down;
if (this.MovementMode != MovementMode.Floating)
{
//Compute the linear jacobians first.
if (isTryingToMove)
{
Vector3 velocityDirection;
Vector3 offVelocityDirection;
//Project the movement direction onto the support plane defined by the support normal.
//This projection is NOT along the support normal to the plane; that would cause the character to veer off course when moving on slopes.
//Instead, project along the sweep direction to the plane.
//For a 6DOF character controller, the lineStart would be different; it must be perpendicular to the local up.
Vector3 lineStart = movementDirectionIn3D;
Vector3 lineEnd;
Vector3.Add(ref lineStart, ref downDirection, out lineEnd);
Plane plane = new Plane(this.supportData.Normal, 0);
float t;
//This method can return false when the line is parallel to the plane, but previous tests and the slope limit guarantee that it won't happen.
Toolbox.GetLinePlaneIntersection(ref lineStart, ref lineEnd, ref plane, out t, out velocityDirection);
//The origin->intersection line direction defines the horizontal velocity direction in 3d space.
velocityDirection.Normalize();
//The normal and velocity direction are perpendicular and normal, so the off velocity direction doesn't need to be normalized.
Vector3.Cross(ref velocityDirection, ref this.supportData.Normal, out offVelocityDirection);
this.linearJacobianA1 = velocityDirection;
this.linearJacobianA2 = offVelocityDirection;
this.linearJacobianB1 = -velocityDirection;
this.linearJacobianB2 = -offVelocityDirection;
}
else
{
//If the character isn't trying to move, then the velocity directions are not well defined.
//Instead, pick two arbitrary vectors on the support plane.
//First guess will be based on the previous jacobian.
//Project the old linear jacobian onto the support normal plane.
float dot;
Vector3.Dot(ref this.linearJacobianA1, ref this.supportData.Normal, out dot);
Vector3 toRemove;
Vector3.Multiply(ref this.supportData.Normal, dot, out toRemove);
Vector3.Subtract(ref this.linearJacobianA1, ref toRemove, out this.linearJacobianA1);
//Vector3.Cross(ref linearJacobianA2, ref supportData.Normal, out linearJacobianA1);
float length = this.linearJacobianA1.LengthSquared();
if (length < Toolbox.Epsilon)
{
//First guess failed. Try the right vector.
Vector3.Cross(ref Toolbox.RightVector, ref this.supportData.Normal, out this.linearJacobianA1);
length = this.linearJacobianA1.LengthSquared();
if (length < Toolbox.Epsilon)
{
//Okay that failed too! try the forward vector.
Vector3.Cross(ref Toolbox.ForwardVector, ref this.supportData.Normal, out this.linearJacobianA1);
length = this.linearJacobianA1.LengthSquared();
//Unless something really weird is happening, we do not need to test any more axes.
}
}
Vector3.Divide(ref this.linearJacobianA1, (float)Math.Sqrt(length), out this.linearJacobianA1);
//Pick another perpendicular vector. Don't need to normalize it since the normal and A1 are already normalized and perpendicular.
Vector3.Cross(ref this.linearJacobianA1, ref this.supportData.Normal, out this.linearJacobianA2);
//B's linear jacobians are just -A's.
this.linearJacobianB1 = -this.linearJacobianA1;
this.linearJacobianB2 = -this.linearJacobianA2;
}
if (this.supportEntity != null)
{
//Compute the angular jacobians.
Vector3 supportToContact = this.supportData.Position - this.supportEntity.Position;
//Since we treat the character to have infinite inertia, we're only concerned with the support's angular jacobians.
//Note the order of the cross product- it is reversed to negate the result.
Vector3.Cross(ref this.linearJacobianA1, ref supportToContact, out this.angularJacobianB1);
Vector3.Cross(ref this.linearJacobianA2, ref supportToContact, out this.angularJacobianB2);
}
else
{
//If we're not standing on an entity, there are no angular jacobians.
this.angularJacobianB1 = new Vector3();
this.angularJacobianB2 = new Vector3();
}
}
else
{
//If the character is floating, then the jacobians are simply the 3d movement direction and the perpendicular direction on the character's horizontal plane.
this.linearJacobianA1 = movementDirectionIn3D;
this.linearJacobianA2 = Vector3.Cross(this.linearJacobianA1, this.character.Down);
}
//Compute the target velocity (in constraint space) for this frame. The hard work has already been done.
this.targetVelocity.X = this.maxSpeed;
this.targetVelocity.Y = 0;
//the mass matrix is defined entirely by the character.
Matrix2x2.CreateScale(this.character.Body.Mass, out this.massMatrix);
//If we're trying to stand still on an object that's moving, use a position correction term to keep the character
//from drifting due to accelerations.
//First thing to do is to check to see if we're moving into a traction/trying to stand still state from a
//non-traction || trying to move state. Either that, or we've switched supports and need to update the offset.
if (this.supportEntity != null &&
((this.wasTryingToMove && !isTryingToMove) || (!this.hadTraction && this.supportData.HasTraction) ||
this.supportEntity != this.previousSupportEntity))
{
//We're transitioning into a new 'use position correction' state.
//Force a recomputation of the local offset.
//The time since transition is used as a flag.
this.timeSinceTransition = 0;
}
//The state is now up to date. Compute an error and velocity bias, if needed.
if (!isTryingToMove && this.supportData.HasTraction && this.supportEntity != null)
{
//Compute the time it usually takes for the character to slow down while it has traction.
var tractionDecelerationTime = this.speed / (this.maximumForce * this.character.Body.InverseMass);
var characterBody = this.character.Body;
var characterHalfHeight = characterBody.Radius + characterBody.Length * .5f;
if (this.timeSinceTransition >= 0 && this.timeSinceTransition < tractionDecelerationTime)
{
this.timeSinceTransition += dt;
}
if (this.timeSinceTransition >= tractionDecelerationTime)
{
Vector3.Multiply(ref downDirection, characterHalfHeight, out this.positionLocalOffset);
this.positionLocalOffset = (this.positionLocalOffset + this.character.Body.Position) - this.supportEntity.Position;
this.positionLocalOffset = Matrix3x3.TransformTranspose(this.positionLocalOffset, this.supportEntity.OrientationMatrix);
this.timeSinceTransition = -1; //Negative 1 means that the offset has been computed.
}
if (this.timeSinceTransition < 0)
{
Vector3 targetPosition;
Vector3.Multiply(ref downDirection, characterHalfHeight, out targetPosition);
targetPosition += characterBody.Position;
Vector3 worldSupportLocation = Matrix3x3.Transform(this.positionLocalOffset, this.supportEntity.OrientationMatrix) + this.supportEntity.Position;
Vector3 error;
Vector3.Subtract(ref targetPosition, ref worldSupportLocation, out error);
//If the error is too large, then recompute the offset. We don't want the character rubber banding around.
if (error.LengthSquared() > .15f * .15f)
{
Vector3.Multiply(ref downDirection, characterHalfHeight, out this.positionLocalOffset);
this.positionLocalOffset = (this.positionLocalOffset + characterBody.Position) - this.supportEntity.Position;
this.positionLocalOffset = Matrix3x3.TransformTranspose(this.positionLocalOffset, this.supportEntity.OrientationMatrix);
this.positionCorrectionBias = new Vector2();
}
else
{
//The error in world space is now available. We can't use this error to directly create a velocity bias, though.
//It needs to be transformed into constraint space where the constraint operates.
//Use the jacobians!
Vector3.Dot(ref error, ref this.linearJacobianA1, out this.positionCorrectionBias.X);
Vector3.Dot(ref error, ref this.linearJacobianA2, out this.positionCorrectionBias.Y);
//Scale the error so that a portion of the error is resolved each frame.
Vector2.Multiply(ref this.positionCorrectionBias, .2f / dt, out this.positionCorrectionBias);
}
}
}
else
{
this.timeSinceTransition = 0;
this.positionCorrectionBias = new Vector2();
}
this.wasTryingToMove = isTryingToMove;
this.hadTraction = this.supportData.HasTraction;
this.previousSupportEntity = this.supportEntity;
}