2016-05-17 03:04:11 +12:00
|
|
|
#pragma once
|
2016-03-19 06:57:51 +13:00
|
|
|
|
|
|
|
#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
|
|
|
|
//*****************************************************************************
|
|
|
|
|
2016-06-15 18:43:10 +12:00
|
|
|
namespace tcg {
|
2016-03-19 06:57:51 +13:00
|
|
|
|
|
|
|
/*!
|
|
|
|
Contains useful functions to deal with \a planar objects
|
|
|
|
(\a two dimensions) up to points and lines.
|
|
|
|
*/
|
|
|
|
|
2016-06-15 18:43:10 +12:00
|
|
|
namespace point_ops {
|
2016-03-19 06:57:51 +13:00
|
|
|
|
|
|
|
template <typename Point>
|
2016-06-15 18:43:10 +12:00
|
|
|
inline Point NaP() {
|
|
|
|
return Point(numeric_ops::NaN<typename point_traits<Point>::value_type>(),
|
|
|
|
numeric_ops::NaN<typename point_traits<Point>::value_type>());
|
|
|
|
}
|
2016-03-19 06:57:51 +13:00
|
|
|
|
|
|
|
//-------------------------------------------------------------------------------------------
|
|
|
|
|
|
|
|
template <typename Point>
|
2016-06-15 18:43:10 +12:00
|
|
|
inline Point operator+(const Point &a, const Point &b) {
|
|
|
|
return Point(a.x + b.x, a.y + b.y);
|
|
|
|
}
|
2016-03-19 06:57:51 +13:00
|
|
|
|
|
|
|
//-------------------------------------------------------------------------------------------
|
|
|
|
|
|
|
|
template <typename Point>
|
2016-06-15 18:43:10 +12:00
|
|
|
inline Point operator-(const Point &a, const Point &b) {
|
|
|
|
return Point(a.x - b.x, a.y - b.y);
|
|
|
|
}
|
2016-03-19 06:57:51 +13:00
|
|
|
|
|
|
|
//-------------------------------------------------------------------------------------------
|
|
|
|
|
|
|
|
template <typename Point, typename Scalar>
|
2016-06-15 18:43:10 +12:00
|
|
|
inline Point operator*(Scalar a, const Point &b) {
|
|
|
|
return Point(a * b.x, a * b.y);
|
2016-03-19 06:57:51 +13:00
|
|
|
}
|
|
|
|
|
|
|
|
//-------------------------------------------------------------------------------------------
|
|
|
|
|
|
|
|
template <typename Point, typename Scalar>
|
2016-06-15 18:43:10 +12:00
|
|
|
inline Point operator/(const Point &a, Scalar b) {
|
|
|
|
return Point(a.x / b, a.y / b);
|
2016-03-19 06:57:51 +13:00
|
|
|
}
|
|
|
|
|
|
|
|
//-------------------------------------------------------------------------------------------
|
|
|
|
|
|
|
|
template <typename Point>
|
2016-06-15 18:43:10 +12:00
|
|
|
inline typename point_traits<Point>::value_type operator*(const Point &a,
|
|
|
|
const Point &b) {
|
|
|
|
return a.x * b.x + a.y * b.y;
|
2016-03-19 06:57:51 +13:00
|
|
|
}
|
|
|
|
|
|
|
|
//-------------------------------------------------------------------------------------------
|
|
|
|
|
|
|
|
template <typename Point>
|
2016-06-15 18:43:10 +12:00
|
|
|
inline typename point_traits<Point>::value_type cross(const Point &a,
|
|
|
|
const Point &b) {
|
|
|
|
return a.x * b.y - a.y * b.x;
|
2016-03-19 06:57:51 +13:00
|
|
|
}
|
|
|
|
|
|
|
|
//-------------------------------------------------------------------------------------------
|
|
|
|
|
|
|
|
template <typename Point>
|
2016-06-15 18:43:10 +12:00
|
|
|
inline typename point_traits<Point>::value_type norm2(const Point &a) {
|
|
|
|
return a * a;
|
|
|
|
}
|
2016-03-19 06:57:51 +13:00
|
|
|
|
|
|
|
//-------------------------------------------------------------------------------------------
|
|
|
|
|
|
|
|
template <typename Point>
|
2016-06-15 18:43:10 +12:00
|
|
|
inline typename point_traits<Point>::float_type norm(const Point &a) {
|
|
|
|
return sqrt(typename point_traits<Point>::float_type(a * a));
|
2016-03-19 06:57:51 +13:00
|
|
|
}
|
|
|
|
|
|
|
|
//-------------------------------------------------------------------------------------------
|
|
|
|
|
|
|
|
template <typename Point>
|
2016-06-15 18:43:10 +12:00
|
|
|
inline typename point_traits<Point>::value_type dist2(const Point &a,
|
|
|
|
const Point &b) {
|
|
|
|
return norm2(b - a);
|
2016-03-19 06:57:51 +13:00
|
|
|
}
|
|
|
|
|
|
|
|
//-------------------------------------------------------------------------------------------
|
|
|
|
|
|
|
|
template <typename Point>
|
2016-06-15 18:43:10 +12:00
|
|
|
inline typename point_traits<Point>::value_type dist(const Point &a,
|
|
|
|
const Point &b) {
|
|
|
|
return norm(b - a);
|
2016-03-19 06:57:51 +13:00
|
|
|
}
|
|
|
|
|
|
|
|
//-------------------------------------------------------------------------------------------
|
|
|
|
|
|
|
|
template <typename Point>
|
2016-06-15 18:43:10 +12:00
|
|
|
inline typename point_traits<Point>::value_type absDist(const Point &a,
|
|
|
|
const Point &b) {
|
|
|
|
typename point_traits<Point>::value_type xDist = std::abs(b.x - a.x),
|
|
|
|
yDist = std::abs(b.y - a.y);
|
|
|
|
return (xDist < yDist) ? yDist : xDist;
|
2016-03-19 06:57:51 +13:00
|
|
|
}
|
|
|
|
|
|
|
|
//-------------------------------------------------------------------------------------------
|
|
|
|
|
|
|
|
template <typename Point>
|
2016-06-15 18:43:10 +12:00
|
|
|
inline Point normalized(const Point &v) {
|
|
|
|
return v / norm(v);
|
2016-03-19 06:57:51 +13:00
|
|
|
}
|
|
|
|
|
|
|
|
//-------------------------------------------------------------------------------------------
|
|
|
|
|
|
|
|
template <typename Point>
|
2016-06-15 18:43:10 +12:00
|
|
|
inline Point normalized(const Point &v,
|
|
|
|
typename point_traits<Point>::value_type tol) {
|
|
|
|
typename point_traits<Point>::value_type vNorm = norm(v);
|
|
|
|
return (vNorm >= tol) ? v / vNorm : NaP<Point>();
|
2016-03-19 06:57:51 +13:00
|
|
|
}
|
|
|
|
|
|
|
|
//-------------------------------------------------------------------------------------------
|
|
|
|
|
|
|
|
template <typename Point>
|
2016-06-15 18:43:10 +12:00
|
|
|
inline void normalize(Point &v) {
|
|
|
|
v = normalized(v);
|
2016-03-19 06:57:51 +13:00
|
|
|
}
|
|
|
|
|
|
|
|
//-------------------------------------------------------------------------------------------
|
|
|
|
|
|
|
|
template <typename Point>
|
2016-06-15 18:43:10 +12:00
|
|
|
inline bool normalize(Point &v, typename point_traits<Point>::value_type tol) {
|
|
|
|
typename point_traits<Point>::value_type vNorm = norm(v);
|
|
|
|
return (vNorm >= tol) ? (v = v / vNorm, true) : (v = NaP<Point>(), false);
|
2016-03-19 06:57:51 +13:00
|
|
|
}
|
|
|
|
|
|
|
|
//-------------------------------------------------------------------------------------------
|
|
|
|
|
|
|
|
template <typename Point>
|
2016-06-15 18:43:10 +12:00
|
|
|
inline Point direction(const Point &a, const Point &b) {
|
|
|
|
return normalized(b - a);
|
2016-03-19 06:57:51 +13:00
|
|
|
}
|
|
|
|
|
|
|
|
//-------------------------------------------------------------------------------------------
|
|
|
|
|
|
|
|
template <typename Point>
|
2016-06-15 18:43:10 +12:00
|
|
|
inline Point direction(const Point &a, const Point &b,
|
|
|
|
typename point_traits<Point>::value_type tol) {
|
|
|
|
return normalized<Point>(b - a, tol);
|
2016-03-19 06:57:51 +13:00
|
|
|
}
|
|
|
|
|
|
|
|
//-------------------------------------------------------------------------------------------
|
|
|
|
|
|
|
|
//! Returns the left orthogonal of passed point
|
|
|
|
template <typename Point>
|
2016-06-15 18:43:10 +12:00
|
|
|
inline Point ortLeft(const Point &p) {
|
|
|
|
return Point(-p.y, p.x);
|
|
|
|
}
|
2016-03-19 06:57:51 +13:00
|
|
|
|
|
|
|
//-------------------------------------------------------------------------------------------
|
|
|
|
|
|
|
|
//! Returns the right orthogonal of passed point
|
|
|
|
template <typename Point>
|
2016-06-15 18:43:10 +12:00
|
|
|
inline Point ortRight(const Point &p) {
|
|
|
|
return Point(p.y, -p.x);
|
|
|
|
}
|
2016-03-19 06:57:51 +13:00
|
|
|
|
|
|
|
//-------------------------------------------------------------------------------------------
|
|
|
|
|
|
|
|
/*!
|
2016-06-15 18:43:10 +12:00
|
|
|
Returns the bisecting direction \p bd between \p d0 and \p d1, in the plane
|
|
|
|
region
|
2016-03-19 06:57:51 +13:00
|
|
|
where <TT>d x d1 > 0</TT>
|
|
|
|
*/
|
|
|
|
template <typename Point>
|
2016-06-15 18:43:10 +12:00
|
|
|
inline Point bisecant(const Point &d0, const Point &d1) {
|
|
|
|
Point sum = d0 + d1;
|
|
|
|
double norm = sum * sum;
|
2016-03-19 06:57:51 +13:00
|
|
|
|
2016-06-15 18:43:10 +12:00
|
|
|
if (norm < 1)
|
|
|
|
sum = ortLeft(d0 - d1), norm = sum * sum;
|
|
|
|
else if (cross(d0, d1) < 0)
|
|
|
|
sum.x = -sum.x, sum.y = -sum.y;
|
2016-03-19 06:57:51 +13:00
|
|
|
|
2016-06-15 18:43:10 +12:00
|
|
|
norm = sqrt(norm);
|
|
|
|
sum.x /= norm, sum.y /= norm;
|
2016-03-19 06:57:51 +13:00
|
|
|
|
2016-06-15 18:43:10 +12:00
|
|
|
return sum;
|
2016-03-19 06:57:51 +13:00
|
|
|
}
|
|
|
|
|
|
|
|
//-------------------------------------------------------------------------------------------
|
|
|
|
|
|
|
|
//! Returns the projection of point \p p on line <TT>a -> d</TT>
|
|
|
|
template <typename Point>
|
2016-06-15 18:43:10 +12:00
|
|
|
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;
|
2016-03-19 06:57:51 +13:00
|
|
|
}
|
|
|
|
|
|
|
|
//-------------------------------------------------------------------------------------------
|
|
|
|
|
|
|
|
//! Returns res: <TT>p = p0 + res.x * (p1 - p0) + res.y * (p1 - p0)_orth</TT>
|
|
|
|
template <typename Point>
|
2016-06-15 18:43:10 +12:00
|
|
|
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);
|
2016-03-19 06:57:51 +13:00
|
|
|
|
2016-06-15 18:43:10 +12:00
|
|
|
typename point_traits<Point>::value_type den = v01.x * v01.x + v01.y * v01.y;
|
2016-03-19 06:57:51 +13:00
|
|
|
|
2016-06-15 18:43:10 +12:00
|
|
|
return Point((v01.x * v02.x + v01.y * v02.y) / den,
|
|
|
|
(v01.y * v02.x - v01.x * v02.y) / den);
|
2016-03-19 06:57:51 +13:00
|
|
|
}
|
|
|
|
|
|
|
|
//-------------------------------------------------------------------------------------------
|
|
|
|
|
|
|
|
//! Returns res: <TT>p = p0 + res.x * (p1 - p0) + res.y * (p2 - p0)</TT>
|
|
|
|
template <typename Point>
|
2016-06-15 18:43:10 +12:00
|
|
|
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);
|
2016-03-19 06:57:51 +13:00
|
|
|
|
2016-06-15 18:43:10 +12:00
|
|
|
typename point_traits<Point>::value_type det = v01.x * v02.y - v02.x * v01.y;
|
2016-03-19 06:57:51 +13:00
|
|
|
|
2016-06-15 18:43:10 +12:00
|
|
|
return Point((v02.y * p_p0.x - v02.x * p_p0.y) / det,
|
|
|
|
(v01.x * p_p0.y - v01.y * p_p0.x) / det);
|
2016-03-19 06:57:51 +13:00
|
|
|
}
|
|
|
|
|
|
|
|
//-------------------------------------------------------------------------------------------
|
|
|
|
|
2016-06-15 18:43:10 +12:00
|
|
|
//! Returns the signed distance of point \p p from line <TT>a->d</TT>, where
|
|
|
|
//! positive means <I>at its left</I>.
|
2016-03-19 06:57:51 +13:00
|
|
|
template <typename Point>
|
2016-06-15 18:43:10 +12:00
|
|
|
inline typename point_traits<Point>::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;
|
2016-03-19 06:57:51 +13:00
|
|
|
}
|
|
|
|
|
|
|
|
//-------------------------------------------------------------------------------------------
|
|
|
|
|
|
|
|
//! Returns the distance of point \p p from line <TT>a->d</TT>
|
|
|
|
template <typename Point>
|
2016-06-15 18:43:10 +12:00
|
|
|
inline typename point_traits<Point>::value_type lineDist(const Point &p,
|
|
|
|
const Point &a,
|
|
|
|
const Point &d) {
|
|
|
|
return std::abs(lineSignedDist(p, a, d));
|
2016-03-19 06:57:51 +13:00
|
|
|
}
|
|
|
|
|
|
|
|
//-------------------------------------------------------------------------------------------
|
|
|
|
|
|
|
|
/*!
|
2016-06-15 18:43:10 +12:00
|
|
|
\brief Returns the side of line <TT>a->d</TT> where point \p p lies, where \p
|
|
|
|
1 means left,
|
2016-03-19 06:57:51 +13:00
|
|
|
\p -1 right, and \p 0 below tolerance (ie on the line).
|
|
|
|
*/
|
|
|
|
template <typename Point>
|
|
|
|
inline int lineSide(const Point &p, const Point &a, const Point &d,
|
2016-06-15 18:43:10 +12:00
|
|
|
typename point_traits<Point>::value_type tol = 0) {
|
|
|
|
typename point_traits<Point>::value_type dist = lineSignedDist(p, a, d);
|
|
|
|
return (dist > tol) ? 1 : (dist < -tol) ? -1 : 0;
|
2016-03-19 06:57:51 +13:00
|
|
|
}
|
|
|
|
|
|
|
|
//-------------------------------------------------------------------------------------------
|
|
|
|
|
|
|
|
template <typename Point>
|
2016-06-15 18:43:10 +12:00
|
|
|
typename point_traits<Point>::value_type segDist(const Point &a, const Point &b,
|
|
|
|
const Point &p) {
|
|
|
|
Point v(b - a), w;
|
|
|
|
v = v / norm(v);
|
2016-03-19 06:57:51 +13:00
|
|
|
|
2016-06-15 18:43:10 +12:00
|
|
|
if ((w = p - a) * v < 0) return norm(w);
|
|
|
|
if ((w = p - b) * (-v) < 0) return norm(w);
|
2016-03-19 06:57:51 +13:00
|
|
|
|
2016-06-15 18:43:10 +12:00
|
|
|
v = Point(-v.y, v.x);
|
|
|
|
return std::abs((p - a) * v);
|
2016-03-19 06:57:51 +13:00
|
|
|
}
|
|
|
|
|
|
|
|
//-------------------------------------------------------------------------------------------
|
|
|
|
|
|
|
|
template <typename Point>
|
2016-06-15 18:43:10 +12:00
|
|
|
inline typename point_traits<Point>::value_type rad(const Point &p) {
|
|
|
|
return atan2(p.y, p.x);
|
2016-03-19 06:57:51 +13:00
|
|
|
}
|
|
|
|
|
|
|
|
//-------------------------------------------------------------------------------------------
|
|
|
|
|
|
|
|
/*!
|
2016-06-15 18:43:10 +12:00
|
|
|
Computes the angle, in radians, between \a unnormalized directions \p v1 and
|
|
|
|
\p v2.
|
|
|
|
\return The angle between the specified directions, in the range
|
|
|
|
<TT>[-consts::pi, consts::pi]</TT>.
|
2016-03-19 06:57:51 +13:00
|
|
|
*/
|
|
|
|
template <typename Point>
|
2016-06-15 18:43:10 +12:00
|
|
|
inline typename point_traits<Point>::value_type angle(const Point &v1,
|
|
|
|
const Point &v2) {
|
|
|
|
return numeric_ops::mod<typename point_traits<Point>::value_type>(
|
|
|
|
rad(v2) - rad(v1), -M_PI, M_PI);
|
2016-03-19 06:57:51 +13:00
|
|
|
}
|
|
|
|
|
|
|
|
//-------------------------------------------------------------------------------------------
|
|
|
|
|
|
|
|
template <typename Point>
|
2016-06-15 18:43:10 +12:00
|
|
|
inline typename point_traits<Point>::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);
|
2016-03-19 06:57:51 +13:00
|
|
|
}
|
|
|
|
|
|
|
|
//-------------------------------------------------------------------------------------------
|
|
|
|
|
|
|
|
/*!
|
|
|
|
Calculates scalars \p s and \p t: <TT>a + s * v == c + t * w</TT>
|
2016-06-15 18:43:10 +12:00
|
|
|
\return Whether the calculation succeeded with the specified tolerance
|
|
|
|
parameter.
|
2016-03-19 06:57:51 +13:00
|
|
|
|
2016-06-15 18:43:10 +12:00
|
|
|
\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.
|
2016-03-19 06:57:51 +13:00
|
|
|
*/
|
|
|
|
template <typename Point>
|
2016-06-15 18:43:10 +12:00
|
|
|
bool intersectionCoords(const Point &a, const Point &v, const Point &c,
|
|
|
|
const Point &w,
|
|
|
|
typename point_traits<Point>::value_type &s,
|
|
|
|
typename point_traits<Point>::value_type &t,
|
|
|
|
typename point_traits<Point>::value_type detTol = 0) {
|
|
|
|
typedef typename point_traits<Point>::value_type value_type;
|
2016-03-19 06:57:51 +13:00
|
|
|
|
2016-06-15 18:43:10 +12:00
|
|
|
value_type det = v.y * w.x - v.x * w.y;
|
|
|
|
if (std::abs(det) < detTol) {
|
|
|
|
s = t = numeric_ops::NaN<value_type>();
|
|
|
|
return false;
|
|
|
|
}
|
2016-03-19 06:57:51 +13:00
|
|
|
|
2016-06-15 18:43:10 +12:00
|
|
|
value_type c_aX = c.x - a.x, c_aY = c.y - a.y;
|
2016-03-19 06:57:51 +13:00
|
|
|
|
2016-06-15 18:43:10 +12:00
|
|
|
s = (w.x * c_aY - w.y * c_aX) / det;
|
|
|
|
t = (v.x * c_aY - v.y * c_aX) / det;
|
2016-03-19 06:57:51 +13:00
|
|
|
|
2016-06-15 18:43:10 +12:00
|
|
|
return true;
|
2016-03-19 06:57:51 +13:00
|
|
|
}
|
|
|
|
|
|
|
|
//-------------------------------------------------------------------------------------------
|
|
|
|
|
|
|
|
/*!
|
|
|
|
Calculates scalars \p s and \p t: <TT>a + s * (b-a) == c + t * (d-c)</TT>
|
2016-06-15 18:43:10 +12:00
|
|
|
\return Whether the calculation succeeded with the specified tolerance
|
|
|
|
parameter.
|
2016-03-19 06:57:51 +13:00
|
|
|
|
2016-06-15 18:43:10 +12:00
|
|
|
\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.
|
2016-03-19 06:57:51 +13:00
|
|
|
*/
|
|
|
|
template <typename Point>
|
2016-06-15 18:43:10 +12:00
|
|
|
inline bool intersectionSegCoords(
|
|
|
|
const Point &a, const Point &b, const Point &c, const Point &d,
|
|
|
|
typename point_traits<Point>::value_type &s,
|
|
|
|
typename point_traits<Point>::value_type &t,
|
|
|
|
typename point_traits<Point>::value_type detTol = 0) {
|
|
|
|
return intersectionCoords(a, b - a, c, d - c, s, t, detTol);
|
2016-03-19 06:57:51 +13:00
|
|
|
}
|
|
|
|
|
|
|
|
//-------------------------------------------------------------------------------------------
|
|
|
|
|
|
|
|
/*!
|
|
|
|
\brief Stores the 6 values of the bidimensional affine transform mapping
|
|
|
|
\p p0, \p p1, \p p2 into \p q0, \p q1, \p q1.
|
|
|
|
|
2016-06-15 18:43:10 +12:00
|
|
|
Returned values are in <I>row-major</I> order, with the translational
|
|
|
|
component
|
2016-03-19 06:57:51 +13:00
|
|
|
at multiple of 3 indexes.
|
|
|
|
|
2016-06-15 18:43:10 +12:00
|
|
|
Returns numeric_ops::NaN() on each entry if the system could not be solved
|
|
|
|
under
|
2016-03-19 06:57:51 +13:00
|
|
|
the specified determinant threshold.
|
|
|
|
|
|
|
|
\return Whether the affine could be solved under the specified constraints
|
|
|
|
*/
|
|
|
|
template <typename Point>
|
|
|
|
bool affineMap(const Point &p0, const Point &p1, const Point &p2,
|
2016-06-15 18:43:10 +12:00
|
|
|
const Point &q0, const Point &q1, const Point &q2,
|
|
|
|
typename point_traits<Point>::value_type affValues[],
|
|
|
|
typename point_traits<Point>::value_type detTol = 0) {
|
|
|
|
typedef typename point_traits<Point>::value_type value_type;
|
2016-03-19 06:57:51 +13:00
|
|
|
|
2016-06-15 18:43:10 +12:00
|
|
|
// 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;
|
2016-03-19 06:57:51 +13:00
|
|
|
|
2016-06-15 18:43:10 +12:00
|
|
|
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<value_type>();
|
|
|
|
return false;
|
|
|
|
}
|
2016-03-19 06:57:51 +13:00
|
|
|
|
2016-06-15 18:43:10 +12:00
|
|
|
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;
|
2016-03-19 06:57:51 +13:00
|
|
|
|
2016-06-15 18:43:10 +12:00
|
|
|
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;
|
2016-03-19 06:57:51 +13:00
|
|
|
|
2016-06-15 18:43:10 +12:00
|
|
|
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];
|
2016-03-19 06:57:51 +13:00
|
|
|
|
2016-06-15 18:43:10 +12:00
|
|
|
// 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;
|
2016-03-19 06:57:51 +13:00
|
|
|
|
2016-06-15 18:43:10 +12:00
|
|
|
return true;
|
2016-03-19 06:57:51 +13:00
|
|
|
}
|
|
|
|
|
|
|
|
//-------------------------------------------------------------------------------------------
|
|
|
|
|
|
|
|
/*!
|
2016-06-15 18:43:10 +12:00
|
|
|
\brief Calculates the best fit line passing through \p p whose \p n samples
|
|
|
|
have the specified
|
2016-03-19 06:57:51 +13:00
|
|
|
coordinate sums
|
|
|
|
|
2016-06-15 18:43:10 +12:00
|
|
|
\note Returned unnormalized direction \p v may be NaP() in case no
|
|
|
|
preferential direction could
|
2016-03-19 06:57:51 +13:00
|
|
|
be chosen
|
|
|
|
*/
|
|
|
|
template <typename Point>
|
|
|
|
bool bestFit(const Point &p, Point &v,
|
2016-06-15 18:43:10 +12:00
|
|
|
typename point_traits<Point>::value_type sums_x,
|
|
|
|
typename point_traits<Point>::value_type sums_y,
|
|
|
|
typename point_traits<Point>::value_type sums2_x,
|
|
|
|
typename point_traits<Point>::value_type sums2_y,
|
|
|
|
typename point_traits<Point>::value_type sums_xy,
|
|
|
|
typename point_traits<Point>::value_type n) {
|
|
|
|
typedef typename point_traits<Point>::value_type value_type;
|
2016-03-19 06:57:51 +13:00
|
|
|
|
2016-06-15 18:43:10 +12:00
|
|
|
sums_x /= n;
|
|
|
|
sums_y /= n;
|
|
|
|
sums2_x /= n;
|
|
|
|
sums2_y /= n;
|
|
|
|
sums_xy /= n;
|
2016-03-19 06:57:51 +13:00
|
|
|
|
2016-06-15 18:43:10 +12:00
|
|
|
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
|
2016-03-19 06:57:51 +13:00
|
|
|
|
2016-06-15 18:43:10 +12:00
|
|
|
// The best-fit direction is an eigenvector of matrix [a, b; b, c]'s highest
|
|
|
|
// eigenvalue.
|
2016-03-19 06:57:51 +13:00
|
|
|
|
2016-06-15 18:43:10 +12:00
|
|
|
double trace = 0.5 * (a + c);
|
|
|
|
double det = a * c - b * b;
|
2016-03-19 06:57:51 +13:00
|
|
|
|
2016-06-15 18:43:10 +12:00
|
|
|
double traceSq = trace * trace;
|
|
|
|
if (traceSq < det) {
|
|
|
|
v = NaP<Point>();
|
|
|
|
return false;
|
|
|
|
}
|
2016-03-19 06:57:51 +13:00
|
|
|
|
2016-06-15 18:43:10 +12:00
|
|
|
double eigen = trace + sqrt(traceSq - det); // plus takes the greater eigen
|
|
|
|
a -= eigen, c -= eigen;
|
2016-03-19 06:57:51 +13:00
|
|
|
|
2016-06-15 18:43:10 +12:00
|
|
|
// 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;
|
2016-03-19 06:57:51 +13:00
|
|
|
|
2016-06-15 18:43:10 +12:00
|
|
|
return true;
|
2016-03-19 06:57:51 +13:00
|
|
|
}
|
|
|
|
|
|
|
|
//-------------------------------------------------------------------------------------------
|
|
|
|
|
|
|
|
/*!
|
2016-06-15 18:43:10 +12:00
|
|
|
Calculates the best fit line whose n samples have the specified coordinate
|
|
|
|
sums.
|
2016-03-19 06:57:51 +13:00
|
|
|
|
2016-06-15 18:43:10 +12:00
|
|
|
\note Returned point \p p is the samples mean, while (not normalized)
|
|
|
|
direction \p v
|
2016-03-19 06:57:51 +13:00
|
|
|
may be NaP in case no preferential direction could be chosen.
|
|
|
|
*/
|
|
|
|
template <typename Point>
|
|
|
|
bool bestFit(Point &p, Point &v,
|
2016-06-15 18:43:10 +12:00
|
|
|
typename point_traits<Point>::value_type sums_x,
|
|
|
|
typename point_traits<Point>::value_type sums_y,
|
|
|
|
typename point_traits<Point>::value_type sums2_x,
|
|
|
|
typename point_traits<Point>::value_type sums2_y,
|
|
|
|
typename point_traits<Point>::value_type sums_xy,
|
|
|
|
typename point_traits<Point>::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);
|
2016-03-19 06:57:51 +13:00
|
|
|
}
|
|
|
|
|
|
|
|
//-------------------------------------------------------------------------------------------
|
|
|
|
|
|
|
|
/*!
|
|
|
|
Calculates the best fit line of the specified points.
|
|
|
|
This is a mostly overloaded version of bestFit without explicit sums.
|
|
|
|
*/
|
|
|
|
template <typename Point_ref, typename Point, typename point_iterator>
|
2016-06-15 18:43:10 +12:00
|
|
|
bool bestFit(Point_ref p, Point &d, point_iterator begin, point_iterator end) {
|
|
|
|
typedef typename point_traits<Point>::value_type value_type;
|
2016-03-19 06:57:51 +13:00
|
|
|
|
2016-06-15 18:43:10 +12:00
|
|
|
value_type sums_x, sums_y, sums2_x, sums2_y, sums_xy;
|
|
|
|
sums_x = sums_y = sums2_x = sums2_y = sums_xy = 0;
|
2016-03-19 06:57:51 +13:00
|
|
|
|
2016-06-15 18:43:10 +12:00
|
|
|
size_t n = 0;
|
2016-03-19 06:57:51 +13:00
|
|
|
|
2016-06-15 18:43:10 +12:00
|
|
|
point_iterator it;
|
|
|
|
for (it = begin; it != end; ++it, ++n) {
|
|
|
|
const Point &p = *it;
|
2016-03-19 06:57:51 +13:00
|
|
|
|
2016-06-15 18:43:10 +12:00
|
|
|
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;
|
|
|
|
}
|
2016-03-19 06:57:51 +13:00
|
|
|
|
2016-06-15 18:43:10 +12:00
|
|
|
return bestFit(p, d, sums_x, sums_y, sums2_x, sums2_y, sums_xy, n);
|
2016-03-19 06:57:51 +13:00
|
|
|
}
|
|
|
|
}
|
2016-06-15 18:43:10 +12:00
|
|
|
} // namespace tcg::point_ops
|
2016-03-19 06:57:51 +13:00
|
|
|
|
2016-06-15 18:43:10 +12:00
|
|
|
#endif // TCG_POINT_OPS_H
|