private void Setup(Vector3 delta, Vector3 compensation, bool ccw, AxisState.Plane plane, RTMovementOptions opts, MachineParameters config)
{
Plane = plane;
Delta = delta;
Compensation = compensation;
CCW = ccw;
Config = config;
Options = opts;
DeltaProj = VectorPlaneProj(Delta + Compensation, Plane);
Height = VectorPlaneNormalProj(Delta + Compensation, Plane);
}
private decimal VectorPlaneNormalProj(Vector3 delta, AxisState.Plane plane)
{
switch (plane)
{
case AxisState.Plane.XY:
return(delta.z);
case AxisState.Plane.YZ:
return(delta.x);
case AxisState.Plane.ZX:
return(delta.y);
default:
throw new ArgumentOutOfRangeException("Invalid axis");
}
}
private Vector2 VectorPlaneProj(Vector3 delta, AxisState.Plane plane)
{
switch (plane)
{
case AxisState.Plane.XY:
return(new Vector2(delta.x, delta.y));
case AxisState.Plane.YZ:
return(new Vector2(delta.y, delta.z));
case AxisState.Plane.ZX:
return(new Vector2(delta.z, delta.x));
default:
throw new ArgumentOutOfRangeException("Invalid axis");
}
}
public RTArcMoveCommand(Vector3 delta, Vector3 compensation, Vector3 startToCenter, bool ccw, AxisState.Plane plane,
RTMovementOptions opts, MachineParameters config)
{
Setup(delta, compensation, ccw, plane, opts, config);
startToCenterProj = VectorPlaneProj(startToCenter, Plane);
endToCenterProj = startToCenterProj - DeltaProj;
decimal nd = Vector2.Cross(DeltaProj, startToCenterProj);
if (nd > 0 && CCW || nd < 0 && !CCW)
{
bigArc = false;
}
else
{
bigArc = true;
}
R = startToCenter.Length();
FinalSetup();
}
public RTArcMoveCommand(Vector3 delta, Vector3 compensation, decimal r, bool ccw, AxisState.Plane plane,
RTMovementOptions opts, MachineParameters config)
{
Setup(delta, compensation, ccw, plane, opts, config);
R = Math.Abs(r);
bigArc = (r < 0);
decimal d = DeltaProj.Length();
decimal hcl = (decimal)Math.Sqrt((double)(R * R - d * d / 4));
if (ccw && !bigArc || !ccw && bigArc)
{
hcl = -hcl;
}
startToCenterProj = DeltaProj / 2 + Vector2.Normalize(DeltaProj.Right()) * hcl;
endToCenterProj = startToCenterProj - DeltaProj;
FinalSetup();
}
