inline bool projected_to_surface(Point3d const& origin, Point3d const& direction, Point3d & result1, Point3d & result2, Spheroid const& spheroid) { typedef typename coordinate_type<Point3d>::type coord_t; coord_t const c0 = 0; coord_t const c1 = 1; coord_t const c2 = 2; coord_t const c4 = 4; // calculate the point of intersection of a ray and spheroid's surface //(x*x+y*y)/(a*a) + z*z/(b*b) = 1 // x = o.x + d.x * t // y = o.y + d.y * t // z = o.z + d.z * t coord_t const ox = get<0>(origin); coord_t const oy = get<1>(origin); coord_t const oz = get<2>(origin); coord_t const dx = get<0>(direction); coord_t const dy = get<1>(direction); coord_t const dz = get<2>(direction); //coord_t const a_sqr = math::sqr(get_radius<0>(spheroid)); //coord_t const b_sqr = math::sqr(get_radius<2>(spheroid)); // "unit" spheroid, a = 1 coord_t const a_sqr = 1; coord_t const b_sqr = math::sqr(formula::unit_spheroid_b<coord_t>(spheroid)); coord_t const param_a = (dx*dx + dy*dy) / a_sqr + dz*dz / b_sqr; coord_t const param_b = c2 * ((ox*dx + oy*dy) / a_sqr + oz*dz / b_sqr); coord_t const param_c = (ox*ox + oy*oy) / a_sqr + oz*oz / b_sqr - c1; coord_t const delta = math::sqr(param_b) - c4 * param_a*param_c; // equals() ? if (delta < c0 || param_a == 0) { return false; } // result = origin + direction * t coord_t const sqrt_delta = math::sqrt(delta); coord_t const two_a = c2 * param_a; coord_t const t1 = (-param_b + sqrt_delta) / two_a; result1 = direction; multiply_value(result1, t1); add_point(result1, origin); coord_t const t2 = (-param_b - sqrt_delta) / two_a; result2 = direction; multiply_value(result2, t2); add_point(result2, origin); return true; }
inline bool planes_spheroid_intersection(Point3d const& o1, Point3d const& n1, Point3d const& o2, Point3d const& n2, Point3d & ip1, Point3d & ip2, Spheroid const& spheroid) { typedef typename coordinate_type<Point3d>::type coord_t; coord_t c0 = 0; coord_t c1 = 1; // Below // n . (p - o) = 0 // n . p - n . o = 0 // n . p + d = 0 // n . p = h // intersection direction Point3d id = cross_product(n1, n2); if (math::equals(dot_product(id, id), c0)) { return false; } coord_t dot_n1_n2 = dot_product(n1, n2); coord_t dot_n1_n2_sqr = math::sqr(dot_n1_n2); coord_t h1 = dot_product(n1, o1); coord_t h2 = dot_product(n2, o2); coord_t denom = c1 - dot_n1_n2_sqr; coord_t C1 = (h1 - h2 * dot_n1_n2) / denom; coord_t C2 = (h2 - h1 * dot_n1_n2) / denom; // C1 * n1 + C2 * n2 Point3d C1_n1 = n1; multiply_value(C1_n1, C1); Point3d C2_n2 = n2; multiply_value(C2_n2, C2); Point3d io = C1_n1; add_point(io, C2_n2); if (! projected_to_surface(io, id, ip1, ip2, spheroid)) { return false; } return true; }
inline Point3d projected_to_surface(Point3d const& direction, Spheroid const& spheroid) { typedef typename coordinate_type<Point3d>::type coord_t; //coord_t const c0 = 0; coord_t const c2 = 2; coord_t const c4 = 4; // calculate the point of intersection of a ray and spheroid's surface // the origin is the origin of the coordinate system //(x*x+y*y)/(a*a) + z*z/(b*b) = 1 // x = d.x * t // y = d.y * t // z = d.z * t coord_t const dx = get<0>(direction); coord_t const dy = get<1>(direction); coord_t const dz = get<2>(direction); //coord_t const a_sqr = math::sqr(get_radius<0>(spheroid)); //coord_t const b_sqr = math::sqr(get_radius<2>(spheroid)); // "unit" spheroid, a = 1 coord_t const a_sqr = 1; coord_t const b_sqr = math::sqr(formula::unit_spheroid_b<coord_t>(spheroid)); coord_t const param_a = (dx*dx + dy*dy) / a_sqr + dz*dz / b_sqr; coord_t const delta = c4 * param_a; // delta >= 0 coord_t const t = math::sqrt(delta) / (c2 * param_a); // result = direction * t Point3d result = direction; multiply_value(result, t); return result; }
inline calculation_type apply(Point const& p, PointOfSegment const& p1, PointOfSegment const& p2) const { assert_dimension_equal<Point, PointOfSegment>(); /* Algorithm [p1: (x1,y1), p2: (x2,y2), p: (px,py)] VECTOR v(x2 - x1, y2 - y1) VECTOR w(px - x1, py - y1) c1 = w . v c2 = v . v b = c1 / c2 RETURN POINT(x1 + b * vx, y1 + b * vy) */ // v is multiplied below with a (possibly) FP-value, so should be in FP // For consistency we define w also in FP fp_vector_type v, w; geometry::convert(p2, v); geometry::convert(p, w); subtract_point(v, p1); subtract_point(w, p1); Strategy strategy; boost::ignore_unused_variable_warning(strategy); calculation_type const zero = calculation_type(); fp_type const c1 = dot_product(w, v); if (c1 <= zero) { return strategy.apply(p, p1); } fp_type const c2 = dot_product(v, v); if (c2 <= c1) { return strategy.apply(p, p2); } // See above, c1 > 0 AND c2 > c1 so: c2 != 0 fp_type const b = c1 / c2; fp_strategy_type fp_strategy = strategy::distance::services::get_similar < Strategy, Point, fp_point_type >::apply(strategy); fp_point_type projected; geometry::convert(p1, projected); multiply_value(v, b); add_point(projected, v); //std::cout << "distance " << dsv(p) << " .. " << dsv(projected) << std::endl; return fp_strategy.apply(p, projected); }
inline return_type operator()(P const& p, Segment const& s) const { assert_dimension_equal<P, Segment>(); /* Algorithm POINT v(x2 - x1, y2 - y1); POINT w(px - x1, py - y1); c1 = w . v c2 = v . v b = c1 / c2 RETURN POINT(x1 + b * vx, y1 + b * vy); */ typedef typename boost::remove_const < typename point_type<Segment>::type >::type segment_point_type; segment_point_type v, w; // TODO // ASSUMPTION: segment // SOLVE THIS USING OTHER FUNCTIONS using get<,> copy_coordinates(s.second, v); copy_coordinates(p, w); subtract_point(v, s.first); subtract_point(w, s.first); Strategy strategy; coordinate_type c1 = dot_product(w, v); if (c1 <= 0) { return strategy(p, s.first); } coordinate_type c2 = dot_product(v, v); if (c2 <= c1) { return strategy(p, s.second); } // Even in case of char's, we have to turn to a point<double/float> // because of the division. coordinate_type b = c1 / c2; // Note that distances with integer coordinates do NOT work because // - the project point is integer // - if we solve that, the used distance_strategy cannot handle double points segment_point_type projected; copy_coordinates(s.first, projected); multiply_value(v, b); add_point(projected, v); return strategy(p, projected); }
inline return_type apply(Point const& p, PointOfSegment const& p1, PointOfSegment const& p2) const { assert_dimension_equal<Point, PointOfSegment>(); /* Algorithm POINT v(x2 - x1, y2 - y1); POINT w(px - x1, py - y1); c1 = w . v c2 = v . v b = c1 / c2 RETURN POINT(x1 + b * vx, y1 + b * vy); */ // Take here the first point type. It should have a default constructor. // That is not required for the second point type. Point v, w; copy_coordinates(p2, v); copy_coordinates(p, w); subtract_point(v, p1); subtract_point(w, p1); Strategy strategy; coordinate_type zero = coordinate_type(); coordinate_type c1 = dot_product(w, v); if (c1 <= zero) { return strategy.apply(p, p1); } coordinate_type c2 = dot_product(v, v); if (c2 <= c1) { return strategy.apply(p, p2); } // Even in case of char's, we have to turn to a point<double/float> // because of the division. typedef typename geometry::select_most_precise<coordinate_type, double>::type divisor_type; divisor_type b = c1 / c2; // Note that distances with integer coordinates do NOT work because // - the project point is integer // - if we solve that, the used distance_strategy cannot handle double points PointOfSegment projected; copy_coordinates(p1, projected); multiply_value(v, b); add_point(projected, v); return strategy.apply(p, projected); }
void centroid(PC& point) { point = sum_ms; if (get<0>(sum_m) != 0 && get<1>(sum_m) != 0) { multiply_value(point, 0.5); divide_point(point, sum_m); } else { // exception? } }
inline bool great_elliptic_intersection(Point3d const& a1, Point3d const& a2, Point3d const& b1, Point3d const& b2, Point3d & result, Spheroid const& spheroid) { typedef typename coordinate_type<Point3d>::type coord_t; coord_t c0 = 0; coord_t c1 = 1; Point3d n1 = cross_product(a1, a2); Point3d n2 = cross_product(b1, b2); // intersection direction Point3d id = cross_product(n1, n2); coord_t id_len_sqr = dot_product(id, id); if (math::equals(id_len_sqr, c0)) { return false; } // no need to normalize a1 and a2 because the intersection point on // the opposite side of the globe is at the same distance from the origin coord_t cos_a1i = dot_product(a1, id); coord_t cos_a2i = dot_product(a2, id); coord_t gri = math::detail::greatest(cos_a1i, cos_a2i); Point3d neg_id = id; multiply_value(neg_id, -c1); coord_t cos_a1ni = dot_product(a1, neg_id); coord_t cos_a2ni = dot_product(a2, neg_id); coord_t grni = math::detail::greatest(cos_a1ni, cos_a2ni); if (gri >= grni) { result = projected_to_surface(id, spheroid); } else { result = projected_to_surface(neg_id, spheroid); } return true; }
inline void experimental_elliptic_plane(Point3d const& p1, Point3d const& p2, Point3d & v1, Point3d & v2, Point3d & origin, Point3d & normal, Spheroid const& spheroid) { typedef typename coordinate_type<Point3d>::type coord_t; Point3d xy1 = projected_to_xy(p1, spheroid); Point3d xy2 = projected_to_xy(p2, spheroid); // origin = (xy1 + xy2) / 2 origin = xy1; add_point(origin, xy2); multiply_value(origin, coord_t(0.5)); // v1 = p1 - origin v1 = p1; subtract_point(v1, origin); // v2 = p2 - origin v2 = p2; subtract_point(v2, origin); normal = cross_product(v1, v2); }
inline void mid_points(Point const& vertex, PointP const& perpendicular, Point1 const& p1, Point2 const& p2, coordinate_type const& buffer_distance, coordinate_type const& max_distance, OutputIterator out, int level = 1) const { // Generate 'vectors' coordinate_type vp1_x = get<0>(p1) - get<0>(vertex); coordinate_type vp1_y = get<1>(p1) - get<1>(vertex); coordinate_type vp2_x = (get<0>(p2) - get<0>(vertex)); coordinate_type vp2_y = (get<1>(p2) - get<1>(vertex)); // Average them to generate vector in between coordinate_type two = 2; coordinate_type v_x = (vp1_x + vp2_x) / two; coordinate_type v_y = (vp1_y + vp2_y) / two; coordinate_type between_length = sqrt(v_x * v_x + v_y * v_y); coordinate_type const positive_buffer_distance = geometry::math::abs(buffer_distance); coordinate_type prop = positive_buffer_distance / between_length; PointOut mid_point; set<0>(mid_point, get<0>(vertex) + v_x * prop); set<1>(mid_point, get<1>(vertex) + v_y * prop); if (buffer_distance > max_distance) { // Calculate point projected on original perpendicular segment, // using vector maths vector_type v = create_vector<vector_type>(perpendicular, vertex); vector_type w = create_vector<vector_type>(mid_point, vertex); coordinate_type c1 = dot_product(w, v); if (c1 > 0) { coordinate_type c2 = dot_product(v, v); if (c2 > c1) { coordinate_type b = c1 / c2; PointOut projected_point; multiply_value(v, b); geometry::convert(vertex, projected_point); add_point(projected_point, v); coordinate_type projected_distance = geometry::distance(projected_point, mid_point); if (projected_distance > max_distance) { // Do not generate from here on. return; } } } } if (level < m_max_level) { mid_points(vertex, perpendicular, p1, mid_point, positive_buffer_distance, max_distance, out, level + 1); } *out++ = mid_point; if (level < m_max_level) { mid_points(vertex, perpendicular, mid_point, p2, positive_buffer_distance, max_distance, out, level + 1); } }