public override Plane CartesianLerp(Plane a, Plane b, double t, double min, double max)
{
t = (t - min) / (max - min);
if (double.IsNaN(t))
{
t = 0;
}
var anglesA = RobotCellKuka.PlaneToEuler(a);
var anglesB = RobotCellKuka.PlaneToEuler(b);
var result = new double[anglesA.Length];
for (int i = 0; i < anglesA.Length; i++)
{
if (i < 3)
{
result[i] = anglesA[i] * (1.0 - t) + anglesB[i] * t;
}
else
{
double angleA = anglesA[i].ToRadians();
double angleB = anglesB[i].ToRadians();
double delta = angleB - angleA;
double absDelta = Abs(delta);
if (absDelta > PI)
{
delta = Sign(delta) * (absDelta - PI * 2);
}
angleB = angleA + delta;
double angle = angleA * (1.0 - t) + angleB * t;
// if (i == 4) angle = Abs(angle);
var results = JointTarget.GetAbsoluteJoints(new double[] { angle }, new double[] { angleB });
result[i] = results[0].ToDegrees();
}
}
return(RobotCellKuka.EulerToPlane(result[0], result[1], result[2], result[3], result[4], result[5]));
// return base.CartesianLerp(a, b, t, min, max);
}
internal KRLPostProcessor(RobotCellKuka robotCell, Program program)
{
this.cell = robotCell;
this.program = program;
this.Code = new List <List <List <string> > >();
for (int i = 0; i < cell.MechanicalGroups.Count; i++)
{
var groupCode = new List <List <string> >();
groupCode.Add(MainFile(i));
groupCode.Add(DatFile(i));
for (int j = 0; j < program.MultiFileIndices.Count; j++)
{
groupCode.Add(SrcFile(j, i));
}
Code.Add(groupCode);
}
}
internal KRLPostProcessor(RobotCellKuka robotCell, Program program)
{
this.cell = robotCell;
this.program = program;
this.Code = new List<List<List<string>>>();
for (int i = 0; i < cell.MechanicalGroups.Count; i++)
{
var groupCode = new List<List<string>>();
groupCode.Add(MainFile(i));
groupCode.Add(DatFile(i));
for (int j = 0; j < program.MultiFileIndices.Count; j++)
groupCode.Add(SrcFile(j, i));
Code.Add(groupCode);
}
}
