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