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