GCC Code Coverage Report

Directory: ./
File: lib/geogram/basic/geometry_nd.h
Date: 2026-09-07 02:28:19
Exec Total Coverage
Lines: 155 197 78.7%
Functions: 17 54 31.5%
Branches: 92 132 69.7%
Line Branch Exec Source
1 /*
2 * Copyright (c) 2000-2022 Inria
3 * All rights reserved.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions are met:
7 *
8 * * Redistributions of source code must retain the above copyright notice,
9 * this list of conditions and the following disclaimer.
10 * * Redistributions in binary form must reproduce the above copyright notice,
11 * this list of conditions and the following disclaimer in the documentation
12 * and/or other materials provided with the distribution.
13 * * Neither the name of the ALICE Project-Team nor the names of its
14 * contributors may be used to endorse or promote products derived from this
15 * software without specific prior written permission.
16 *
17 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
21 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27 * POSSIBILITY OF SUCH DAMAGE.
28 *
29 * Contact: Bruno Levy
30 *
31 * https://www.inria.fr/fr/bruno-levy
32 *
33 * Inria,
34 * Domaine de Voluceau,
35 * 78150 Le Chesnay - Rocquencourt
36 * FRANCE
37 *
38 */
39
40 #ifndef GEOGRAM_BASIC_GEOMETRY_ND
41 #define GEOGRAM_BASIC_GEOMETRY_ND
42
43 #include <geogram/basic/common.h>
44 #include <geogram/basic/geometry.h>
45 #include <geogram/basic/memory.h>
46
47 /**
48 * \file geogram/basic/geometry_nd.h
49 * \brief Geometric functions in arbitrary dimension
50 */
51
52 namespace GEO {
53
54 namespace Geom {
55
56 /**
57 * \brief Computes the squared distance between two nd points.
58 * \param[in] p1 a pointer to the coordinates of the first point
59 * \param[in] p2 a pointer to the coordinates of the second point
60 * \param[in] dim dimension (number of coordinates of the points)
61 * \return the squared distance between \p p1 and \p p2
62 * \tparam COORD_T the numeric type of the point coordinates
63 */
64 template <class COORD_T>
65 inline double distance2(
66 const COORD_T* p1, const COORD_T* p2, coord_index_t dim
67 ) {
68 double result = 0.0;
69
30/58
✓ Branch 0 taken 724965394 times.
✓ Branch 1 taken 165297516 times.
✓ Branch 2 taken 193851 times.
✓ Branch 3 taken 64617 times.
✓ Branch 4 taken 87938841 times.
✓ Branch 5 taken 29312947 times.
✓ Branch 6 taken 22602624 times.
✓ Branch 7 taken 7534208 times.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✓ Branch 12 taken 352821114 times.
✓ Branch 13 taken 58803519 times.
✓ Branch 14 taken 91022610 times.
✓ Branch 15 taken 15170435 times.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 31 not taken.
✓ Branch 32 taken 43903008 times.
✓ Branch 33 taken 14634336 times.
✓ Branch 34 taken 5698095 times.
✓ Branch 35 taken 1899365 times.
✓ Branch 36 taken 89731840 times.
✓ Branch 37 taken 22432960 times.
✓ Branch 38 taken 11246712 times.
✓ Branch 39 taken 2811678 times.
✓ Branch 40 taken 141657732 times.
✓ Branch 41 taken 23609622 times.
✓ Branch 42 taken 18585828 times.
✓ Branch 43 taken 3097638 times.
✓ Branch 44 taken 189469408 times.
✓ Branch 45 taken 23683676 times.
✓ Branch 46 taken 25110328 times.
✓ Branch 47 taken 3138791 times.
✗ Branch 48 not taken.
✗ Branch 49 not taken.
✗ Branch 50 not taken.
✗ Branch 51 not taken.
✗ Branch 52 not taken.
✗ Branch 53 not taken.
✗ Branch 54 not taken.
✗ Branch 55 not taken.
✓ Branch 56 taken 295395810 times.
✓ Branch 57 taken 52280469 times.
 2524114972 for(coord_index_t i = 0; i < dim; i++) {
70 2100343195 result += GEO::geo_sqr(double(p2[i]) - double(p1[i]));
71 }
72 return result;
73 }
74
75 /**
76 * \brief Computes the distance between two nd points.
77 * \param[in] p1 a pointer to the coordinates of the first point
78 * \param[in] p2 a pointer to the coordinates of the second point
79 * \param[in] dim dimension (number of coordinates of the points)
80 * \return the distance between \p p1 and \p p2
81 * \tparam COORD_T the numeric type of the point coordinates
82 */
83 template <class COORD_T>
84 54433269 inline double distance(
85 const COORD_T* p1, const COORD_T* p2, coord_index_t dim
86 ) {
87 54433269 return ::sqrt(distance2(p1, p2, dim));
88 }
89
90 /**
91 * \brief Computes the squared distance between two nd points.
92 * \param[in] p1 first point
93 * \param[in] p2 second point
94 * \tparam VEC the class that represents the points. VEC needs to
95 * implement data(), that returns a pointer to the coordinates
96 * of the point.
97 * \return the squared distance between \p p1 and \p p2
98 */
99 template <class VEC>
100 inline double distance2(
101 const VEC& p1, const VEC& p2
102 ) {
103 geo_debug_assert(p1.dimension() == p2.dimension());
104 return distance2(
105 p1.data(), p2.data(), coord_index_t(p1.dimension())
106 );
107 }
108
109 /**
110
111 * \brief Computes the distance between two nd points.
112 * \param[in] p1 first point
113 * \param[in] p2 second point
114 * \tparam VEC the class that represents the points. VEC needs to
115 * implement data(), that returns a pointer to the coordinates
116 * of the point.
117 * \return the distance between \p p1 and \p p2
118 */
119 template <class VEC>
120 inline double distance(
121 const VEC& p1, const VEC& p2
122 ) {
123 geo_debug_assert(p1.dimension() == p2.dimension());
124 ✗ return distance(p1.data(), p2.data(), coord_index_t(p1.dimension()));
125 }
126
127 /**
128 * \brief Computes the area of a nd triangle
129 * \details Uses Heron formula (that computes the area
130 * from the lengths of the three edges).
131 * \param[in] p1 a pointer to the coordinates of the
132 * first vertex of the triangle
133 * \param[in] p2 a pointer to the coordinates of the
134 * second vertex of the triangle
135 * \param[in] p3 a pointer to the coordinates of the
136 * third vertex of the triangle
137 * \param[in] dim dimension of the points
138 * \tparam COORD_T the numeric type that represents the coordinates
139 * of the points
140 * \return the area of triangle ( \p p1, \p p2, \p p3)
141 */
142 template <class COORD_T>
143 8608939 inline double triangle_area(
144 const COORD_T* p1,
145 const COORD_T* p2,
146 const COORD_T* p3,
147 coord_index_t dim
148 ) {
149 8608939 double a = distance(p1, p2, dim);
150 8608939 double b = distance(p2, p3, dim);
151 8608939 double c = distance(p3, p1, dim);
152 8608939 double s = double(0.5) * (a + b + c);
153 8608939 double A2 = s * (s - a) * (s - b) * (s - c);
154 // the max is there to avoid some numerical problems.
155 8608939 return ::sqrt(std::max(A2, 0.0));
156 }
157
158 /**
159 * \brief Computes the centroid of a 3d triangle with weighted points.
160 * \details The integrated weight varies linearly in the triangle.
161 * \param[in] p a pointer to the coordinates of the
162 * first vertex of the triangle
163 * \param[in] q a pointer to the coordinates of the
164 * second vertex of the triangle
165 * \param[in] r a pointer to the coordinates of the
166 * third vertex of the triangle
167 * \param[in] a the weight associated with vertex \p p
168 * \param[in] b the weight associated with vertex \p q
169 * \param[in] c the weight associated with vertex \p r
170 * \param[out] Vg the total weight times the centroid (
171 * a pointer to a caller-allocated array of dim COORD_T%s)
172 * \param[out] V the total weight
173 * \param[in] dim the dimension of the vertices
174 * \tparam COORD_T the numeric type that represents the coordinates
175 * of the points
176 */
177 template <class COORD_T>
178 ✗ inline void triangle_centroid(
179 const COORD_T* p,
180 const COORD_T* q,
181 const COORD_T* r,
182 COORD_T a, COORD_T b, COORD_T c,
183 double* Vg,
184 double& V,
185 coord_index_t dim
186 ) {
187 ✗ double abc = a + b + c;
188 ✗ double area = Geom::triangle_area(p, q, r, dim);
189 ✗ V = area / 3.0 * abc;
190 ✗ double wp = a + abc;
191 ✗ double wq = b + abc;
192 ✗ double wr = c + abc;
193 ✗ double s = area / 12.0;
194 ✗ for(coord_index_t i = 0; i < dim; i++) {
195 ✗ Vg[i] = s * (wp * p[i] + wq * q[i] + wr * r[i]);
196 }
197 ✗ }
198
199 /********************************************************************/
200
201 /**
202 * \brief Computes the area of a nd triangle.
203 * \details Uses Heron formula (that compures the area
204 * from the lengths of the three edges).
205 * \param[in] p1 first vertex of the triangle
206 * \param[in] p2 second vertex of the triangle
207 * \param[in] p3 third vertex of the triangle
208 * \tparam VEC the class used to represent the vertices of the triangle
209 * \return the area of triangle (\p p1, \p p2, \p p3)
210 */
211 template <class VEC>
212 29360 inline double triangle_area(
213 const VEC& p1, const VEC& p2, const VEC& p3
214 ) {
215 // Heron formula
216 double a = distance(p1, p2);
217 double b = distance(p2, p3);
218 double c = distance(p3, p1);
219 29360 double s = double(0.5) * (a + b + c);
220 29360 return ::sqrt(s * (s - a) * (s - b) * (s - c));
221 }
222
223 /**
224 * \brief Computes the mass of a nd triangle with weighted points.
225 * \details The integrated weight varies linearly in the triangle.
226 * \param[in] p first vertex of the triangle
227 * \param[in] q second vertex of the triangle
228 * \param[in] r third vertex of the triangle
229 * \param[in] a the weight associated with vertex \p p
230 * \param[in] b the weight associated with vertex \p q
231 * \param[in] c the weight associated with vertex \p r
232 * \tparam VEC the class used to represent the vertices of the triangle
233 * \return the mass of the weighted triangle ( \p p, \p a),
234 * ( \p q, \p b), ( \p r, \p c)
235 */
236 template <class VEC>
237 ✗ inline double triangle_mass(
238 const VEC& p, const VEC& q, const VEC& r,
239 double a, double b, double c
240 ) {
241 // TODO: try to better understand the formula and
242 // determine why there are these sqrt's
243 // (probably due to the relation between the
244 // user-provided density and the one achieved
245 // by CVT), but I'm pretty sure that the formula
246 // is correct (at least, dimensions match).
247 // Note: the ::fabs() are there to avoid numerical
248 // errors.
249 ✗ return Geom::triangle_area(p, q, r) / 3.0 * (
250 ✗ ::sqrt(::fabs(a)) + sqrt(::fabs(b)) + sqrt(::fabs(c))
251 ✗ );
252 }
253
254 /**
255 * \brief Computes the center of the circumscribed circle of
256 * a nd triangle.
257 * \param[in] Q1 first vertex of the triangle
258 * \param[in] Q2 second vertex of the triangle
259 * \param[in] Q3 third vertex of the triangle
260 * \param[out] denom if the parameter is non null, it is set to the
261 * denominator of the barycentric coordinates of the circumcenter.
262 * \tparam POINT the class used to represent the vertices
263 * of the triangle
264 * \return the circumcenter of the triangle (\p p1, \p p2, \p p3).
265 */
266 template <class POINT>
267 POINT triangle_circumcenter(
268 const POINT& Q1,
269 const POINT& Q2,
270 const POINT& Q3,
271 double* denom = nullptr
272 ) {
273 const POINT q2 = Q2 - Q1;
274 const POINT q3 = Q3 - Q1;
275
276 double l2 = length2(q2);
277 double l3 = length2(q3);
278
279 double a12 = -2.0 * dot(q2, q2);
280 double a13 = -2.0 * dot(q3, q2);
281 double a22 = -2.0 * dot(q2, q3);
282 double a23 = -2.0 * dot(q3, q3);
283
284 double c31 = (a23 * a12 - a22 * a13);
285 double d = c31;
286 double s = 1.0 / d;
287 double lambda1 = s * ((a23 - a22) * l2 + (a12 - a13) * l3 + c31);
288 double lambda2 = s * ((-a23) * l2 + (a13) * l3);
289 double lambda3 = s * ((a22) * l2 + (-a12) * l3);
290 if(denom != nullptr) {
291 *denom = d;
292 }
293 return lambda1 * Q1 + lambda2 * Q2 + lambda3 * Q3;
294 }
295
296 /**
297 * \brief Computes the centroid of a nd triangle with weighted points.
298 * \details The integrated weight varies linearly in the triangle.
299 * \param[in] p first vertex of the triangle
300 * \param[in] q second vertex of the triangle
301 * \param[in] r third vertex of the triangle
302 * \param[in] a the weight associated with vertex \p p
303 * \param[in] b the weight associated with vertex \p q
304 * \param[in] c the weight associated with vertex \p r
305 * \param[out] Vg the total weight times the centroid
306 * \param[out] V the total weight
307 * \tparam VEC the class used to represent the vertices
308 * of the triangle
309 */
310 template <class VEC>
311 inline void triangle_centroid(
312 const VEC& p, const VEC& q, const VEC& r,
313 double a, double b, double c,
314 VEC& Vg, double& V
315 ) {
316 double abc = a + b + c;
317 double area = Geom::triangle_area(p, q, r);
318 V = area / 3.0 * abc;
319 double wp = a + abc;
320 double wq = b + abc;
321 double wr = c + abc;
322 double s = area / 12.0;
323 Vg = s * (wp * p + wq * q + wr * r);
324 }
325
326 /**
327 * \brief Generates a random point in a nd triangle.
328 * \details Uses Greg Turk's second method
329 * (see article in Graphic Gems).
330 * \param[in] p1 first vertex of the triangle
331 * \param[in] p2 second vertex of the triangle
332 * \param[in] p3 third vertex of the triangle
333 * \return a random point in triangle ( \p p1, \p p2, \p p3 )
334 * \tparam VEC the class used to represent the vertices
335 * of the triangle
336 */
337 template <class VEC>
338 40000 inline VEC random_point_in_triangle(
339 const VEC& p1, const VEC& p2, const VEC& p3
340 ) {
341 40000 double l1 = Numeric::random_float64();
342 40000 double l2 = Numeric::random_float64();
343
2/2
✓ Branch 0 taken 10100 times.
✓ Branch 1 taken 9900 times.
 40000 if(l1 + l2 > 1.0) {
344 20200 l1 = 1.0 - l1;
345 20200 l2 = 1.0 - l2;
346 }
347 40000 double l3 = 1.0 - l1 - l2;
348 40000 return l1 * p1 + l2 * p2 + l3 * p3;
349 }
350
351 /**
352 * \brief Generates a random point in a nd tetrahedron.
353 * \details Uses Greg Turk's second method
354 * (see article in Graphic Gems).
355 * \param[in] p1 first vertex of the triangle
356 * \param[in] p2 second vertex of the triangle
357 * \param[in] p3 third vertex of the triangle
358 * \param[in] p4 fourth vertex of the triangle
359 * \return a random point in tetrahedron ( \p p1, \p p2, \p p3, \p p4)
360 * \tparam VEC the class used to represent the vertices
361 * of the triangle
362 */
363 template <class VEC>
364 2400 inline VEC random_point_in_tetra(
365 const VEC& p1, const VEC& p2, const VEC& p3, const VEC& p4
366 ) {
367 2400 double s = Numeric::random_float64();
368 2400 double t = Numeric::random_float64();
369 2400 double u = Numeric::random_float64();
370
2/2
✓ Branch 0 taken 584 times.
✓ Branch 1 taken 616 times.
 2400 if(s + t > 1.0) {
371 1168 s = 1.0 - s;
372 1168 t = 1.0 - t;
373 }
374
2/2
✓ Branch 0 taken 412 times.
✓ Branch 1 taken 788 times.
 2400 if(t + u > 1.0) {
375 double tmp = u;
376 824 u = 1.0 - s - t;
377 824 t = 1.0 - tmp;
378
2/2
✓ Branch 0 taken 448 times.
✓ Branch 1 taken 340 times.
 1576 } else if(s + t + u > 1.0) {
379 double tmp = u;
380 896 u = s + t + u - 1.0;
381 896 s = 1.0 - t - tmp;
382 }
383 2400 double a = 1.0 - s - t - u;
384 2400 return a * p1 + s * p2 + t * p3 + u * p4;
385 }
386
387 /**
388 * \brief Computes the point closest to a given point in a nd segment
389 * \param[in] point the query point
390 * \param[in] V0 first extremity of the segment
391 * \param[in] V1 second extremity of the segment
392 * \param[out] closest_point the point closest to \p point in the
393 * segment [\p V0, \p V1]
394 * \param[out] lambda0 barycentric coordinate of the closest point
395 * relative to \p V0
396 * \param[out] lambda1 barycentric coordinate of the closest point
397 * relative to \p V1
398 * \tparam VEC the class that represents the points.
399 * \return the squared distance between the point and
400 * the segment [\p V0, \p V1]
401 */
402 template <class VEC>
403
1/2
✓ Branch 0 taken 388 times.
✗ Branch 1 not taken.
 388 inline double point_segment_squared_distance(
404 const VEC& point,
405 const VEC& V0,
406 const VEC& V1,
407 VEC& closest_point,
408 double& lambda0,
409 double& lambda1
410 ) {
411 double l2 = distance2(V0,V1);
412 ✗ double t = dot(point - V0, V1 - V0);
413
2/4
✓ Branch 0 taken 388 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 388 times.
 388 if(t <= 0.0 || l2 == 0.0) {
414 ✗ closest_point = V0;
415 ✗ lambda0 = 1.0;
416 ✗ lambda1 = 0.0;
417 ✗ return distance2(point, V0);
418
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 388 times.
 388 } else if(t > l2) {
419 ✗ closest_point = V1;
420 ✗ lambda0 = 0.0;
421 ✗ lambda1 = 1.0;
422 ✗ return distance2(point, V1);
423 }
424 388 lambda1 = t / l2;
425 388 lambda0 = 1.0-lambda1;
426 388 closest_point = lambda0 * V0 + lambda1 * V1;
427 388 return distance2(point, closest_point);
428 }
429
430
431 /**
432 * \brief Computes the point closest to a given point in a nd segment
433 * \param[in] point the query point
434 * \param[in] V0 first extremity of the segment
435 * \param[in] V1 second extremity of the segment
436 * \tparam VEC the class that represents the points.
437 * \return the squared distance between the point and
438 * the segment [\p V0, \p V1]
439 */
440 template <class VEC>
441 inline double point_segment_squared_distance(
442 const VEC& point,
443 const VEC& V0,
444 const VEC& V1
445 ) {
446 VEC closest_point;
447 double lambda0;
448 double lambda1;
449 return point_segment_squared_distance(
450 point, V0, V1, closest_point, lambda0, lambda1
451 );
452 }
453
454 /**
455 * \brief Computes the point closest to a given point in a nd triangle
456 * \details See
457 * http://www.geometrictools.com/LibMathematics/Distance/Distance.html
458 * \param[in] point the query point
459 * \param[in] V0 first vertex of the triangle
460 * \param[in] V1 second vertex of the triangle
461 * \param[in] V2 third vertex of the triangle
462 * \param[out] closest_point the point closest to \p point in the
463 * triangle (\p V0, \p V1, \p V2)
464 * \param[out] lambda0 barycentric coordinate of the closest point
465 * relative to \p V0
466 * \param[out] lambda1 barycentric coordinate of the closest point
467 * relative to \p V1
468 * \param[out] lambda2 barycentric coordinate of the closest point
469 * relative to \p V2
470 * \tparam VEC the class that represents the points.
471 * \return the squared distance between the point and
472 * the triangle (\p V0, \p V1, \p V2)
473 */
474 template <class VEC>
475
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 2517568 times.
 2517568 inline double point_triangle_squared_distance(
476 const VEC& point,
477 const VEC& V0,
478 const VEC& V1,
479 const VEC& V2,
480 VEC& closest_point,
481 double& lambda0, double& lambda1, double& lambda2
482 ) {
483 VEC diff = V0 - point;
484 VEC edge0 = V1 - V0;
485 VEC edge1 = V2 - V0;
486 double a00 = length2(edge0);
487 double a01 = dot(edge0, edge1);
488 double a11 = length2(edge1);
489 double b0 = dot(diff, edge0);
490 double b1 = dot(diff, edge1);
491 double c = length2(diff);
492 2517568 double det = ::fabs(a00 * a11 - a01 * a01);
493 2517568 double s = a01 * b1 - a11 * b0;
494 2517568 double t = a01 * b0 - a00 * b1;
495 double sqrDistance;
496
497 // If the triangle is degenerate
498
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 2517568 times.
 2517568 if(det < 1e-30) {
499 double cur_l1, cur_l2;
500 VEC cur_closest;
501 double result;
502 ✗ double cur_dist = point_segment_squared_distance(
503 point, V0, V1, cur_closest, cur_l1, cur_l2
504 );
505 result = cur_dist;
506 ✗ closest_point = cur_closest;
507 ✗ lambda0 = cur_l1;
508 ✗ lambda1 = cur_l2;
509 ✗ lambda2 = 0.0;
510 ✗ cur_dist = point_segment_squared_distance(
511 point, V0, V2, cur_closest, cur_l1, cur_l2
512 );
513 ✗ if(cur_dist < result) {
514 result = cur_dist;
515 ✗ closest_point = cur_closest;
516 ✗ lambda0 = cur_l1;
517 ✗ lambda2 = cur_l2;
518 ✗ lambda1 = 0.0;
519 }
520 ✗ cur_dist = point_segment_squared_distance(
521 point, V1, V2, cur_closest, cur_l1, cur_l2
522 );
523 ✗ if(cur_dist < result) {
524 result = cur_dist;
525 ✗ closest_point = cur_closest;
526 ✗ lambda1 = cur_l1;
527 ✗ lambda2 = cur_l2;
528 ✗ lambda0 = 0.0;
529 }
530 return result;
531 }
532
533
2/2
✓ Branch 0 taken 2031922 times.
✓ Branch 1 taken 485646 times.
 2517568 if(s + t <= det) {
534
2/2
✓ Branch 0 taken 591026 times.
✓ Branch 1 taken 1440896 times.
 2031922 if(s < 0.0) {
535
2/2
✓ Branch 0 taken 18463 times.
✓ Branch 1 taken 572563 times.
 591026 if(t < 0.0) { // region 4
536
2/2
✓ Branch 0 taken 1329 times.
✓ Branch 1 taken 17134 times.
 18463 if(b0 < 0.0) {
537 t = 0.0;
538
2/2
✓ Branch 0 taken 717 times.
✓ Branch 1 taken 612 times.
 1329 if(-b0 >= a00) {
539 s = 1.0;
540 717 sqrDistance = a00 + 2.0 * b0 + c;
541 } else {
542 612 s = -b0 / a00;
543 612 sqrDistance = b0 * s + c;
544 }
545 } else {
546 s = 0.0;
547
2/2
✓ Branch 0 taken 1485 times.
✓ Branch 1 taken 15649 times.
 17134 if(b1 >= 0.0) {
548 t = 0.0;
549 sqrDistance = c;
550
2/2
✓ Branch 0 taken 12 times.
✓ Branch 1 taken 1473 times.
 1485 } else if(-b1 >= a11) {
551 t = 1.0;
552 12 sqrDistance = a11 + 2.0 * b1 + c;
553 } else {
554 1473 t = -b1 / a11;
555 1473 sqrDistance = b1 * t + c;
556 }
557 }
558 } else { // region 3
559 s = 0.0;
560
2/2
✓ Branch 0 taken 560871 times.
✓ Branch 1 taken 11692 times.
 572563 if(b1 >= 0.0) {
561 t = 0.0;
562 sqrDistance = c;
563
2/2
✓ Branch 0 taken 13311 times.
✓ Branch 1 taken 547560 times.
 560871 } else if(-b1 >= a11) {
564 t = 1.0;
565 13311 sqrDistance = a11 + 2.0 * b1 + c;
566 } else {
567 547560 t = -b1 / a11;
568 547560 sqrDistance = b1 * t + c;
569 }
570 }
571
2/2
✓ Branch 0 taken 492434 times.
✓ Branch 1 taken 948462 times.
 1440896 } else if(t < 0.0) { // region 5
572 t = 0.0;
573
2/2
✓ Branch 0 taken 480071 times.
✓ Branch 1 taken 12363 times.
 492434 if(b0 >= 0.0) {
574 s = 0.0;
575 sqrDistance = c;
576
2/2
✓ Branch 0 taken 13507 times.
✓ Branch 1 taken 466564 times.
 480071 } else if(-b0 >= a00) {
577 s = 1.0;
578 13507 sqrDistance = a00 + 2.0 * b0 + c;
579 } else {
580 466564 s = -b0 / a00;
581 466564 sqrDistance = b0 * s + c;
582 }
583 } else { // region 0
584 // minimum at interior point
585 948462 double invDet = double(1.0) / det;
586 948462 s *= invDet;
587 948462 t *= invDet;
588 948462 sqrDistance = s * (a00 * s + a01 * t + 2.0 * b0) +
589 948462 t * (a01 * s + a11 * t + 2.0 * b1) + c;
590 }
591 } else {
592 double tmp0, tmp1, numer, denom;
593
594
2/2
✓ Branch 0 taken 22887 times.
✓ Branch 1 taken 462759 times.
 485646 if(s < 0.0) { // region 2
595 22887 tmp0 = a01 + b0;
596 22887 tmp1 = a11 + b1;
597
2/2
✓ Branch 0 taken 1960 times.
✓ Branch 1 taken 20927 times.
 22887 if(tmp1 > tmp0) {
598 1960 numer = tmp1 - tmp0;
599 1960 denom = a00 - 2.0 * a01 + a11;
600
2/2
✓ Branch 0 taken 600 times.
✓ Branch 1 taken 1360 times.
 1960 if(numer >= denom) {
601 s = 1.0;
602 t = 0.0;
603 600 sqrDistance = a00 + 2.0 * b0 + c;
604 } else {
605 1360 s = numer / denom;
606 1360 t = 1.0 - s;
607 1360 sqrDistance = s * (a00 * s + a01 * t + 2.0 * b0) +
608 1360 t * (a01 * s + a11 * t + 2.0 * b1) + c;
609 }
610 } else {
611 s = 0.0;
612
2/2
✓ Branch 0 taken 18080 times.
✓ Branch 1 taken 2847 times.
 20927 if(tmp1 <= 0.0) {
613 t = 1.0;
614 18080 sqrDistance = a11 + 2.0 * b1 + c;
615 }
616
2/2
✓ Branch 0 taken 2836 times.
✓ Branch 1 taken 11 times.
 2847 else if(b1 >= 0.0) {
617 t = 0.0;
618 sqrDistance = c;
619 } else {
620 2836 t = -b1 / a11;
621 2836 sqrDistance = b1 * t + c;
622 }
623 }
624
2/2
✓ Branch 0 taken 19825 times.
✓ Branch 1 taken 442934 times.
 462759 } else if(t < 0.0) { // region 6
625 19825 tmp0 = a01 + b1;
626 19825 tmp1 = a00 + b0;
627
2/2
✓ Branch 0 taken 709 times.
✓ Branch 1 taken 19116 times.
 19825 if(tmp1 > tmp0) {
628 709 numer = tmp1 - tmp0;
629 709 denom = a00 - 2.0 * a01 + a11;
630
2/2
✓ Branch 0 taken 50 times.
✓ Branch 1 taken 659 times.
 709 if(numer >= denom) {
631 t = 1.0;
632 s = 0.0;
633 50 sqrDistance = a11 + 2.0 * b1 + c;
634 } else {
635 659 t = numer / denom;
636 659 s = 1.0 - t;
637 659 sqrDistance = s * (a00 * s + a01 * t + 2.0 * b0) +
638 659 t * (a01 * s + a11 * t + 2.0 * b1) + c;
639 }
640 } else {
641 t = 0.0;
642
2/2
✓ Branch 0 taken 18129 times.
✓ Branch 1 taken 987 times.
 19116 if(tmp1 <= 0.0) {
643 s = 1.0;
644 18129 sqrDistance = a00 + 2.0 * b0 + c;
645
2/2
✓ Branch 0 taken 955 times.
✓ Branch 1 taken 32 times.
 987 } else if(b0 >= 0.0) {
646 s = 0.0;
647 sqrDistance = c;
648 } else {
649 955 s = -b0 / a00;
650 955 sqrDistance = b0 * s + c;
651 }
652 }
653 } else { // region 1
654 442934 numer = a11 + b1 - a01 - b0;
655
2/2
✓ Branch 0 taken 13739 times.
✓ Branch 1 taken 429195 times.
 442934 if(numer <= 0.0) {
656 s = 0.0;
657 t = 1.0;
658 13739 sqrDistance = a11 + 2.0 * b1 + c;
659 } else {
660 429195 denom = a00 - 2.0 * a01 + a11;
661
2/2
✓ Branch 0 taken 13200 times.
✓ Branch 1 taken 415995 times.
 429195 if(numer >= denom) {
662 s = 1.0;
663 t = 0.0;
664 13200 sqrDistance = a00 + 2.0 * b0 + c;
665 } else {
666 415995 s = numer / denom;
667 415995 t = 1.0 - s;
668 415995 sqrDistance = s * (a00 * s + a01 * t + 2.0 * b0) +
669 415995 t * (a01 * s + a11 * t + 2.0 * b1) + c;
670 }
671 }
672 }
673 }
674
675 // Account for numerical round-off error.
676 if(sqrDistance < 0.0) {
677 sqrDistance = 0.0;
678 }
679
680 2517568 closest_point = V0 + s * edge0 + t * edge1;
681 2517568 lambda0 = 1.0 - s - t;
682 2517568 lambda1 = s;
683 2517568 lambda2 = t;
684 2517568 return sqrDistance;
685 }
686
687 /**
688 * \brief Computes the squared distance between a point and a nd
689 * triangle.
690 * \details See
691 * http://www.geometrictools.com/LibMathematics/Distance/Distance.html
692 * \param[in] p the query point
693 * \param[in] q1 first vertex of the triangle
694 * \param[in] q2 second vertex of the triangle
695 * \param[in] q3 third vertex of the triangle
696 * \tparam VEC the class that represents the points.
697 * \return the squared distance between the point and
698 * the triangle (\p V0, \p V1, \p V2)
699 */
700
701 template <class VEC>
702 inline double point_triangle_squared_distance(
703 const VEC& p, const VEC& q1, const VEC& q2, const VEC& q3
704 ) {
705 VEC closest_point;
706 double lambda1, lambda2, lambda3;
707 1 return point_triangle_squared_distance(
708 p, q1, q2, q3, closest_point, lambda1, lambda2, lambda3
709 );
710 }
711
712 /**
713 * \brief Computes the volume of a tetrahedron from
714 * edge lengths.
715 * \details Uses a form of generalized Heron formula:
716 * W. Kahan, "What has the Volume of a Tetrahedron
717 * to do with Computer Programming Languages?"
718 * \param[in] u distance between p1 and p4
719 * \param[in] U distance between p2 and p3
720 * \param[in] v distance between p2 and p4
721 * \param[in] V distance between p3 and p1
722 * \param[in] w distance between p3 and p4
723 * \param[in] W distance between p1 and p2
724 * \return the volume of the tetrahedron
725 */
726 4765144 inline double tetra_volume_from_edge_lengths(
727 double u, double U,
728 double v, double V,
729 double w, double W
730 ) {
731 4765144 double X = (w - U + v) * (U + v + w);
732 4765144 double x = (U - v + w) * (v - w + U);
733 4765144 double Y = (u - V + w) * (V + w + u);
734 4765144 double y = (V - w + u) * (w - u + V);
735 4765144 double Z = (v - W + u) * (W + u + v);
736 4765144 double z = (W - u + v) * (u - v + W);
737 4765144 double a = ::sqrt(::fabs(x * Y * Z));
738 4765144 double b = ::sqrt(::fabs(y * Z * X));
739 4765144 double c = ::sqrt(::fabs(z * X * Y));
740 4765144 double d = ::sqrt(::fabs(x * y * z));
741 4765144 return ::sqrt(::fabs(
742 4765144 (-a + b + c + d) *
743 4765144 (a - b + c + d) *
744 4765144 (a + b - c + d) *
745 4765144 (a + b + c - d)
746 4765144 )) / (192.0 * u * v * w);
747 }
748
749 /**
750 * \brief Computes the volume of a nd tetrahedron
751 * \details Uses a form of generalized Heron formula:
752 * W. Kahan, "What has the Volume of a Tetrahedron
753 * to do with Computer Programming Languages?"
754 * \param[in] p1 first vertex of the tetrahedron
755 * \param[in] p2 second vertex of the tetrahedron
756 * \param[in] p3 third vertex of the tetrahedron
757 * \param[in] p4 fourth vertex of the tetrahedron
758 * \tparam VEC the class that represents the points
759 * \return the volume of the tetrahedron
760 * (\p p1, \p p2, \p p3, \p p4)
761 */
762 template <class VEC>
763 4608 inline double tetra_volume(
764 const VEC& p1, const VEC& p2, const VEC& p3, const VEC& p4
765 ) {
766 double U = distance(p1, p2);
767 double u = distance(p3, p4);
768 double V = distance(p2, p3);
769 double v = distance(p1, p4);
770 double W = distance(p3, p1);
771 double w = distance(p2, p4);
772 4608 return tetra_volume_from_edge_lengths(u, U, v, V, w, W);
773 }
774
775 /**
776 * \brief Computes the volume of a nd tetrahedron
777 * \details Uses a form of generalized Heron formula:
778 * W. Kahan, "What has the Volume of a Tetrahedron
779 * to do with Computer Programming Languages?"
780 * \param[in] p1 first vertex of the tetrahedron
781 * \param[in] p2 second vertex of the tetrahedron
782 * \param[in] p3 third vertex of the tetrahedron
783 * \param[in] p4 fourth vertex of the tetrahedron
784 * \tparam DIM dimension of the points
785 * \return the volume of the tetrahedron
786 * (\p p1, \p p2, \p p3, \p p4)
787 */
788 template <int DIM>
789 9525680 inline double tetra_volume(
790 const double* p1, const double* p2,
791 const double* p3, const double* p4
792 ) {
793 9525680 double U = distance(p1, p2, DIM);
794 9525680 double u = distance(p3, p4, DIM);
795 9525680 double V = distance(p2, p3, DIM);
796 9525680 double v = distance(p1, p4, DIM);
797 9525680 double W = distance(p3, p1, DIM);
798 9525680 double w = distance(p2, p4, DIM);
799 9525680 return tetra_volume_from_edge_lengths(u, U, v, V, w, W);
800 }
801
802 /**
803 * \brief Computes the volume of a 3d tetrahedron
804 * \details Partial specialization of tetra_volume() for DIM == 3.
805 * It uses the standard formula, much simpler than the
806 * N dimension version.
807 * \param[in] p1 first vertex of the tetrahedron
808 * \param[in] p2 second vertex of the tetrahedron
809 * \param[in] p3 third vertex of the tetrahedron
810 * \param[in] p4 fourth vertex of the tetrahedron
811 * \return the volume of the tetrahedron
812 * (\p p1, \p p2, \p p3, \p p4)
813 */
814 template <>
815 inline double tetra_volume<3>(
816 const double* p1, const double* p2,
817 const double* p3, const double* p4
818 ) {
819 return tetra_volume(
820 *reinterpret_cast<const vec3*>(p1),
821 *reinterpret_cast<const vec3*>(p2),
822 *reinterpret_cast<const vec3*>(p3),
823 *reinterpret_cast<const vec3*>(p4)
824 );
825 }
826 }
827 }
828
829 #endif
830