/// <summary> /// Simulates the specified joints. /// </summary> /// <param name="joints">The joints.</param> /// <param name="links">The links.</param> /// <param name="Forward">The forward.</param> private void Simulate(List <Joint> joints, List <Link> links, Boolean Forward) { var timeStep = (Forward == (InputSpeed > 0)) ? FixedTimeStep : -FixedTimeStep; var startingPosChange = (Forward == (InputSpeed > 0)) ? Constants.DefaultStepSize : -Constants.DefaultStepSize; if (InputJoint.TypeOfJoint == JointType.P) { startingPosChange *= AverageLength; } var maxLengthError = MaxSmoothingError * AverageLength; var currentTime = 0.0; Boolean validPosition; var posFinder = new PositionFinder(joints, links, gearsData, simulationDriveIndex); var velSolver = new VelocitySolver(joints, links, FirstInputJointIndex, simulationDriveIndex, InputLinkIndex, InputSpeed, gearsData, AverageLength); var accelSolver = new AccelerationSolver(joints, links, FirstInputJointIndex, simulationDriveIndex, InputLinkIndex, InputSpeed, gearsData, AverageLength); do { #region Find Next Positions if (useErrorMethod) { var k = 0; double upperError; do { timeStep = startingPosChange / InputSpeed; NumericalPosition(timeStep, joints, links); validPosition = posFinder.DefineNewPositions(startingPosChange, ref IsDyadic); upperError = posFinder.PositionError - maxLengthError; if (validPosition && upperError < 0) { startingPosChange *= Constants.ErrorSizeIncrease; // startingPosChange = startingPosChange * maxLengthError / (maxLengthError + upperError); } else { if (Math.Abs(startingPosChange * Constants.ConservativeErrorEstimation * 0.5) < Constants.MinimumStepSize) { validPosition = false; } else { startingPosChange *= Constants.ConservativeErrorEstimation * 0.5; } } } while ((!validPosition || upperError > 0) && k++ < Constants.MaxItersInPositionError && (Math.Abs(startingPosChange * Constants.ConservativeErrorEstimation * 0.5) >= Constants.MinimumStepSize)); //var tempStep = startingPosChange; //startingPosChange = (Constants.ErrorEstimateInertia * prevStep + startingPosChange) / (1 + Constants.ErrorEstimateInertia); //prevStep = tempStep; } else { // this next function puts the xNumerical and yNumerical values in the joints NumericalPosition(timeStep, joints, links); var delta = InputSpeed * timeStep; // this next function puts the x and y values in the joints validPosition = posFinder.DefineNewPositions(delta, ref IsDyadic); } #endregion if (validPosition) { if (Forward == (InputSpeed > 0)) { lock (InputRange) { InputRange[1] = links[InputLinkIndex].Angle; } } else { lock (InputRange) { InputRange[0] = links[InputLinkIndex].Angle; } } #region Find Velocities for Current Position // this next functions puts the vx and vy values as well as the vx_unit and vy_unit in the joints if (!velSolver.Solve()) { Status += "Instant Centers could not be found at" + currentTime + "."; NumericalVelocity(timeStep, joints, links); } #endregion #region Find Accelerations for Current Position // this next functions puts the ax and ay values in the joints if (!accelSolver.Solve()) { Status += "Analytical acceleration could not be found at" + currentTime + "."; NumericalAcceleration(timeStep, joints, links); } #endregion currentTime += timeStep; var jointParams = WriteJointStatesVariablesToMatrixAndToLast(joints); var linkParams = WriteLinkStatesVariablesToMatrixAndToLast(links); if (Forward == (InputSpeed > 0)) { lock (JointParameters) JointParameters.AddNearEnd(currentTime, jointParams); lock (LinkParameters) LinkParameters.AddNearEnd(currentTime, linkParams); } else { lock (JointParameters) JointParameters.AddNearBegin(currentTime, jointParams); lock (LinkParameters) LinkParameters.AddNearBegin(currentTime, linkParams); } } } while (validPosition && LessThanFullRotation()); }