string AsRelativeMovePosition(INcMachine ncMachine, IMachinePosition previousPosition)
{
string line = "";
switch (ncMachine.MachineGeometry)
{
case MachineGeometry.XA:
line = "X" + (X - previousPosition.X).ToString(linearFormat) + " "
+ "A" + (Adeg - previousPosition.Adeg).ToString(rotaryFormat);
break;
case MachineGeometry.XYZ:
line = line = "X" + (X - previousPosition.X).ToString(linearFormat) + " "
+ "Y" + (Y - previousPosition.Y).ToString(linearFormat) + " "
+ "Z" + (Z - previousPosition.Z).ToString(linearFormat);
break;
case MachineGeometry.XYZBC:
line = line = "X" + (X - previousPosition.X).ToString(linearFormat) + " "
+ "Y" + (Y - previousPosition.Y).ToString(linearFormat) + " "
+ "Z" + (Z - previousPosition.Z).ToString(linearFormat) + " "
+ "B" + (Bdeg - previousPosition.Bdeg).ToString(rotaryFormat) + " "
+ "C" + (Cdeg - previousPosition.Cdeg).ToString(rotaryFormat);
break;
}
return(line);
}
public double DistanceTo(IMachinePosition pos)
{
var v1 = new Vector3(X, Y, Z);
var v2 = new Vector3(pos.X, pos.Y, pos.Z);
double d = v1.DistanceTo(v2);
return(d);
}
public NcPositionCommand(int lineNumber, IMachinePosition machinePosition,
IFeedrate feedrate, BlockType blockType)
{
MachinePosition = machinePosition;
this.blockType = blockType;
moveType = MoveType.ABSOLUTE;
Feedrate = feedrate;
LineNumber = lineNumber;
}
public MachinePosition(IMachinePosition p)
{
Adeg = p.Adeg;
Bdeg = p.Bdeg;
Cdeg = p.Cdeg;
X = p.X;
Y = p.Y;
Z = p.Z;
}
public NcPositionCommand(int lineNumber, IMachinePosition machinePosition, NcPositionCommand previousPositionCommand,
IFeedrate feedrate, BlockType blockType)
{
MachinePosition = machinePosition;
this.blockType = blockType;
moveType = MoveType.RELATIVE;
Feedrate = feedrate;
LineNumber = lineNumber;
prevPosition = previousPositionCommand.MachinePosition;
}
public string AsNcString(INcMachine ncMachine, IMachinePosition prevPosition, BlockType blockType, MoveType moveType)
{
rotaryFormat = ncMachine.MachineCode.RotaryAxisFormat;
linearFormat = ncMachine.MachineCode.LinearAxisFormat;
this.blockType = blockType;
if (moveType == MoveType.RELATIVE)
{
return(AsRelativeMovePosition(ncMachine, prevPosition));
}
else
{
return(AsAbsoluteMovePosition(ncMachine));
}
}
public List <string> AsNcTextFile(INcMachine ncMachine)
{
var file = new List <string>();
IMachinePosition prevPosition = ncMachine.ProgStartPosition;
file.AddRange(GetHeader(ncMachine));
foreach (var ent in data)
{
file.Add(ent.AsNcString(ncMachine));
}
file.AddRange(GetFooter(ncMachine));
file.AddRange(GetEndOfFile(ncMachine));
return(file);
}
/// <summary>
/// return true and blocktype if line is a g-code line
/// </summary>
/// <param name="ncLine">line from program</param>
/// <param name="blockT">block type</param>
/// <returns>true if line is part of path</returns>
private void CalcSweepAngle(ref ArcPathEntity arc, IMachinePosition startPoint)
{
Vector3 vRadius = new Vector3();
double startAngle = 0;
double endAngle = 0;
double radius = 0;
double sweep = 0;
double dxprev = 0;
double dyprev = 0;
double dzprev = 0;
Vector3 vEnd = new GeometryLib.Vector3(arc.Position.X, arc.Position.Y, arc.Position.Z);
Vector3 vStart = new Vector3(startPoint.X, startPoint.Y, startPoint.Z);
switch (arc.ArcType)
{
case ArcSpecType.IJKAbsolute:
arc.CenterPoint = new GeometryLib.Vector3(arc.Position.X - arc.Icoordinate, arc.Position.Y - arc.Jcoordinate, arc.Position.Z - arc.Kcoordinate);
arc.Radius = vEnd.Length;
radius = arc.Radius;
dxprev = startPoint.X - arc.Icoordinate;
dyprev = startPoint.Y - arc.Jcoordinate;
dzprev = startPoint.Z - arc.Kcoordinate;
break;
case ArcSpecType.Radius:
break;
case ArcSpecType.NCI:
vRadius = new Vector3(arc.Position.X - arc.CenterPoint.X, arc.Position.Y - arc.CenterPoint.Y, arc.Position.Z - arc.CenterPoint.Z);
arc.Radius = vRadius.Length;
break;
default:
case ArcSpecType.IJKRelative:
vRadius = new GeometryLib.Vector3(arc.Icoordinate, arc.Jcoordinate, arc.Kcoordinate);
arc.Radius = vRadius.Length;
arc.CenterPoint = new Vector3(startPoint.X + arc.Icoordinate, startPoint.Y + arc.Jcoordinate, startPoint.Z + arc.Kcoordinate);
break;
}
switch (arc.ArcPlane)
{
case ArcPlane.XY:
startAngle = Math.Atan2(vStart.Y - arc.CenterPoint.Y, vStart.X - arc.CenterPoint.X);
endAngle = Math.Atan2(vEnd.Y - arc.CenterPoint.Y, vEnd.X - arc.CenterPoint.X);
break;
case ArcPlane.XZ:
startAngle = Math.Atan2(vStart.Z - arc.CenterPoint.Z, vStart.X - arc.CenterPoint.X);
endAngle = Math.Atan2(vEnd.Z - arc.CenterPoint.Z, vEnd.X - arc.CenterPoint.X);
break;
case ArcPlane.YZ:
startAngle = Math.Atan2(vStart.Z - arc.CenterPoint.Z, vStart.Y - arc.CenterPoint.Y);
endAngle = Math.Atan2(vEnd.Z - arc.CenterPoint.Z, vEnd.Y - arc.CenterPoint.Y);
break;
}
sweep = endAngle - startAngle;
if (arc.Type == BlockType.CWARC)
{
sweep = 2 * Math.PI - sweep;
if (sweep > 0)
{
sweep = -1 * sweep;
}
}
else
{
if (sweep < 0)
{
sweep = Math.Abs(sweep);
}
}
if (arc.FullArcFlag)
{
sweep = 2 * Math.PI;
}
arc.SweepAngle = sweep;
arc.StartAngleRad = startAngle;
}
