#pragma once #ifndef TCG_POINT_OPS_H #define TCG_POINT_OPS_H // tcg includes #include "point.h" #include "numeric_ops.h" /*! \file tcg_point_ops.h \brief This file contains useful functions to deal with \a planar objects (\a two dimensions) up to points and lines. */ //***************************************************************************** // (Planar) Point Operations //***************************************************************************** namespace tcg { /*! Contains useful functions to deal with \a planar objects (\a two dimensions) up to points and lines. */ namespace point_ops { template inline Point NaP() { return Point(numeric_ops::NaN::value_type>(), numeric_ops::NaN::value_type>()); } //------------------------------------------------------------------------------------------- template inline Point operator+(const Point &a, const Point &b) { return Point(a.x + b.x, a.y + b.y); } //------------------------------------------------------------------------------------------- template inline Point operator-(const Point &a, const Point &b) { return Point(a.x - b.x, a.y - b.y); } //------------------------------------------------------------------------------------------- template inline Point operator*(Scalar a, const Point &b) { return Point(a * b.x, a * b.y); } //------------------------------------------------------------------------------------------- template inline Point operator/(const Point &a, Scalar b) { return Point(a.x / b, a.y / b); } //------------------------------------------------------------------------------------------- template inline typename point_traits::value_type operator*(const Point &a, const Point &b) { return a.x * b.x + a.y * b.y; } //------------------------------------------------------------------------------------------- template inline typename point_traits::value_type cross(const Point &a, const Point &b) { return a.x * b.y - a.y * b.x; } //------------------------------------------------------------------------------------------- template inline typename point_traits::value_type norm2(const Point &a) { return a * a; } //------------------------------------------------------------------------------------------- template inline typename point_traits::float_type norm(const Point &a) { return sqrt(typename point_traits::float_type(a * a)); } //------------------------------------------------------------------------------------------- template inline typename point_traits::value_type dist2(const Point &a, const Point &b) { return norm2(b - a); } //------------------------------------------------------------------------------------------- template inline typename point_traits::value_type dist(const Point &a, const Point &b) { return norm(b - a); } //------------------------------------------------------------------------------------------- template inline typename point_traits::value_type absDist(const Point &a, const Point &b) { typename point_traits::value_type xDist = std::abs(b.x - a.x), yDist = std::abs(b.y - a.y); return (xDist < yDist) ? yDist : xDist; } //------------------------------------------------------------------------------------------- template inline Point normalized(const Point &v) { return v / norm(v); } //------------------------------------------------------------------------------------------- template inline Point normalized(const Point &v, typename point_traits::value_type tol) { typename point_traits::value_type vNorm = norm(v); return (vNorm >= tol) ? v / vNorm : NaP(); } //------------------------------------------------------------------------------------------- template inline void normalize(Point &v) { v = normalized(v); } //------------------------------------------------------------------------------------------- template inline bool normalize(Point &v, typename point_traits::value_type tol) { typename point_traits::value_type vNorm = norm(v); return (vNorm >= tol) ? (v = v / vNorm, true) : (v = NaP(), false); } //------------------------------------------------------------------------------------------- template inline Point direction(const Point &a, const Point &b) { return normalized(b - a); } //------------------------------------------------------------------------------------------- template inline Point direction(const Point &a, const Point &b, typename point_traits::value_type tol) { return normalized(b - a, tol); } //------------------------------------------------------------------------------------------- //! Returns the left orthogonal of passed point template inline Point ortLeft(const Point &p) { return Point(-p.y, p.x); } //------------------------------------------------------------------------------------------- //! Returns the right orthogonal of passed point template inline Point ortRight(const Point &p) { return Point(p.y, -p.x); } //------------------------------------------------------------------------------------------- /*! Returns the bisecting direction \p bd between \p d0 and \p d1, in the plane region where d x d1 > 0 */ template inline Point bisecant(const Point &d0, const Point &d1) { Point sum = d0 + d1; double norm = sum * sum; if (norm < 1) sum = ortLeft(d0 - d1), norm = sum * sum; else if (cross(d0, d1) < 0) sum.x = -sum.x, sum.y = -sum.y; norm = sqrt(norm); sum.x /= norm, sum.y /= norm; return sum; } //------------------------------------------------------------------------------------------- //! Returns the projection of point \p p on line a -> d template inline Point projection(const Point &p, const Point &a, const Point &d) { return a + ((p.x - a.x) * d.x + (p.y - a.y) * d.y) * d; } //------------------------------------------------------------------------------------------- //! Returns res: p = p0 + res.x * (p1 - p0) + res.y * (p1 - p0)_orth template Point ortCoords(const Point &p, const Point &p0, const Point &p1) { Point v01(p1.x - p0.x, p1.y - p0.y); Point v02(p.x - p0.x, p.y - p0.y); typename point_traits::value_type den = v01.x * v01.x + v01.y * v01.y; return Point((v01.x * v02.x + v01.y * v02.y) / den, (v01.y * v02.x - v01.x * v02.y) / den); } //------------------------------------------------------------------------------------------- //! Returns res: p = p0 + res.x * (p1 - p0) + res.y * (p2 - p0) template Point triCoords(const Point &p, const Point &p0, const Point &p1, const Point &p2) { Point v01(p1.x - p0.x, p1.y - p0.y); Point v02(p2.x - p0.x, p2.y - p0.y); Point p_p0(p.x - p0.x, p.y - p0.y); typename point_traits::value_type det = v01.x * v02.y - v02.x * v01.y; return Point((v02.y * p_p0.x - v02.x * p_p0.y) / det, (v01.x * p_p0.y - v01.y * p_p0.x) / det); } //------------------------------------------------------------------------------------------- //! Returns the signed distance of point \p p from line a->d, where //! positive means at its left. template inline typename point_traits::value_type lineSignedDist(const Point &p, const Point &a, const Point &d) { return (p.y - a.y) * d.x - (p.x - a.x) * d.y; } //------------------------------------------------------------------------------------------- //! Returns the distance of point \p p from line a->d template inline typename point_traits::value_type lineDist(const Point &p, const Point &a, const Point &d) { return std::abs(lineSignedDist(p, a, d)); } //------------------------------------------------------------------------------------------- /*! \brief Returns the side of line a->d where point \p p lies, where \p 1 means left, \p -1 right, and \p 0 below tolerance (ie on the line). */ template inline int lineSide(const Point &p, const Point &a, const Point &d, typename point_traits::value_type tol = 0) { typename point_traits::value_type dist = lineSignedDist(p, a, d); return (dist > tol) ? 1 : (dist < -tol) ? -1 : 0; } //------------------------------------------------------------------------------------------- template typename point_traits::value_type segDist(const Point &a, const Point &b, const Point &p) { Point v(b - a), w; v = v / norm(v); if ((w = p - a) * v < 0) return norm(w); if ((w = p - b) * (-v) < 0) return norm(w); v = Point(-v.y, v.x); return std::abs((p - a) * v); } //------------------------------------------------------------------------------------------- template inline typename point_traits::value_type rad(const Point &p) { return atan2(p.y, p.x); } //------------------------------------------------------------------------------------------- /*! Computes the angle, in radians, between \a unnormalized directions \p v1 and \p v2. \return The angle between the specified directions, in the range [-consts::pi, consts::pi]. */ template inline typename point_traits::value_type angle(const Point &v1, const Point &v2) { return numeric_ops::mod::value_type>( rad(v2) - rad(v1), -M_PI, M_PI); } //------------------------------------------------------------------------------------------- template inline typename point_traits::value_type segCoord(const Point &p, const Point &p0, const Point &p1) { Point p1_p0 = p1 - p0; return ((p - p0) * p1_p0) / (p1_p0 * p1_p0); } //------------------------------------------------------------------------------------------- /*! Calculates scalars \p s and \p t: a + s * v == c + t * w \return Whether the calculation succeeded with the specified tolerance parameter. \note In case the absolute value of the system's determinant is less or equal to the passed tolerance, the values numeric_ops::NaN() are returned for \p s and \p t. */ template bool intersectionCoords(const Point &a, const Point &v, const Point &c, const Point &w, typename point_traits::value_type &s, typename point_traits::value_type &t, typename point_traits::value_type detTol = 0) { typedef typename point_traits::value_type value_type; value_type det = v.y * w.x - v.x * w.y; if (std::abs(det) < detTol) { s = t = numeric_ops::NaN(); return false; } value_type c_aX = c.x - a.x, c_aY = c.y - a.y; s = (w.x * c_aY - w.y * c_aX) / det; t = (v.x * c_aY - v.y * c_aX) / det; return true; } //------------------------------------------------------------------------------------------- /*! Calculates scalars \p s and \p t: a + s * (b-a) == c + t * (d-c) \return Whether the calculation succeeded with the specified tolerance parameter. \note In case the absolute value of the system's determinant is less or equal to the passed tolerance, the values numeric_ops::NaN() are returned for \p s and \p t. */ template inline bool intersectionSegCoords( const Point &a, const Point &b, const Point &c, const Point &d, typename point_traits::value_type &s, typename point_traits::value_type &t, typename point_traits::value_type detTol = 0) { return intersectionCoords(a, b - a, c, d - c, s, t, detTol); } //------------------------------------------------------------------------------------------- /*! \brief Stores the 6 values of the bidimensional affine transform mapping \p p0, \p p1, \p p2 into \p q0, \p q1, \p q1. Returned values are in row-major order, with the translational component at multiple of 3 indexes. Returns numeric_ops::NaN() on each entry if the system could not be solved under the specified determinant threshold. \return Whether the affine could be solved under the specified constraints */ template bool affineMap(const Point &p0, const Point &p1, const Point &p2, const Point &q0, const Point &q1, const Point &q2, typename point_traits::value_type affValues[], typename point_traits::value_type detTol = 0) { typedef typename point_traits::value_type value_type; // Build the linear transform mapping (p1-p0) and (p2-p0) into (q1-q0) and // (q2-q0) value_type p1_p0_x = p1.x - p0.x, p1_p0_y = p1.y - p0.y; value_type p2_p0_x = p2.x - p0.x, p2_p0_y = p2.y - p0.y; value_type det = p1_p0_x * p2_p0_y - p1_p0_y * p2_p0_x; if (std::abs(det) < detTol) { affValues[0] = affValues[1] = affValues[2] = affValues[3] = affValues[4] = affValues[5] = numeric_ops::NaN(); return false; } value_type q1_q0_x = q1.x - q0.x, q1_q0_y = q1.y - q0.y; value_type q2_q0_x = q2.x - q0.x, q2_q0_y = q2.y - q0.y; affValues[2] = p2_p0_y / det; affValues[5] = -p2_p0_x / det; // 2 and 5 used as temps here affValues[3] = -p1_p0_y / det; affValues[4] = p1_p0_x / det; affValues[0] = q1_q0_x * affValues[2] + q2_q0_x * affValues[3]; affValues[1] = q1_q0_x * affValues[5] + q2_q0_x * affValues[4]; affValues[3] = q1_q0_y * affValues[2] + q2_q0_y * affValues[3]; affValues[4] = q1_q0_y * affValues[5] + q2_q0_y * affValues[4]; // Finally, add the translational component affValues[2] = q0.x - affValues[0] * p0.x - affValues[1] * p0.y; affValues[5] = q0.y - affValues[3] * p0.x - affValues[4] * p0.y; return true; } //------------------------------------------------------------------------------------------- /*! \brief Calculates the best fit line passing through \p p whose \p n samples have the specified coordinate sums \note Returned unnormalized direction \p v may be NaP() in case no preferential direction could be chosen */ template bool bestFit(const Point &p, Point &v, typename point_traits::value_type sums_x, typename point_traits::value_type sums_y, typename point_traits::value_type sums2_x, typename point_traits::value_type sums2_y, typename point_traits::value_type sums_xy, typename point_traits::value_type n) { typedef typename point_traits::value_type value_type; sums_x /= n; sums_y /= n; sums2_x /= n; sums2_y /= n; sums_xy /= n; value_type a = sums2_x - 2.0 * p.x * sums_x + sq(p.x); // Always >= 0 value_type b = sums_xy - p.x * sums_y - p.y * sums_x + p.x * p.y; value_type c = sums2_y - 2.0 * p.y * sums_y + sq(p.y); // Always >= 0 // The best-fit direction is an eigenvector of matrix [a, b; b, c]'s highest // eigenvalue. double trace = 0.5 * (a + c); double det = a * c - b * b; double traceSq = trace * trace; if (traceSq < det) { v = NaP(); return false; } double eigen = trace + sqrt(traceSq - det); // plus takes the greater eigen a -= eigen, c -= eigen; // Take the most 'normed' obvious eigenvector if (std::abs(a) > std::abs(c)) v.x = b, v.y = -a; else v.x = -c, v.y = b; return true; } //------------------------------------------------------------------------------------------- /*! Calculates the best fit line whose n samples have the specified coordinate sums. \note Returned point \p p is the samples mean, while (not normalized) direction \p v may be NaP in case no preferential direction could be chosen. */ template bool bestFit(Point &p, Point &v, typename point_traits::value_type sums_x, typename point_traits::value_type sums_y, typename point_traits::value_type sums2_x, typename point_traits::value_type sums2_y, typename point_traits::value_type sums_xy, typename point_traits::value_type n) { p.x = sums_x / n, p.y = sums_y / n; return bestFit((const Point &)p, v, sums_x, sums_y, sums2_x, sums2_y, sums_xy, n); } //------------------------------------------------------------------------------------------- /*! Calculates the best fit line of the specified points. This is a mostly overloaded version of bestFit without explicit sums. */ template bool bestFit(Point_ref p, Point &d, point_iterator begin, point_iterator end) { typedef typename point_traits::value_type value_type; value_type sums_x, sums_y, sums2_x, sums2_y, sums_xy; sums_x = sums_y = sums2_x = sums2_y = sums_xy = 0; size_t n = 0; point_iterator it; for (it = begin; it != end; ++it, ++n) { const Point &p = *it; sums_x += p.x, sums_y += p.y; sums2_x += p.x * p.x, sums2_y += p.y * p.y; sums_xy += p.x * p.y; } return bestFit(p, d, sums_x, sums_y, sums2_x, sums2_y, sums_xy, n); } } } // namespace tcg::point_ops #endif // TCG_POINT_OPS_H