OGS
GeometricBasics.cpp
Go to the documentation of this file.
1
10#include "GeometricBasics.h"
11
12#include <Eigen/Dense>
13
14#include "BaseLib/Logging.h"
15#include "Point3d.h"
16
17namespace MathLib
18{
20 MathLib::Point3d const& a,
21 MathLib::Point3d const& b,
22 MathLib::Point3d const& c)
23{
24 Eigen::Vector3d const u = p.asEigenVector3d() - a.asEigenVector3d();
25 Eigen::Vector3d const v = p.asEigenVector3d() - b.asEigenVector3d();
26 Eigen::Vector3d const w = p.asEigenVector3d() - c.asEigenVector3d();
27 return u.cross(v).dot(w);
28}
29
31 MathLib::Point3d const& b,
32 MathLib::Point3d const& c,
33 MathLib::Point3d const& d)
34{
35 Eigen::Vector3d const w = b.asEigenVector3d() - a.asEigenVector3d();
36 Eigen::Vector3d const u = c.asEigenVector3d() - a.asEigenVector3d();
37 Eigen::Vector3d const v = d.asEigenVector3d() - a.asEigenVector3d();
38 return std::abs(u.cross(v).dot(w)) / 6.0;
39}
40
42 MathLib::Point3d const& c)
43{
44 Eigen::Vector3d const u = c.asEigenVector3d() - a.asEigenVector3d();
45 Eigen::Vector3d const v = b.asEigenVector3d() - a.asEigenVector3d();
46 Eigen::Vector3d const w = u.cross(v);
47 return 0.5 * w.norm();
48}
49
51 MathLib::Point3d const& b, MathLib::Point3d const& c,
52 MathLib::Point3d const& d, double eps)
53{
54 double const d0(MathLib::orientation3d(d, a, b, c));
55 // if tetrahedron is not coplanar
56 if (std::abs(d0) > std::numeric_limits<double>::epsilon())
57 {
58 bool const d0_sign(d0 > 0);
59 // if p is on the same side of bcd as a
60 double const d1(MathLib::orientation3d(d, p, b, c));
61 if (!(d0_sign == (d1 >= 0) || std::abs(d1) < eps))
62 {
63 return false;
64 }
65 // if p is on the same side of acd as b
66 double const d2(MathLib::orientation3d(d, a, p, c));
67 if (!(d0_sign == (d2 >= 0) || std::abs(d2) < eps))
68 {
69 return false;
70 }
71 // if p is on the same side of abd as c
72 double const d3(MathLib::orientation3d(d, a, b, p));
73 if (!(d0_sign == (d3 >= 0) || std::abs(d3) < eps))
74 {
75 return false;
76 }
77 // if p is on the same side of abc as d
78 double const d4(MathLib::orientation3d(p, a, b, c));
79 return d0_sign == (d4 >= 0) || std::abs(d4) < eps;
80 }
81 return false;
82}
83
85 MathLib::Point3d const& a,
86 MathLib::Point3d const& b,
87 MathLib::Point3d const& c,
88 double eps_pnt_out_of_plane,
89 double eps_pnt_out_of_tri,
90 MathLib::TriangleTest algorithm)
91{
92 switch (algorithm)
93 {
94 case MathLib::GAUSS:
95 return gaussPointInTriangle(p, a, b, c, eps_pnt_out_of_plane,
96 eps_pnt_out_of_tri);
98 return barycentricPointInTriangle(p, a, b, c, eps_pnt_out_of_plane,
99 eps_pnt_out_of_tri);
100 default:
101 ERR("Selected algorithm for point in triangle testing not found, "
102 "falling back on default.");
103 }
104 return gaussPointInTriangle(p, a, b, c, eps_pnt_out_of_plane,
105 eps_pnt_out_of_tri);
106}
107
109 MathLib::Point3d const& a,
110 MathLib::Point3d const& b,
111 MathLib::Point3d const& c,
112 double eps_pnt_out_of_plane,
113 double eps_pnt_out_of_tri)
114{
115 auto const& pa = a.asEigenVector3d();
116 Eigen::Vector3d const v = b.asEigenVector3d() - pa;
117 Eigen::Vector3d const w = c.asEigenVector3d() - pa;
118
119 Eigen::Matrix2d mat;
120 mat(0, 0) = v.squaredNorm();
121 mat(0, 1) = v[0] * w[0] + v[1] * w[1] + v[2] * w[2];
122 mat(1, 0) = mat(0, 1);
123 mat(1, 1) = w.squaredNorm();
124 Eigen::Vector2d y(
125 v[0] * (q[0] - a[0]) + v[1] * (q[1] - a[1]) + v[2] * (q[2] - a[2]),
126 w[0] * (q[0] - a[0]) + w[1] * (q[1] - a[1]) + w[2] * (q[2] - a[2]));
127
128 y = mat.partialPivLu().solve(y);
129
130 const double lower(eps_pnt_out_of_tri);
131 const double upper(1 + lower);
132
133 if (-lower <= y[0] && y[0] <= upper && -lower <= y[1] && y[1] <= upper &&
134 y[0] + y[1] <= upper)
135 {
136 MathLib::Point3d const q_projected(std::array<double, 3>{
137 {a[0] + y[0] * v[0] + y[1] * w[0], a[1] + y[0] * v[1] + y[1] * w[1],
138 a[2] + y[0] * v[2] + y[1] * w[2]}});
139 if (MathLib::sqrDist(q, q_projected) <= eps_pnt_out_of_plane)
140 {
141 return true;
142 }
143 }
144
145 return false;
146}
147
149 MathLib::Point3d const& a,
150 MathLib::Point3d const& b,
151 MathLib::Point3d const& c,
152 double eps_pnt_out_of_plane,
153 double eps_pnt_out_of_tri)
154{
155 if (std::abs(MathLib::orientation3d(p, a, b, c)) > eps_pnt_out_of_plane)
156 {
157 return false;
158 }
159
160 auto const& vp = p.asEigenVector3d();
161 Eigen::Vector3d const& pa = a.asEigenVector3d() - vp;
162 Eigen::Vector3d const& pb = b.asEigenVector3d() - vp;
163 Eigen::Vector3d const& pc = c.asEigenVector3d() - vp;
164 double const area_x_2(calcTriangleArea(a, b, c) * 2);
165
166 double const alpha((pb.cross(pc).norm()) / area_x_2);
167 if (alpha < -eps_pnt_out_of_tri || alpha > 1 + eps_pnt_out_of_tri)
168 {
169 return false;
170 }
171 double const beta((pc.cross(pa).norm()) / area_x_2);
172 if (beta < -eps_pnt_out_of_tri || beta > 1 + eps_pnt_out_of_tri)
173 {
174 return false;
175 }
176 double const gamma(1 - alpha - beta);
177 return !(gamma < -eps_pnt_out_of_tri || gamma > 1 + eps_pnt_out_of_tri);
178}
179
181 MathLib::Point3d const& a,
182 MathLib::Point3d const& b,
183 MathLib::Point3d const& c)
184{
185 // criterion: p-a = u0 * (b-a) + u1 * (c-a); 0 <= u0, u1 <= 1, u0+u1 <= 1
186 Eigen::Matrix2d mat;
187 mat(0, 0) = b[0] - a[0];
188 mat(0, 1) = c[0] - a[0];
189 mat(1, 0) = b[1] - a[1];
190 mat(1, 1) = c[1] - a[1];
191 Eigen::Vector2d y;
192 y << p[0] - a[0], p[1] - a[1];
193
194 y = mat.partialPivLu().solve(y);
195
196 // check if u0 and u1 fulfills the condition
197 return 0 <= y[0] && y[0] <= 1 && 0 <= y[1] && y[1] <= 1 && y[0] + y[1] <= 1;
198}
199
201 const MathLib::Point3d& c, const MathLib::Point3d& d)
202{
203 for (unsigned x = 0; x < 3; ++x)
204 {
205 const unsigned y = (x + 1) % 3;
206 const double abc =
207 (b[x] - a[x]) * (c[y] - a[y]) - (b[y] - a[y]) * (c[x] - a[x]);
208 const double abd =
209 (b[x] - a[x]) * (d[y] - a[y]) - (b[y] - a[y]) * (d[x] - a[x]);
210
211 if ((abc > 0 && abd < 0) || (abc < 0 && abd > 0))
212 {
213 return true;
214 }
215 }
216 return false;
217}
218
220 const MathLib::Point3d& c, const MathLib::Point3d& d)
221{
222 Eigen::Vector3d const ab = b.asEigenVector3d() - a.asEigenVector3d();
223 Eigen::Vector3d const ac = c.asEigenVector3d() - a.asEigenVector3d();
224 Eigen::Vector3d const ad = d.asEigenVector3d() - a.asEigenVector3d();
225
226 auto const eps_squared =
227 std::pow(std::numeric_limits<double>::epsilon(), 2);
228 if (ab.squaredNorm() < eps_squared || ac.squaredNorm() < eps_squared ||
229 ad.squaredNorm() < eps_squared)
230 {
231 return true;
232 }
233
234 // In exact arithmetic <ac*ad^T, ab> should be zero
235 // if all four points are coplanar.
236 const double sqr_scalar_triple(std::pow(ac.cross(ad).dot(ab), 2));
237 // Due to evaluating the above numerically some cancellation or rounding
238 // can occur. For this reason a normalisation factor is introduced.
239 const double normalisation_factor =
240 (ab.squaredNorm() * ac.squaredNorm() * ad.squaredNorm());
241
242 // tolerance 1e-11 is chosen such that
243 // a = (0,0,0), b=(1,0,0), c=(0,1,0) and d=(1,1,1e-6) are considered as
244 // coplanar
245 // a = (0,0,0), b=(1,0,0), c=(0,1,0) and d=(1,1,1e-5) are considered as not
246 // coplanar
247 return (sqr_scalar_triple / normalisation_factor < 1e-11);
248}
249
250} // end namespace MathLib
void ERR(char const *fmt, Args const &... args)
Definition: Logging.h:44
Definition of the Point3d class.
Eigen::Vector3d const & asEigenVector3d() const
Definition: Point3d.h:67
static const double q
double orientation3d(MathLib::Point3d const &p, MathLib::Point3d const &a, MathLib::Point3d const &b, MathLib::Point3d const &c)
bool gaussPointInTriangle(MathLib::Point3d const &q, MathLib::Point3d const &a, MathLib::Point3d const &b, MathLib::Point3d const &c, double eps_pnt_out_of_plane, double eps_pnt_out_of_tri)
double calcTriangleArea(MathLib::Point3d const &a, MathLib::Point3d const &b, MathLib::Point3d const &c)
bool isCoplanar(const MathLib::Point3d &a, const MathLib::Point3d &b, const MathLib::Point3d &c, const MathLib::Point3d &d)
Checks if the four given points are located on a plane.
static const double p
bool dividedByPlane(const MathLib::Point3d &a, const MathLib::Point3d &b, const MathLib::Point3d &c, const MathLib::Point3d &d)
bool isPointInTriangle(MathLib::Point3d const &p, MathLib::Point3d const &a, MathLib::Point3d const &b, MathLib::Point3d const &c, double eps_pnt_out_of_plane, double eps_pnt_out_of_tri, MathLib::TriangleTest algorithm)
static const double u
bool barycentricPointInTriangle(MathLib::Point3d const &p, MathLib::Point3d const &a, MathLib::Point3d const &b, MathLib::Point3d const &c, double eps_pnt_out_of_plane, double eps_pnt_out_of_tri)
static const double v
double sqrDist(MathLib::Point3d const &p0, MathLib::Point3d const &p1)
Definition: Point3d.cpp:26
double calcTetrahedronVolume(MathLib::Point3d const &a, MathLib::Point3d const &b, MathLib::Point3d const &c, MathLib::Point3d const &d)
bool isPointInTetrahedron(MathLib::Point3d const &p, MathLib::Point3d const &a, MathLib::Point3d const &b, MathLib::Point3d const &c, MathLib::Point3d const &d, double eps)
bool isPointInTriangleXY(MathLib::Point3d const &p, MathLib::Point3d const &a, MathLib::Point3d const &b, MathLib::Point3d const &c)