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); }