ug4
Loading...
Searching...
No Matches
math_util_impl.hpp
Go to the documentation of this file.
1/*
2 * Copyright (c) 2010-2015: G-CSC, Goethe University Frankfurt
3 * Author: Sebastian Reiter
4 *
5 * This file is part of UG4.
6 *
7 * UG4 is free software: you can redistribute it and/or modify it under the
8 * terms of the GNU Lesser General Public License version 3 (as published by the
9 * Free Software Foundation) with the following additional attribution
10 * requirements (according to LGPL/GPL v3 §7):
11 *
12 * (1) The following notice must be displayed in the Appropriate Legal Notices
13 * of covered and combined works: "Based on UG4 (www.ug4.org/license)".
14 *
15 * (2) The following notice must be displayed at a prominent place in the
16 * terminal output of covered works: "Based on UG4 (www.ug4.org/license)".
17 *
18 * (3) The following bibliography is recommended for citation and must be
19 * preserved in all covered files:
20 * "Reiter, S., Vogel, A., Heppner, I., Rupp, M., and Wittum, G. A massively
21 * parallel geometric multigrid solver on hierarchically distributed grids.
22 * Computing and visualization in science 16, 4 (2013), 151-164"
23 * "Vogel, A., Reiter, S., Rupp, M., Nägel, A., and Wittum, G. UG4 -- a novel
24 * flexible software system for simulating pde based models on high performance
25 * computers. Computing and visualization in science 16, 4 (2013), 165-179"
26 *
27 * This program is distributed in the hope that it will be useful,
28 * but WITHOUT ANY WARRANTY; without even the implied warranty of
29 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
30 * GNU Lesser General Public License for more details.
31 */
32
33#ifndef __H__UGMATH__MATH_UTIL_IMPL__
34#define __H__UGMATH__MATH_UTIL_IMPL__
35
36#include <algorithm>
37#include "common/common.h"
41
42namespace ug
43{
44
46// deg_to_rad
47template <class TNumber>
48inline TNumber
49deg_to_rad(TNumber deg)
50{
51 return deg * PI / 180.;
52}
53
55// rad_to_deg
56template <class TNumber>
57inline TNumber
58rad_to_deg(TNumber rad)
59{
60 return rad * 180. / PI;
61}
62
64// urand
65template <class TNumber>
66TNumber
67urand(TNumber lowerBound, TNumber upperBound)
68{
69 long t = rand();
70 if(t == RAND_MAX)
71 t -= 1;
72
73 return lowerBound + (TNumber)((upperBound - lowerBound) * ((double)t / (double)RAND_MAX));
74}
75
77// clip
78template <class TNumber>
79TNumber
80clip(TNumber val, TNumber lowerBound, TNumber upperBound)
81{
82 if(val > upperBound)
83 return upperBound;
84 else if(val < lowerBound)
85 return lowerBound;
86 return val;
87}
88
90template <class TNumber>
91inline TNumber sq(TNumber val)
92{
93 return val * val;
94}
95
97template <class vector_t>
98void CalculateCenter(vector_t& centerOut, const vector_t* pointSet,
99 size_t numPoints)
100{
101 for(size_t i = 0; i < centerOut.size(); ++i)
102 centerOut[i] = 0;
103
104 if(numPoints > 0){
105 for(size_t i = 0; i < numPoints; ++i)
106 VecAdd(centerOut, centerOut, pointSet[i]);
107
108 VecScale(centerOut, centerOut, 1. / (number)numPoints);
109 }
110}
111
112template <class vector_t>
113vector_t
114TriangleBarycenter(const vector_t& p1, const vector_t& p2, const vector_t& p3)
115{
116 vector_t bc;
117 VecScaleAdd(bc, 1./3., p1, 1./3., p2, 1./3., p3);
118 return bc;
119}
120
122template <class vector_t>
123number DropAPerpendicular(vector_t& vOut, const vector_t& v,
124 const vector_t& v0, const vector_t& v1)
125{
126// project v onto v' on the edge (v0, v1) so that (v'-v)*(v0-v1) = 0
127 vector_t e[2];
128 VecSubtract(e[0], v, v0);
129 VecSubtract(e[1], v1, v0);
130
131 number d1 = VecDot(e[0], e[1]);
132 number d2 = VecDot(e[1], e[1]);
133
134// avoid division by zero
135 if(fabs(d2) > SMALL)
136 {
137 // calculate the projection p'
138 number s = d1/d2;
139 VecScale(e[1], e[1], s);
140 VecAdd(vOut, v0, e[1]);
141 return s;
142 }
143 else
144 vOut = v0;
145 return 0;
146}
147
149template <class vector_t>
150vector_t PointOnRay(const vector_t& from, const vector_t& dir, number s)
151{
152 vector_t v = dir;
153 v *= s;
154 v += from;
155 return v;
156}
157
159template <class vector_t>
160number ProjectPointToRay(vector_t& vOut, const vector_t& v,
161 const vector_t& from, const vector_t& dir)
162{
163 vector_t tmpDir;
164 VecSubtract(tmpDir, v, from);
165
166 number d1 = VecDot(tmpDir, dir);
167 number d2 = VecDot(dir, dir);
168
169// avoid division by zero
170 if(fabs(d2) > SMALL)
171 {
172 // calculate the projection p'
173 number s = d1/d2;
174 VecScale(tmpDir, dir, s);
175 VecAdd(vOut, from, tmpDir);
176 return s;
177 }
178 else{
179 vOut = from;
180 }
181 return 0;
182}
183
185template <class vector_t>
186number ProjectPointToLine(vector_t& vOut, const vector_t& v,
187 const vector_t& from, const vector_t& to)
188{
189 vector_t dir;
190 VecSubtract(dir, to, from);
191 return ProjectPointToRay(vOut, v, from, dir);
192}
193
195template <class vector_t>
196inline
197number DistancePointToLine(const vector_t& v, const vector_t& v1,
198 const vector_t& v2)
199{
200 number t;
201 return DistancePointToLine(t, v, v1, v2);
202}
203
204template <class vector_t>
205number DistancePointToLine(number& tOut, const vector_t& v,
206 const vector_t& v1, const vector_t& v2)
207{
208 vector_t tmp;
209 tOut = DropAPerpendicular(tmp, v, v1, v2);
210 if(tOut > 1){
211 tOut = 1;
212 return VecDistance(v, v2);
213 }
214 else if(tOut < 0){
215 tOut = 0;
216 return VecDistance(v, v1);
217 }
218 else
219 return VecDistance(v, tmp);
220}
221
223template <class vector_t>
224inline
225number DistancePointToRay(const vector_t& v, const vector_t& from,
226 const vector_t& dir)
227{
228 vector_t tmp;
229 ProjectPointToRay(tmp, v, from, dir);
230 return VecDistance(v, tmp);
231}
232
233template <class vector_t>
234inline
235number DistancePointToRay(vector_t& vOut, number& tOut, const vector_t& v,
236 const vector_t& from, const vector_t& dir)
237{
238 tOut = ProjectPointToRay(vOut, v, from, dir);
239 return VecDistance(v, vOut);
240}
241
242template <class vector_t>
243number DistancePointToTriangle(vector_t& vOut, number& bc1Out, number& bc2Out,
244 const vector_t& p, const vector_t& v1, const vector_t& v2,
245 const vector_t& v3, const vector_t& n)
246{
247// parameter of line-intersection and of point-ray-projection.
248 number t;
249
250// first try an orthogonal projection of p onto the triangle
251 if(RayTriangleIntersection(vOut, bc1Out, bc2Out, t, v1, v2, v3, p, n))
252 { //min distance found
253 return VecDistance(vOut, p);
254 }
255
256// if ortho projection is not in triangle perform edge-point distance
257 number d, tmpDist, tmpT;
258 vector_t vDir, vTmp;
259 int bestIndex = 0;
260
261 VecSubtract(vDir, v2, v1);
262 d = DistancePointToRay(vOut, t, p, v1, vDir);
263 bc1Out = t; bc2Out = 0;
264
265 VecSubtract(vDir, v3, v1);
266 tmpDist = DistancePointToRay(vTmp, tmpT, p, v1, vDir);
267 if(tmpDist < d){
268 bestIndex = 1;
269 d = tmpDist;
270 t = tmpT;
271 bc1Out = 0; bc2Out = tmpT;
272 vOut = vTmp;
273 }
274
275 VecSubtract(vDir, v3, v2);
276 tmpDist = DistancePointToRay(vTmp, tmpT, p, v2, vDir);
277 if(tmpDist < d){
278 bestIndex = 2;
279 d = tmpDist;
280 t = tmpT;
281 bc1Out = 1. - t; bc2Out = t;
282 vOut = vTmp;
283 }
284
285// we now have to check whether the projection to an edge was
286// orthogonal. If not we'll return the distance to a point.
287 if(t > 0 && t < 1){
288 return d;
289 }
290 else{
291 switch(bestIndex){
292 case 0:
293 if(t < 0.5){
294 vOut = v1;
295 bc1Out = 0; bc2Out = 0;
296 }
297 else{
298 vOut = v2;
299 bc1Out = 1; bc2Out = 0;
300 }
301 break;
302
303 case 1:
304 if(t < 0.5){
305 vOut = v1;
306 bc1Out = 0; bc2Out = 0;
307 }
308 else{
309 vOut = v3;
310 bc1Out = 0; bc2Out = 1;
311 }
312 break;
313 case 2:
314 if(t < 0.5){
315 vOut = v2;
316 bc1Out = 1; bc2Out = 0;
317 }
318 else{
319 vOut = v3;
320 bc1Out = 0; bc2Out = 1;
321 }
322 break;
323 }
324 }
325
326// return the distance
327 return VecDistance(p, vOut);
328}
329
331template <class vector_t>
332number DistancePointToPlane(const vector_t& v, const vector_t& p,
333 const vector_t& n)
334{
335 vector_t vTmp;
336 ProjectPointToPlane(vTmp, v, p, n);
337 return VecDistance(vTmp, v);
338}
339
341template <class vector_t>
342void ProjectPointToPlane(vector_t& vOut, const vector_t& v,
343 const vector_t& p, const vector_t& n)
344{
345// the vector from p to v
346 vector_t t, norm;
347 VecSubtract(t, v, p);
348
349 VecNormalize(norm, n);
350
351// scale the normal with the dot-product with the direction
352 VecScale(t, norm, VecDot(norm, t));
353
354// subtract the scaled normal from the original point
355 VecSubtract(vOut, v, t);
356}
357
359template <class vector_t>
360bool RayPlaneIntersection(vector_t& vOut, number& tOut,
361 const vector_t& rayFrom, const vector_t& rayDir,
362 const vector_t& p, const vector_t& n)
363{
364// solve: t = (p-rayFrom)*n / rayDir*n
365 number denom = VecDot(rayDir, n);
366 if(fabs(denom) < SMALL_SQ)
367 return false;
368
369// calculate intersection parameter
370 vector_t v;
371 VecSubtract(v, p, rayFrom);
372 tOut = VecDot(v, n) / denom;
373
374// calculate intersection point
375 VecScale(v, rayDir, tOut);
376 VecAdd(vOut, rayFrom, v);
377 return true;
378}
379
380
382template <class vector_t>
383bool RayRayIntersection2d(vector_t &vOut, number& t0Out, number& t1Out,
384 const vector_t &p0, const vector_t &dir0,
385 const vector_t &p1, const vector_t &dir1)
386{
387 // we search for the intersection of the ray vFrom + tOut*vDir with the
388 // Line p0 + bcOut*(p1-p0). Intersection is true, if c0 in [0,1].
389 // We set up the system
390 // | dir0[0] , - dir1[0] | ( t0Out ) = ( (p1 - p0)[0] )
391 // | dir0[1] , - dir1[1] | ( t1Out ) = ( (p1 - p0)[1] )
392
393 vector_t v, b;
394 MathMatrix<2,2> m, mInv;
395
396 // set up matrix
397 m[0][0] = dir0[0]; m[0][1] = -1 * dir1[0];
398 m[1][0] = dir0[1]; m[1][1] = -1 * dir1[1];
399
400 // invert matrix
401 number det = Determinant(m);
402
403 // if det == 0.0 lines are parallel
404 if(det == 0.0) return false;
405
406 // compute inverse
407 Inverse(mInv, m);
408
409 // compute rhs of system
410 VecSubtract(b, p1, p0);
411
412 // solve system
413 MatVecMult(v, mInv, b);
414
415 // parameters of the intersection
416 t0Out = v[0];
417 t1Out = v[1];
418
419 // compute intersection point
420 vOut = p0;
421 VecScaleAppend(vOut, t0Out, dir0);
422
423 return true;
424}
425
427template <class vector_t>
428bool RayLineIntersection2d(vector_t &vOut, number& bcOut, number& tOut,
429 const vector_t &p0, const vector_t &p1,
430 const vector_t &vFrom, const vector_t &vDir,
431 number sml)
432{
433 vector_t dir0;
434 VecSubtract(dir0, p1, p0);
435 if(RayRayIntersection2d(vOut, bcOut, tOut, p0, dir0, vFrom, vDir)){
436 if(bcOut >= -sml && bcOut <= 1.+sml)
437 return true;
438 }
439 return false;
440}
441
442template <class vector_t>
443bool LineLineIntersection2d(vector_t &vOut, number& t0Out, number& t1Out,
444 const vector_t &from0, const vector_t &to0,
445 const vector_t &from1, const vector_t &to1,
446 const number threshold)
447{
448 vector_t dir0, dir1;
449 VecSubtract(dir0, to0, from0);
450 VecSubtract(dir1, to1, from1);
451 if(RayRayIntersection2d(vOut, t0Out, t1Out, from0, dir0, from1, dir1)){
452 if((t0Out >= -threshold) && (t0Out <= (1. + threshold))
453 && (t1Out >= -threshold) && (t1Out <= (1. + threshold)))
454 {
455 return true;
456 }
457 }
458 return false;
459}
460
461template <class vector_t>
462bool RayRayProjection(number& t1Out, number& t2Out,
463 const vector_t& from1, const vector_t& dir1,
464 const vector_t& from2, const vector_t& dir2)
465{
466 vector_t ab;
467 VecSubtract(ab, from2, from1);
468 number l11 = VecDot(dir1, dir1);
469 number l12 = -VecDot(dir1, dir2);
470 number l22 = VecDot(dir2, dir2);
471 number ra = VecDot(dir1, ab);
472 number rb = -VecDot(dir2, ab);
473
474// l11 and l22 are always >= 0
475 if((l11 < SMALL_SQ) || (l22 < SMALL_SQ))
476 return false;
477
478 number tmp = l11 * l22 - l12 * l12;
479 if(fabs(tmp) < SMALL)
480 return false;
481
482 t2Out = (l11*rb - l12*ra) / tmp;
483 t1Out = (ra - l12*t2Out) / l11;
484 return true;
485}
486
487template <class vector_t>
488bool LineLineProjection(number& t1Out, number& t2Out,
489 const vector_t& a1, const vector_t& a2,
490 const vector_t& b1, const vector_t& b2)
491{
492 vector_t dirA, dirB;
493 VecSubtract(dirA, a2, a1);
494 VecSubtract(dirB, b2, b1);
495
496 if(RayRayProjection(t1Out, t2Out, a1, dirA, b1, dirB)){
497 if((t1Out >= 0) && (t1Out <= 1.) && (t2Out >= 0) && (t2Out <= 1.))
498 return true;
499 return false;
500 }
501 return false;
502}
503
505template <class vector_t>
506bool RayTriangleIntersection(vector_t &vOut, number& bc1Out, number& bc2Out, number& tOut,
507 const vector_t &p0, const vector_t &p1, const vector_t &p2,
508 const vector_t &vFrom, const vector_t &vDir,
509 const number small)
510{
511// this piece of code is rather old and should probably be replaced by a
512// new method to improve speed and robustness.
513// The current implementation solves a 3x3 linear system using gaussian
514// elimination with pivoting to find the intersection of the ray with
515// the plane in which the triangle lies.
516// The local coordinates r and s, that describe the position of the
517// intersection point in the planes parameter-form, are then examined to
518// check whether the intersection-point lies inside the triangle or not
519// (r and s can at the same time be seen as the barycentric coordinates of
520// the triangle).
521// Division by zero is avoided (currently a == check is used - should be
522// replaced by a comparision to small)
523
524/*
525 p2
526 |\
527 v1| \v2
528 |____\
529 p0 v0 p1
530
531
532 r*v0 + s*v1 - t* vDir = b
533
534 m00 m01 m02
535 m10 m11 m12
536 m20 m21 m22
537*/
538
539 number m[3][3];
540 number b[3];
541
542 m[0][0] = p1.x() - p0.x(); m[0][1] = p2.x() - p0.x(); m[0][2] = -vDir.x(); b[0] = vFrom.x() - p0.x();
543 m[1][0] = p1.y() - p0.y(); m[1][1] = p2.y() - p0.y(); m[1][2] = -vDir.y(); b[1] = vFrom.y() - p0.y();
544 m[2][0] = p1.z() - p0.z(); m[2][1] = p2.z() - p0.z(); m[2][2] = -vDir.z(); b[2] = vFrom.z() - p0.z();
545
546 int i1, i2, i3, j;
547 number fac;
548 bc1Out = 0;
549 bc2Out = 0;
550 tOut = 0;
551
552 number dBestEntry = fabs(m[0][0]);
553 i1 = 0;
554 fac = fabs(m[1][0]);
555 if(fac > dBestEntry){
556 dBestEntry = fac;
557 i1 = 1;
558 }
559
560 if(fabs(m[2][0]) > dBestEntry)
561 i1 = 2;
562
563
564 if(m[i1][0]){
565 for(i2 = 0; i2 < 3; ++i2){
566 if(i2!=i1){
567 if(m[i2][0]){
568 fac = -m[i2][0]/m[i1][0];
569 for(j = 0; j < 3; ++j)
570 m[i2][j] = m[i2][j] + fac*m[i1][j];
571 b[i2] = b[i2] + fac * b[i1];
572 }
573 }
574 }
575
576 i2 = (i1 + 1) % 3;
577 i3 = (i1 + 2) % 3;
578 if(fabs(m[i2][1]) < fabs(m[i3][1])){
579 int ti = i2;
580 i2 = i3;
581 i3 = ti;
582 }
583
584 if((m[i2][1]!=0) && (m[i3][1]!=0)){
585 fac = -m[i3][1]/m[i2][1];
586 for(j = 1; j < 3; ++j)
587 m[i3][j] = m[i3][j] + fac*m[i2][j];
588 b[i3] = b[i3] + fac * b[i2];
589 }
590
591 //calculate tOut (t)
592 if(m[i3][2])
593 tOut = b[i3] / m[i3][2];
594 else if(b[i3] != 0)
595 return false;
596
597 //calculate bc2Out (s)
598 b[i2] -= tOut*m[i2][2];
599 if(m[i2][1])
600 bc2Out = b[i2] / m[i2][1];
601 else if(b[i2] != 0)
602 return false;
603
604 //calculate bc1Out (r)
605 b[i1] -= (tOut*m[i1][2] + bc2Out*m[i1][1]);
606 if(m[i1][0])
607 bc1Out = b[i1] / m[i1][0];
608 else if(b[i1] != 0)
609 return false;
610
611 if((bc1Out >=-small ) && (bc2Out >= -small ) && ((bc1Out + bc2Out) <= 1.+small))
612 {
613 vOut.x() = vFrom.x() + tOut*vDir.x();
614 vOut.y() = vFrom.y() + tOut*vDir.y();
615 vOut.z() = vFrom.z() + tOut*vDir.z();
616 return true;
617 }
618 }
619 return false;
620}
621
622template <class vector_t> inline
623bool RayTriangleIntersection(vector_t &vOut, const vector_t &p0,
624 const vector_t &p1, const vector_t &p2,
625 const vector_t &vFrom, const vector_t &vDir)
626{
627 number r, s, t;
628 return RayTriangleIntersection(vOut, r, s, t, p0, p1, p2, vFrom, vDir);
629}
630
631// ////////////////////////////////////////////////////////////////////////
632// template <class vector_t>
633// bool RayBoxIntersection(const vector_t& rayFrom, const vector_t& rayDir,
634// const vector_t& boxMin, const vector_t& boxMax,
635// number* tNearOut, number* tFarOut)
636// {
637// number tMin = 0, tMax = 0;// initialized only to avoid mislead compiler warnings...
638// number t1, t2;
639// bool bMinMaxSet = false;
640
641// if(fabs(rayDir.x()) > SMALL)
642// {
643// //get xNear and xFar
644// t1 = (boxMin.x() - rayFrom.x()) / rayDir.x();
645// t2 = (boxMax.x() - rayFrom.x()) / rayDir.x();
646// if(t1 > t2)
647// std::swap(t1, t2);
648// tMin = t1;
649// tMax = t2;
650// bMinMaxSet = true;
651// }
652// else
653// {
654// if(rayFrom.x() < boxMin.x())
655// return false;
656// if(rayFrom.x() > boxMax.x())
657// return false;
658// }
659
660// if(fabs(rayDir.y()) > SMALL)
661// {
662// //get yNear and yFar
663// t1 = (boxMin.y() - rayFrom.y()) / rayDir.y();
664// t2 = (boxMax.y() - rayFrom.y()) / rayDir.y();
665// if(t1 > t2)
666// std::swap(t1, t2);
667// if(bMinMaxSet)
668// {
669// if((t1 <= tMax) && (t2 >= tMin))
670// {
671// tMin = std::max(t1, tMin);
672// tMax = std::min(t2, tMax);
673// }
674// else
675// return false;
676// }
677// else
678// {
679// tMin = t1;
680// tMax = t2;
681// }
682// bMinMaxSet = true;
683// }
684// else
685// {
686// if(rayFrom.y() < boxMin.y())
687// return false;
688// if(rayFrom.y() > boxMax.y())
689// return false;
690// }
691
692// if(fabs(rayDir.z()) > SMALL)
693// {
694// //get zNear and zFar
695// t1 = (boxMin.z() - rayFrom.z()) / rayDir.z();
696// t2 = (boxMax.z() - rayFrom.z()) / rayDir.z();
697// if(t1 > t2)
698// std::swap(t1, t2);
699// if(bMinMaxSet)
700// {
701// if((t1 <= tMax) && (t2 >= tMin))
702// {
703// tMin = std::max(t1, tMin);
704// tMax = std::min(t2, tMax);
705// }
706// else
707// return false;
708// }
709// else
710// {
711// tMin = t1;
712// tMax = t2;
713// }
714// bMinMaxSet = true;
715// }
716// else
717// {
718// if(rayFrom.z() < boxMin.z())
719// return false;
720// if(rayFrom.z() > boxMax.z())
721// return false;
722// }
723
724// if(bMinMaxSet)
725// {
726// // the ray intersects the box
727// // lets calculate tNear and tFar and return true
728// if(fabs(tMin) > fabs(tMax))
729// std::swap(tMin, tMax);
730// if(tNearOut)
731// *tNearOut = tMin;
732// if(tFarOut)
733// *tFarOut = tMax;
734// return true;
735// }
736// else
737// {
738// // the ray has no direction -> we'll check if the From-point lies inside the box
739// if(BoxBoundProbe(rayFrom, boxMin, boxMax)){
740
741// if(*tNearOut)
742// *tNearOut = 0;
743// if(*tFarOut)
744// *tFarOut = 0;
745// return true;
746// }
747// else
748// return false;
749// }
750// }
751
753template <class vector_t>
754bool RayBoxIntersection(const vector_t& rayFrom, const vector_t& rayDir,
755 const vector_t& boxMin, const vector_t& boxMax,
756 number* tMinOut, number* tMaxOut)
757{
758 number tMin = 0, tMax = 0;// initialized only to avoid mislead compiler warnings...
759 number t1, t2;
760 bool bMinMaxSet = false;
761
762 for(size_t i = 0; i < vector_t::Size; ++i){
763 if(fabs(rayDir[i]) > SMALL)
764 {
765 //get near and far
766 t1 = (boxMin[i] - rayFrom[i]) / rayDir[i];
767 t2 = (boxMax[i] - rayFrom[i]) / rayDir[i];
768 if(t1 > t2)
769 std::swap(t1, t2);
770 if(bMinMaxSet)
771 {
772 if((t1 <= tMax) && (t2 >= tMin))
773 {
774 tMin = std::max(t1, tMin);
775 tMax = std::min(t2, tMax);
776 }
777 else
778 return false;
779 }
780 else
781 {
782 tMin = t1;
783 tMax = t2;
784 }
785 bMinMaxSet = true;
786 }
787 else if((rayFrom[i] < boxMin[i]) || (rayFrom[i] > boxMax[i]))
788 return false;
789 }
790
791 if(bMinMaxSet)
792 {
793 // the ray intersects the box
794 // lets calculate tNear and tFar and return true
795 if(tMin > tMax)
796 std::swap(tMin, tMax);
797 if(tMinOut)
798 *tMinOut = tMin;
799 if(tMaxOut)
800 *tMaxOut = tMax;
801 return true;
802 }
803 else
804 {
805 // the ray has no direction -> we'll check if the From-point lies inside the box
806 if(BoxBoundProbe(rayFrom, boxMin, boxMax)){
807
808 if(tMinOut)
809 *tMinOut = 0;
810 if(tMaxOut)
811 *tMaxOut = 0;
812 return true;
813 }
814 else{
815 if(tMinOut)
816 *tMinOut = 0;
817 if(tMaxOut)
818 *tMaxOut = 0;
819 return false;
820 }
821 }
822}
823
825template <class vector_t>
826bool LineBoxIntersection(const vector_t& v1, const vector_t& v2,
827 const vector_t& boxMin, const vector_t& boxMax)
828{
829 number tmin, tmax;
830
831 vector_t vDir;
832 VecSubtract(vDir, v2, v1);
833 if(RayBoxIntersection(v1, vDir, boxMin, boxMax, &tmin, &tmax))
834 {
835 return ((tmin * tmax < 0) || (tmin >= 0 && tmin <= 1.));
836 }
837
838 return false;
839}
840
842template <class vector_t>
844 const vector_t& v, const vector_t& dir,
845 const vector_t& center, number radius)
846{
847 vector_t p;
848 number s = ProjectPointToRay(p, center, v, dir);
849 number h = VecDistance(p, center);
850 if(h > radius)
851 return 0;
852 if(h > radius - SMALL){
853 s1Out = s;
854 s2Out = s;
855 return 1;
856 }
857
858 number dirLen = VecLength(dir);
859 if(dirLen == 0){
860 s1Out = s2Out = 0;
861 return 0;
862 }
863
864 number a = sqrt(radius * radius - h * h);
865
866 number sa = a / dirLen;
867 s1Out = s + sa;
868 s2Out = s - sa;
869 return 2;
870}
871
872template <class vector_t>
874 const vector_t& v1, const vector_t& v2,
875 const vector_t& center, number radius)
876{
877 vector_t dir;
878 VecSubtract(dir, v2, v1);
879 int num = RaySphereIntersection(s1Out, s2Out, v1, dir, center, radius);
880
881 switch(num){
882 case 0: return 0;
883 case 1: if((s1Out >= 0) && (s1Out <= 1))
884 return 1;
885 return 0;
886 case 2:{
887 if((s1Out < 0) || (s1Out > 1))
888 --num;
889 if((s2Out < 0) || (s2Out > 1))
890 --num;
891 else if(num == 1)
892 s1Out = s2Out;
893 return num;
894 }
895 }
896 return 0;
897}
898
900template <class vector_t>
901bool BoxBoxIntersection(const vector_t& box1Min, const vector_t& box1Max,
902 const vector_t& box2Min, const vector_t& box2Max)
903{
904 for(size_t i = 0; i < box1Min.size(); ++i){
905 if(box1Min[i] > box2Max[i] || box1Max[i] < box2Min[i])
906 return false;
907 }
908
909 return true;
910}
911
913template <class vector_t>
914number TriangleArea(const vector_t& p1, const vector_t& p2, const vector_t& p3)
915{
916 vector_t e[3];
917 VecSubtract(e[0], p2, p1);
918 VecSubtract(e[1], p3, p1);
919 GenVecCross(e[2], e[0], e[1]);
920 return 0.5 * VecLength(e[2]);
921}
922
923template <class vector_t>
924number QuadrilateralArea(const vector_t& p1, const vector_t& p2,
925 const vector_t& p3, const vector_t& p4)
926{
927 return TriangleArea(p1, p2, p3) + TriangleArea(p3, p4, p1);
928}
929
931template <class vector_t>
932number GeometricApproximationDegree(vector_t& n1, vector_t& n2, vector_t& n3,
933 vector_t& tn)
934{
935 return std::min(VecDot(n1, tn), std::min(VecDot(n2, tn), VecDot(n3, tn)));
936}
937
939template <class vector_t>
940number TriangleQuality_Area(const vector_t& p1, const vector_t& p2,
941 const vector_t& p3)
942{
943 number edgeSum = VecDistanceSq(p1, p2) + VecDistanceSq(p2, p3)
944 + VecDistanceSq(p1, p3);
945 if(edgeSum > SMALL)
946 {
947 // 4*sqrt(3) = 6.9282032
948 return 6.9282032 * TriangleArea(p1, p2, p3) / edgeSum;
949 }
950
951// a triangle whose sides have all zero length is considered to be a bad triangle.
952 return 0;
953}
954
956// BoxBoundProbe
957template <class vector_t>
958bool BoxBoundProbe(const vector_t& v, const vector_t& boxMin,
959 const vector_t& boxMax)
960{
961 for(size_t i = 0; i < v.size(); ++i){
962 if(v[i] < boxMin[i] || v[i] > boxMax[i])
963 return false;
964 }
965 return true;
966}
967
969// PointIsInsideTriangle
970template <class vector_t>
971bool PointIsInsideTriangle(const vector_t& v, const vector_t& v0,
972 const vector_t& v1, const vector_t& v2)
973{
974// we'll check for each side of the tri, whether v and the point of
975// the tri, that does not lie on the edge, do lie on the same side.
976 vector_t e; // the examined edge
977 vector_t edgeNorm; // the normal of the examined edge
978 vector_t tv1, tv2; // the direction of a tri-point to v
979
980// We compare against a small-constant which is
981// relative to the first edge of the triangle under examination.
982// Note: If we do not do this in a relative fashion, sufficiently
983// small-scale triangles will always return true!
984 VecSubtract(e, v1, v0);
985 number locSmall = VecTwoNormSq(e);
986 locSmall = locSmall * locSmall * SMALL;
987 // const number locSmall = VecTwoNorm(e) * SMALL;
988
989 //VecSubtract(e, v1, v0);
990 edgeNorm.x() = e.y();
991 edgeNorm.y() = -e.x();
992 VecSubtract(tv1, v2, v0);
993 VecSubtract(tv2, v, v0);
994 if(VecDot(tv1, edgeNorm) * VecDot(tv2, edgeNorm) < -locSmall)
995 return false;
996
997 VecSubtract(e, v2, v1);
998 edgeNorm.x() = e.y();
999 edgeNorm.y() = -e.x();
1000 VecSubtract(tv1, v0, v1);
1001 VecSubtract(tv2, v, v1);
1002 if(VecDot(tv1, edgeNorm) * VecDot(tv2, edgeNorm) < -locSmall)
1003 return false;
1004
1005 VecSubtract(e, v0, v2);
1006 edgeNorm.x() = e.y();
1007 edgeNorm.y() = -e.x();
1008 VecSubtract(tv1, v1, v2);
1009 VecSubtract(tv2, v, v2);
1010 if(VecDot(tv1, edgeNorm) * VecDot(tv2, edgeNorm) < -locSmall)
1011 return false;
1012
1013// all tests succeeded. return true.
1014 return true;
1015}
1016
1017// prevent usage with MathVector<3> (otherwise: undefined behavior due to uninit'ed edgeNorm[2]),
1018// even if all vertices are located in the same xy plane, edgeNorm[2] might be inf or nan!
1019template <>
1020inline bool PointIsInsideTriangle(const MathVector<3>& v, const MathVector<3>& v0,
1021 const MathVector<3>& v1, const MathVector<3>& v2)
1022{
1023 UG_THROW("Not implemented for 3D!");
1024 return false;
1025}
1026
1027
1029// PointIsInsideTriangle_HighAcc
1030template <class vector_t>
1031bool PointIsInsideTriangle_HighAcc(const vector_t& v, const vector_t& v0,
1032 const vector_t& v1, const vector_t& v2)
1033{
1034 return PointIsInsideTriangle(v, v0, v1, v2);
1035}
1036
1038// PointIsInsideQuadrilateral (for convex quads only)
1039template <class vector_t>
1040bool PointIsInsideQuadrilateral(const vector_t& v, const vector_t& v0,
1041 const vector_t& v1, const vector_t& v2,
1042 const vector_t& v3)
1043{
1044// we'll check for each side of the quad, whether v and a point of
1045// the quad, that does not lie on the edge, do lie on the same side.
1046 vector_t e; // the examined edge
1047 vector_t edgeNorm; // the normal of the examined edge
1048 vector_t tv1, tv2; // the direction of a quad-point to v
1049
1050// we compare against a small-constant which is
1051// relative to the first edge of the examined triangle
1052// Note: If we do not do this in a relative fashion, small-scale quads
1053// will always return true!
1054 VecSubtract(e, v1, v0);
1055 number locSmall = VecTwoNormSq(e);
1056 locSmall = locSmall * locSmall * SMALL;
1057
1058 //VecSubtract(e, v1, v0);
1059 edgeNorm.x() = e.y();
1060 edgeNorm.y() = -e.x();
1061 VecSubtract(tv1, v2, v0);
1062 VecSubtract(tv2, v, v0);
1063 if(VecDot(tv1, edgeNorm) * VecDot(tv2, edgeNorm) < -locSmall)
1064 return false;
1065
1066 VecSubtract(e, v2, v1);
1067 edgeNorm.x() = e.y();
1068 edgeNorm.y() = -e.x();
1069 VecSubtract(tv1, v3, v1);
1070 VecSubtract(tv2, v, v1);
1071 if(VecDot(tv1, edgeNorm) * VecDot(tv2, edgeNorm) < -locSmall)
1072 return false;
1073
1074 VecSubtract(e, v3, v2);
1075 edgeNorm.x() = e.y();
1076 edgeNorm.y() = -e.x();
1077 VecSubtract(tv1, v0, v2);
1078 VecSubtract(tv2, v, v2);
1079 if(VecDot(tv1, edgeNorm) * VecDot(tv2, edgeNorm) < -locSmall)
1080 return false;
1081
1082 VecSubtract(e, v0, v3);
1083 edgeNorm.x() = e.y();
1084 edgeNorm.y() = -e.x();
1085 VecSubtract(tv1, v1, v3);
1086 VecSubtract(tv2, v, v3);
1087 if(VecDot(tv1, edgeNorm) * VecDot(tv2, edgeNorm) < -locSmall)
1088 return false;
1089
1090// all tests succeeded. return true.
1091 return true;
1092}
1093
1094
1095// prevent usage with MathVector<3> (otherwise: undefined behavior due to uninit'ed edgeNorm[2]),
1096// even if all vertices are located in the same xy plane, edgeNorm[2] might be inf or nan!
1097template <>
1099 const MathVector<3>& v1, const MathVector<3>& v2,
1100 const MathVector<3>& v3)
1101{
1102 UG_THROW("Not implemented for 3D!");
1103 return false;
1104}
1105
1106
1108// PointIsInsideTetrahedron
1109template <class vector_t>
1110bool PointIsInsideTetrahedron(const vector_t& v, const vector_t& v0, const vector_t& v1,
1111 const vector_t& v2, const vector_t& v3)
1112{
1113// we'll check for each side of the tet, whether v and the point of
1114// the tet, that does not lie in the plane, lie on the same side.
1115 vector_t n; // the normal of the examined face
1116 vector_t e1, e2; // directions of two edges of a face
1117 number pn; // dot product of a point in the plane with the normal
1118
1119// we compare against a small-constant which is
1120// relative to the first edge of the examined triangle
1121// Note: If we do not do this in a relative fashion, small-scale quads
1122// will always return true!
1123 VecSubtract(e2, v1, v0);
1124 number locSmall = VecTwoNormSq(e2);
1125 locSmall = locSmall * locSmall * locSmall * SMALL;
1126
1127// check side 0, 2, 1
1128 VecSubtract(e1, v2, v0);
1129 //VecSubtract(e2, v1, v0);
1130 VecCross(n, e1, e2);
1131 pn = VecDot(v0, n);
1132 if((VecDot(v3, n) - pn) * (VecDot(v, n) - pn) < -locSmall)
1133 return false;
1134
1135// check side 0, 1, 3
1136 VecSubtract(e1, v1, v0);
1137 VecSubtract(e2, v3, v0);
1138 VecCross(n, e1, e2);
1139 pn = VecDot(v0, n);
1140 if((VecDot(v2, n) - pn) * (VecDot(v, n) - pn) < -locSmall)
1141 return false;
1142
1143// check side 1, 2, 3
1144 VecSubtract(e1, v2, v1);
1145 VecSubtract(e2, v3, v1);
1146 VecCross(n, e1, e2);
1147 pn = VecDot(v1, n);
1148 if((VecDot(v0, n) - pn) * (VecDot(v, n) - pn) < -locSmall)
1149 return false;
1150
1151// check side 0, 3, 2
1152 VecSubtract(e1, v3, v0);
1153 VecSubtract(e2, v2, v0);
1154 VecCross(n, e1, e2);
1155 pn = VecDot(v0, n);
1156 if((VecDot(v1, n) - pn) * (VecDot(v, n) - pn) < -locSmall)
1157 return false;
1158
1159// all tests succeeded. return true.
1160 return true;
1161}
1162
1164// ReflectVectorAtPlane
1165template <class vector_t>
1166void ReflectVectorAtPlane(vector_t& vReflectedOut, const vector_t& v,
1167 const vector_t& n, const vector_t& r0)
1168{
1169 const number s = 2 * (VecDot(v, n) - VecDot(n, r0)) / VecDot(n, n);
1170 VecScaleAdd(vReflectedOut, 1.0, v, -s, n);
1171}
1172
1173}// end of namespace
1174
1175#endif
parameterString p
parameterString s
A class for fixed size, dense matrices.
Definition math_matrix.h:63
a mathematical Vector with N entries.
Definition math_vector.h:97
MathMatrix< N, N, T >::value_type Determinant(const MathMatrix< N, N, T > &m)
Determinant of a matrix.
Definition math_matrix_functions_common_impl.hpp:382
MathMatrix< N, M, T >::value_type Inverse(MathMatrix< N, M, T > &mOut, const MathMatrix< M, N, T > &m)
Inverse of a matrix.
Definition math_matrix_functions_common_impl.hpp:560
#define UG_THROW(msg)
Definition error.h:57
double number
Definition types.h:124
void MatVecMult(vector_t_out &vOut, const matrix_t &m, const vector_t_in &v)
Matrix - Vector Multiplication.
Definition math_matrix_vector_functions_common_impl.hpp:49
number TriangleArea(const vector_t &p1, const vector_t &p2, const vector_t &p3)
calculates the are of the triangle defined by p1, p2 and p3
Definition math_util_impl.hpp:914
number ProjectPointToLine(vector_t &vOut, const vector_t &v, const vector_t &from, const vector_t &to)
finds the projection of v onto the line defined by from and to
Definition math_util_impl.hpp:186
bool LineLineIntersection2d(vector_t &vOut, number &t0Out, number &t1Out, const vector_t &from0, const vector_t &to0, const vector_t &from1, const vector_t &to1, const number threshold=0)
calculates the intersection of a two lines in 2d
Definition math_util_impl.hpp:443
bool PointIsInsideQuadrilateral(const vector_t &v, const vector_t &v0, const vector_t &v1, const vector_t &v2, const vector_t &v3)
Returns true if the point lies inside or on the boundary of a quadrilateral.
Definition math_util_impl.hpp:1040
bool LineLineProjection(number &t1Out, number &t2Out, const vector_t &a1, const vector_t &a2, const vector_t &b1, const vector_t &b2)
calculates the closest point between the rays through the given lines.
Definition math_util_impl.hpp:488
number QuadrilateralArea(const vector_t &p1, const vector_t &p2, const vector_t &p3, const vector_t &p4)
calculates the are of the triangle defined by p1, p2 and p3
Definition math_util_impl.hpp:924
vector_t TriangleBarycenter(const vector_t &p1, const vector_t &p2, const vector_t &p3)
Calculates the barycenter of a triangle (1/3) * (p1+p2+p3)
Definition math_util_impl.hpp:114
bool RayPlaneIntersection(vector_t &vOut, number &tOut, const vector_t &rayFrom, const vector_t &rayDir, const vector_t &p, const vector_t &n)
calculates the intersection of the ray rayFrom+t*rayDir and the plane (x-p)*n=0.
Definition math_util_impl.hpp:360
bool RayBoxIntersection(const vector_t &rayFrom, const vector_t &rayDir, const vector_t &boxMin, const vector_t &boxMax, number *tNearOut=NULL, number *tFarOut=NULL)
checks if a ray is intersecting a box
Definition math_util_impl.hpp:754
bool RayRayIntersection2d(vector_t &vOut, number &t0Out, number &t1Out, const vector_t &p0, const vector_t &dir0, const vector_t &p1, const vector_t &dir1)
calculates the intersection of two Rays in 2d
Definition math_util_impl.hpp:383
number DistancePointToRay(const vector_t &v, const vector_t &from, const vector_t &dir)
calculates the distance of a point to a ray
Definition math_util_impl.hpp:225
TNumber urand(TNumber lowerBound, TNumber upperBound)
uniform distributed random numbers in [lowerBound, upperBound[. Use srand to set a seed.
Definition math_util_impl.hpp:67
number DistancePointToLine(const vector_t &v, const vector_t &v1, const vector_t &v2)
calculates the distance of a point to a line segment
Definition math_util_impl.hpp:197
number DropAPerpendicular(vector_t &vOut, const vector_t &v, const vector_t &v0, const vector_t &v1)
finds the projection of v onto the line defined by v0 and v1
Definition math_util_impl.hpp:123
bool PointIsInsideTriangle(const vector_t &v, const vector_t &v0, const vector_t &v1, const vector_t &v2)
Returns true if the point lies inside or on the boundary of a triangle.
Definition math_util_impl.hpp:971
vector_t PointOnRay(const vector_t &from, const vector_t &dir, number s)
returns the point described by a relative ray coordinate
Definition math_util_impl.hpp:150
TNumber sq(TNumber val)
returns the square of a value (val*val)
Definition math_util_impl.hpp:91
number GeometricApproximationDegree(vector_t &n1, vector_t &n2, vector_t &n3, vector_t &tn)
the returned degree lies between 0 and 1. The closer to 1 the better.
Definition math_util_impl.hpp:932
bool BoxBoundProbe(const vector_t &v, const vector_t &boxMin, const vector_t &boxMax)
Returns true if the point lies inside or on the boundary of the box.
Definition math_util_impl.hpp:958
bool RayLineIntersection2d(vector_t &vOut, number &bcOut, number &tOut, const vector_t &p0, const vector_t &p1, const vector_t &vFrom, const vector_t &vDir, number sml=0)
calculates the intersection of a ray with a Line in 2d
Definition math_util_impl.hpp:428
void CalculateCenter(vector_t &centerOut, const vector_t *pointSet, size_t numPoints)
calculates the center of a point-set
Definition math_util_impl.hpp:98
int RaySphereIntersection(number &s1Out, number &s2Out, const vector_t &v, const vector_t &dir, const vector_t &center, number radius)
checks whether the line segment (v1, v2) intersect the given sphere.
Definition math_util_impl.hpp:843
bool RayTriangleIntersection(vector_t &vOut, number &bc1Out, number &bc2Out, number &tOut, const vector_t &p0, const vector_t &p1, const vector_t &p2, const vector_t &vFrom, const vector_t &vDir, const number small=SMALL)
calculates the intersection of a ray with a triangle
Definition math_util_impl.hpp:506
TNumber clip(TNumber val, TNumber lowerBound, TNumber upperBound)
clips a number to the given interval [lowerBound, upperBound].
Definition math_util_impl.hpp:80
bool LineBoxIntersection(const vector_t &v1, const vector_t &v2, const vector_t &boxMin, const vector_t &boxMax)
checks whether the given line-segment (v1, v2) intersect the given box.
Definition math_util_impl.hpp:826
number TriangleQuality_Area(const vector_t &p1, const vector_t &p2, const vector_t &p3)
returns a value between 0 and 1. The higher the better.
Definition math_util_impl.hpp:940
void ProjectPointToPlane(vector_t &vOut, const vector_t &v, const vector_t &p, const vector_t &n)
projects v onto the plane defined by the point p and the planes normal n.
Definition math_util_impl.hpp:342
bool PointIsInsideTriangle_HighAcc(const vector_t &v, const vector_t &v0, const vector_t &v1, const vector_t &v2)
Returns true if the point lies inside or on a side of the given triangle.
Definition math_util_impl.hpp:1031
TNumber deg_to_rad(TNumber deg)
Definition math_util_impl.hpp:49
bool PointIsInsideTetrahedron(const vector_t &v, const vector_t &v0, const vector_t &v1, const vector_t &v2, const vector_t &v3)
Returns true if the point lies inside or on the boundary of a tetrahedron.
Definition math_util_impl.hpp:1110
number DistancePointToPlane(const vector_t &v, const vector_t &p, const vector_t &n)
Calculates the distance between the specified point and the plane.
Definition math_util_impl.hpp:332
void ReflectVectorAtPlane(vector_t &vReflectedOut, const vector_t &v, const vector_t &n, const vector_t &r0)
reflects a vector at a plane
Definition math_util_impl.hpp:1166
bool BoxBoxIntersection(const vector_t &box1Min, const vector_t &box1Max, const vector_t &box2Min, const vector_t &box2Max)
checks whether two boxes intersect.
Definition math_util_impl.hpp:901
int LineSphereIntersection(number &s1Out, number &s2Out, const vector_t &v1, const vector_t &v2, const vector_t &center, number radius)
Definition math_util_impl.hpp:873
bool RayRayProjection(number &t1Out, number &t2Out, const vector_t &from1, const vector_t &dir1, const vector_t &from2, const vector_t &dir2)
Calculates the parameter values at wich two rays are closest.
Definition math_util_impl.hpp:462
TNumber rad_to_deg(TNumber rad)
Definition math_util_impl.hpp:58
number ProjectPointToRay(vector_t &vOut, const vector_t &v, const vector_t &from, const vector_t &dir)
finds the projection of v onto the ray defined by from and dir
Definition math_util_impl.hpp:160
number DistancePointToTriangle(vector_t &vOut, number &bc1Out, number &bc2Out, const vector_t &p, const vector_t &v1, const vector_t &v2, const vector_t &v3, const vector_t &n)
calculates the minimal distance of a point to a triangle.
Definition math_util_impl.hpp:243
vector_t::value_type VecLength(const vector_t &v)
returns the length of v. Slower than VecLengthSq.
Definition math_vector_functions_common_impl.hpp:341
void VecScaleAppend(vector_t &vOut, typename vector_t::value_type s1, const vector_t &v1)
Scales a Vector and adds it to a second vector.
Definition math_vector_functions_common_impl.hpp:126
void GenVecCross(MathVector< dim > &result, const MathVector< dim > &v_1, const MathVector< dim > &v_2)
calculates the usual cross product in 3d, and the (det, 0) vector as a cross product in 2d
Definition math_vector_functions_common_impl.hpp:465
void VecScaleAdd(vector_t &vOut, typename vector_t::value_type s1, const vector_t &v1, typename vector_t::value_type s2, const vector_t &v2)
Scales two Vectors, adds them and returns the sum in a third vector.
Definition math_vector_functions_common_impl.hpp:265
vector_t::value_type VecTwoNormSq(const vector_t &v)
Definition math_vector_functions_common_impl.hpp:585
void VecNormalize(vector_t &vOut, const vector_t &v)
scales a vector_t to unit length
Definition math_vector_functions_common_impl.hpp:501
void VecSubtract(vector_t &vOut, const vector_t &v1, const vector_t &v2)
subtracts v2 from v1 and stores the result in a vOut
Definition math_vector_functions_common_impl.hpp:226
vector_t::value_type VecDistanceSq(const vector_t &v1, const vector_t &v2)
returns the squared distance of two vector_ts.
Definition math_vector_functions_common_impl.hpp:351
vector_t::value_type VecDistance(const vector_t &v1, const vector_t &v2)
returns the distance of two vector_ts.
Definition math_vector_functions_common_impl.hpp:375
void VecAdd(vector_t &vOut, const vector_t &v1, const vector_t &v2)
adds two MathVector<N>s and stores the result in a third one
Definition math_vector_functions_common_impl.hpp:185
void VecScale(vector_t &vOut, const vector_t &v, typename vector_t::value_type s)
scales a MathVector<N>
Definition math_vector_functions_common_impl.hpp:252
void VecCross(vector_t &vOut, const vector_t &v1, const vector_t &v2)
calculates the cross product of two Vectors of dimension 3. It makes no sense to use VecCross for vec...
Definition math_vector_functions_common_impl.hpp:437
vector_t::value_type VecDot(const vector_t &v1, const vector_t &v2)
returns the dot-product of two vector_ts
Definition math_vector_functions_common_impl.hpp:385
the ug namespace
const number PI
Definition math_constants.h:45
const number SMALL
Definition math_constants.h:41
const number SMALL_SQ
Definition math_constants.h:42
Definition file_io_grdecl.cpp:60