public static void sortByDistance()
{
List <BlockData> result = new List <BlockData>();
xyPoint first = new xyPoint();
double distanceReverse;
BlockData tmp;
bool allowReverse = !VisuGCode.containsG2G3Command() && !VisuGCode.containsG91Command() && !VisuGCode.containsTangential();
while (listFigures.Count > 0)
{
for (int i = 0; i < listFigures.Count; i++)
{
tmp = listFigures[i];
tmp.distance = first.DistanceTo(tmp.posStart);
distanceReverse = first.DistanceTo(tmp.posEnd);
if (allowReverse && (distanceReverse < tmp.distance))
{
tmp.distance = distanceReverse;
tmp.reverse = true;
}
listFigures[i] = tmp;
}
listFigures.Sort((x, y) => x.distance.CompareTo(y.distance));
result.Add(listFigures[0]);
first = listFigures[0].posEnd;
listFigures.RemoveAt(0);
}
listFigures.Clear();
foreach (BlockData item in result)
{
listFigures.Add(item);
}
}
private void teachpoint2_process_Click(object sender, EventArgs e)
{
teachPoint2 = grbl.posMarker;
double angle1 = teachPoint1.AngleTo(teachPoint2);
double dist1 = teachPoint1.DistanceTo(teachPoint2);
double angle2 = teachPoint1.AngleTo((xyPoint)grbl.posWork);
double dist2 = teachPoint1.DistanceTo((xyPoint)grbl.posWork);
double angle = angle1 - angle2;
lblAngle.Text = String.Format("{0:0.00}°", angle);
OnRaiseXYEvent(new XYEventArgs(angle, dist2 / dist1, teachPoint1, "a")); // rotate arround TP1
}
private static bool calcIntermediatePos()
{
updateFeedRate();
remainingStep = stepWidth;
double deltaA = codeNext.alpha - posA;
if (codeNext.motionMode < 2)
{
double deltaS = posXY.DistanceTo((xyPoint)codeNext.actualPos); // XY remaining max distance
if (tangentialAxisName != "Z")
{
deltaS = Math.Max(deltaS, Math.Abs(codeNext.actualPos.Z - posZ)); // Z remaining max distance
}
// Logger.Trace("calcIntermediatePos deltaS {0:0.00} codeLast.alpha {1:0.00} codeNext.alpha {2:0.00} posA {3:0.00} ", deltaS, codeLast.alpha, codeNext.alpha, posA);
if ((deltaS < remainingStep) && ((deltaA) < 0.1)) // return false if finish with intermediate
{
remainingStep -= deltaS;
return(false);
}
double deltaX = codeNext.actualPos.X - posXY.X; // get remaining distance
double deltaY = codeNext.actualPos.Y - posXY.Y;
double deltaZ = codeNext.actualPos.Z - posZ;
// double deltaA = codeNext.alpha - posA;
double dX = 0, dY = 0, dZ = 0, aStep = 10;
if (deltaS != 0)
{
dX = deltaX * remainingStep / deltaS; // get step width relativ to max distance
dY = deltaY * remainingStep / deltaS;
dZ = deltaZ * remainingStep / deltaS;
aStep = diff.Max / remainingStep; // amount of steps to reach end-value
}
posXY.X += dX;
posXY.Y += dY;
posA += (codeNext.alpha - codeLast.alpha) / aStep; // step width = 1/10 dA;
if ((codeNext.z != null) && !isTangentialZ)
{
posZ += dZ;
}
return(true);
}
else
{
double aStep2 = remainingStep / arcMove.radius; // get delta angle
double turnAngle = arcMove.angleEnd - arcMove.angleStart;
if (codeNext.motionMode == 2)
{
if (turnAngle > 0)
{
turnAngle -= 2 * Math.PI;
}
}
else
{
if (turnAngle < 0)
{
turnAngle += 2 * Math.PI;
}
}
if (turnAngle == 0)
{
turnAngle = 2 * Math.PI;
}
double dA = Math.Abs((codeNext.alpha - codeLast.alpha) / (turnAngle / aStep2)); // get step width
if (arcMove.angleDiff > 0)
{
if (angleTmp < (arcMove.angleStart + arcMove.angleDiff))
{
angleTmp += aStep2;
}
if (posA < codeNext.alpha)
{
posA += dA;
}
if ((angleTmp >= (arcMove.angleStart + arcMove.angleDiff)) && (posA >= codeNext.alpha)) // return false if finish with intermediate
{
return(false);
}
}
else
{
if (angleTmp > (arcMove.angleStart + arcMove.angleDiff))
{
angleTmp -= aStep2;
}
if (posA > codeNext.alpha)
{
posA -= dA;
}
if ((angleTmp <= (arcMove.angleStart + arcMove.angleDiff)) && (posA <= codeNext.alpha)) // return false if finish with intermediate
{
return(false);
}
}
posXY.X = arcMove.center.X + arcMove.radius * Math.Cos(angleTmp);
posXY.Y = arcMove.center.Y + arcMove.radius * Math.Sin(angleTmp);
// Logger.Trace(" arcMove.angleStart {0:0.00} arcMove.angleDiff {1:0.00} codeLast.alpha {2:0.00} codeNext.alpha {3:0.00} posA {4:0.00} angleTmp {5:0.00} dA {6:0.00}", arcMove.angleStart, arcMove.angleDiff, codeLast.alpha, codeNext.alpha, posA, angleTmp, dA);
return(true);
}
}
