public ArcInterpolation(CPointF origin, CPointF center, CPointF end, bool clockwise)
{
Origin = origin;
Center = center;
End = end;
Clockwise = clockwise;
Initialize();
}
public CPointF GetArcPoint(float t)
{
float Delta = Gamma * t + Alpha;
float x = Distance * (float)Math2.Cos(Delta);
float y = Distance * (float)Math2.Sin(Delta);
CPointF result = new CPointF(Center.X + x, Center.Y + y);
return(result);
}
public ArcInterpolation(CPointF origin, CPointF end, float radius, bool clockwise)
{
// PhlatScript uses the radius format, so we need to support it.
Origin = origin;
End = end;
Distance = radius;
Clockwise = !clockwise;
// Calculate center. Best explanation found here:
// http://mathforum.org/library/drmath/view/53027.html
float x1 = Origin.X;
float y1 = Origin.Y;
float x2 = End.X;
float y2 = End.Y;
float r = Distance;
// Distance between start and end
float q = Math2.Sqrt((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1));
// middle ploint between both points
float x3 = (x1 + x2) / 2;
float y3 = (y1 + y2) / 2;
if (!Clockwise)
{
Center = new CPointF(
x3 - Math2.Sqrt(r * r - (q / 2) * (q / 2)) * (y1 - y2) / q,
y3 - Math2.Sqrt(r * r - (q / 2) * (q / 2)) * (x2 - x1) / q);
}
else
{
Center = new CPointF(
x3 + Math2.Sqrt(r * r - (q / 2) * (q / 2)) * (y1 - y2) / q,
y3 + Math2.Sqrt(r * r - (q / 2) * (q / 2)) * (x2 - x1) / q);
}
const string E_NO_ARC_CENTER = "Could not find a suitable center for arc";
if (Center.X == float.MinValue)
{
Machine.Error(E_NO_ARC_CENTER);
}
if (Center.Y == float.MinValue)
{
Machine.Error(E_NO_ARC_CENTER);
}
Initialize();
}
private static void ArcMove(float x, float y, float z, float i, float j, float k, float radius, bool clockwise)
{
// Only XY plane supported for now.
CPointF start = new CPointF(mDev.mCurrentX, mDev.mCurrentY), end;
ArcInterpolation arc = null;
if (radius == float.MinValue)
{
// Center format arc.
if (i == float.MinValue && j == float.MinValue)
{
Error("G2/3: I and J are missing");
}
if (i == float.MinValue)
{
i = 0;
}
if (j == float.MinValue)
{
j = 0;
}
if (x == float.MinValue)
{
x = mDev.mCurrentX;
}
if (y == float.MinValue)
{
y = mDev.mCurrentY;
}
if (z == float.MinValue)
{
z = mDev.mCurrentZ;
}
CPointF center = new CPointF(mDev.mCurrentX + i, mDev.mCurrentY + j);
end = new CPointF(x, y);
arc = new ArcInterpolation(start, center, end, clockwise);
}
else
{
// Radius format arc
// XYZ are the endpoint. R is the radius.
if (x == float.MinValue && y == float.MinValue)
{
Error("G2/3: X and Y are missing");
}
if (x == float.MinValue)
{
x = mDev.mCurrentX;
}
if (y == float.MinValue)
{
y = mDev.mCurrentY;
}
if (mDistanceMode == DistanceMode.Absolute)
{
end = new CPointF(x, y);
}
else
{
end = new CPointF(mDev.mCurrentX + x, mDev.mCurrentY + y);
}
arc = new ArcInterpolation(start, end, radius, clockwise);
}
if (arc == null)
{
Error("G2/3: could not find an arc solution");
}
for (float t = ArcStep; t <= 1.0 + (ArcStep / 2); t += ArcStep)
{
CPointF target = arc.GetArcPoint(t);
// Only XY supported
mDev.MoveAbsoluteLinear(target.X, target.Y, mDev.mCurrentZ);
}
}
