static bool compare(const Sequence& n, const Sequence& low, const Sequence& high, const NumberComparisonPolicy& nCompare)
{
if (nCompare.less_than(get<D>(n), get<D>(low)) ||
nCompare.greater_than(get<D>(n), get<D>(high)))
return false;
return dimension_processor<Sequence, D - 1>::compare(n, low, high, nCompare);
}
inline bool point_in_triangle( const Point1& p, const Point2& A, const Point3& B, const Point4& C, const NumberComparisonPolicy& cmp, dimension<3> )
{
typedef typename select_arithmetic_type_from_sequences<Point1, Point2>::type arithmetic_type;
// Translate point and triangle so that point lies at origin
vector<arithmetic_type, 3> a = A - p;
vector<arithmetic_type, 3> b = B - p;
vector<arithmetic_type, 3> c = C - p;
// Compute normal vectors for triangles pab and pbc
auto u = cross_product( b, c );
auto v = cross_product( c, a );
// Make sure they are both pointing in the same direction
if( cmp.less_than(dot_product(u, v), constants::zero<decltype(dot_product(u,v))>() ) )
return false;
// Compute normal vector for triangle pca
auto w = cross_product(a, b);
// Make sure it points in the same direction as the first two
if( cmp.less_than(dot_product(u, w), constants::zero<decltype(dot_product(u,w))>() ) )
return false;
// Otherwise P must be in (or on) the triangle
return true;
}
static bool compare(const Sequence& n, const Sequence& low, const Sequence& high, const NumberComparisonPolicy& nCompare)
{
if (nCompare.less_than(get<0>(n), get<0>(low)) ||
nCompare.greater_than(get<0>(n), get<0>(high)))
return false;
else
return true;
}
bool numeric_sequence_equals_3d( const NumericSequence1& A,
const NumericSequence2& B,
const NumberComparisonPolicy& compare )
{
return compare.equals( get<0>( A ), get<0>( B ) ) &&
compare.equals( get<1>( A ), get<1>( B ) ) &&
compare.equals( get<2>( A ), get<2>( B ) );
}
bool intersects(const NumericType& t, const NumberComparisonPolicy& compare, typename boost::enable_if< is_numeric<NumericType> >::type* = 0) const
{
typedef typename geometric_traits<NumericSequence>::coordinate_type coordinate_type;
coordinate_type& lowD = get<D>(m_low);
coordinate_type& highD = get<D>(m_high);
bool lessThanHigh = compare.less_than(highD, t);
bool greaterThanLow = compare.greater_than(lowD, t);
return (lessThanHigh && greaterThanLow);
}
inline bool point_in_triangle( const Point1& p, const Point2& A, const Point3& B, const Point4& C, const NumberComparisonPolicy& cmp, dimension<2> )
{
//! From real time collision detection.
// If P to the right of AB then outside triangle
if( cmp.less_than( psuedo_cross_2d( p - A, B - A ), constants::zero<typename result_of::psuedo_cross_2d<decltype(p - A), decltype(B - A)>::type>() ) )
return false;
// If P to the right of BC then outside triangle
if( cmp.less_than( psuedo_cross_2d( p - B, C - B ), constants::zero<typename result_of::psuedo_cross_2d<decltype(p - B), decltype(C - B)>::type>()) )
return false;
// If P to the right of CA then outside triangle
if( cmp.less_than( psuedo_cross_2d( p - C, A - C ), constants::zero<typename result_of::psuedo_cross_2d<decltype(p - C), decltype(A - C)>::type>()) )
return false;
// Otherwise P must be in (or on) the triangle
return true;
}
inline bool bounding_box_intersection( const Point& p1, const Point& p2, const Point& p3, const Point& p4, const NumberComparisonPolicy& compare )
{
typedef typename geometric_traits<Point>::coordinate_type coordinate_type;
BOOST_CONCEPT_ASSERT((Point2DConcept<Point>));
coordinate_type xmax1, xmax2, ymax1, ymax2, xmin1, xmin2, ymin1, ymin2;
boost::tie( xmin1, xmax1 ) = boost::minmax( get<0>( p1 ), get<0>( p2 ) );
boost::tie( ymin1, ymax1 ) = boost::minmax( get<1>( p1 ), get<1>( p2 ) );
boost::tie( xmin2, xmax2 ) = boost::minmax( get<0>( p3 ), get<0>( p4 ) );
boost::tie( ymin2, ymax2 ) = boost::minmax( get<1>( p3 ), get<1>( p4 ) );
return ( compare.greater_than_or_equal( xmax1, xmin2 ) &&
compare.greater_than_or_equal( xmax2, xmin1 ) &&
compare.greater_than_or_equal( ymax1, ymin2 ) &&
compare.greater_than_or_equal( ymax2, ymin1 ) );
}
inline plane_orientation classify_point_to_plane(const Point& p, const Hyperplane& plane, const NumberComparisonPolicy& cmp)
{
using access = hyperplane_access_traits<typename std::decay<Hyperplane>::type>;
// Compute signed distance of point from plane
auto dist = scalar_projection(as_vector(p), access::get_normal_vector(plane)) - access::get_distance_to_origin(plane);
// Classify p based on the signed distance
using number_t = typename std::decay<decltype(dist)>::type;
auto zero = constants::zero<number_t>();
if (cmp.greater_than(dist, zero))
return plane_orientation::in_front_of_plane;
if (cmp.less_than(dist, zero))
return plane_orientation::in_back_of_plane;
return plane_orientation::coplanar_with_plane;
}
bool operator()( const Point1& a, const Point2& b ) const
{
BOOST_CONCEPT_ASSERT((Point2DConcept<Point1>));
BOOST_CONCEPT_ASSERT((Point2DConcept<Point2>));
const vector_double_2d da = a - m_origin, db = b - m_origin;
const double detb = exterior_product_area( m_reference, db );
//! If v2 is along reference it is smallest.
if( m_cmp.equals(detb, 0) && m_cmp.greater_than_or_equal(dot_product( db, m_reference ), 0) )
return false;
const double deta = exterior_product_area( m_reference, da );
//! If v1 is along reference it is smallest.
if( m_cmp.equals(deta, 0) && m_cmp.greater_than_or_equal(dot_product( da, m_reference ), 0) )
return true;
//! If detv1 and detv2 have the same sign, they are on the same side of reference and can be compared directly.
if( m_cmp.greater_than_or_equal(deta * detb, 0) )
{
//! both on same side of reference: compare to each other
return m_cmp.greater_than(exterior_product_area( da, db ), 0);
}
//! At this point one of the two detvX is negative. A negative detvX means a large angle WRT reference.
//! If v1 is positive it must be smaller than v2, else the opposite must be true.
return m_cmp.greater_than(deta, 0);
}
sphere_sphere_intersection_result<point<typename geometric_traits<SphereA>::radius_type, 2>> sphere_sphere_intersection(const SphereA& A, const SphereB& B, const NumberComparisonPolicy& cmp)
{
using namespace geometrix;
using std::sqrt;
using std::abs;
using length_t = typename geometric_traits<SphereA>::radius_type;
using dimensionless_t = decltype(std::declval<length_t>() / std::declval<length_t>());
using point_t = point<length_t, 2>;
using vector_t = vector<length_t, 2>;
auto d = point_point_distance(get_center(A), get_center(B));
if (cmp.greater_than(d, get_radius(A) + get_radius(B)))
return sphere_sphere_intersection_result<point_t>(sphere_intersection_state::non_intersecting);
else if (cmp.less_than(d, abs(get_radius(A) - get_radius(B))))
return sphere_sphere_intersection_result<point_t>(sphere_intersection_state::circumscribed);
else if (cmp.equals(d, constants::zero<length_t>()))
return sphere_sphere_intersection_result<point_t>(sphere_intersection_state::coincident);
auto r1 = get_radius(A) * get_radius(A);
auto a = (r1 - get_radius(B) * get_radius(B) + d * d) / (constants::two<dimensionless_t>() * d);
auto h = sqrt(r1 - a*a);
vector_t v = get_center(B) - get_center(A);
auto dinv = constants::one<dimensionless_t>() / d;
point_t m = get_center(A) + (a * dinv) * v;
auto f = h * dinv;
point_t s1 = m + f * right_normal(v);
if (cmp.equals(h, constants::zero<length_t>()))
return sphere_sphere_intersection_result<point_t>(s1);
point_t s2 = m + f * left_normal(v);
return sphere_sphere_intersection_result<point_t>(s1, s2);
}
inline polygon_containment point_polygon_containment_or_on_border(const Point& p, const Polygon& poly, const NumberComparisonPolicy& cmp )
{
typedef point_sequence_traits<Polygon> access;
typedef typename access::point_type point_type;
typedef typename geometric_traits<point_type>::arithmetic_type arithmetic_type;
using dimensionless_type = typename geometric_traits<point_type>::dimensionless_type;
std::size_t size = access::size(poly);
GEOMETRIX_ASSERT(size > 2);
std::size_t rcross = 0;
std::size_t lcross = 0;
bool rstrad, lstrad;
for (std::size_t i = 0; i < size; ++i)
{
point_type pointi = access::get_point(poly, i);
if (numeric_sequence_equals(p, pointi, cmp))
return polygon_containment::vertex;
std::size_t h = (i + size - 1) % size;
point_type pointh = access::get_point(poly, h);
rstrad = cmp.greater_than(get<1>(pointi), get<1>(p)) != cmp.greater_than(get<1>(pointh), get<1>(p));
lstrad = cmp.less_than(get<1>(pointi), get<1>(p)) != cmp.less_than(get<1>(pointh), get<1>(p));
if (rstrad || lstrad)
{
arithmetic_type ydiff = get<1>(pointh) - get<1>(pointi);
arithmetic_type denom = (ydiff == constants::zero<arithmetic_type>()) ? std::numeric_limits<arithmetic_type>::epsilon() : ydiff;
dimensionless_type slopeInverse = (get<0>(pointh) - get<0>(pointi)) / denom;
arithmetic_type x = slopeInverse * (get<1>(p) - get<1>(pointi)) + get<0>(pointi);
if (rstrad && cmp.greater_than(x, get<0>(p)))
++rcross;
if (lstrad && cmp.less_than(x, get<0>(p)))
++lcross;
}
}
if ((rcross % 2) != (lcross % 2))
return polygon_containment::border;
if ((rcross % 2) == 1)
return polygon_containment::interior;
return polygon_containment::exterior;
}
inline orientation_type point_polyline_orientation(const Point& p, const Polyline& pline, const NumberComparisonPolicy& cmp)
{
BOOST_CONCEPT_ASSERT((PointSequenceConcept<Polyline>));
BOOST_CONCEPT_ASSERT((NumberComparisonPolicyConcept<NumberComparisonPolicy>));
using access = point_sequence_traits<Polyline>;
GEOMETRIX_ASSERT(access::size(pline) > 1);
using length_t = typename select_arithmetic_type_from_sequences<Point, typename access::point_type>::type;
using area_t = decltype(std::declval<length_t>() * std::declval<length_t>());
using dimensionless_t = decltype(std::declval<length_t>() / std::declval<length_t>());
using vector_t = vector<length_t, dimension_of<Point>::value>;
auto minDistance = (std::numeric_limits<area_t>::max)();
std::size_t segIndex;
for (std::size_t i = 0, j = 1; j < access::size(pline); i = j++)
{
auto d2 = point_segment_distance_sqrd(p, access::get_point(pline, i), access::get_point(pline, j));
if (d2 < minDistance)
{
minDistance = d2;
segIndex = i;
}
}
if (cmp.greater_than(minDistance, constants::zero<area_t>()))
{
GEOMETRIX_ASSERT(minDistance < (std::numeric_limits<area_t>::max)());
auto pi = access::get_point(pline, segIndex);
auto pj = access::get_point(pline, segIndex + 1);
dimensionless_t t;
point_segment_closest_point_and_param(p, pi, pj, t);
if (t > constants::zero<dimensionless_t>() && t < constants::one<dimensionless_t>())
return get_orientation(pi, pj, p, cmp);
else if (cmp.equals(t, constants::zero<dimensionless_t>()))
{
//! Closest point is a vertex i.
if (segIndex != 0)
{
auto ph = access::get_point(pline, segIndex - 1);
auto o = get_orientation(ph, pi, pj, cmp);
if (o == oriented_left)
{
vector_t vpi = p - pi;
vector_t lo = pj - pi;
vector_t hi = ph - pi;
if (is_vector_between(lo, hi, vpi, false, cmp))
return oriented_left;
else
return oriented_right;
}
else if (o == oriented_right)
{
vector_t vpi = p - pi;
vector_t hi = pj - pi;
vector_t lo = ph - pi;
if (is_vector_between(lo, hi, vpi, false, cmp))
return oriented_right;
else
return oriented_left;
}
else
return get_orientation(pi, pj, p, cmp);
}
else
return get_orientation(pi, pj, p, cmp);
}
else
{
//! pj is the closest.
if ((segIndex + 2) < access::size(pline))
{
auto pk = access::get_point(pline, segIndex + 2);
auto o = get_orientation(pi, pj, pk, cmp);
if (o == oriented_left)
{
vector_t vpj = p - pj;
vector_t lo = pk - pj;
vector_t hi = pi - pj;
if (is_vector_between(lo, hi, vpj, false, cmp))
return oriented_left;
else
return oriented_right;
}
else if (o == oriented_right)
{
vector_t vpj = p - pj;
vector_t hi = pk - pj;
vector_t lo = pi - pj;
if (is_vector_between(lo, hi, vpj, false, cmp))
return oriented_right;
else
return oriented_left;
}
else
return get_orientation(pi, pj, p, cmp);
}
else
return get_orientation(pi, pj, p, cmp);
}
}
return oriented_collinear;
}
