/// <summary>
/// (Iteratively) merges the given constraint with the previous constraint the the lower MaxVel of both.
/// </summary>
/// <param name="constraint">Constraint to merge with its previous constraint</param>
/// <param name="index">Index of the given constraint within the EffectiveConstraints list</param>
/// <param name="forceMerge">Forces a complete merge without iteratively approaching to the previous constraint</param>
/// <returns>True if constraint was completely merged, otherwise false</returns>
private bool MergeWithPreviousConstraint(VelocityConstraint constraint, int index, bool forceMerge = false)
{
const double reduceByDistance = 100; // mm
const double reduceByVelocity = 50; // mm/s
if (index == 0)
{
// first constraint -> no previous constraint -> try reducing velocity for reachability
constraint.ReduceBy(reduceByVelocity);
}
else
{
var previousConstraint = EffectiveConstraints[index - 1];
if (!forceMerge && previousConstraint > constraint && previousConstraint.Length > reduceByDistance)
{
// The previous constraint is higher than the given constraint. As it is not allowed
// to increase the MaxVel of a constraint, the MaxVel of the previous constraint must be reduced.
// Because the previous constraint may be a very long one, a complete merge at one time could waste
// precious "high velocity time". Therefore, the merging is done iteratively.
constraint.Start -= reduceByDistance;
constraint.Length += reduceByDistance;
previousConstraint.Length -= reduceByDistance;
}
else if (!forceMerge && previousConstraint < constraint && Math.Abs(previousConstraint - constraint) > reduceByVelocity)
{
// The previous constraint is below the given constraint. Therefore, the given constraints MaxVelo
// must be reduced. Because that could be a waste of "high velocity time", this is done interatively.
constraint.ReduceBy(reduceByVelocity);
}
else
{
// Either foreMerge is true or the constraint which should be reduced is not long / high enough anymore
// -> now completely merge the constraints by removing the given constraint and adding its length to the previous constraint
// To be safe, the minimum of the both MaxVels is taken
EffectiveConstraints.RemoveAt(index);
previousConstraint.Length += constraint.Length;
previousConstraint.MaximumVelocity = Math.Min(constraint.MaximumVelocity, previousConstraint.MaximumVelocity);
return(true);
}
}
return(false);
}
/// <summary>
/// Removes the given constraint, moves the start of the next constraint to the start of the
/// given constraint and adds its length to the next constraint.
/// </summary>
private void MergeWithNextConstraint(VelocityConstraint constraint, int index)
{
EffectiveConstraints.RemoveAt(index);
EffectiveConstraints[index].Start -= constraint.Length;
EffectiveConstraints[index].Length += constraint.Length;
}