public void StepUp(float dt)
{
Matrix transform = RigidBody.MotionState.WorldTransform;
if (!_stepping)
{
_stepping = true;
_steppingTo.Y += CapsuleHalfHeight + CapsuleRadius;
}
/* Bullet reacts with velocity also with kinematic bodies, althogh this
* should not change anything, until we do not go back to dynamic body. */
Vector3 origin = transform.Origin;
float hor = Vector3.Dot(origin - _steppingTo, _steppingInvNormal);
Vector3 stepDir = _steppingTo - origin;
stepDir.Y = 0;
stepDir.Normalize();
float speed = Vector3.Dot(stepDir, Vector3.TransformNormal(_targetSpeed, transform.Basis))
* SteppingSpeed;
origin.Y += speed * dt * 2;
float dv = _steppingTo.Y - origin.Y;
if (dv <= -1 * MaxStepHeight || _targetSpeed.LengthSquared < 0.0001f || speed <= 0)
{
CancelStep();
}
else if (dv < CapsuleRadius)
{
float dh = (float)System.Math.Sqrt(dv * (2 * CapsuleRadius - dv));
if (dh < System.Math.Abs(hor))
{
float advance = System.Math.Min(dh * System.Math.Sign(hor) - hor, speed * dt);
var addVec = _steppingInvNormal * advance;
transform.Origin = transform.Origin + addVec;
}
}
RigidBody.MotionState.WorldTransform = transform;
}
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);
}
