private TimestepResult ByPhysics(TimestepStartingCondition startCondition, JoystickOutput output, TimeSpan stepEndTime, float dt) { Vector3 newPosition; Vector3 newVelocity; ClassicalMechanics.ConstantAcceleration( dt, startCondition.Position, startCondition.Velocity, startCondition.Acceleration, out newPosition, out newVelocity); Quaternion newOrientation, afterJoystick; RotateByJoystickInput(startCondition.Orientation, dt, output, out afterJoystick); newOrientation = afterJoystick; return(new TimestepResult(newPosition, newVelocity, newOrientation, stepEndTime)); }
private TimestepResult SimulateStep(TimestepStartingCondition startCondition, JoystickOutput output, TimeSpan stepDuration, TimeSpan stepEndTime) { // Find new position and velocity from a constant acceleration over timestep var dt = (float)stepDuration.TotalSeconds; // TimestepResult result; if (_useTerrainCollision) { // Note we override gravity here because the gravity acceleration is already accounted for in startCondition.Acceleration vector _collision.SetGravity(Vector3.Zero); float heliMass = _collision.HelicopterBody.Mass; _collision.HelicopterBody.Position = Conversion.ToJitterVector(startCondition.Position); _collision.HelicopterBody.LinearVelocity = Conversion.ToJitterVector(startCondition.Velocity); _collision.HelicopterBody.AddForce(Conversion.ToJitterVector(heliMass * startCondition.Acceleration)); var localAngVelocity = new Vector3( output.Pitch * PhysicsConstants.MaxPitchRate, output.Yaw * PhysicsConstants.MaxYawRate, -output.Roll * PhysicsConstants.MaxRollRate); var worldAngVelocity = VectorHelper.MapFromWorld(localAngVelocity, startCondition.Orientation); _collision.HelicopterBody.AngularVelocity = Conversion.ToJitterVector(worldAngVelocity); // Simulate physics // Vector3 preForward = Vector3.Transform(Vector3.Forward, startCondition.Orientation); _collision.World.Step(dt, false); // TODO Testing with Jitter Physics return(new TimestepResult( Conversion.ToXNAVector(_collision.HelicopterBody.Position), Conversion.ToXNAVector(_collision.HelicopterBody.LinearVelocity), Quaternion.CreateFromRotationMatrix(Conversion.ToXNAMatrix(_collision.HelicopterBody.Orientation)), stepEndTime)); // Vector3 postForward = Vector3.Transform(Vector3.Forward, result.Orientation); } else { Vector3 newPosition; Vector3 newVelocity; ClassicalMechanics.ConstantAcceleration( dt, startCondition.Position, startCondition.Velocity, startCondition.Acceleration, out newPosition, out newVelocity); // TODO Add wind // Rotate helicopter by joystick input after moving it // Vector3 airVelocity = -startCondition.Velocity + Vector3.Zero; // TODO Re-apply rotation by air friction as soon as the Kalman Filter works fine without it Quaternion newOrientation, afterJoystick; RotateByJoystickInput(startCondition.Orientation, dt, output, out afterJoystick); // RotateByAirFriction(afterJoystick, airVelocity, dt, out newOrientation); newOrientation = afterJoystick; return(new TimestepResult(newPosition, newVelocity, newOrientation, stepEndTime)); } }