private static double CalcLengthAtSmallCircle(double bigR, double smallR, double d, double tol) { if (smallR <= 0 || d <= 0) { return(double.NaN); } Equation.Func func = smallLength => { var rate = GetCurvatureRate(smallLength, smallR); var bigLength = GetLength(rate, bigR); return(d - (GetCurvatureCenter(rate, bigLength, bigR) .DistanceTo(GetCurvatureCenter(rate, smallLength, smallR)))); }; var result = double.NaN; // Equation func is periodic, so we are checking each period of two full revolutions of clothoid for (var i = 0; i < 4; i++) { result = Equation.SolveBrent(func, i * 2 * Math.PI * smallR, (i + 1) * 2 * Math.PI * smallR, tol, 0); if (!double.IsNaN(result)) { return(result); } } return(result); }
public void TestSolveNewton() { Equation.Func func = x => x - 1; Assert.AreEqual(1, Equation.SolveNewton(func, 10, 0, Eps), Eps); Assert.AreEqual(1, Equation.SolveNewton(func, 0, 10, Eps), Eps); Assert.IsNaN(Equation.SolveNewton(func, -10, 0, Eps)); func = x => x * x - 1; Assert.AreEqual(1, Equation.SolveNewton(func, 0, 10, Eps), Eps); Assert.AreEqual(-1, Equation.SolveNewton(func, -10, 0, Eps), Eps); Assert.AreEqual(-1, Equation.SolveNewton(func, -10, -1 + double.Epsilon, Eps), Eps); Assert.AreEqual(-1, Equation.SolveNewton(func, -10, -1, Eps), Eps); Assert.IsNaN(Equation.SolveNewton(func, -10, 10, Eps)); }
private static double CalcLength(double radius, double h, double tol) { Debug.Assert(radius > 0 && h > 0, "Arguments must be positive"); Equation.Func func = length => { var rate = GetCurvatureRate(length, radius); var angle = GetAngle(rate, length); return(h - (GetY(rate, length) + radius * Math.Cos(angle))); }; // uncomment for debug purposes, or if Bent will fail for some reason // return Equation.SolveBisection(func, 0, 2 * Math.PI * radius, tol); return(Equation.SolveBrent(func, 0, 2 * Math.PI * radius, tol, 0)); }
public void TestSolveBisection() { Equation.Func func = x => - Math.Sqrt(1.5 * 1.5 - (x - 1) * (x - 1)) + 1; Assert.IsTrue(Equation.SolveBisection(func, 0.5, 2.3, Eps) > 0); func = x => x - 1; Assert.AreEqual(1, Equation.SolveBisection(func, 0, 2, Eps), Eps); Assert.AreEqual(1, Equation.SolveBisection(func, 1, 2, Eps), Eps); Assert.IsNaN(Equation.SolveBisection(func, 3, 4, Eps)); func = x => x * x - 1; Assert.AreEqual(1, Equation.SolveBisection(func, 0, 10, Eps), Eps); Assert.AreEqual(-1, Equation.SolveBisection(func, -10, 0, Eps), Eps); Assert.AreEqual(-1, Equation.SolveBisection(func, -10, -1 + double.Epsilon, Eps), Eps); Assert.AreEqual(-1, Equation.SolveBisection(func, -10, -1, Eps), Eps); Assert.IsNaN(Equation.SolveBisection(func, -10, 10, Eps)); }
/// <summary> /// Fits a circle at a point providing G2 connection: coincidence of Continuity+Tangency+Curvature. /// <para/>The specified tolerance actually refers to clothoid parametric equation parameter value, rather than a true result spatial accuracy. /// </summary> public static bool FitCircle(Clothoid clothoid, Point point, double tol, out Circle circle, out double clothoidLength) { circle = new Circle(Point.NaP, double.NaN); clothoidLength = double.NaN; if (double.IsNaN(clothoid.CurvatureRate)) { return(false); } var newXAxis = new Line(clothoid.Origin, clothoid.OriginDirection); var vx = newXAxis.GetProjection(point) - clothoid.Origin; var pt = new Point(vx.Length * Math.Sign(vx * newXAxis.Direction), newXAxis.DistanceTo(point)); if (pt.X * pt.Y * clothoid.CurvatureRate <= 0.0) { return(false); } Equation.Func func = length => Math.Abs(GetRadius(clothoid.CurvatureRate, length)) - (GetCurvatureCenter(clothoid.CurvatureRate, length) - pt).Length; var sign = Math.Sign(clothoid.CurvatureRate * pt.Y); var boundL = sqrtPi / Math.Abs(clothoid.CurvatureRate) * sign; clothoidLength = Equation.SolveBisection(func, 0 + tol * sign, boundL - tol * sign, tol); // Brent method does not work since the func changes it's 2nd derivative sign if (double.IsNaN(clothoidLength)) { return(false); } var r = GetRadius(clothoid.CurvatureRate, clothoidLength); var c = clothoid.GetPoint(clothoidLength) + clothoid.GetNormal(clothoidLength) * r; circle = new Circle(c, r); return(true); }