diff options
Diffstat (limited to 'geometry/distance.hpp')
-rw-r--r-- | geometry/distance.hpp | 38 |
1 files changed, 38 insertions, 0 deletions
diff --git a/geometry/distance.hpp b/geometry/distance.hpp new file mode 100644 index 0000000000..629ae09774 --- /dev/null +++ b/geometry/distance.hpp @@ -0,0 +1,38 @@ +#include "../base/base.hpp" + +// Similarly to namespace m2 - 2d math, this is a namespace for nd math. +namespace mn +{ + +template <typename PointT> class DistanceToLineSquare +{ +public: + DistanceToLineSquare(PointT p0, PointT p1) + : m_P0(p0), m_P1(p1), m_D(m_P1 - m_P0), m_D2(DotProduct(m_D, m_D)), m_InvD2(1.0 / m_D2) + { + } + + double operator () (PointT Y) const + { + PointT const YmP0 = Y - m_P0; + double const t = DotProduct(m_D, YmP0); + if (t <= 0) + { + // Y is closest to P0. + return DotProduct(YmP0, YmP0); + } + if (t >= m_D2) + { + // Y is closest to P1. + PointT const YmP1 = Y - m_P1; + return DotProduct(YmP1, YmP1); + } + // Closest point is interior to segment. + return DotProduct(YmP0, YmP0) - t * t * m_InvD2; + } +private: + PointT m_P0, m_P1, m_D; + double m_D2, m_InvD2; +}; + +} |