private Point[] BackwardIncrements(double[] lengths, Point target)
{
target = target.Scale(new Size(-1, -1));
lengths = lengths.Reverse().ToArray();
List<Point> calculatedPoints = new List<Point>();
double reachedDistance = lengths[0];
double angle = 0;
calculatedPoints.Add(new Point());
Point[] trianglePoints = null;
for (int i = 0; i < lengths.Length - 1; i++)
{
trianglePoints = CalculateTriangleIK(target, reachedDistance, lengths[i + 1]);
reachedDistance = trianglePoints.Last().DistanceFromZero;
angle = Vector.Angle(target.ToVector(), trianglePoints[1].ToVector());
angle = double.IsNaN(angle) ? 0 : angle;
RotatePoints(calculatedPoints, angle);
calculatedPoints.Add(trianglePoints[1]);
}
var lastPoint = trianglePoints.Last();
calculatedPoints.Add(lastPoint);
var yAxisAngle = -Vector.AngleOriented(new Vector(0, 1), lastPoint.ToVector());
return calculatedPoints
.Select(o => { return o - lastPoint; })
.Reverse()
.Select(o => { return o.GetRotated(yAxisAngle); })
.Select(o => { return new Point(-o.X, o.Y); })
.Select(o => { return o.GetRotated(-yAxisAngle); })
.ToArray();
}
public Line()
{
Start = new Point();
End = new Point();
}
public Line(Point start, Point end)
{
Start = start;
End = end;
}
public Rectangle()
{
Min = new Point();
Max = new Point();
}
public Rectangle(Point min, Point max)
{
Min = min;
Max = max;
}
public EL_Point(Point p)
{
Position = p;
}
private Point[] CalculateTriangleIK(Point target, double firstLength, double secondLength)
{
return IKUtils.GetTriangleIKPositions(target, firstLength, secondLength, false);
}
private Point[] ForwardIncrements(double[] lengths, Point target)
{
//TODO
//dodat lubovolny pociatok
//tak nejak imnplicitne sa berie (0,0)
List<Point> calculatedPoints = new List<Point>();
double reachedDistance = lengths[0];
double angle = 0;
calculatedPoints.Add(new Point());
Point[] trianglePoints = null;
for (int i = 0; i < lengths.Length - 1; i++)
{
trianglePoints = CalculateTriangleIK(target, reachedDistance, lengths[i + 1]);
reachedDistance = trianglePoints.Last().DistanceFromZero;
angle = Vector.Angle(target.ToVector(), trianglePoints[1].ToVector());
angle = double.IsNaN(angle) ? 0 : angle;
RotatePoints(calculatedPoints, angle);
calculatedPoints.Add(trianglePoints[1]);
}
calculatedPoints.Add(trianglePoints.Last());
return calculatedPoints.ToArray();
}