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