public SimulationStepResults(PhysicalHeliState startState, TimeSpan startTime) { StartingCondition = new TimestepStartingCondition(startState.Position, startState.Velocity, startState.Acceleration, startState.Orientation, startTime); Result = new TimestepResult(startState.Position, startState.Velocity, startState.Orientation, startTime); StartTime = startTime; EndTime = startTime; Duration = TimeSpan.Zero; }
public SimulationStepResults(PhysicalHeliState startState, TimeSpan startTime) { StartingCondition = new TimestepStartingCondition(startState.Position, startState.Velocity, startState.Acceleration, startState.Orientation, startTime); Result = new TimestepResult(startState.Position, startState.Velocity, startState.Orientation, startTime); StartTime = startTime; EndTime = startTime; Duration = TimeSpan.Zero; }
public void Test() { var simSettings = new SimulationSettings(); simSettings.RenderMode = RenderModes.Normal; var game = new SimulatorGame(simSettings, IntPtr.Zero); _collision = new TerrainCollision(game, game); _collision.Initialize(); _collision.TmpBuildScene(); // Find new position and velocity from a constant acceleration over timestep const float dt = 0.017f; var a = new Vector3(0, 9.81f, 0); var startCondition1 = new TimestepStartingCondition(Vector3.Zero, Vector3.Zero, a, Quaternion.Identity, TimeSpan.Zero); TimestepStartingCondition startCondition2 = startCondition1; var joystickOutput = new JoystickOutput(0.1f, 0.1f, 0, 0.5f); for (int i = 0; i < 100; i++) { TimestepResult jitterResult = ByJitter(startCondition1, joystickOutput, startCondition1.StartTime + TimeSpan.FromSeconds(dt), dt); TimestepResult physicsResult = ByPhysics(startCondition2, joystickOutput, startCondition2.StartTime + TimeSpan.FromSeconds(dt), dt); Vector3 dPos = jitterResult.Position - physicsResult.Position; Vector3 dVel = jitterResult.Velocity - physicsResult.Velocity; if (jitterResult.Orientation != physicsResult.Orientation) { float dPitchDeg = MathHelper.ToDegrees(VectorHelper.GetPitchAngle(jitterResult.Orientation) - VectorHelper.GetPitchAngle(physicsResult.Orientation)); float dRollDeg = MathHelper.ToDegrees(VectorHelper.GetRollAngle(jitterResult.Orientation) - VectorHelper.GetRollAngle(physicsResult.Orientation)); float dYawDeg = MathHelper.ToDegrees(VectorHelper.GetHeadingAngle(jitterResult.Orientation) - VectorHelper.GetHeadingAngle(physicsResult.Orientation)); Console.WriteLine("YPR delta " + dPitchDeg + " " + dRollDeg + " " + dYawDeg); } TimeSpan nextStartTime = physicsResult.EndTime; startCondition1 = new TimestepStartingCondition(jitterResult.Position, jitterResult.Velocity, a, jitterResult.Orientation, nextStartTime); startCondition2 = new TimestepStartingCondition(physicsResult.Position, physicsResult.Velocity, a, physicsResult.Orientation, nextStartTime); } }
public SimulationStepResults(TimestepStartingCondition startingCondition, TimestepResult result, TimeSpan startTime, TimeSpan endTime) { Duration = endTime - startTime; StartTime = startTime; EndTime = endTime; // Substeps = new List<SimulationStepResults>(); StartingCondition = startingCondition; Result = result; }
public SimulationStepResults(TimestepStartingCondition startingCondition, TimestepResult result, TimeSpan startTime, TimeSpan endTime) { Duration = endTime - startTime; StartTime = startTime; EndTime = endTime; // Substeps = new List<SimulationStepResults>(); StartingCondition = startingCondition; Result = result; }
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 ByJitter(TimestepStartingCondition startCondition, JoystickOutput output, TimeSpan stepEndTime, float dt) { // 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); Vector3 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); }
/// <summary>Calculates the input and external forces.</summary> /// <returns>The new orientation and the total acceleration for this orientation in this timestep.</returns> public SimulationStepResults PerformTimestep(PhysicalHeliState prev, JoystickOutput output, TimeSpan stepDuration, TimeSpan stepEndTime) { if (!_isInitialized) { // startCondition.Acceleration = CalculateAcceleration(startCondition.Orientation, startCondition.Velocity, input); _prevTimestepResult = new TimestepResult(prev.Position, prev.Velocity, prev.Orientation, stepEndTime - stepDuration); _isInitialized = true; } // If the number of substeps is 0 then only the state at the end of the timestep will be calculated. // If the number is greater than 1, then the timestep will 1 then a substep will be calculated in the middle of the timestep. const int substeps = 0; if (substeps < 0) throw new Exception("The number of substeps is invalid."); TimeSpan substepDuration = stepDuration.Divide(1 + substeps); Vector3 initialAcceleration = CalculateAcceleration(prev.Orientation, prev.Velocity, output); var initialState = new TimestepStartingCondition(_prevTimestepResult, initialAcceleration); //_prevTimestepResult.Result; // var substepResults = new List<SubstepResults> {initialState}; // We always need to calculate at least the timestep itself, plus any optional substeps. // Substeps are used to provide sensors with a higher frequency of data than the simulator is capable of rendering real-time. // const int stepsToCalculate = substeps + 1; // SubstepResults prevSubstep = initialState; // for (int i = 0; i < stepsToCalculate; i++) // { // prevSubstep.Acceleration = CalculateAcceleration(prevSubstep.Orientation, prevSubstep.Velocity, input); // SubstepResults r = SimulateStep(prevSubstep, prevSubstep.Acceleration, input, substepDuration, stepEndTime); // // substepResults.Add(r); // prevSubstep = r; // } TimestepResult result = SimulateStep(initialState, output, substepDuration, stepEndTime); //new SimulationStepResults(stepDuration, substepDuration, substepResults); _prevTimestepResult = result; // DebugInformation.Time1 = stepEndTime; // DebugInformation.Q1 = result.Orientation; // // DebugInformation.Vectors["Pos"] = result.Position; // DebugInformation.Vectors["Vel"] = result.Velocity; // DebugInformation.Vectors["Acc"] = initialAcceleration; return new SimulationStepResults(initialState, result, stepEndTime - stepDuration, 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); } }