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; }