internal void AddSegment(ToolPathSegment segment)
{
if (_lastAddedSegment != null)
{
var edgeLimit = PathSpeedLimitCalculator.CalculateEdgeLimit(_lastAddedSegment, segment);
_edgeLimits[segment] = edgeLimit;
}
_lastAddedSegment = segment;
var segmentInfo = new SegmentPlanningInfo(segment);
_workSegments.Enqueue(segmentInfo);
foreach (var limitCalculator in _openLimitCalculators)
{
limitCalculator.AddLookaheadSegments(new[] { segment }, _edgeLimits);
}
_openLimitCalculators.Add(segmentInfo.LimitCalculator);
foreach (var limitCalculator in _openLimitCalculators.ToArray())
{
if (!limitCalculator.NeedsMoreFollowingSegments)
{
_openLimitCalculators.Remove(limitCalculator);
}
}
}
internal InstructionCNC GenerateNextInstruction(double desiredSpeed)
{
if (_currentSlicer == null || _currentSlicer.IsComplete)
{
if (_currentSlicer != null)
{
_completedLength += _currentSlicer.CompletedLength;
}
_currentSegmentInfo = _workSegments[_nextWorkSegmentIndex];
_currentSlicer = new ToolPathSegmentSlicer(_currentSegmentInfo.Segment);
_nextWorkSegmentIndex += 1;
}
var limitCalculator = _currentSegmentInfo.LimitCalculator;
var currentSegment = _currentSegmentInfo.Segment;
var smoothingLookahead = 5.0 / currentSegment.Length;
var speedLimit = limitCalculator.GetLimit(_currentSlicer.SliceProgress);
double speedLimitLookahead;
if (_currentSpeed < speedLimit)
{
// lookahead is useful to prevent short term accelerations - call it only when acceleration is possible
speedLimitLookahead = limitCalculator.GetLimit(Math.Min(1.0, _currentSlicer.SliceProgress + smoothingLookahead));
}
else
{
speedLimitLookahead = speedLimit;
}
var targetSpeed = Math.Min(desiredSpeed, speedLimit);
if (_currentSpeed < speedLimitLookahead)
{
//allow acceleration only when limit is far enough
_currentSpeed = PathSpeedLimitCalculator.TransitionSpeedTo(_currentSpeed, targetSpeed);
}
var newSpeed = Math.Min(_currentSpeed, speedLimit); //speed limits are valid (acceleration limits accounted)
_currentSpeed = newSpeed;
return(_currentSlicer.Slice(_currentSpeed, PathSpeedLimitCalculator.TimeGrain));
}
internal SegmentPlanningInfo(ToolPathSegment segment)
{
Segment = segment;
LimitCalculator = new PathSpeedLimitCalculator(segment);
}