[MaL] Reimpl. calcProjPntToLineAndDists using Eigen.

MathLib/MathTools.cpp

@@ -7,32 +7,39 @@
  *              http://www.opengeosys.org/project/license
  */
 
+#include "MathTools.h"
+
+#include <Eigen/Dense>
 #include <cmath>
 
-#include "MathTools.h"
+#include "Point3d.h"
 
 namespace MathLib
 {
 
-double calcProjPntToLineAndDists(const double p[3], const double a[3],
-        const double b[3], double &lambda, double &d0)
+double calcProjPntToLineAndDists(Point3d const& pp, Point3d const& pa,
+                                 Point3d const& pb, double& lambda, double& d0)
 {
-    // g (lambda) = a + lambda v, v = b-a
-    double v[3] = {b[0] - a[0], b[1] - a[1], b[2] - a[2]};
-    // orthogonal projection: (g(lambda)-p) * v = 0 => in order to compute lambda we define a help vector u
-    double u[3] = {p[0] - a[0], p[1] - a[1], p[2] - a[2]};
-    lambda = scalarProduct<double,3> (u, v) / scalarProduct<double,3> (v, v);
+    auto const a =
+        Eigen::Map<Eigen::Vector3d>(const_cast<double*>(pa.getCoords()));
+    auto const b =
+        Eigen::Map<Eigen::Vector3d>(const_cast<double*>(pb.getCoords()));
+    auto const p =
+        Eigen::Map<Eigen::Vector3d>(const_cast<double*>(pp.getCoords()));
+    // g(lambda) = a + lambda v, v = b-a
+    Eigen::Vector3d const v = b - a;
+
+    // orthogonal projection: (p - g(lambda))^T * v = 0
+    // <=> (a-p - lambda (b-a))^T * (b-a) = 0
+    // <=> (a-p)^T * (b-a) = lambda (b-a)^T ) (b-a)
+    lambda = (((p - a).transpose() * v) / v.squaredNorm())(0, 0);
 
     // compute projected point
-    double proj_pnt[3];
-    for (std::size_t k(0); k < 3; k++)
-    {
-        proj_pnt[k] = a[k] + lambda * v[k];
-    }
+    Eigen::Vector3d const proj_pnt = a + lambda * v;
 
-    d0 = std::sqrt (sqrDist (proj_pnt, a));
+    d0 = (proj_pnt - a).norm();
 
-    return std::sqrt (sqrDist (p, proj_pnt));
+    return (p - proj_pnt).norm();
 }
 
 double getAngle (const double p0[3], const double p1[3], const double p2[3])

MathLib/MathTools.h

@@ -17,6 +17,9 @@
 
 namespace MathLib
 {
+template <typename T, std::size_t DIM> class TemplatePoint;
+using Point3d = MathLib::TemplatePoint<double, 3>;
+
 /**
  * standard inner product in R^N
  * \param v0 array of type T representing the vector
@@ -65,16 +68,18 @@ inline T scalarProduct(T const* const v0, T const* const v1, int const n)
  * \f$g(\lambda) = a + \lambda (b - a)\f$,
  * the distance between p and the projected point
  * and the distances between the projected point and the end
- * points a, b of the line
- * \param p the (mesh) point
- * \param a first point of line
- * \param b second point of line
+ * points pa, pb of the line
+ * \param pp the (mesh) point
+ * \param pa first point of line
+ * \param pb second point of line
  * \param lambda the projected point described by the line equation above
  * \param d0 distance to the line point a
- * \returns the distance between p and the orthogonal projection of p
+ * \returns the distance between pp and the orthogonal projection of pp
  */
-double calcProjPntToLineAndDists(const double p[3], const double a[3],
-                                 const double b[3], double &lambda, double &d0);
+double calcProjPntToLineAndDists(MathLib::Point3d const& pp,
+                                 MathLib::Point3d const& pa,
+                                 MathLib::Point3d const& pb, double& lambda,
+                                 double& d0);
 
 /** squared dist between double arrays p0 and p1 (size of arrays is 3) */
 inline