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