public PathEntity5Axis()
{
ActiveMcodes = new List <string>();
Position = new CNCLib.XYZBCMachPosition();
PrevPosition = new CNCLib.XYZBCMachPosition();
Type = BlockType.Unknown;
Ccomp = CComp.NoChange;
ControlFlag = CtrlFlag.Unknown;
DirVector = new Vector3();
JetVector = new Vector3();
SurfNormal = new Vector3();
Feedrate = new Feedrate();
Depth = 0;
}
public ModelPathEntity()
{
IntersectionIndices = new List <int>();
Ccomp = new CComp();
CcompTangent = false;
ControlFlag = new CtrlFlag();
Depth = 0;
DirVector = new Vector3();
Position = new CNCLib.XYZBCMachPosition();
Feedrate = new ToolpathLib.Feedrate();
JetOn = false;
JetVector = new Vector3();
PathNumber = 0;
SurfNormal = new Vector3();
Type = BlockType.Unknown;
TravelTime = 0;
CumulativeTime = 0;
}
public ModelPathEntity(PathEntity5Axis pEnt)
{
Ccomp = pEnt.Ccomp;
CcompTangent = pEnt.CcompTangent;
ControlFlag = pEnt.ControlFlag;
Depth = pEnt.Depth;
DirVector = new Vector3(pEnt.DirVector);
Position = new CNCLib.XYZBCMachPosition(pEnt.Position);
Feedrate = new ToolpathLib.Feedrate(pEnt.Feedrate);
JetOn = pEnt.JetOn;
JetVector = new Vector3(pEnt.JetVector);
PathNumber = pEnt.PathNumber;
LineNumber = pEnt.LineNumber;
SurfNormal = new Vector3(pEnt.SurfNormal);
Type = pEnt.Type;
TravelTime = pEnt.TravelTime;
CumulativeTime = pEnt.CumulativeTime;
}
private CNCLib.XYZBCMachPosition interpolatePosition(PathEntity5Axis p1, PathEntity5Axis p2, double currentTime)
{
double dx = p2.Position.X - p1.Position.X;
double dy = p2.Position.Y - p1.Position.Y;
double dz = p2.Position.Z - p1.Position.Z;
double db = p2.Position.Bdeg - p1.Position.Bdeg;
double dc = p2.Position.Cdeg - p1.Position.Cdeg;
double t = interpolateTime(p1, currentTime);
var Position = new CNCLib.XYZBCMachPosition();
Position.X = p1.Position.X + t * dx;
Position.Y = p1.Position.Y + t * dy;
Position.Z = p1.Position.Z + t * dz;
Position.Bdeg = p1.Position.Bdeg + t * db;
Position.Cdeg = p1.Position.Cdeg + t * dc;
if (Math.Abs(dc) > .5 || Math.Abs(db) > .5)
{
isFiveAxis = true;
}
return(Position);
}
public CNCLib.XYZBCMachPosition getNewArcEndpoint(ArcPathEntity arc, double newSweepAngle)
{
double coord1 = 0;
double coord2 = 0;
var mp = new CNCLib.XYZBCMachPosition();
switch (arc.Type)
{
case BlockType.CCWArc:
coord1 = arc.Radius * Math.Cos(arc.StartAngleRad + newSweepAngle);
coord2 = arc.Radius * Math.Sin(arc.StartAngleRad + newSweepAngle);
break;
case BlockType.CWArc:
coord1 = arc.Radius * Math.Cos(arc.StartAngleRad - newSweepAngle);
coord2 = arc.Radius * Math.Sin(arc.StartAngleRad - newSweepAngle);
break;
}
switch (arc.ArcPlane)
{
case ArcPlane.XY:
mp.X = coord1 + arc.CenterPoint.X;
mp.Y = coord2 + arc.CenterPoint.Y;
mp.Z = arc.Position.Z;
break;
case ArcPlane.XZ:
mp.X = coord1 + arc.CenterPoint.X;
mp.Z = coord2 + arc.CenterPoint.Z;
mp.Y = arc.Position.Y;
break;
case ArcPlane.YZ:
mp.Y = coord1 + arc.CenterPoint.Y;
mp.Z = coord2 + arc.CenterPoint.Z;
mp.X = arc.Position.X;
break;
}
return(mp);
}
private List <ModelPathEntity> parseArc(double increment, PathEntity5Axis entity, CNCLib.XYZBCMachPosition startPoint)
{
try
{
List <ModelPathEntity> path = new List <ModelPathEntity>();
ArcPathEntity arc = entity as ArcPathEntity;
Vector3 vEnd = new GeometryLib.Vector3(arc.Position.X, arc.Position.Y, arc.Position.Z);
Vector3 vStart = new Vector3(startPoint.X, startPoint.Y, startPoint.Z);
double segLength = Math.Abs(arc.SweepAngle * arc.Radius);
int parseCount = (int)Math.Round(segLength / increment);
if (parseCount == 0)
{
parseCount = 1;
}
double dA = arc.SweepAngle / parseCount;
for (int j = 0; j < parseCount; j++)
{
ModelPathEntity pathSeg = new ModelPathEntity(arc);
pathSeg.Position = getNewArcEndpoint(arc, j * dA);
if (entity.JetOn == true)
{
pathSeg.JetOn = true;
}
else
{
pathSeg.JetOn = false;
}
path.Add(pathSeg);
}
return(path);
}
catch (Exception)
{
throw;
}
}
/// <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, CNCLib.XYZBCMachPosition 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;
}
