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