Geogram  Version 1.9.1
A programming library of geometric algorithms
geometry_nd.h
Go to the documentation of this file.
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 
52 namespace GEO {
53 
54  namespace Geom {
55 
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  for(coord_index_t i = 0; i < dim; i++) {
70  result += GEO::geo_sqr(double(p2[i]) - double(p1[i]));
71  }
72  return result;
73  }
74 
83  template <class COORD_T>
84  inline double distance(
85  const COORD_T* p1, const COORD_T* p2, coord_index_t dim
86  ) {
87  return ::sqrt(distance2(p1, p2, dim));
88  }
89 
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 
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 
142  template <class COORD_T>
143  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  double a = distance(p1, p2, dim);
150  double b = distance(p2, p3, dim);
151  double c = distance(p3, p1, dim);
152  double s = double(0.5) * (a + b + c);
153  double A2 = s * (s - a) * (s - b) * (s - c);
154  // the max is there to avoid some numerical problems.
155  return ::sqrt(std::max(A2, 0.0));
156  }
157 
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 
211  template <class VEC>
212  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  double s = double(0.5) * (a + b + c);
220  return ::sqrt(s * (s - a) * (s - b) * (s - c));
221  }
222 
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 
266  template <class POINT>
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 
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 
337  template <class VEC>
339  const VEC& p1, const VEC& p2, const VEC& p3
340  ) {
341  double l1 = Numeric::random_float64();
342  double l2 = Numeric::random_float64();
343  if(l1 + l2 > 1.0) {
344  l1 = 1.0 - l1;
345  l2 = 1.0 - l2;
346  }
347  double l3 = 1.0 - l1 - l2;
348  return l1 * p1 + l2 * p2 + l3 * p3;
349  }
350 
363  template <class VEC>
365  const VEC& p1, const VEC& p2, const VEC& p3, const VEC& p4
366  ) {
367  double s = Numeric::random_float64();
368  double t = Numeric::random_float64();
369  double u = Numeric::random_float64();
370  if(s + t > 1.0) {
371  s = 1.0 - s;
372  t = 1.0 - t;
373  }
374  if(t + u > 1.0) {
375  double tmp = u;
376  u = 1.0 - s - t;
377  t = 1.0 - tmp;
378  } else if(s + t + u > 1.0) {
379  double tmp = u;
380  u = s + t + u - 1.0;
381  s = 1.0 - t - tmp;
382  }
383  double a = 1.0 - s - t - u;
384  return a * p1 + s * p2 + t * p3 + u * p4;
385  }
386 
402  template <class VEC>
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  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  } else if(t > l2) {
419  closest_point = V1;
420  lambda0 = 0.0;
421  lambda1 = 1.0;
422  return distance2(point, V1);
423  }
424  lambda1 = t / l2;
425  lambda0 = 1.0-lambda1;
426  closest_point = lambda0 * V0 + lambda1 * V1;
427  return distance2(point, closest_point);
428  }
429 
430 
440  template <class VEC>
442  const VEC& point,
443  const VEC& V0,
444  const VEC& V1
445  ) {
446  VEC closest_point;
447  double lambda0;
448  double lambda1;
450  point, V0, V1, closest_point, lambda0, lambda1
451  );
452  }
453 
474  template <class VEC>
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  double det = ::fabs(a00 * a11 - a01 * a01);
493  double s = a01 * b1 - a11 * b0;
494  double t = a01 * b0 - a00 * b1;
495  double sqrDistance;
496 
497  // If the triangle is degenerate
498  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(point, V0, V1, cur_closest, cur_l1, cur_l2);
503  result = cur_dist;
504  closest_point = cur_closest;
505  lambda0 = cur_l1;
506  lambda1 = cur_l2;
507  lambda2 = 0.0;
508  cur_dist = point_segment_squared_distance(point, V0, V2, cur_closest, cur_l1, cur_l2);
509  if(cur_dist < result) {
510  result = cur_dist;
511  closest_point = cur_closest;
512  lambda0 = cur_l1;
513  lambda2 = cur_l2;
514  lambda1 = 0.0;
515  }
516  cur_dist = point_segment_squared_distance(point, V1, V2, cur_closest, cur_l1, cur_l2);
517  if(cur_dist < result) {
518  result = cur_dist;
519  closest_point = cur_closest;
520  lambda1 = cur_l1;
521  lambda2 = cur_l2;
522  lambda0 = 0.0;
523  }
524  return result;
525  }
526 
527  if(s + t <= det) {
528  if(s < 0.0) {
529  if(t < 0.0) { // region 4
530  if(b0 < 0.0) {
531  t = 0.0;
532  if(-b0 >= a00) {
533  s = 1.0;
534  sqrDistance = a00 + 2.0 * b0 + c;
535  } else {
536  s = -b0 / a00;
537  sqrDistance = b0 * s + c;
538  }
539  } else {
540  s = 0.0;
541  if(b1 >= 0.0) {
542  t = 0.0;
543  sqrDistance = c;
544  } else if(-b1 >= a11) {
545  t = 1.0;
546  sqrDistance = a11 + 2.0 * b1 + c;
547  } else {
548  t = -b1 / a11;
549  sqrDistance = b1 * t + c;
550  }
551  }
552  } else { // region 3
553  s = 0.0;
554  if(b1 >= 0.0) {
555  t = 0.0;
556  sqrDistance = c;
557  } else if(-b1 >= a11) {
558  t = 1.0;
559  sqrDistance = a11 + 2.0 * b1 + c;
560  } else {
561  t = -b1 / a11;
562  sqrDistance = b1 * t + c;
563  }
564  }
565  } else if(t < 0.0) { // region 5
566  t = 0.0;
567  if(b0 >= 0.0) {
568  s = 0.0;
569  sqrDistance = c;
570  } else if(-b0 >= a00) {
571  s = 1.0;
572  sqrDistance = a00 + 2.0 * b0 + c;
573  } else {
574  s = -b0 / a00;
575  sqrDistance = b0 * s + c;
576  }
577  } else { // region 0
578  // minimum at interior point
579  double invDet = double(1.0) / det;
580  s *= invDet;
581  t *= invDet;
582  sqrDistance = s * (a00 * s + a01 * t + 2.0 * b0) +
583  t * (a01 * s + a11 * t + 2.0 * b1) + c;
584  }
585  } else {
586  double tmp0, tmp1, numer, denom;
587 
588  if(s < 0.0) { // region 2
589  tmp0 = a01 + b0;
590  tmp1 = a11 + b1;
591  if(tmp1 > tmp0) {
592  numer = tmp1 - tmp0;
593  denom = a00 - 2.0 * a01 + a11;
594  if(numer >= denom) {
595  s = 1.0;
596  t = 0.0;
597  sqrDistance = a00 + 2.0 * b0 + c;
598  } else {
599  s = numer / denom;
600  t = 1.0 - s;
601  sqrDistance = s * (a00 * s + a01 * t + 2.0 * b0) +
602  t * (a01 * s + a11 * t + 2.0 * b1) + c;
603  }
604  } else {
605  s = 0.0;
606  if(tmp1 <= 0.0) {
607  t = 1.0;
608  sqrDistance = a11 + 2.0 * b1 + c;
609  }
610  else if(b1 >= 0.0) {
611  t = 0.0;
612  sqrDistance = c;
613  } else {
614  t = -b1 / a11;
615  sqrDistance = b1 * t + c;
616  }
617  }
618  } else if(t < 0.0) { // region 6
619  tmp0 = a01 + b1;
620  tmp1 = a00 + b0;
621  if(tmp1 > tmp0) {
622  numer = tmp1 - tmp0;
623  denom = a00 - 2.0 * a01 + a11;
624  if(numer >= denom) {
625  t = 1.0;
626  s = 0.0;
627  sqrDistance = a11 + 2.0 * b1 + c;
628  } else {
629  t = numer / denom;
630  s = 1.0 - t;
631  sqrDistance = s * (a00 * s + a01 * t + 2.0 * b0) +
632  t * (a01 * s + a11 * t + 2.0 * b1) + c;
633  }
634  } else {
635  t = 0.0;
636  if(tmp1 <= 0.0) {
637  s = 1.0;
638  sqrDistance = a00 + 2.0 * b0 + c;
639  } else if(b0 >= 0.0) {
640  s = 0.0;
641  sqrDistance = c;
642  } else {
643  s = -b0 / a00;
644  sqrDistance = b0 * s + c;
645  }
646  }
647  } else { // region 1
648  numer = a11 + b1 - a01 - b0;
649  if(numer <= 0.0) {
650  s = 0.0;
651  t = 1.0;
652  sqrDistance = a11 + 2.0 * b1 + c;
653  } else {
654  denom = a00 - 2.0 * a01 + a11;
655  if(numer >= denom) {
656  s = 1.0;
657  t = 0.0;
658  sqrDistance = a00 + 2.0 * b0 + c;
659  } else {
660  s = numer / denom;
661  t = 1.0 - s;
662  sqrDistance = s * (a00 * s + a01 * t + 2.0 * b0) +
663  t * (a01 * s + a11 * t + 2.0 * b1) + c;
664  }
665  }
666  }
667  }
668 
669  // Account for numerical round-off error.
670  if(sqrDistance < 0.0) {
671  sqrDistance = 0.0;
672  }
673 
674  closest_point = V0 + s * edge0 + t * edge1;
675  lambda0 = 1.0 - s - t;
676  lambda1 = s;
677  lambda2 = t;
678  return sqrDistance;
679  }
680 
695  template <class VEC>
697  const VEC& p, const VEC& q1, const VEC& q2, const VEC& q3
698  ) {
699  VEC closest_point;
700  double lambda1, lambda2, lambda3;
702  p, q1, q2, q3, closest_point, lambda1, lambda2, lambda3
703  );
704  }
705 
721  double u, double U,
722  double v, double V,
723  double w, double W
724  ) {
725  double X = (w - U + v) * (U + v + w);
726  double x = (U - v + w) * (v - w + U);
727  double Y = (u - V + w) * (V + w + u);
728  double y = (V - w + u) * (w - u + V);
729  double Z = (v - W + u) * (W + u + v);
730  double z = (W - u + v) * (u - v + W);
731  double a = ::sqrt(::fabs(x * Y * Z));
732  double b = ::sqrt(::fabs(y * Z * X));
733  double c = ::sqrt(::fabs(z * X * Y));
734  double d = ::sqrt(::fabs(x * y * z));
735  return ::sqrt(::fabs(
736  (-a + b + c + d) *
737  (a - b + c + d) *
738  (a + b - c + d) *
739  (a + b + c - d)
740  )) / (192.0 * u * v * w);
741  }
742 
756  template <class VEC>
757  inline double tetra_volume(
758  const VEC& p1, const VEC& p2, const VEC& p3, const VEC& p4
759  ) {
760  double U = distance(p1, p2);
761  double u = distance(p3, p4);
762  double V = distance(p2, p3);
763  double v = distance(p1, p4);
764  double W = distance(p3, p1);
765  double w = distance(p2, p4);
766  return tetra_volume_from_edge_lengths(u, U, v, V, w, W);
767  }
768 
782  template <int DIM>
783  inline double tetra_volume(
784  const double* p1, const double* p2,
785  const double* p3, const double* p4
786  ) {
787  double U = distance(p1, p2, DIM);
788  double u = distance(p3, p4, DIM);
789  double V = distance(p2, p3, DIM);
790  double v = distance(p1, p4, DIM);
791  double W = distance(p3, p1, DIM);
792  double w = distance(p2, p4, DIM);
793  return tetra_volume_from_edge_lengths(u, U, v, V, w, W);
794  }
795 
808  template <>
809  inline double tetra_volume<3>(
810  const double* p1, const double* p2,
811  const double* p3, const double* p4
812  ) {
813  return tetra_volume(
814  *reinterpret_cast<const vec3*>(p1),
815  *reinterpret_cast<const vec3*>(p2),
816  *reinterpret_cast<const vec3*>(p3),
817  *reinterpret_cast<const vec3*>(p4)
818  );
819  }
820  }
821 }
822 
823 #endif
#define geo_debug_assert(x)
Verifies that a condition is met.
Definition: assert.h:196
Common include file, providing basic definitions. Should be included before anything else by all head...
Geometric functions in 2d and 3d.
Types and functions for memory manipulation.
double point_segment_squared_distance(const VEC &point, const VEC &V0, const VEC &V1, VEC &closest_point, double &lambda0, double &lambda1)
Computes the point closest to a given point in a nd segment.
Definition: geometry_nd.h:403
double distance(const COORD_T *p1, const COORD_T *p2, coord_index_t dim)
Computes the distance between two nd points.
Definition: geometry_nd.h:84
double det(const vec2 &a, const vec2 &b)
Computes the determinant of two vectors.
Definition: geometry.h:292
double tetra_volume< 3 >(const double *p1, const double *p2, const double *p3, const double *p4)
Computes the volume of a 3d tetrahedron.
Definition: geometry_nd.h:809
vec3 random_point_in_triangle(const vec3 &p1, const vec3 &p2, const vec3 &p3)
Generates a random point in a 3d triangle.
Definition: geometry.h:582
double distance2(const COORD_T *p1, const COORD_T *p2, coord_index_t dim)
Computes the squared distance between two nd points.
Definition: geometry_nd.h:65
double tetra_volume(const vec3 &p1, const vec3 &p2, const vec3 &p3, const vec3 &p4)
Computes the volume of a 3d tetrahedron.
Definition: geometry.h:500
double triangle_mass(const vec3 &p, const vec3 &q, const vec3 &r, double a, double b, double c)
Computes the mass of a 3d triangle with weighted points.
Definition: geometry.h:563
double triangle_area(const vec3 &p1, const vec3 &p2, const vec3 &p3)
Computes the area of a 3d triangle.
Definition: geometry.h:348
void triangle_centroid(const vec3 &p, const vec3 &q, const vec3 &r, double a, double b, double c, vec3 &Vg, double &V)
Computes the centroid of a 3d triangle with weighted points.
Definition: geometry.h:534
double tetra_volume_from_edge_lengths(double u, double U, double v, double V, double w, double W)
Computes the volume of a tetrahedron from edge lengths.
Definition: geometry_nd.h:720
double point_triangle_squared_distance(const VEC &point, const VEC &V0, const VEC &V1, const VEC &V2, VEC &closest_point, double &lambda0, double &lambda1, double &lambda2)
Computes the point closest to a given point in a nd triangle.
Definition: geometry_nd.h:475
VEC random_point_in_tetra(const VEC &p1, const VEC &p2, const VEC &p3, const VEC &p4)
Generates a random point in a nd tetrahedron.
Definition: geometry_nd.h:364
vec2 triangle_circumcenter(const vec2 &p1, const vec2 &p2, const vec2 &p3)
Computes the center of the circumscribed circle of a 2d triangle.
float64 random_float64()
Returns a 64 bits float between 0 and 1.
Global Vorpaline namespace.
Definition: basic.h:55
T geo_sqr(T x)
Gets the square value of a value.
Definition: numeric.h:301
geo_coord_index_t coord_index_t
The type for storing coordinate indices, and iterating on the coordinates of a point.
Definition: numeric.h:363