public void UpdateVelocity(float dt)
{
var basis = RigidBody.MotionState.WorldTransform;
Matrix inv = basis;
inv.Invert();
Vector3 linearVelocity = Vector3.TransformNormal(RigidBody.LinearVelocity, inv);
if (_targetSpeed.LengthSquared < 0.0001f && OnGround)
{
linearVelocity *= SpeedDamping;
}
else if (OnGround || linearVelocity.Y > 0)
{
var velXz = new Vector3(linearVelocity.X, 0, linearVelocity.Z);
var moveXz = new Vector3(_targetSpeed.X, 0, _targetSpeed.Z);
float velDelta = (moveXz - velXz).Length;
float t = System.Math.Min(1, WalkAcceleration / (velDelta + 0.00001f));
velXz = Vector3.Lerp(velXz, moveXz, t);
float speed2 = velXz.X * velXz.X + velXz.Z * velXz.Z;
float correction = 1;
if (velXz.LengthSquared > _maxLinearVelocitySquared)
{
correction = (float)System.Math.Sqrt(_maxLinearVelocitySquared / speed2);
}
linearVelocity.X = velXz.X * correction;
linearVelocity.Z = velXz.Z * correction;
}
if (_isJumping)
{
linearVelocity += JumpSpeed * _jumpDir;
_isJumping = false;
CancelStep();
}
RigidBody.LinearVelocity = Vector3.TransformNormal(linearVelocity, RigidBody.MotionState.WorldTransform);
}
