/// <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());
}
/// <summary>
/// Sets the initial velocity and acceleration.
/// </summary>
/// <param name="joints">The joints.</param>
/// <param name="links">The links.</param>
/// <param name="initJointParams">The initialize joint parameters.</param>
/// <param name="initLinkParams">The initialize link parameters.</param>
private void SetInitialVelocityAndAcceleration(List <Joint> joints, List <Link> links,
out double[,] initJointParams, out double[,] initLinkParams)
{
var posFinder = new PositionFinder(joints, links, gearsData, simulationDriveIndex);
posFinder.UpdateSliderPosition();
var velSolver = new VelocitySolver(joints, links, FirstInputJointIndex, simulationDriveIndex, InputLinkIndex,
InputSpeed, gearsData, AverageLength);
var accelSolver = new AccelerationSolver(joints, links, FirstInputJointIndex, simulationDriveIndex,
InputLinkIndex, InputSpeed, gearsData, AverageLength);
initJointParams = WriteJointStatesVariablesToMatrixAndToLast(joints);
initLinkParams = WriteLinkStatesVariablesToMatrixAndToLast(links);
var smallTimeStep = (double.IsNaN(FixedTimeStep))
? Constants.SmallPerturbationFraction
: Constants.SmallPerturbationFraction * FixedTimeStep;
if (velSolver.Solve())
{
for (var i = 0; i <= simulationDriveIndex; i++)
{
initJointParams[oIOSJ[i], 2] = joints[i].VxLast = joints[i].Vx;
initJointParams[oIOSJ[i], 3] = joints[i].VyLast = joints[i].Vy;
}
for (var i = 0; i <= InputLinkIndex; i++)
{
initLinkParams[oIOSL[i], 1] = links[i].VelocityLast = links[i].Velocity;
}
if (accelSolver.Solve())
{
for (var i = 0; i <= simulationDriveIndex; i++)
{
initJointParams[oIOSJ[i], 4] = joints[i].Ax;
initJointParams[oIOSJ[i], 5] = joints[i].Ay;
}
for (var i = 0; i <= InputLinkIndex; i++)
{
initLinkParams[oIOSL[i], 2] = links[i].Acceleration;
}
}
else
{
/* velocity was successfully found, but not acceleration. */
if (posFinder.DefineNewPositions(smallTimeStep * InputSpeed, ref IsDyadic) &&
velSolver.Solve())
{
/* forward difference on velocities to create accelerations. */
for (var i = 0; i <= simulationDriveIndex; i++)
{
initJointParams[oIOSJ[i], 4] = joints[i].Ax = (joints[i].Vx - joints[i].VxLast) / smallTimeStep;
initJointParams[oIOSJ[i], 5] = joints[i].Ay = (joints[i].Vy - joints[i].VyLast) / smallTimeStep;
}
for (var i = 0; i <= InputLinkIndex; i++)
{
initLinkParams[oIOSL[i], 2] = links[i].Acceleration = (links[i].Velocity - links[i].VelocityLast) / smallTimeStep;
}
/* since the position solving wrote values to joints[i].x and .y, we need to reset them, for further work. */
foreach (var joint in SimulationJoints)
{
joint.X = joint.XInitial;
joint.Y = joint.YInitial;
}
foreach (var link in SimulationLinks)
{
link.Angle = link.AngleInitial;
}
}
}
return;
}
var ForwardJointParams = new double[NumAllJoints, 2];
var ForwardLinkParams = new double[NumLinks];
/*** Stepping Forward in Time ***/
var forwardSuccess = posFinder.DefineNewPositions(smallTimeStep * InputSpeed, ref IsDyadic);
if (forwardSuccess)
{
for (var i = 0; i < NumAllJoints; i++)
{
ForwardJointParams[oIOSJ[i], 0] = joints[i].X;
ForwardJointParams[oIOSJ[i], 1] = joints[i].Y;
}
for (var i = 0; i < NumLinks; i++)
{
ForwardLinkParams[oIOSL[i]] = links[i].Angle;
}
}
/*** Stepping Backward in Time ***/
var BackwardJointParams = new double[NumAllJoints, 2];
var BackwardLinkParams = new double[NumLinks];
var backwardSuccess = posFinder.DefineNewPositions(-smallTimeStep * InputSpeed, ref IsDyadic);
if (backwardSuccess)
{
for (var i = 0; i < NumAllJoints; i++)
{
BackwardJointParams[oIOSJ[i], 0] = joints[i].X;
BackwardJointParams[oIOSJ[i], 1] = joints[i].Y;
}
for (var i = 0; i < NumLinks; i++)
{
BackwardLinkParams[oIOSL[i]] = links[i].Angle;
}
}
if (forwardSuccess && backwardSuccess)
{
/* central difference puts values in init parameters. */
for (var i = 0; i < NumAllJoints; i++)
{
/* first-order central finite difference */
initJointParams[i, 2] = (ForwardJointParams[i, 0] - BackwardJointParams[i, 0]) / (2 * smallTimeStep);
initJointParams[i, 3] = (ForwardJointParams[i, 1] - BackwardJointParams[i, 1]) / (2 * smallTimeStep);
/* second-order central finite difference */
initJointParams[i, 4] = (ForwardJointParams[i, 0] - 2 * initJointParams[i, 0] +
BackwardJointParams[i, 0]) / (smallTimeStep * smallTimeStep);
initJointParams[i, 5] = (ForwardJointParams[i, 1] - 2 * initJointParams[i, 1] +
BackwardJointParams[i, 1]) / (smallTimeStep * smallTimeStep);
}
for (var i = 0; i < NumAllLinks; i++)
{
/* first-order central finite difference */
initLinkParams[i, 1] = (ForwardLinkParams[i] - BackwardLinkParams[i]) / (2 * smallTimeStep);
/* second-order central finite difference */
initLinkParams[i, 2] = (ForwardLinkParams[i] - 2 * initLinkParams[i, 0] + BackwardLinkParams[i])
/ (smallTimeStep * smallTimeStep);
}
}
else if (forwardSuccess)
{
/* forward difference puts values in init parameters. */
for (var i = 0; i < NumAllJoints; i++)
{
/* first-order forward finite difference */
initJointParams[i, 2] = (ForwardJointParams[i, 0] - initJointParams[i, 0]) / smallTimeStep;
initJointParams[i, 3] = (ForwardJointParams[i, 1] - initJointParams[i, 1]) / smallTimeStep;
}
for (var i = 0; i < NumAllLinks; i++)
{
/* first-order forward finite difference */
initLinkParams[i, 1] = (ForwardLinkParams[i] - initLinkParams[i, 0]) / smallTimeStep;
}
}
else if (backwardSuccess)
{
/* backward difference puts values in init parameters. */
for (var i = 0; i < NumAllJoints; i++)
{
/* first-order backward finite difference */
initJointParams[i, 2] = (initJointParams[i, 0] - BackwardJointParams[i, 0]) / smallTimeStep;
initJointParams[i, 3] = (initJointParams[i, 1] - BackwardJointParams[i, 1]) / smallTimeStep;
}
for (var i = 0; i <= NumAllLinks; i++)
{
/* first-order backward finite difference */
initLinkParams[i, 1] = (initLinkParams[i, 0] - BackwardLinkParams[i]) / smallTimeStep;
}
}
/* since the position solving wrote values to joints[i].x and .y, we need to reset them, for further work. */
foreach (var joint in SimulationJoints)
{
joint.X = joint.XInitial;
joint.Y = joint.YInitial;
}
foreach (var link in SimulationLinks)
{
link.Angle = link.AngleInitial;
}
}
