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(TimestepStartingCondition startingCondition, TimestepResult result, TimeSpan startTime, TimeSpan endTime) { Duration = endTime - startTime; StartTime = startTime; EndTime = endTime; // Substeps = new List<SimulationStepResults>(); StartingCondition = startingCondition; Result = result; }
public TimestepStartingCondition(TimestepResult result, Vector3 acceleration) : this(result.Position, result.Velocity, acceleration, result.Orientation, result.EndTime) { }
/// <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); }