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