diff --git a/include/world_builder/objects/bezier_curve.h b/include/world_builder/objects/bezier_curve.h index fd326ec36..0c607511c 100644 --- a/include/world_builder/objects/bezier_curve.h +++ b/include/world_builder/objects/bezier_curve.h @@ -31,22 +31,20 @@ namespace WorldBuilder { /** - * @brief Class for circle line/spline, including interpolation on it + * @brief A Class representing bezier curves. * */ class BezierCurve { public: /** - * @brief Construct a new Bezier Curve object + * @brief Construct a new Bezier Curve object. * - * @param p - * @param angle_constrains */ BezierCurve() {}; /** - * @brief Construct a new Bezier Curve object + * @brief Construct a new Bezier Curve object from a list of points and a list of angle constrains. * * @param p * @param angle_constrains @@ -66,7 +64,7 @@ namespace WorldBuilder ClosestPointOnCurve closest_point_on_curve_segment(const Point<2> &p, const bool verbose = false) const; /** - * @brief + * @brief Compute a point on the curve at a given parameter value and section. * * @param i * @param x diff --git a/source/world_builder/objects/bezier_curve.cc b/source/world_builder/objects/bezier_curve.cc index 4a6afe9fd..bbab1370c 100644 --- a/source/world_builder/objects/bezier_curve.cc +++ b/source/world_builder/objects/bezier_curve.cc @@ -177,44 +177,48 @@ namespace WorldBuilder ClosestPointOnCurve - BezierCurve::closest_point_on_curve_segment(const Point<2> &check_point, + BezierCurve::closest_point_on_curve_segment(const Point<2> &p, const bool verbose) const { ClosestPointOnCurve closest_point_on_curve; - const Point<2> &cp = check_point; double min_squared_distance = std::numeric_limits::infinity(); - if (check_point.get_coordinate_system() == CoordinateSystem::cartesian) + if (p.get_coordinate_system() == CoordinateSystem::cartesian) { - for ( size_t cp_i = 0; cp_i < control_points.size(); ++cp_i) + const size_t n_control_points = points.size(); + for (size_t cp_i = 0; cp_i < n_control_points; ++cp_i) { + const Point<2> &p1 = points[cp_i]; + const Point<2> &p2 = points[cp_i+1]; + + const Point<2> &cp1 = control_points[cp_i][0]; + const Point<2> &cp2 = control_points[cp_i][1]; + #ifndef NDEBUG std::stringstream output; #endif - const Point<2> &p1 = points[cp_i]; - const Point<2> &p2 = points[cp_i+1]; - //min_squared_distance = std::min(std::min(min_squared_distance,(check_point-p1).norm_square()),(check_point-p1).norm_square()); // Getting an estimate for where the closest point is with a linear approximation const Point<2> P1P2 = p2-p1; - const Point<2> P1Pc = check_point-p1; + const Point<2> P1Pc = p-p1; const double P2P2_dot = P1P2*P1P2; - double est = P2P2_dot > 0.0 ? std::min(1.,std::max(0.,(P1Pc*P1P2) / P2P2_dot)) : 1.0; // est=estimate of solution + // estimated curve parameter of solution + double est = P2P2_dot > 0.0 ? std::min(1.,std::max(0.,(P1Pc*P1P2) / P2P2_dot)) : 1.0; bool found = false; // based on https://stackoverflow.com/questions/2742610/closest-point-on-a-cubic-bezier-curve - const double a_0 = 3.*control_points[cp_i][0][0]-3.*control_points[cp_i][1][0]+points[cp_i+1][0]-points[cp_i][0]; - const double a_1 = 3.*control_points[cp_i][0][1]-3.*control_points[cp_i][1][1]+points[cp_i+1][1]-points[cp_i][1]; - const double b_0 = 3.*points[cp_i][0] - 6.*control_points[cp_i][0][0]+3.*control_points[cp_i][1][0]; - const double b_1 = 3.*points[cp_i][1] - 6.*control_points[cp_i][0][1]+3.*control_points[cp_i][1][1]; - const double c_0 = -3.*points[cp_i][0] + 3.*control_points[cp_i][0][0]; - const double c_1 = -3.*points[cp_i][1] + 3.*control_points[cp_i][0][1]; - const double d_0 = points[cp_i][0]; - const double d_1 = points[cp_i][1]; - - const double d_min_cp_0 = d_0-cp[0]; - const double d_min_cp_1 = d_1-cp[1]; + const double a_0 = 3.*cp1[0] - 3.*cp2[0] + p2[0] - p1[0]; + const double a_1 = 3.*cp1[1] - 3.*cp2[1] + p2[1] - p1[1]; + const double b_0 = 3.*p1[0] - 6.*cp1[0] + 3.*cp2[0]; + const double b_1 = 3.*p1[1] - 6.*cp1[1] + 3.*cp2[1]; + const double c_0 = -3.*p1[0] + 3.*cp1[0]; + const double c_1 = -3.*p1[1] + 3.*cp1[1]; + const double d_0 = p1[0]; + const double d_1 = p1[1]; + + const double d_min_cp_0 = d_0-p[0]; + const double d_min_cp_1 = d_1-p[1]; #ifndef NDEBUG double estimate_point_min_cp_0_dg = a_0*est*est*est+b_0*est*est+c_0*est+d_min_cp_0; @@ -222,7 +226,7 @@ namespace WorldBuilder double min_squared_distance_cartesian_temp_dg = (estimate_point_min_cp_0_dg*estimate_point_min_cp_0_dg)+(estimate_point_min_cp_1_dg*estimate_point_min_cp_1_dg); #endif - for (size_t newton_i = 0; newton_i < 150; newton_i++) + for (size_t newton_iteration = 0; newton_iteration < 150; newton_iteration++) { #ifndef NDEBUG output << " wolfram alpha: (" << a_0 << "*x^3+" << b_0 << "*x^2+"<< c_0 << "*x+" << d_0 << "-" << cp[0] << ")^2+(" << a_1 << "*x^3+" << b_1 << "*x^2+"<< c_1 << "*x+" << d_1 << "-" << cp[1] << ")^2 with x=" << est << std::endl; @@ -265,20 +269,20 @@ namespace WorldBuilder squared_distance_cartesian_test = (estimate_point_min_cp_test_0*estimate_point_min_cp_test_0)+(estimate_point_min_cp_test_1*estimate_point_min_cp_test_1); #ifndef NDEBUG - const Point<2> a = 3.*control_points[cp_i][0]-3.*control_points[cp_i][1]+points[cp_i+1]-points[cp_i]; - const Point<2> b = 3.*points[cp_i] - 6.*control_points[cp_i][0]+3.*control_points[cp_i][1]; - const Point<2> c = -3.*points[cp_i] + 3.*control_points[cp_i][0]; - const Point<2> d = points[cp_i]; + const Point<2> a = 3.*cp1-3.*cp2+p2-p1; + const Point<2> b = 3.*p1 - 6.*cp1+3.*cp2; + const Point<2> c = -3.*p1 + 3.*cp1; + const Point<2> d = p1; const double squared_distance_cartesian_derivative_test = 2.0*(3.0*a_0*est_test*est_test+2.0*b_0*est_test+c_0)*(a_0*est_test*est_test*est_test+b_0*est_test*est_test+c_0*est_test+d_0-cp[0]) + 2.0*(3.0*a_1*est_test*est_test+2.0*b_1*est_test+c_1)*(a_1*est_test*est_test*est_test+b_1*est_test*est_test+c_1*est_test+d_1-cp[1]); const double squared_distance_cartesian_second_derivative_test = 2.0*(6.0*a_0*est_test + 2.0*b_0)*(a_0*est_test*est_test*est_test+b_0*est_test*est_test+c_0*est_test+d_0-cp[0]) + 2.0*(3.0*a_0*est_test*est_test + 2.0*b_0*est_test + c_0)*(3.0*a_0*est_test*est_test + 2.0*b_0*est_test + c_0) + 2.0*(6.0*a_1*est_test + 2.0*b_1)*(a_1*est_test*est_test*est_test+b_1*est_test*est_test+c_1*est_test+d_1-cp[1]) + 2.0*(3.0*a_1*est_test*est_test + 2.0*b_1*est_test + c_1)*(3.0*a_1*est_test*est_test + 2.0*b_1*est_test + c_1) ; - output << " i: " << cp_i << ", ni: " << newton_i<< ", lsi: " << i << ", line_search_step=" << 2./3. << ": squared_distance_cartesian_test = " << squared_distance_cartesian_test << ", diff= " << squared_distance_cartesian_test-squared_distance_cartesian + output << " i: " << cp_i << ", ni: " << newton_iteration<< ", lsi: " << i << ", line_search_step=" << 2./3. << ": squared_distance_cartesian_test = " << squared_distance_cartesian_test << ", diff= " << squared_distance_cartesian_test-squared_distance_cartesian << ", tests: " << (squared_distance_cartesian_test_previous < squared_distance_cartesian ? "true" : "false") << ":" << (squared_distance_cartesian_test > squared_distance_cartesian_test_previous ? "true" : "false") << ", est_test=" << est_test << ", update=" << update << ", ls=" << line_search << ", up*ls=" << update *line_search << ", test deriv =" << squared_distance_cartesian_derivative_test << ", test update=" << squared_distance_cartesian_derivative_test/fabs(squared_distance_cartesian_second_derivative_test) - << ", p1=" << p1 << ", p2= " << p2 << ", poc= " << a *est_test *est_test *est_test + b *est_test *est_test+c *est_test+d << ", cp= " << check_point << ", ds:" << ((a*est_test*est_test*est_test+b*est_test*est_test+c*est_test+d)-check_point).norm_square() << ":" << min_squared_distance_cartesian_temp_dg + << ", p1=" << p1 << ", p2= " << p2 << ", poc= " << a *est_test *est_test *est_test + b *est_test *est_test+c *est_test+d << ", p= " << p << ", ds:" << ((a*est_test*est_test*est_test+b*est_test*est_test+c*est_test+d)-p).norm_square() << ":" << min_squared_distance_cartesian_temp_dg << ", diff = " << squared_distance_cartesian_test-min_squared_distance_cartesian_temp_dg << std::endl; #endif if (i > 0 && (squared_distance_cartesian_test > squared_distance_cartesian_test_previous)) @@ -317,17 +321,17 @@ namespace WorldBuilder if (est >= -1e-8 && cp_i+est > 0 && est-1. <= 1e-8 && est-1. < cp_i) { min_squared_distance = min_squared_distance_temp; - const Point<2> point_on_curve = Point<2>(a_0*est*est*est+b_0*est*est+c_0*est+d_0,a_1*est*est*est+b_1*est*est+c_1*est+d_1,cp.get_coordinate_system()); + const Point<2> point_on_curve = Point<2>(a_0*est*est*est+b_0*est*est+c_0*est+d_0,a_1*est*est*est+b_1*est*est+c_1*est+d_1,p.get_coordinate_system()); WBAssert(!std::isnan(point_on_curve[0]) && !std::isnan(point_on_curve[1]), "Point on curve has NAN entries: " << point_on_curve); // the sign is rotating the derivative by 90 degrees. // When moving in the direction of increasing t, left is positive and right is negative. // https://www.wolframalpha.com/input?i=d%2Fdt+%281-t%29*%281-t%29*%281-t%29*a+%2B+3*%281-t%29*%281-t%29*t*b+%2B+3.*%281-t%29*t*t*c%2Bt*t*t*d - const Point<2> derivative_point = points[cp_i]*((6.-3.*est)*est-3.) + control_points[cp_i][0]*(est*(9*est-12)+3) - + control_points[cp_i][1]*(6.-9.*est)*est + points[cp_i+1]*3.*est*est; + const Point<2> derivative_point = p1*((6.-3.*est)*est-3.) + cp1*(est*(9*est-12)+3) + + cp2*(6.-9.*est)*est + p2*3.*est*est; Point<2> tangent_point = derivative_point - point_on_curve; // if angle between check point and tangent point is larger than 90 degrees, return a negative distance - const double dot_product = (tangent_point*(check_point-point_on_curve)); + const double dot_product = (tangent_point*(p-point_on_curve)); const double sign = dot_product < 0. ? -1. : 1.; tangent_point = Point<2>(-tangent_point[1],tangent_point[0],tangent_point.get_coordinate_system()); @@ -339,7 +343,7 @@ namespace WorldBuilder WBAssert(!std::isnan(point_on_curve[0]) && !std::isnan(point_on_curve[1]), "Point on curve has NAN entries: " << point_on_curve); Point<2> normal = point_on_curve; { - Point<2> derivative = Point<2>(a_0*est*est+b_0*est+c_0,a_1*est*est+b_1*est+c_1,cp.get_coordinate_system()); + Point<2> derivative = Point<2>(a_0*est*est+b_0*est+c_0,a_1*est*est+b_1*est+c_1,p.get_coordinate_system()); normal=derivative; const double normal_size = derivative.norm(); if (normal_size > 0.) @@ -353,59 +357,74 @@ namespace WorldBuilder } } } - else + else // spherical coordinates { - const double cos_cp_lat = cos(cp[1]); - for ( size_t cp_i = 0; cp_i < control_points.size(); ++cp_i) + const double cos_cp_lat = cos(p[1]); + const size_t n_control_points = control_points.size(); + for (size_t cp_i = 0; cp_i < n_control_points; ++cp_i) { const Point<2> &p1 = points[cp_i]; const Point<2> &p2 = points[cp_i+1]; + + const Point<2> &cp1 = control_points[cp_i][0]; + const Point<2> &cp2 = control_points[cp_i][1]; + // Getting an estimate for where the closest point is with a linear approximation const Point<2> P1P2 = p2-p1; - const Point<2> P1Pc = check_point-p1; + const Point<2> P1Pc = p-p1; + + // Making sure we do not run into numerical accuracy issues + WBAssertThrow(P1P2.norm_square() > std::numeric_limits::epsilon(), "The distance between the two control points is too small. P1P2.norm_square() = " << P1P2.norm_square() << "."); + WBAssertThrow(P1P2.norm_square() > p1.norm_square()*1e-10, "The distance between the two control points is too small. P1P2.norm_square() = " << P1P2.norm_square() << ", p1.norm_square()*1e-10 = " << p1.norm_square()*1e-10 << ", p1 = " << p1 << "."); + WBAssertThrow(P1P2.norm_square() > p2.norm_square()*1e-10, "The distance between the two control points is too small. P1P2.norm_square() = " << P1P2.norm_square() << ", p2.norm_square()*1e-10 = " << p2.norm_square()*1e-10 << ", p2 = " << p2 << "."); + WBAssertThrow(P1P2.norm_square() > P1Pc.norm_square()*1e-10, "The distance between the two control points is too small. P1Pc.norm_square() = " << P1Pc.norm_square() << ", P1P2.norm_square()*1e-10 = " << P1P2.norm_square()*1e-10 << ", p1 = " << p1 << ", p2 = " << p2 << ", cp = " << p << "."); const double P2P2_dot = P1P2*P1P2; - double est = P2P2_dot > 0.0 ? std::min(1.,std::max(0.,(P1Pc*P1P2) / P2P2_dot)) : 1.0; // est=estimate of solution + // estimated curve parameter of solution + double est = std::min(1.,std::max(0.,(P1Pc*P1P2) / P2P2_dot)); bool found = false; // only used if verbose is true std::stringstream output; - Point<2> a = 3.*control_points[cp_i][0]-3.*control_points[cp_i][1]+points[cp_i+1]-points[cp_i]; - Point<2> b = 3.*points[cp_i] - 6.*control_points[cp_i][0]+3.*control_points[cp_i][1]; - Point<2> c = -3.*points[cp_i] + 3.*control_points[cp_i][0]; - const Point<2> d = points[cp_i]; + Point<2> a = 3.*cp1-3.*cp2+p2-p1; + Point<2> b = 3.*p1 - 6.*cp1+3.*cp2; + Point<2> c = -3.*p1 + 3.*cp1; + const Point<2> d = p1; Point<2> estimate_point = a*est*est*est+b*est*est+c*est+d; - double cos_lat_dg; - double sin_d_long_h_dg; - double sin_d_lat_h_dg; double min_squared_distance_cartesian_temp_dg; if (verbose == true) { - cos_lat_dg = cos(estimate_point[1]); - sin_d_long_h_dg = sin((estimate_point[0]-cp[0])*0.5); - sin_d_lat_h_dg = sin((estimate_point[1]-cp[1])*0.5); + const double cos_lat_dg = cos(estimate_point[1]); + const double sin_d_long_h_dg = sin((estimate_point[0]-p[0])*0.5); + const double sin_d_lat_h_dg = sin((estimate_point[1]-p[1])*0.5); min_squared_distance_cartesian_temp_dg = sin_d_lat_h_dg*sin_d_lat_h_dg+sin_d_long_h_dg*sin_d_long_h_dg*cos_cp_lat*cos_lat_dg; - output << "cp_i=" << cp_i << ", init est = " << est << ", min_squared_distance = " << min_squared_distance << ", min_squared_distance_cartesian_temp_dg: " << min_squared_distance_cartesian_temp_dg << ", p1: " << p1 << ", p2: " << p2 << std::endl; - output << std::setprecision(6) << " wolfram: sin((" << a[1] << "*x^3+" << b[1] << "*x^2+"<< c[1] << "*x+" << d[1] << "-" << cp[1] << ")*.5)^2+sin((" << a[0] << "*x^3+" << b[0] << "*x^2+"<< c[0] << "*x+" << d[0] << "-" << cp[0] << ")*.5)^2*cos(" << cp[1] << ")*cos(" << a[1] << "*x^3+" << b[1] << "*x^2+"<< c[1] << "*x+" << d[1] << "-" << cp[1] << ") with x=" << est << std::endl; - output << std::setprecision(10) << " python: y=np.sin((" << a[1] << "*x**3+" << b[1] << "*x**2+"<< c[1] << "*x+" << d[1] << "-" << cp[1] << ")*.5)**2+np.sin((" << a[0] << "*x**3+" << b[0] << "*x**2+"<< c[0] << "*x+" << d[0] << "-" << cp[0] << ")*.5)**2*np.cos(" << cp[1] << ")*np.cos(" << a[1] << "*x**3+" << b[1] << "*x**2+"<< c[1] << "*x+" << d[1] << "-" << cp[1] << "); x=" << est << std::endl; + output << "cp_i=" << cp_i + << ", init est = " << est + << ", min_squared_distance = " << min_squared_distance + << ", min_squared_distance_cartesian_temp_dg: " << min_squared_distance_cartesian_temp_dg + << ", p1: " << p1 + << ", p2: " << p2 << std::endl; + output << std::setprecision(6) << " wolfram: sin((" << a[1] << "*x^3+" << b[1] << "*x^2+"<< c[1] << "*x+" << d[1] << "-" << p[1] << ")*.5)^2+sin((" << a[0] << "*x^3+" << b[0] << "*x^2+"<< c[0] << "*x+" << d[0] << "-" << p[0] << ")*.5)^2*cos(" << p[1] << ")*cos(" << a[1] << "*x^3+" << b[1] << "*x^2+"<< c[1] << "*x+" << d[1] << "-" << p[1] << ") with x=" << est << std::endl; + output << std::setprecision(10) << " python: y=np.sin((" << a[1] << "*x**3+" << b[1] << "*x**2+"<< c[1] << "*x+" << d[1] << "-" << p[1] << ")*.5)**2+np.sin((" << a[0] << "*x**3+" << b[0] << "*x**2+"<< c[0] << "*x+" << d[0] << "-" << p[0] << ")*.5)**2*np.cos(" << p[1] << ")*np.cos(" << a[1] << "*x**3+" << b[1] << "*x**2+"<< c[1] << "*x+" << d[1] << "-" << p[1] << "); x=" << est << std::endl; } - for (size_t newton_i = 0; newton_i < 150; newton_i++) + + for (size_t newton_iteration = 0; newton_iteration < 150; newton_iteration++) { // based on https://stackoverflow.com/questions/2742610/closest-point-on-a-cubic-bezier-curve estimate_point = a*est*est*est+b*est*est+c*est+d; - double sin_d_long_h = sin((estimate_point[0]-cp[0])*0.5); - double sin_d_lat_h = sin((estimate_point[1]-cp[1])*0.5); - const double cos_d_lat = cos(estimate_point[1]-cp[1]); + double sin_d_long_h = sin((estimate_point[0]-p[0])*0.5); + double sin_d_lat_h = sin((estimate_point[1]-p[1])*0.5); + const double cos_d_lat = cos(estimate_point[1]-p[1]); const double squared_distance_cartesian = sin_d_lat_h*sin_d_lat_h+sin_d_long_h*sin_d_long_h*cos_cp_lat*cos_d_lat; - double sin_dlat = sin(estimate_point[1]-cp[1]); - const double cos_dlong_h = cos(0.5*(estimate_point[0]-cp[0])); - double cos_dlat_h = cos(0.5*(estimate_point[1]-cp[1])); + double sin_dlat = sin(estimate_point[1]-p[1]); + const double cos_dlong_h = cos(0.5*(estimate_point[0]-p[0])); + double cos_dlat_h = cos(0.5*(estimate_point[1]-p[1])); double deriv_long = (3.0*a[0]*est*est+2.0*b[0]*est+c[0]); double deriv_lat = (3.0*a[1]*est*est+2.0*b[1]*est+c[1]); @@ -417,12 +436,13 @@ namespace WorldBuilder if (verbose == true) { - const double squared_distance_cartesian_full = sin((a[1]*est*est*est+b[1]*est*est+c[1]*est+d[1]-cp[1])*0.5)*sin((a[1]*est*est*est+b[1]*est*est+c[1]*est+d[1]-cp[1])*0.5)+sin((a[0]*est*est*est+b[0]*est*est+c[0]*est+d[0]-cp[0])*0.5)*sin((a[0]*est*est*est+b[0]*est*est+c[0]*est+d[0]-cp[0])*0.5)*cos(cp[1])*cos(a[1]*est*est*est+b[1]*est*est+c[1]*est+d[1]-cp[1]); - const double squared_distance_cartesian_derivative_full = cos(cp[1])*(-(3.0*a[1]*est*est+2.0*b[1]*est+c[1]))*sin(0.5*(a[0]*est*est*est+b[0]*est*est+c[0]*est+d[0]-cp[0]))*sin(0.5*(a[0]*est*est*est+b[0]*est*est+c[0]*est+d[0]-cp[0]))*sin(a[1]*est*est*est+b[1]*est*est+c[1]*est+d[1]-cp[1])+cos(cp[1])*(3.0*a[0]*est*est+2.0*b[0]*est+c[0])*sin(0.5*(a[0]*est*est*est+b[0]*est*est+c[0]*est+d[0]-cp[0]))*cos(0.5*(a[0]*est*est*est+b[0]*est*est+c[0]*est+d[0]-cp[0]))*cos(a[1]*est*est*est+b[1]*est*est+c[1]*est+d[1]-cp[1])+(3.0*a[1]*est*est+2.0*b[1]*est+c[1])*sin(0.5*(a[1]*est*est*est+b[1]*est*est+c[1]*est+d[1]-cp[1]))*cos(0.5*(a[1]*est*est*est+b[1]*est*est+c[1]*est+d[1]-cp[1])); - const double squared_distance_cartesian_second_derivative_full = cos(cp[1])*cos(a[1]*est*est*est+b[1]*est*est+c[1]*est+d[1]-cp[1])*(-0.5*(3.0*a[0]*est*est+2.0*b[0]*est+c[0])*(3.0*a[0]*est*est+2.0*b[0]*est+c[0])*sin(0.5*(a[0]*est*est*est+b[0]*est*est+c[0]*est+d[0]-cp[0]))*sin(0.5*(a[0]*est*est*est+b[0]*est*est+c[0]*est+d[0]-cp[0]))+0.5*(3.0*a[0]*est*est+2.0*b[0]*est+c[0])*(3.0*a[0]*est*est+2.0*b[0]*est+c[0])*cos(0.5*(a[0]*est*est*est+b[0]*est*est+c[0]*est+d[0]-cp[0]))*cos(0.5*(a[0]*est*est*est+b[0]*est*est+c[0]*est+d[0]-cp[0]))+(6.0*a[0]*est+2.0*b[0])*sin(0.5*(a[0]*est*est*est+b[0]*est*est+c[0]*est+d[0]-cp[0]))*cos(0.5*(a[0]*est*est*est+b[0]*est*est+c[0]*est+d[0]-cp[0])))+cos(cp[1])*sin(0.5*(a[0]*est*est*est+b[0]*est*est+c[0]*est+d[0]-cp[0]))*sin(0.5*(a[0]*est*est*est+b[0]*est*est+c[0]*est+d[0]-cp[0]))*((3.0*a[1]*est*est+2.0*b[1]*est+c[1])*(3.0*a[1]*est*est+2.0*b[1]*est+c[1])*(-cos(a[1]*est*est*est+b[1]*est*est+c[1]*est+d[1]-cp[1]))-(6.0*a[1]*est+2.0*b[1])*sin(a[1]*est*est*est+b[1]*est*est+c[1]*est+d[1]-cp[1]))-2.0*cos(cp[1])*(3.0*a[0]*est*est+2.0*b[0]*est+c[0])*(3.0*a[1]*est*est+2.0*b[1]*est+c[1])*sin(0.5*(a[0]*est*est*est+b[0]*est*est+c[0]*est+d[0]-cp[0]))*cos(0.5*(a[0]*est*est*est+b[0]*est*est+c[0]*est+d[0]-cp[0]))*sin(a[1]*est*est*est+b[1]*est*est+c[1]*est+d[1]-cp[1])-0.5*(3.0*a[1]*est*est+2.0*b[1]*est+c[1])*(3.0*a[1]*est*est+2.0*b[1]*est+c[1])*sin(0.5*(a[1]*est*est*est+b[1]*est*est+c[1]*est+d[1]-cp[1]))*sin(0.5*(a[1]*est*est*est+b[1]*est*est+c[1]*est+d[1]-cp[1]))+0.5*(3.0*a[1]*est*est+2.0*b[1]*est+c[1])*(3.0*a[1]*est*est+2.0*b[1]*est+c[1])*cos(0.5*(a[1]*est*est*est+b[1]*est*est+c[1]*est+d[1]-cp[1]))*cos(0.5*(a[1]*est*est*est+b[1]*est*est+c[1]*est+d[1]-cp[1]))+(6.0*a[1]*est+2.0*b[1])*sin(0.5*(a[1]*est*est*est+b[1]*est*est+c[1]*est+d[1]-cp[1]))*cos(0.5*(a[1]*est*est*est+b[1]*est*est+c[1]*est+d[1]-cp[1])); + const double squared_distance_cartesian_full = sin((a[1]*est*est*est+b[1]*est*est+c[1]*est+d[1]-p[1])*0.5)*sin((a[1]*est*est*est+b[1]*est*est+c[1]*est+d[1]-p[1])*0.5)+sin((a[0]*est*est*est+b[0]*est*est+c[0]*est+d[0]-p[0])*0.5)*sin((a[0]*est*est*est+b[0]*est*est+c[0]*est+d[0]-p[0])*0.5)*cos(p[1])*cos(a[1]*est*est*est+b[1]*est*est+c[1]*est+d[1]-p[1]); + const double squared_distance_cartesian_derivative_full = cos(p[1])*(-(3.0*a[1]*est*est+2.0*b[1]*est+c[1]))*sin(0.5*(a[0]*est*est*est+b[0]*est*est+c[0]*est+d[0]-p[0]))*sin(0.5*(a[0]*est*est*est+b[0]*est*est+c[0]*est+d[0]-p[0]))*sin(a[1]*est*est*est+b[1]*est*est+c[1]*est+d[1]-p[1])+cos(p[1])*(3.0*a[0]*est*est+2.0*b[0]*est+c[0])*sin(0.5*(a[0]*est*est*est+b[0]*est*est+c[0]*est+d[0]-p[0]))*cos(0.5*(a[0]*est*est*est+b[0]*est*est+c[0]*est+d[0]-p[0]))*cos(a[1]*est*est*est+b[1]*est*est+c[1]*est+d[1]-p[1])+(3.0*a[1]*est*est+2.0*b[1]*est+c[1])*sin(0.5*(a[1]*est*est*est+b[1]*est*est+c[1]*est+d[1]-p[1]))*cos(0.5*(a[1]*est*est*est+b[1]*est*est+c[1]*est+d[1]-p[1])); + const double squared_distance_cartesian_second_derivative_full = cos(p[1])*cos(a[1]*est*est*est+b[1]*est*est+c[1]*est+d[1]-p[1])*(-0.5*(3.0*a[0]*est*est+2.0*b[0]*est+c[0])*(3.0*a[0]*est*est+2.0*b[0]*est+c[0])*sin(0.5*(a[0]*est*est*est+b[0]*est*est+c[0]*est+d[0]-p[0]))*sin(0.5*(a[0]*est*est*est+b[0]*est*est+c[0]*est+d[0]-p[0]))+0.5*(3.0*a[0]*est*est+2.0*b[0]*est+c[0])*(3.0*a[0]*est*est+2.0*b[0]*est+c[0])*cos(0.5*(a[0]*est*est*est+b[0]*est*est+c[0]*est+d[0]-p[0]))*cos(0.5*(a[0]*est*est*est+b[0]*est*est+c[0]*est+d[0]-p[0]))+(6.0*a[0]*est+2.0*b[0])*sin(0.5*(a[0]*est*est*est+b[0]*est*est+c[0]*est+d[0]-p[0]))*cos(0.5*(a[0]*est*est*est+b[0]*est*est+c[0]*est+d[0]-p[0])))+cos(p[1])*sin(0.5*(a[0]*est*est*est+b[0]*est*est+c[0]*est+d[0]-p[0]))*sin(0.5*(a[0]*est*est*est+b[0]*est*est+c[0]*est+d[0]-p[0]))*((3.0*a[1]*est*est+2.0*b[1]*est+c[1])*(3.0*a[1]*est*est+2.0*b[1]*est+c[1])*(-cos(a[1]*est*est*est+b[1]*est*est+c[1]*est+d[1]-p[1]))-(6.0*a[1]*est+2.0*b[1])*sin(a[1]*est*est*est+b[1]*est*est+c[1]*est+d[1]-p[1]))-2.0*cos(p[1])*(3.0*a[0]*est*est+2.0*b[0]*est+c[0])*(3.0*a[1]*est*est+2.0*b[1]*est+c[1])*sin(0.5*(a[0]*est*est*est+b[0]*est*est+c[0]*est+d[0]-p[0]))*cos(0.5*(a[0]*est*est*est+b[0]*est*est+c[0]*est+d[0]-p[0]))*sin(a[1]*est*est*est+b[1]*est*est+c[1]*est+d[1]-p[1])-0.5*(3.0*a[1]*est*est+2.0*b[1]*est+c[1])*(3.0*a[1]*est*est+2.0*b[1]*est+c[1])*sin(0.5*(a[1]*est*est*est+b[1]*est*est+c[1]*est+d[1]-p[1]))*sin(0.5*(a[1]*est*est*est+b[1]*est*est+c[1]*est+d[1]-p[1]))+0.5*(3.0*a[1]*est*est+2.0*b[1]*est+c[1])*(3.0*a[1]*est*est+2.0*b[1]*est+c[1])*cos(0.5*(a[1]*est*est*est+b[1]*est*est+c[1]*est+d[1]-p[1]))*cos(0.5*(a[1]*est*est*est+b[1]*est*est+c[1]*est+d[1]-p[1]))+(6.0*a[1]*est+2.0*b[1])*sin(0.5*(a[1]*est*est*est+b[1]*est*est+c[1]*est+d[1]-p[1]))*cos(0.5*(a[1]*est*est*est+b[1]*est*est+c[1]*est+d[1]-p[1])); output <<"sqd = " << squared_distance_cartesian <<":" << squared_distance_cartesian_full << ", diff=" << squared_distance_cartesian-squared_distance_cartesian_full << ", sqdd: " << squared_distance_cartesian_derivative <<":" << squared_distance_cartesian_derivative_full << ", diff="<< squared_distance_cartesian_derivative-squared_distance_cartesian_derivative_full << ", sqdd: " << squared_distance_cartesian_second_derivative << ":" << squared_distance_cartesian_second_derivative_full << ", diff= " << squared_distance_cartesian_second_derivative-squared_distance_cartesian_second_derivative_full << ", est: " << est << std::endl; } - // the local minimum is where squared_distance_cartesian_derivative=0 and squared_distance_cartesian_derivative>=0 + // the local minimum is where squared_distance_cartesian_derivative=0 and squared_distance_cartesian_second_derivative>=0 + WBAssertThrow(std::fabs(squared_distance_cartesian_second_derivative) > std::fabs(squared_distance_cartesian_derivative) * std::numeric_limits::min(), "The second derivative is zero. This should not happen."); update = std::min(0.5,std::max(-0.5,squared_distance_cartesian_derivative/std::fabs(squared_distance_cartesian_second_derivative))); double line_search = 1.; double est_test = est-update*line_search; @@ -430,40 +450,40 @@ namespace WorldBuilder double squared_distance_cartesian_test_previous = squared_distance_cartesian; double line_search_step = 2./3.; - for (unsigned int i = 0; i < 10; i++) + for (unsigned int line_search_iteration = 0; line_search_iteration < 10; line_search_iteration++) { est_test = est-update*line_search; estimate_point = a*est_test*est_test*est_test+b*est_test*est_test+c*est_test+d; - sin_d_long_h = sin((estimate_point[0]-cp[0])*0.5); - sin_d_lat_h = sin((estimate_point[1]-cp[1])*0.5); - squared_distance_cartesian_test = sin_d_lat_h*sin_d_lat_h+sin_d_long_h*sin_d_long_h*cos_cp_lat*cos(estimate_point[1]-cp[1]); + sin_d_long_h = sin((estimate_point[0]-p[0])*0.5); + sin_d_lat_h = sin((estimate_point[1]-p[1])*0.5); + squared_distance_cartesian_test = sin_d_lat_h*sin_d_lat_h+sin_d_long_h*sin_d_long_h*cos_cp_lat*cos(estimate_point[1]-p[1]); if (verbose == true) { - sin_dlat = sin(estimate_point[1]-cp[1]); + sin_dlat = sin(estimate_point[1]-p[1]); deriv_long = (3.0*a[0]*est_test*est_test+2.0*b[0]*est_test+c[0]); deriv_lat = (3.0*a[1]*est_test*est_test+2.0*b[1]*est_test+c[1]); const double squared_distance_cartesian_derivative_test = cos_cp_lat*(-deriv_lat)*sin_d_long_h*sin_d_long_h*sin_dlat+cos_cp_lat*deriv_long*sin_d_long_h*cos_dlong_h*cos_d_lat+deriv_lat*sin_d_lat_h*cos_dlat_h; const double squared_distance_cartesian_second_derivative_test = cos_cp_lat*cos_d_lat*(-0.5*deriv_long*deriv_long*sin_d_long_h*sin_d_long_h+0.5*deriv_long*deriv_long*cos_dlong_h*cos_dlong_h+(6.0*a[0]*est_test+2.0*b[0])*sin_d_long_h*cos_dlong_h)+cos_cp_lat*sin_d_long_h*sin_d_long_h*(deriv_lat*deriv_lat*(-cos_d_lat)-(6.0*a[1]*est_test+2.0*b[1])*sin_dlat)-2.0*cos_cp_lat*deriv_long*deriv_lat*sin_d_long_h*cos_dlong_h*sin_dlat-0.5*deriv_lat*deriv_lat*sin_d_lat_h*sin_d_lat_h+0.5*deriv_lat*deriv_lat*cos_dlat_h*cos_dlat_h+(6.0*a[1]*est_test+2.0*b[1])*sin_d_lat_h*cos_dlat_h; - output << " i: " << cp_i << ", ni: " << newton_i<< ", lsi: " << i << ", line_search_step=" << line_search_step << ": squared_distance_cartesian_test = " << squared_distance_cartesian_test << ", diff= " << squared_distance_cartesian_test-squared_distance_cartesian << ", tests: " << (squared_distance_cartesian_test_previous < squared_distance_cartesian ? "true" : "false") << ":" << (squared_distance_cartesian_test > squared_distance_cartesian_test_previous ? "true" : "false") << ", est_test=" << est_test << ", update=" << update << ", ls=" << line_search << ", up*ls=" << update *line_search << ", test deriv =" << squared_distance_cartesian_derivative_test << ", test update=" << squared_distance_cartesian_derivative_test/fabs(squared_distance_cartesian_second_derivative_test) << ", p1=" << p1 << ", p2= " << p2 << ", poc= " << a *est_test *est_test *est_test+b *est_test *est_test+c *est_test+d << ", cp= " << check_point << ", ds:" << ((a*est_test*est_test*est_test+b*est_test*est_test+c*est_test+d)-check_point).norm_square() << ":" << min_squared_distance_cartesian_temp_dg << ", diff = " << squared_distance_cartesian_test-min_squared_distance_cartesian_temp_dg<< std::endl; + output << " i: " << cp_i << ", ni: " << newton_iteration<< ", lsi: " << line_search_iteration << ", line_search_step=" << line_search_step << ": squared_distance_cartesian_test = " << squared_distance_cartesian_test << ", diff= " << squared_distance_cartesian_test-squared_distance_cartesian << ", tests: " << (squared_distance_cartesian_test_previous < squared_distance_cartesian ? "true" : "false") << ":" << (squared_distance_cartesian_test > squared_distance_cartesian_test_previous ? "true" : "false") << ", est_test=" << est_test << ", update=" << update << ", ls=" << line_search << ", up*ls=" << update *line_search << ", test deriv =" << squared_distance_cartesian_derivative_test << ", test update=" << squared_distance_cartesian_derivative_test/fabs(squared_distance_cartesian_second_derivative_test) << ", p1=" << p1 << ", p2= " << p2 << ", poc= " << a *est_test *est_test *est_test+b *est_test *est_test+c *est_test+d << ", cp= " << p << ", ds:" << ((a*est_test*est_test*est_test+b*est_test*est_test+c*est_test+d)-p).norm_square() << ":" << min_squared_distance_cartesian_temp_dg << ", diff = " << squared_distance_cartesian_test-min_squared_distance_cartesian_temp_dg<< std::endl; } - if (i > 0 && (squared_distance_cartesian_test > squared_distance_cartesian_test_previous)) + if (line_search_iteration > 0 && (squared_distance_cartesian_test > squared_distance_cartesian_test_previous)) { if (squared_distance_cartesian_test_previous-squared_distance_cartesian < 0) { line_search *= 1/line_search_step; break; } - if (i> 1) + if (line_search_iteration > 1) { line_search *= (1/line_search_step)*(1/line_search_step); est_test = est-update*line_search; estimate_point = a*est_test*est_test*est_test+b*est_test*est_test+c*est_test+d; - sin_d_long_h = sin((estimate_point[0]-cp[0])*0.5); - sin_d_lat_h = sin((estimate_point[1]-cp[1])*0.5); - squared_distance_cartesian_test_previous = sin_d_lat_h*sin_d_lat_h+sin_d_long_h*sin_d_long_h*cos_cp_lat*cos(estimate_point[1]-cp[1]); + sin_d_long_h = sin((estimate_point[0]-p[0])*0.5); + sin_d_lat_h = sin((estimate_point[1]-p[1])*0.5); + squared_distance_cartesian_test_previous = sin_d_lat_h*sin_d_lat_h+sin_d_long_h*sin_d_long_h*cos_cp_lat*cos(estimate_point[1]-p[1]); line_search_step = std::min(line_search_step*(11./10.),0.95); continue; } @@ -474,12 +494,12 @@ namespace WorldBuilder } if (verbose == true) { - output << " i: " << cp_i << ", ni: " << newton_i<< ", est= " << est-update *line_search << ", ls:" << line_search << ": squared_distance_cartesian_test = " << squared_distance_cartesian_test << ", diff= " << squared_distance_cartesian_test-squared_distance_cartesian << std::endl; + output << " i: " << cp_i << ", ni: " << newton_iteration<< ", est= " << est-update *line_search << ", ls:" << line_search << ": squared_distance_cartesian_test = " << squared_distance_cartesian_test << ", diff= " << squared_distance_cartesian_test-squared_distance_cartesian << std::endl; } est -= update*line_search; } - if (std::fabs(squared_distance_cartesian_derivative) <= 1e-16 || std::fabs(update) < 1e-4 || est < -0.1 || est > 1.1) + if (std::fabs(squared_distance_cartesian_derivative) <= 1e-14 || std::fabs(update) < 1e-4 || est < -0.1 || est > 1.1) { found = true; break; @@ -494,15 +514,15 @@ namespace WorldBuilder else if (verbose == false && found == false) { // redo the iteration with verbose=true to be able to report the error - return closest_point_on_curve_segment(check_point, true); + return closest_point_on_curve_segment(p, true); } estimate_point = a*est*est*est+b*est*est+c*est+d; - const double sin_d_long_h = sin((estimate_point[0]-cp[0])*0.5); - const double sin_d_lat_h = sin((estimate_point[1]-cp[1])*0.5); + const double sin_d_long_h = sin((estimate_point[0]-p[0])*0.5); + const double sin_d_lat_h = sin((estimate_point[1]-p[1])*0.5); - const double min_squared_distance_cartesian_temp = sin_d_lat_h*sin_d_lat_h+sin_d_long_h*sin_d_long_h*cos_cp_lat*cos(estimate_point[1]-cp[1]); + const double min_squared_distance_cartesian_temp = sin_d_lat_h*sin_d_lat_h+sin_d_long_h*sin_d_long_h*cos_cp_lat*cos(estimate_point[1]-p[1]); if (min_squared_distance_cartesian_temp < min_squared_distance) { @@ -514,12 +534,12 @@ namespace WorldBuilder // the sign is rotating the derivative by 90 degrees. // When moving in the direction of increasing t, left is positive and right is negative. // https://www.wolframalpha.com/input?i=d%2Fdt+%281-t%29*%281-t%29*%281-t%29*a+%2B+3*%281-t%29*%281-t%29*t*b+%2B+3.*%281-t%29*t*t*c%2Bt*t*t*d - const Point<2> derivative_point = points[cp_i]*((6.-3.*est)*est-3.) + control_points[cp_i][0]*(est*(9*est-12)+3) - + control_points[cp_i][1]*(6.-9.*est)*est + points[cp_i+1]*3.*est*est; + const Point<2> derivative_point = p1*((6.-3.*est)*est-3.) + cp1*(est*(9*est-12)+3) + + cp2*(6.-9.*est)*est + p2*3.*est*est; Point<2> tangent_point = derivative_point - point_on_curve; // if angle between check point and tangent point is larger than 90 degrees, return a negative distance - const double dot_product = (tangent_point*(check_point-point_on_curve)); + const double dot_product = (tangent_point*(p-point_on_curve)); const double sign = dot_product < 0. ? -1. : 1.; tangent_point = Point<2>(-tangent_point[1],tangent_point[0],tangent_point.get_coordinate_system());