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 void MoveToCompletedLength(double targetLength)
{
--_nextWorkSegmentIndex;
if (_nextWorkSegmentIndex < 1)
{
_completedLength = 0;
_nextWorkSegmentIndex = 0;
}
while (_completedLength > targetLength)
{
--_nextWorkSegmentIndex;
_currentSegmentInfo = _workSegments[_nextWorkSegmentIndex];
_completedLength -= _currentSegmentInfo.Segment.Length;
}
while (_completedLength < targetLength && _nextWorkSegmentIndex < _workSegments.Length)
{
_currentSegmentInfo = _workSegments[_nextWorkSegmentIndex];
if (_completedLength + _currentSegmentInfo.Segment.Length >= targetLength)
{
// we found the correct segment
break;
}
++_nextWorkSegmentIndex;
_completedLength += _currentSegmentInfo.Segment.Length;
}
_currentSegmentInfo = _workSegments[_nextWorkSegmentIndex];
_currentSlicer = new ToolPathSegmentSlicer(_currentSegmentInfo.Segment);
++_nextWorkSegmentIndex;
while (_currentSlicer.SliceProgress * _currentSlicer.Segment.Length + _completedLength < targetLength)
{
_currentSlicer.Slice(_currentSpeed, PathSpeedLimitCalculator.TimeGrain);
if (_currentSlicer.IsComplete)
{
break;
}
}
}