OGS
MathTools.cpp
Go to the documentation of this file.
1 
10 #include "MathTools.h"
11 
12 #include <Eigen/Dense>
13 #include <cmath>
14 
15 #include "Point3d.h"
16 
17 namespace MathLib
18 {
19 double calcProjPntToLineAndDists(Point3d const& pp, Point3d const& pa,
20  Point3d const& pb, double& lambda, double& d0)
21 {
22  auto const a = Eigen::Map<Eigen::Vector3d const>(pa.getCoords());
23  auto const b = Eigen::Map<Eigen::Vector3d const>(pb.getCoords());
24  auto const p = Eigen::Map<Eigen::Vector3d const>(pp.getCoords());
25 
26  // g(lambda) = a + lambda v, v = b-a
27  Eigen::Vector3d const v = b - a;
28 
29  // orthogonal projection: (p - g(lambda))^T * v = 0
30  // <=> (a-p - lambda (b-a))^T * (b-a) = 0
31  // <=> (a-p)^T * (b-a) = lambda (b-a)^T ) (b-a)
32  lambda = (((p - a).transpose() * v) / v.squaredNorm())(0, 0);
33 
34  // compute projected point
35  Eigen::Vector3d const proj_pnt = a + lambda * v;
36 
37  d0 = (proj_pnt - a).norm();
38 
39  return (p - proj_pnt).norm();
40 }
41 
42 double getAngle(Point3d const& p0, Point3d const& p1, Point3d const& p2)
43 {
44  auto const a = Eigen::Map<Eigen::Vector3d const>(p0.getCoords());
45  auto const b = Eigen::Map<Eigen::Vector3d const>(p1.getCoords());
46  auto const c = Eigen::Map<Eigen::Vector3d const>(p2.getCoords());
47  Eigen::Vector3d const v0 = a - b;
48  Eigen::Vector3d const v1 = c - b;
49 
50  // apply Cauchy Schwarz inequality
51  return std::acos(v0.dot(v1) / (v0.norm() * v1.norm()));
52 }
53 
54 } // namespace MathLib
Definition of the Point3d class.
const T * getCoords() const
Definition: TemplatePoint.h:75
double norm(MatrixOrVector const &x, MathLib::VecNormType type)
Definition: LinAlg.h:88
static const double p
double calcProjPntToLineAndDists(Point3d const &pp, Point3d const &pa, Point3d const &pb, double &lambda, double &d0)
Definition: MathTools.cpp:19
double getAngle(Point3d const &p0, Point3d const &p1, Point3d const &p2)
Definition: MathTools.cpp:42
static const double v