protected override Point[] GetPositionsImpl()
{
#if DEBUG
DebugGeo.Clear();
#endif
var count = Lengths.Length + 1;
var start = new Point(0, 0);
if ((Target - start).Magnitude > Lengths.Sum())
{
var dir = (Target - start).GetNormalized();
Point lastPos = start;
return new Point[] { start }.Concat(
Enumerable
.Range(0, Lengths.Length)
.Select(i =>
{
var newPos = lastPos + dir * Lengths[i];
lastPos = newPos;
return newPos;
})
)
.ToArray();
}
Vector tan;
if (double.IsNaN(Angle))
tan = -(Target - start).Perpendicular;
else
tan = new Vector(1, 0).GetRotated(Angle / 180.0 * Math.PI);
var res = GetPositions(start, Target, Lengths, tan, tan);
if (res == null)
return null;
if (TargetCorrections)
{
IKUtils.ApplyTargetCorrections(res, Lengths, Target);
}
if (ConvexityCorrections)
{
IKUtils.ApplyConvexityCorrections(res, Lengths, Target);
}
return res;
}
protected override Point[] GetPositionsImpl()
{
DebugGeo.Clear();
if (Lengths == null || Lengths.Length < 2)
return new Point[0];
var iterCount = IterationCount < 1 ? 1 : IterationCount;
var start = new Point(0, 0);
Point[] joints = _oldPositions;
var axisVector = new Vector(1, 0);
int compStepCounter = 0;
for (int i = 0; i < iterCount; i++)
{
for (int j = joints.Length - 2; j >= 0; j--)
{
var endJoint = joints[joints.Length - 1];
var cur = joints[j];
var bone = endJoint - cur;
DebugGeo.Add(new BezierSpline(cur, cur, endJoint, endJoint));
var lastSegAngle = Vector.AngleOriented(bone, axisVector);
var targetAngle = Vector.AngleOriented(Target - cur, axisVector);
var angleDif = lastSegAngle - targetAngle;
for (int k = joints.Length - 1; k > j; k--)
{
var boneRotated = joints[k] - cur;
joints[k] = cur + boneRotated.GetRotated(angleDif);
}
compStepCounter++;
}
if (ApplyConvexityCorrections)
{
IKUtils.ApplyConvexityCorrections(joints, Lengths, Target);
}
}
_oldPositions = joints;
return joints;
}
private Point[] GetPositions(Point start, Point end, double[] lengths, Vector startTangent, Vector endTangent)
{
var lengthSum = lengths.Sum();
var dist = (end - start).Magnitude;
BezierSpline spline = null;
double relEps = 0.001;
var startDir = startTangent.GetNormalized();
var endDir = endTangent.GetNormalized();
Point[] resPoints = null;
int lengthSampleCount = (int)(lengthSum / lengths.Min());
#if DEBUG
#region Time
double startTime = Reporting.Seconds;
double prevTime = startTime;
double curTime;
Action<string, bool> printTime = (message, fromStart) =>
{
curTime = Reporting.Seconds;
var lastTime = fromStart ? startTime : prevTime;
Reporting.LogMessage("{0} = {1} [ms] at {2} [fps]", message, Math.Round((curTime - lastTime) * 1000, 2), Math.Round(1.0 / (curTime - lastTime), 2));
if (!fromStart)
prevTime = curTime;
};
#endregion Time
#endif
Func<double, double> func = (p) =>
{
spline = new BezierSpline(start, start + startDir * p, end + endDir * p, end);
#if DEBUG
DebugGeo.Add(spline);
#endif
return spline.GetLength(lengthSampleCount * 2) - lengthSum;
};
double param = 0;
double eps = lengthSum * relEps; ;
double minParam = (lengthSum - dist) / 2;
double maxParam = Math.Sqrt(lengthSum * lengthSum - dist * dist);
var success = MathUtils.SolveByBisection(func, minParam, maxParam, out param, eps);
if (!success)
{
success = MathUtils.SolveByBisection(func, minParam / 2, maxParam * 2, out param, eps);
if (!success)
return null;
}
#if DEBUG
printTime("SplinePreparation", false);
#endif
spline = new BezierSpline(start, start + startDir * param, end + endDir * param, end);
resPoints = spline.GetResampledByDistance(lengths, true);
#if DEBUG
printTime("Resample", false);
#endif
#if DEBUG
printTime("Finished", true);
#endif
return resPoints;
}
public void Translate(Vector offset)
{
X += offset.X;
Y += offset.Y;
}
private static double VectorSizeRatio(Vector a, Vector b)
{
if (Determinant(a, b) != 0)
throw new ArgumentException("Determinant of vectors must be zero.");
double ratioX = double.NaN;
double ratioY = double.NaN;
if (b.X != 0)
{
ratioX = a.X / b.X;
}
if (b.Y != 0)
{
ratioY = a.Y / b.Y;
}
if (double.IsNaN(ratioX) && double.IsNaN(ratioY))
return 0;
if (double.IsNaN(ratioX))
return ratioY;
if (double.IsNaN(ratioY))
return ratioX;
return 0;
}
public static Vector Project(Vector a, Vector b)
{
return b * Vector.DotProduct(a, b) / Vector.DotProduct(b, b);
}
public static double DotProduct(Vector a, Vector b)
{
return a.X * b.X + a.Y * b.Y;
}
public static double Determinant(Vector a, Vector b)
{
return a.X * b.Y - a.Y * b.X;
}
public static double ComputeDistance(Vector a, Vector b)
{
return (a - b).Magnitude;
}
public static double AngleOriented(Vector a, Vector b)
{
var angle = Vector.Angle(a, b);
if (double.IsNaN(angle))
return Math.PI;
double deter = Vector.Determinant(a, b);
if (deter < 0)
return Math.PI * 2 - angle;
if (deter > 0)
return angle;
if (deter == 0)
{
var ratio = VectorSizeRatio(a, b);
if (ratio < 0)
return Math.PI;
if (ratio > 0)
return 0;
}
return 0;
}
public static double Angle(Vector a, Vector b)
{
double angle = Math.Acos(Vector.DotProduct(a, b) / (a.Magnitude * b.Magnitude));
return angle;
}