private void generateMovement(MachineState state, Point3D lastPosition, ToolPath result)
{
var startPosition = lastPosition;
var endPosition = state.CurrentPosition;
switch (state.MotionMode)
{
case MotionMode.IsLinear:
case MotionMode.IsLinearRapid:
//for now aproximate by a simple line
result.AddLine(endPosition, state);
break;
case MotionMode.IsCircularCW:
addCircularApproximation(result, state, startPosition, endPosition, clockwise: true);
break;
case MotionMode.IsCircularCCW:
addCircularApproximation(result, state, startPosition, endPosition, clockwise: false);
break;
default:
throw new NotImplementedException();
}
}
private void addCircularApproximation(ToolPath result, MachineState state, Point3D startPosition, Point3D endPosition, bool clockwise)
{
if (state.PlaneSelectionMode != PlaneSelectionMode.XY)
{
throw new NotImplementedException();
}
//if (state.DistanceMode != DistanceMode.Relative)
//TODO check specification properly regarding IJK behaviour - it seems inconsistent
Point3D centerPoint;
if (state.BufferI.HasValue)
{
var i = state.BufferI.Value;
var j = state.BufferJ.Value;
centerPoint = new Point3D(startPosition.X + i, startPosition.Y + j, startPosition.Z); //TODO this changes with selected plane
}
else
{
var inputRadius = state.BufferR.Value;
// Negative R is g-code-alese for "I want a circle with more than 180 degrees of travel" (go figure!),
// even though it is advised against ever generating such circles in a single line of g-code. By
// inverting the sign of h_x2_div_d the center of the circles is placed on the opposite side of the line of
// travel and thus we get the unadvisably long arcs as prescribed.
var x1 = startPosition.X;
var y1 = startPosition.Y;
var x2 = endPosition.X;
var y2 = endPosition.Y;
var q = Math.Sqrt(Math.Pow((x2 - x1), 2) + Math.Pow((y2 - y1), 2));
var y3 = (y1 + y2) / 2;
var x3 = (x1 + x2) / 2;
var basex = Math.Sqrt(Math.Pow(inputRadius, 2) - Math.Pow((q / 2), 2)) * (y1 - y2) / q; //calculate once
var basey = Math.Sqrt(Math.Pow(inputRadius, 2) - Math.Pow((q / 2), 2)) * (x2 - x1) / q; //calculate once
var centerx1 = x3 + basex; //center x of circle 1
var centery1 = y3 + basey; //center y of circle 1
var centerx2 = x3 - basex; //center x of circle 2
var centery2 = y3 - basey; //center y of circle 2
//TODO negative radius and center selection
centerPoint = new Point3D(centerx1, centery1, startPosition.Z);
}
var radius = (startPosition - centerPoint).Length;
var radius2 = (endPosition - centerPoint).Length;
if (Math.Abs(radius - radius2) > _precision)
{
throw new NotSupportedException("Invalid arc detected");
}
var totalC = (startPosition - endPosition).Length;
var totalAngle = 2 * Math.Asin(totalC / 2 / radius);
var stepAngle = 2 * Math.Acos(1 - _precision / radius);
var initialAngle = Math.Atan2(startPosition.Y - centerPoint.Y, startPosition.X - centerPoint.X); //REALLY, the coordinates are reversed
var finalAngle = Math.Atan2(endPosition.Y - centerPoint.Y, endPosition.X - centerPoint.X);
var pi2 = Math.PI * 2;
initialAngle = (initialAngle + pi2) % pi2;
finalAngle = (finalAngle + pi2) % pi2;
double direction;
if (clockwise)
{
direction = -1.0;
if (initialAngle < finalAngle)
{
//rotating clockwise == subtract steps from initial angle
initialAngle += pi2;
}
}
else
{
direction = 1.0;
if (initialAngle > finalAngle)
{
//rotating ccw == add steps to initial angle
initialAngle -= pi2;
}
}
totalAngle = Math.Abs(finalAngle - initialAngle);
var segmentCount = Math.Ceiling(totalAngle / stepAngle);
var lastPosition = startPosition;
for (var segmentIndex = 1; segmentIndex < segmentCount; ++segmentIndex)
{
var actualAngle = initialAngle + direction * totalAngle * segmentIndex / segmentCount;
var x = Math.Cos(actualAngle) * radius;
var y = Math.Sin(actualAngle) * radius;
var nextPosition = new Point3D();
nextPosition.X = centerPoint.X + x;
nextPosition.Y = centerPoint.Y + y;
nextPosition.Z = centerPoint.Z; //TODO this changes with plane selection
result.AddLine(nextPosition, state);
lastPosition = nextPosition;
}
result.AddLine(endPosition, state);
}
