private ModelPathEntity interpolateLine(PathEntity5Axis p1, PathEntity5Axis p2, double currentTime)
{
ModelPathEntity mpe = new ModelPathEntity(p2);
double dt = currentTime - p1.CumulativeTime;
if (p1.Type == BlockType.FiveAxis)
{
isFiveAxis = true;
}
else
{
p1.JetVector = GeometryLib.Vector3.ZAxis;
}
if (p2.Type == BlockType.FiveAxis)
{
isFiveAxis = true;
}
else
{
p2.JetVector = GeometryLib.Vector3.ZAxis;
}
mpe.Position = interpolatePosition(p1, p2, currentTime);
mpe.JetVector = interpolateVector(p1, p2, currentTime);
return(mpe);
}
string buildLine(PathEntity5Axis pe)
{
string line = "";
switch (pe.Type)
{
case BlockType.CCWArc:
case BlockType.CWArc:
var ape = (ArcPathEntity)pe;
break;
case BlockType.Command:
break;
case BlockType.Comment:
break;
case BlockType.Delay:
var dpe = (DelayPathEntity)pe;
line = addDelay(dpe.Delay);
break;
case BlockType.FiveAxis:
case BlockType.Linear:
case BlockType.Rapid:
var lpe = (LinePathEntity)pe;
line = addMove(lpe);
break;
case BlockType.Unknown:
break;
}
return(line);
}
private ModelPathEntity interpolateArc(PathEntity5Axis p1, PathEntity5Axis p2, double currentTime)
{
ModelPathEntity mpe = new ModelPathEntity(p2);
ArcPathEntity arc = p2 as ArcPathEntity;
double dt = currentTime - p1.CumulativeTime;
mpe.Position = getNewArcEndpoint(arc, dt);
return(mpe);
}
private void AddEntity(PathEntity5Axis pe)
{
if (toolpath.Count > 0)
{
pe.PrevPosition = new CNCLib.XYZBCMachPosition(toolpath.Last().Position);
}
else
{
pe.PrevPosition = new CNCLib.XYZBCMachPosition(pe.Position);
}
toolpath.Add(pe);
}
private Vector3 interpolateVector(PathEntity5Axis p1, PathEntity5Axis p2, double currentTime)
{
double dvx = p2.JetVector.X - p1.JetVector.X;
double dvy = p2.JetVector.Y - p1.JetVector.Y;
double dvz = p2.JetVector.Z - p1.JetVector.Z;
double t = interpolateTime(p1, currentTime);
double vx = p1.JetVector.X + t * dvx;
double vy = p1.JetVector.Y + t * dvy;
double vz = p1.JetVector.Z + t * dvz;
return(new Vector3(vx, vy, vz));
}
void appendPositions(PathEntity5Axis pe, ref StringBuilder line)
{
line.Append("X");
line.Append(pe.Position.X.ToString(machine.PFormat) + machine.Sp);
line.Append("Y");
line.Append(pe.Position.Y.ToString(machine.PFormat) + machine.Sp);
line.Append("Z");
line.Append(pe.Position.Z.ToString(machine.PFormat) + machine.Sp);
line.Append("B");
line.Append(pe.Position.Bdeg.ToString(machine.PFormat) + machine.Sp);
line.Append("C");
line.Append(pe.Position.Cdeg.ToString(machine.PFormat) + machine.Sp);
}
string addMove(PathEntity5Axis pe)
{
StringBuilder line = new StringBuilder();
appendLineNumber(ref line);
appendGCode(pe.Type, ref line);
appendPositions(pe, ref line);
if (pe.Type != BlockType.Rapid)
{
appendFeedrate(pe.Feedrate, ref line);
}
return(line.ToString());
}
private List <ModelPathEntity> parseDelay(double increment, PathEntity5Axis entity, DelayPathEntity delayEnt)
{
try
{
List <ModelPathEntity> path = new List <ModelPathEntity>();
int parseCount = (int)Math.Round(delayEnt.Delay / increment);
for (int i = 0; i < parseCount; i++)
{
path.Add(new ModelPathEntity(entity));
}
return(path);
}
catch (Exception)
{
throw;
}
}
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 ModelPathEntity interpolate(PathEntity5Axis p1, PathEntity5Axis p2, double currentTime, double timeInc)
{
ModelPathEntity mpe = new ModelPathEntity(p1);
if (p2 is LinePathEntity)
{
mpe = interpolateLine(p1, p2, currentTime);
}
if (p2 is ArcPathEntity)
{
mpe = interpolateArc(p1, p2, currentTime);
}
if (p2 is DelayPathEntity)
{
mpe = interpolateDelay(p1, p2, currentTime);
}
mpe.CumulativeTime = currentTime;
mpe.TravelTime = timeInc;
return(mpe);
}
internal ToolPath5Axis ParsePath(List <string> file)
{
jeton = false;
int pathNumber = 0;
foreach (string line in file)
{
BlockType blockT = BlockType.Unknown;
if (!jeton)
{
jeton = isPathStart(line);
}
else
{
jeton = !isPathEnd(line);
}
if (isPath(line, out blockT))
{
PathEntity5Axis pe = parseString(line, blockT, jeton);
pe.PathNumber = pathNumber;
path.Add(pe);
pathNumber++;
}
}
for (int i = 1; i < path.Count; i++)
{
if (path[i].JetOn && !path[i - 1].JetOn)
{
path[i - 1].JetOn = true;
path[i - 1].Feedrate = path[i].Feedrate;
}
}
// calc arc sweep angle and travel time for each segment
calcPathLengths();
fillMissingCoords();
return(path);
}
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);
}
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;
}
}
private List <ModelPathEntity> parseLine(double increment, PathEntity5Axis p2, PathEntity5Axis p1)
{
try
{
List <ModelPathEntity> path = new List <ModelPathEntity>();
double segLength = p2.Position.DistanceTo(p1.Position);
int parseCount = (int)Math.Round(segLength / increment);
if (parseCount == 0)
{
parseCount = 1;
}
if (p1.Type == BlockType.FiveAxis)
{
isFiveAxis = true;
}
else
{
p1.JetVector = GeometryLib.Vector3.ZAxis;
}
if (p2.Type == BlockType.FiveAxis)
{
isFiveAxis = true;
}
else
{
p2.JetVector = GeometryLib.Vector3.ZAxis;
}
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 dvx = p2.JetVector.X - p1.JetVector.X;
double dvy = p2.JetVector.Y - p1.JetVector.Y;
double dvz = p2.JetVector.Z - p1.JetVector.Z;
if (Math.Abs(dc) > .5 || Math.Abs(db) > .5)
{
isFiveAxis = true;
}
double pathsegF = p2.Feedrate.Value;
for (int j = 0; j < parseCount; j++)
{
ModelPathEntity pathSeg = new ModelPathEntity(p2);
//calc new position
pathSeg.Position = new CNCLib.XYZBCMachPosition();
pathSeg.Position.X = p1.Position.X + j * dx / parseCount;
pathSeg.Position.Y = p1.Position.Y + j * dy / parseCount;
pathSeg.Position.Z = p1.Position.Z + j * dz / parseCount;
pathSeg.Position.Bdeg = p1.Position.Bdeg + j * db / parseCount;
pathSeg.Position.Cdeg = p1.Position.Cdeg + j * dc / parseCount;
//calc new jet vector
double vx = p1.JetVector.X + j * dvx / parseCount;
double vy = p1.JetVector.Y + j * dvy / parseCount;
double vz = p1.JetVector.Z + j * dvz / parseCount;
pathSeg.JetVector = new GeometryLib.Vector3(vx, vy, vz);
if (p2.Feedrate.Inverted)
{
pathSeg.Feedrate.Value = pathsegF;
}
else
{
pathSeg.Feedrate.Value = p2.Feedrate.Value;
}
if (p2.JetOn == true)
{
pathSeg.JetOn = true;
}
else
{
pathSeg.JetOn = false;
}
path.Add(pathSeg);
}
return(path);
}
catch (Exception)
{
throw;
}
}
private ModelPathEntity interpolateDelay(PathEntity5Axis p1, PathEntity5Axis p2, double currentTime)
{
ModelPathEntity mpe = new ModelPathEntity(p1);
return(mpe);
}
private double interpolateTime(PathEntity5Axis p1, double currentTime)
{
return(currentTime - p1.CumulativeTime);
}
