1 /* 2 * Copyright (c) 2012-2014, Bruno Levy 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 * If you modify this software, you should include a notice giving the 30 * name of the person performing the modification, the date of modification, 31 * and the reason for such modification. 32 * 33 * Contact: Bruno Levy 34 * 35 * Bruno.Levy@inria.fr 36 * http://www.loria.fr/~levy 37 * 38 * ALICE Project 39 * LORIA, INRIA Lorraine, 40 * Campus Scientifique, BP 239 41 * 54506 VANDOEUVRE LES NANCY CEDEX 42 * FRANCE 43 * 44 */ 45 46 #ifndef GEOGRAM_BASIC_GEOMETRY_ND 47 #define GEOGRAM_BASIC_GEOMETRY_ND 48 49 #include <geogram/basic/common.h> 50 #include <geogram/basic/geometry.h> 51 #include <geogram/basic/memory.h> 52 53 /** 54 * \file geogram/basic/geometry_nd.h 55 * \brief Geometric functions in arbitrary dimension 56 */ 57 58 namespace GEO { 59 60 namespace Geom { 61 62 /** 63 * \brief Computes the squared distance between two nd points. 64 * \param[in] p1 a pointer to the coordinates of the first point 65 * \param[in] p2 a pointer to the coordinates of the second point 66 * \param[in] dim dimension (number of coordinates of the points) 67 * \return the squared distance between \p p1 and \p p2 68 * \tparam COORD_T the numeric type of the point coordinates 69 */ 70 template <class COORD_T> distance2(const COORD_T * p1,const COORD_T * p2,coord_index_t dim)71 inline double distance2( 72 const COORD_T* p1, const COORD_T* p2, coord_index_t dim 73 ) { 74 double result = 0.0; 75 for(coord_index_t i = 0; i < dim; i++) { 76 result += GEO::geo_sqr(double(p2[i]) - double(p1[i])); 77 } 78 return result; 79 } 80 81 /** 82 * \brief Computes the distance between two nd points. 83 * \param[in] p1 a pointer to the coordinates of the first point 84 * \param[in] p2 a pointer to the coordinates of the second point 85 * \param[in] dim dimension (number of coordinates of the points) 86 * \return the distance between \p p1 and \p p2 87 * \tparam COORD_T the numeric type of the point coordinates 88 */ 89 template <class COORD_T> distance(const COORD_T * p1,const COORD_T * p2,coord_index_t dim)90 inline double distance( 91 const COORD_T* p1, const COORD_T* p2, coord_index_t dim 92 ) { 93 return ::sqrt(distance2(p1, p2, dim)); 94 } 95 96 /** 97 * \brief Computes the squared distance between two nd points. 98 * \param[in] p1 first point 99 * \param[in] p2 second point 100 * \tparam VEC the class that represents the points. VEC needs to 101 * implement data(), that returns a pointer to the coordinates 102 * of the point. 103 * \return the squared distance between \p p1 and \p p2 104 */ 105 template <class VEC> distance2(const VEC & p1,const VEC & p2)106 inline double distance2( 107 const VEC& p1, const VEC& p2 108 ) { 109 geo_debug_assert(p1.dimension() == p2.dimension()); 110 return distance2( 111 p1.data(), p2.data(), coord_index_t(p1.dimension()) 112 ); 113 } 114 115 /** 116 117 * \brief Computes the distance between two nd points. 118 * \param[in] p1 first point 119 * \param[in] p2 second point 120 * \tparam VEC the class that represents the points. VEC needs to 121 * implement data(), that returns a pointer to the coordinates 122 * of the point. 123 * \return the distance between \p p1 and \p p2 124 */ 125 template <class VEC> distance(const VEC & p1,const VEC & p2)126 inline double distance( 127 const VEC& p1, const VEC& p2 128 ) { 129 geo_debug_assert(p1.dimension() == p2.dimension()); 130 return distance(p1.data(), p2.data(), p1.dimension()); 131 } 132 133 /** 134 * \brief Computes the area of a nd triangle 135 * \details Uses Heron formula (that computes the area 136 * from the lengths of the three edges). 137 * \param[in] p1 a pointer to the coordinates of the 138 * first vertex of the triangle 139 * \param[in] p2 a pointer to the coordinates of the 140 * second vertex of the triangle 141 * \param[in] p3 a pointer to the coordinates of the 142 * third vertex of the triangle 143 * \param[in] dim dimension of the points 144 * \tparam COORD_T the numeric type that represents the coordinates 145 * of the points 146 * \return the area of triangle ( \p p1, \p p2, \p p3) 147 */ 148 template <class COORD_T> triangle_area(const COORD_T * p1,const COORD_T * p2,const COORD_T * p3,coord_index_t dim)149 inline double triangle_area( 150 const COORD_T* p1, 151 const COORD_T* p2, 152 const COORD_T* p3, 153 coord_index_t dim 154 ) { 155 double a = distance(p1, p2, dim); 156 double b = distance(p2, p3, dim); 157 double c = distance(p3, p1, dim); 158 double s = double(0.5) * (a + b + c); 159 double A2 = s * (s - a) * (s - b) * (s - c); 160 // the max is there to avoid some numerical problems. 161 return ::sqrt(std::max(A2, 0.0)); 162 } 163 164 /** 165 * \brief Computes the centroid of a 3d triangle with weighted points. 166 * \details The integrated weight varies linearly in the triangle. 167 * \param[in] p a pointer to the coordinates of the 168 * first vertex of the triangle 169 * \param[in] q a pointer to the coordinates of the 170 * second vertex of the triangle 171 * \param[in] r a pointer to the coordinates of the 172 * third vertex of the triangle 173 * \param[in] a the weight associated with vertex \p p 174 * \param[in] b the weight associated with vertex \p q 175 * \param[in] c the weight associated with vertex \p r 176 * \param[out] Vg the total weight times the centroid ( 177 * a pointer to a caller-allocated array of dim COORD_T%s) 178 * \param[out] V the total weight 179 * \param[in] dim the dimension of the vertices 180 * \tparam COORD_T the numeric type that represents the coordinates 181 * of the points 182 */ 183 template <class COORD_T> triangle_centroid(const COORD_T * p,const COORD_T * q,const COORD_T * r,COORD_T a,COORD_T b,COORD_T c,double * Vg,double & V,coord_index_t dim)184 inline void triangle_centroid( 185 const COORD_T* p, 186 const COORD_T* q, 187 const COORD_T* r, 188 COORD_T a, COORD_T b, COORD_T c, 189 double* Vg, 190 double& V, 191 coord_index_t dim 192 ) { 193 double abc = a + b + c; 194 double area = Geom::triangle_area(p, q, r, dim); 195 V = area / 3.0 * abc; 196 double wp = a + abc; 197 double wq = b + abc; 198 double wr = c + abc; 199 double s = area / 12.0; 200 for(coord_index_t i = 0; i < dim; i++) { 201 Vg[i] = s * (wp * p[i] + wq * q[i] + wr * r[i]); 202 } 203 } 204 205 /********************************************************************/ 206 207 /** 208 * \brief Computes the area of a nd triangle. 209 * \details Uses Heron formula (that compures the area 210 * from the lengths of the three edges). 211 * \param[in] p1 first vertex of the triangle 212 * \param[in] p2 second vertex of the triangle 213 * \param[in] p3 third vertex of the triangle 214 * \tparam VEC the class used to represent the vertices of the triangle 215 * \return the area of triangle (\p p1, \p p2, \p p3) 216 */ 217 template <class VEC> triangle_area(const VEC & p1,const VEC & p2,const VEC & p3)218 inline double triangle_area( 219 const VEC& p1, const VEC& p2, const VEC& p3 220 ) { 221 // Heron formula 222 double a = distance(p1, p2); 223 double b = distance(p2, p3); 224 double c = distance(p3, p1); 225 double s = double(0.5) * (a + b + c); 226 return ::sqrt(s * (s - a) * (s - b) * (s - c)); 227 } 228 229 /** 230 * \brief Computes the mass of a nd triangle with weighted points. 231 * \details The integrated weight varies linearly in the triangle. 232 * \param[in] p first vertex of the triangle 233 * \param[in] q second vertex of the triangle 234 * \param[in] r third vertex of the triangle 235 * \param[in] a the weight associated with vertex \p p 236 * \param[in] b the weight associated with vertex \p q 237 * \param[in] c the weight associated with vertex \p r 238 * \tparam VEC the class used to represent the vertices of the triangle 239 * \return the mass of the weighted triangle ( \p p, \p a), 240 * ( \p q, \p b), ( \p r, \p c) 241 */ 242 template <class VEC> triangle_mass(const VEC & p,const VEC & q,const VEC & r,double a,double b,double c)243 inline double triangle_mass( 244 const VEC& p, const VEC& q, const VEC& r, 245 double a, double b, double c 246 ) { 247 // TODO: try to better understand the formula and 248 // determine why there are these sqrt's 249 // (probably due to the relation between the 250 // user-provided density and the one achieved 251 // by CVT), but I'm pretty sure that the formula 252 // is correct (at least, dimensions match). 253 // Note: the ::fabs() are there to avoid numerical 254 // errors. 255 return Geom::triangle_area(p, q, r) / 3.0 * ( 256 ::sqrt(::fabs(a)) + sqrt(::fabs(b)) + sqrt(::fabs(c)) 257 ); 258 } 259 260 /** 261 * \brief Computes the center of the circumscribed circle of 262 * a nd triangle. 263 * \param[in] Q1 first vertex of the triangle 264 * \param[in] Q2 second vertex of the triangle 265 * \param[in] Q3 third vertex of the triangle 266 * \param[out] denom if the parameter is non null, it is set to the 267 * denominator of the barycentric coordinates of the circumcenter. 268 * \tparam POINT the class used to represent the vertices 269 * of the triangle 270 * \return the circumcenter of the triangle (\p p1, \p p2, \p p3). 271 */ 272 template <class POINT> 273 POINT triangle_circumcenter( 274 const POINT& Q1, 275 const POINT& Q2, 276 const POINT& Q3, 277 double* denom = nullptr 278 ) { 279 const POINT q2 = Q2 - Q1; 280 const POINT q3 = Q3 - Q1; 281 282 double l2 = length2(q2); 283 double l3 = length2(q3); 284 285 double a12 = -2.0 * dot(q2, q2); 286 double a13 = -2.0 * dot(q3, q2); 287 double a22 = -2.0 * dot(q2, q3); 288 double a23 = -2.0 * dot(q3, q3); 289 290 double c31 = (a23 * a12 - a22 * a13); 291 double d = c31; 292 double s = 1.0 / d; 293 double lambda1 = s * ((a23 - a22) * l2 + (a12 - a13) * l3 + c31); 294 double lambda2 = s * ((-a23) * l2 + (a13) * l3); 295 double lambda3 = s * ((a22) * l2 + (-a12) * l3); 296 if(denom != nullptr) { 297 *denom = d; 298 } 299 return lambda1 * Q1 + lambda2 * Q2 + lambda3 * Q3; 300 } 301 302 /** 303 * \brief Computes the centroid of a nd triangle with weighted points. 304 * \details The integrated weight varies linearly in the triangle. 305 * \param[in] p first vertex of the triangle 306 * \param[in] q second vertex of the triangle 307 * \param[in] r third vertex of the triangle 308 * \param[in] a the weight associated with vertex \p p 309 * \param[in] b the weight associated with vertex \p q 310 * \param[in] c the weight associated with vertex \p r 311 * \param[out] Vg the total weight times the centroid 312 * \param[out] V the total weight 313 * \tparam VEC the class used to represent the vertices 314 * of the triangle 315 */ 316 template <class VEC> triangle_centroid(const VEC & p,const VEC & q,const VEC & r,double a,double b,double c,VEC & Vg,double & V)317 inline void triangle_centroid( 318 const VEC& p, const VEC& q, const VEC& r, 319 double a, double b, double c, 320 VEC& Vg, double& V 321 ) { 322 double abc = a + b + c; 323 double area = Geom::triangle_area(p, q, r); 324 V = area / 3.0 * abc; 325 double wp = a + abc; 326 double wq = b + abc; 327 double wr = c + abc; 328 double s = area / 12.0; 329 Vg = s * (wp * p + wq * q + wr * r); 330 } 331 332 /** 333 * \brief Generates a random point in a nd triangle. 334 * \details Uses Greg Turk's second method 335 * (see article in Graphic Gems). 336 * \param[in] p1 first vertex of the triangle 337 * \param[in] p2 second vertex of the triangle 338 * \param[in] p3 third vertex of the triangle 339 * \return a random point in triangle ( \p p1, \p p2, \p p3 ) 340 * \tparam VEC the class used to represent the vertices 341 * of the triangle 342 */ 343 template <class VEC> random_point_in_triangle(const VEC & p1,const VEC & p2,const VEC & p3)344 inline VEC random_point_in_triangle( 345 const VEC& p1, const VEC& p2, const VEC& p3 346 ) { 347 double l1 = Numeric::random_float64(); 348 double l2 = Numeric::random_float64(); 349 if(l1 + l2 > 1.0) { 350 l1 = 1.0 - l1; 351 l2 = 1.0 - l2; 352 } 353 double l3 = 1.0 - l1 - l2; 354 return l1 * p1 + l2 * p2 + l3 * p3; 355 } 356 357 /** 358 * \brief Generates a random point in a nd tetrahedron. 359 * \details Uses Greg Turk's second method 360 * (see article in Graphic Gems). 361 * \param[in] p1 first vertex of the triangle 362 * \param[in] p2 second vertex of the triangle 363 * \param[in] p3 third vertex of the triangle 364 * \param[in] p4 fourth vertex of the triangle 365 * \return a random point in tetrahedron ( \p p1, \p p2, \p p3, \p p4) 366 * \tparam VEC the class used to represent the vertices 367 * of the triangle 368 */ 369 template <class VEC> random_point_in_tetra(const VEC & p1,const VEC & p2,const VEC & p3,const VEC & p4)370 inline VEC random_point_in_tetra( 371 const VEC& p1, const VEC& p2, const VEC& p3, const VEC& p4 372 ) { 373 double s = Numeric::random_float64(); 374 double t = Numeric::random_float64(); 375 double u = Numeric::random_float64(); 376 if(s + t > 1.0) { 377 s = 1.0 - s; 378 t = 1.0 - t; 379 } 380 if(t + u > 1.0) { 381 double tmp = u; 382 u = 1.0 - s - t; 383 t = 1.0 - tmp; 384 } else if(s + t + u > 1.0) { 385 double tmp = u; 386 u = s + t + u - 1.0; 387 s = 1.0 - t - tmp; 388 } 389 double a = 1.0 - s - t - u; 390 return a * p1 + s * p2 + t * p3 + u * p4; 391 } 392 393 /** 394 * \brief Computes the point closest to a given point in a nd segment 395 * \param[in] point the query point 396 * \param[in] V0 first extremity of the segment 397 * \param[in] V1 second extremity of the segment 398 * \param[out] closest_point the point closest to \p point in the 399 * segment [\p V0, \p V1] 400 * \param[out] lambda0 barycentric coordinate of the closest point 401 * relative to \p V0 402 * \param[out] lambda1 barycentric coordinate of the closest point 403 * relative to \p V1 404 * \tparam VEC the class that represents the points. 405 * \return the squared distance between the point and 406 * the segment [\p V0, \p V1] 407 */ 408 template <class VEC> point_segment_squared_distance(const VEC & point,const VEC & V0,const VEC & V1,VEC & closest_point,double & lambda0,double & lambda1)409 inline double point_segment_squared_distance( 410 const VEC& point, 411 const VEC& V0, 412 const VEC& V1, 413 VEC& closest_point, 414 double& lambda0, 415 double& lambda1 416 ) { 417 double l2 = distance2(V0,V1); 418 double t = dot(point - V0, V1 - V0); 419 if(t <= 0.0 || l2 == 0.0) { 420 closest_point = V0; 421 lambda0 = 1.0; 422 lambda1 = 0.0; 423 return distance2(point, V0); 424 } else if(t > l2) { 425 closest_point = V1; 426 lambda0 = 0.0; 427 lambda1 = 1.0; 428 return distance2(point, V1); 429 } 430 lambda1 = t / l2; 431 lambda0 = 1.0-lambda1; 432 closest_point = lambda0 * V0 + lambda1 * V1; 433 return distance2(point, closest_point); 434 } 435 436 437 /** 438 * \brief Computes the point closest to a given point in a nd segment 439 * \param[in] point the query point 440 * \param[in] V0 first extremity of the segment 441 * \param[in] V1 second extremity of the segment 442 * \tparam VEC the class that represents the points. 443 * \return the squared distance between the point and 444 * the segment [\p V0, \p V1] 445 */ 446 template <class VEC> point_segment_squared_distance(const VEC & point,const VEC & V0,const VEC & V1)447 inline double point_segment_squared_distance( 448 const VEC& point, 449 const VEC& V0, 450 const VEC& V1 451 ) { 452 VEC closest_point; 453 double lambda0; 454 double lambda1; 455 return point_segment_squared_distance( 456 point, V0, V1, closest_point, lambda0, lambda1 457 ); 458 } 459 460 /** 461 * \brief Computes the point closest to a given point in a nd triangle 462 * \details See 463 * http://www.geometrictools.com/LibMathematics/Distance/Distance.html 464 * \param[in] point the query point 465 * \param[in] V0 first vertex of the triangle 466 * \param[in] V1 second vertex of the triangle 467 * \param[in] V2 third vertex of the triangle 468 * \param[out] closest_point the point closest to \p point in the 469 * triangle (\p V0, \p V1, \p V2) 470 * \param[out] lambda0 barycentric coordinate of the closest point 471 * relative to \p V0 472 * \param[out] lambda1 barycentric coordinate of the closest point 473 * relative to \p V1 474 * \param[out] lambda2 barycentric coordinate of the closest point 475 * relative to \p V2 476 * \tparam VEC the class that represents the points. 477 * \return the squared distance between the point and 478 * the triangle (\p V0, \p V1, \p V2) 479 */ 480 template <class VEC> 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)481 inline double point_triangle_squared_distance( 482 const VEC& point, 483 const VEC& V0, 484 const VEC& V1, 485 const VEC& V2, 486 VEC& closest_point, 487 double& lambda0, double& lambda1, double& lambda2 488 ) { 489 VEC diff = V0 - point; 490 VEC edge0 = V1 - V0; 491 VEC edge1 = V2 - V0; 492 double a00 = length2(edge0); 493 double a01 = dot(edge0, edge1); 494 double a11 = length2(edge1); 495 double b0 = dot(diff, edge0); 496 double b1 = dot(diff, edge1); 497 double c = length2(diff); 498 double det = ::fabs(a00 * a11 - a01 * a01); 499 double s = a01 * b1 - a11 * b0; 500 double t = a01 * b0 - a00 * b1; 501 double sqrDistance; 502 503 // If the triangle is degenerate 504 if(det < 1e-30) { 505 double cur_l1, cur_l2; 506 VEC cur_closest; 507 double result; 508 double cur_dist = point_segment_squared_distance(point, V0, V1, cur_closest, cur_l1, cur_l2); 509 result = cur_dist; 510 closest_point = cur_closest; 511 lambda0 = cur_l1; 512 lambda1 = cur_l2; 513 lambda2 = 0.0; 514 cur_dist = point_segment_squared_distance(point, V0, V2, cur_closest, cur_l1, cur_l2); 515 if(cur_dist < result) { 516 result = cur_dist; 517 closest_point = cur_closest; 518 lambda0 = cur_l1; 519 lambda2 = cur_l2; 520 lambda1 = 0.0; 521 } 522 cur_dist = point_segment_squared_distance(point, V1, V2, cur_closest, cur_l1, cur_l2); 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 if(s + t <= det) { 534 if(s < 0.0) { 535 if(t < 0.0) { // region 4 536 if(b0 < 0.0) { 537 t = 0.0; 538 if(-b0 >= a00) { 539 s = 1.0; 540 sqrDistance = a00 + 2.0 * b0 + c; 541 } else { 542 s = -b0 / a00; 543 sqrDistance = b0 * s + c; 544 } 545 } else { 546 s = 0.0; 547 if(b1 >= 0.0) { 548 t = 0.0; 549 sqrDistance = c; 550 } else if(-b1 >= a11) { 551 t = 1.0; 552 sqrDistance = a11 + 2.0 * b1 + c; 553 } else { 554 t = -b1 / a11; 555 sqrDistance = b1 * t + c; 556 } 557 } 558 } else { // region 3 559 s = 0.0; 560 if(b1 >= 0.0) { 561 t = 0.0; 562 sqrDistance = c; 563 } else if(-b1 >= a11) { 564 t = 1.0; 565 sqrDistance = a11 + 2.0 * b1 + c; 566 } else { 567 t = -b1 / a11; 568 sqrDistance = b1 * t + c; 569 } 570 } 571 } else if(t < 0.0) { // region 5 572 t = 0.0; 573 if(b0 >= 0.0) { 574 s = 0.0; 575 sqrDistance = c; 576 } else if(-b0 >= a00) { 577 s = 1.0; 578 sqrDistance = a00 + 2.0 * b0 + c; 579 } else { 580 s = -b0 / a00; 581 sqrDistance = b0 * s + c; 582 } 583 } else { // region 0 584 // minimum at interior point 585 double invDet = double(1.0) / det; 586 s *= invDet; 587 t *= invDet; 588 sqrDistance = s * (a00 * s + a01 * t + 2.0 * b0) + 589 t * (a01 * s + a11 * t + 2.0 * b1) + c; 590 } 591 } else { 592 double tmp0, tmp1, numer, denom; 593 594 if(s < 0.0) { // region 2 595 tmp0 = a01 + b0; 596 tmp1 = a11 + b1; 597 if(tmp1 > tmp0) { 598 numer = tmp1 - tmp0; 599 denom = a00 - 2.0 * a01 + a11; 600 if(numer >= denom) { 601 s = 1.0; 602 t = 0.0; 603 sqrDistance = a00 + 2.0 * b0 + c; 604 } else { 605 s = numer / denom; 606 t = 1.0 - s; 607 sqrDistance = s * (a00 * s + a01 * t + 2.0 * b0) + 608 t * (a01 * s + a11 * t + 2.0 * b1) + c; 609 } 610 } else { 611 s = 0.0; 612 if(tmp1 <= 0.0) { 613 t = 1.0; 614 sqrDistance = a11 + 2.0 * b1 + c; 615 } 616 else if(b1 >= 0.0) { 617 t = 0.0; 618 sqrDistance = c; 619 } else { 620 t = -b1 / a11; 621 sqrDistance = b1 * t + c; 622 } 623 } 624 } else if(t < 0.0) { // region 6 625 tmp0 = a01 + b1; 626 tmp1 = a00 + b0; 627 if(tmp1 > tmp0) { 628 numer = tmp1 - tmp0; 629 denom = a00 - 2.0 * a01 + a11; 630 if(numer >= denom) { 631 t = 1.0; 632 s = 0.0; 633 sqrDistance = a11 + 2.0 * b1 + c; 634 } else { 635 t = numer / denom; 636 s = 1.0 - t; 637 sqrDistance = s * (a00 * s + a01 * t + 2.0 * b0) + 638 t * (a01 * s + a11 * t + 2.0 * b1) + c; 639 } 640 } else { 641 t = 0.0; 642 if(tmp1 <= 0.0) { 643 s = 1.0; 644 sqrDistance = a00 + 2.0 * b0 + c; 645 } else if(b0 >= 0.0) { 646 s = 0.0; 647 sqrDistance = c; 648 } else { 649 s = -b0 / a00; 650 sqrDistance = b0 * s + c; 651 } 652 } 653 } else { // region 1 654 numer = a11 + b1 - a01 - b0; 655 if(numer <= 0.0) { 656 s = 0.0; 657 t = 1.0; 658 sqrDistance = a11 + 2.0 * b1 + c; 659 } else { 660 denom = a00 - 2.0 * a01 + a11; 661 if(numer >= denom) { 662 s = 1.0; 663 t = 0.0; 664 sqrDistance = a00 + 2.0 * b0 + c; 665 } else { 666 s = numer / denom; 667 t = 1.0 - s; 668 sqrDistance = s * (a00 * s + a01 * t + 2.0 * b0) + 669 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 closest_point = V0 + s * edge0 + t * edge1; 681 lambda0 = 1.0 - s - t; 682 lambda1 = s; 683 lambda2 = t; 684 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> point_triangle_squared_distance(const VEC & p,const VEC & q1,const VEC & q2,const VEC & q3)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 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 */ tetra_volume_from_edge_lengths(double u,double U,double v,double V,double w,double W)726 inline double tetra_volume_from_edge_lengths( 727 double u, double U, 728 double v, double V, 729 double w, double W 730 ) { 731 double X = (w - U + v) * (U + v + w); 732 double x = (U - v + w) * (v - w + U); 733 double Y = (u - V + w) * (V + w + u); 734 double y = (V - w + u) * (w - u + V); 735 double Z = (v - W + u) * (W + u + v); 736 double z = (W - u + v) * (u - v + W); 737 double a = ::sqrt(::fabs(x * Y * Z)); 738 double b = ::sqrt(::fabs(y * Z * X)); 739 double c = ::sqrt(::fabs(z * X * Y)); 740 double d = ::sqrt(::fabs(x * y * z)); 741 return ::sqrt(::fabs( 742 (-a + b + c + d) * 743 (a - b + c + d) * 744 (a + b - c + d) * 745 (a + b + c - d) 746 )) / (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> tetra_volume(const VEC & p1,const VEC & p2,const VEC & p3,const VEC & p4)763 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 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> tetra_volume(const double * p1,const double * p2,const double * p3,const double * p4)789 inline double tetra_volume( 790 const double* p1, const double* p2, 791 const double* p3, const double* p4 792 ) { 793 double U = distance(p1, p2, DIM); 794 double u = distance(p3, p4, DIM); 795 double V = distance(p2, p3, DIM); 796 double v = distance(p1, p4, DIM); 797 double W = distance(p3, p1, DIM); 798 double w = distance(p2, p4, DIM); 799 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 831