OGS
MathTools.cpp
Go to the documentation of this file.
1
10#include "MathTools.h"
11
12#include <Eigen/Core>
13#include <cmath>
14
15#include "Point3d.h"
16
17namespace MathLib
18{
19double calcProjPntToLineAndDists(Point3d const& pp, Point3d const& pa,
20 Point3d const& pb, double& lambda, double& d0)
21{
22 auto const& a = pa.asEigenVector3d();
23 auto const& b = pb.asEigenVector3d();
24 auto const& p = pp.asEigenVector3d();
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
42double getAngle(Point3d const& p0, Point3d const& p1, Point3d const& p2)
43{
44 auto const& b = p1.asEigenVector3d();
45 Eigen::Vector3d const v0 = p0.asEigenVector3d() - b;
46 Eigen::Vector3d const v1 = p2.asEigenVector3d() - b;
47
48 // apply Cauchy Schwarz inequality
49 return std::acos(v0.dot(v1) / (v0.norm() * v1.norm()));
50}
51
52} // namespace MathLib
Definition of the Point3d class.
Eigen::Vector3d const & asEigenVector3d() const
Definition Point3d.h:64
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