public void Advance()
{
if (_rotationLookVelocity == 0 && _rotationVelocity == 0 && _forwardVelocity == 0 && _rightVelocity == 0)
{
_timeLastUpdate = DateTime.Now;
return;
}
DateTime now = DateTime.Now;
TimeSpan span = now - _timeLastUpdate;
float seconds = Math.Min(span.Milliseconds / 1000.0f, 0.5f) * 20;
_timeLastUpdate = now;
Vector3D localUp = this.World.LocalUpAxis;
Vector3D localRight = this.World.LocalRightAxis;
Vector3D localForward = this.World.LocalForwardAxis;
// updating rotations are easy...
_rotationBody += seconds * (_rotationVelocity * 30);
_rotationLook = Math2.Clamp(_rotationLook + seconds * _rotationLookVelocity * 30, -1.4f, 1.4f);
Matrix3D xfrm = Matrix3D.FromRotation(localUp, _rotationBody);
Vector3D localVelocity =
seconds * _forwardVelocity * localForward * 35 +
seconds * _rightVelocity * localRight * 35;
Vector3D absoluteVelocity = xfrm * localVelocity;
Vector3D absoluteUp = xfrm * this.World.LocalUpAxis;
Vector3D absoluteRight = xfrm * this.World.LocalRightAxis;
Vector3D absoluteForward = xfrm * this.World.LocalForwardAxis;
//Debug.WriteLine( "Velocity: " + velocityBody );
//Debug.WriteLine( "TimeStep Velocity: " + incrementalMovement );
//Vector3D extentCenter = xfrm * ( localForward * _bodyRadius );
//Vector3D extentTop = localUp * _bodyRadius;
//Vector3D extentBottom = - localUp * ( _bodyHeight - _liftSize );
//Debug.WriteLine( "Old Position: " + _translationBody );
if (absoluteVelocity.GetMagnitude() > 0)
{
Vector3D moveDirection = absoluteVelocity.GetUnit();
Vector3D centerDelta = SmartStep(_translationBody, moveDirection, _bodyRadius, absoluteVelocity.GetMagnitude()); //+ extentCenter, _translationBody + extentCenter + absoluteVelocity ) - ( _translationBody + extentCenter ) - forwardStep;
Vector3D topDelta = SmartStep(_translationBody + absoluteUp * _bodyRadius, moveDirection, _bodyRadius, absoluteVelocity.GetMagnitude()); //MoveTo( _translationBody + extentTop, _translationBody + extentTop + absoluteVelocity ) - ( _translationBody + extentTop ) - forwardStep;
Vector3D bottomDelta = SmartStep(_translationBody - absoluteUp * (_bodyHeight - _liftSize), moveDirection, _bodyRadius, absoluteVelocity.GetMagnitude()); // MoveTo( _translationBody + extentBottom, _translationBody + extentBottom + absoluteVelocity ) - ( _translationBody + extentBottom ) - forwardStep;
_translationBody += Vector3D.Min(Vector3D.Min(centerDelta, topDelta), bottomDelta);
}
_translationBody += SmartStep(_translationBody, absoluteRight, _bodyRadius, 0);
_translationBody += SmartStep(_translationBody, -absoluteRight, _bodyRadius, 0);
_translationBody += SmartStep(_translationBody, -absoluteUp, _bodyHeight, _liftSize);
}
