Line data Source code
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"
38 : #include "common/math/math_vector_matrix/math_vector_functions.h"
39 : #include "common/math/math_vector_matrix/math_matrix_functions.h"
40 : #include "common/math/math_vector_matrix/math_matrix_vector_functions.h"
41 :
42 : namespace ug
43 : {
44 :
45 : ////////////////////////////////////////////////////////////////////////
46 : // deg_to_rad
47 : template <class TNumber>
48 : inline TNumber
49 : deg_to_rad(TNumber deg)
50 : {
51 0 : return deg * PI / 180.;
52 : }
53 :
54 : ////////////////////////////////////////////////////////////////////////
55 : // rad_to_deg
56 : template <class TNumber>
57 : inline TNumber
58 : rad_to_deg(TNumber rad)
59 : {
60 0 : return rad * 180. / PI;
61 : }
62 :
63 : ////////////////////////////////////////////////////////////////////////
64 : // urand
65 : template <class TNumber>
66 : TNumber
67 : urand(TNumber lowerBound, TNumber upperBound)
68 : {
69 1800 : long t = rand();
70 1800 : if(t == RAND_MAX)
71 : t -= 1;
72 :
73 1800 : return lowerBound + (TNumber)((upperBound - lowerBound) * ((double)t / (double)RAND_MAX));
74 : }
75 :
76 : ////////////////////////////////////////////////////////////////////////
77 : // clip
78 : template <class TNumber>
79 : TNumber
80 : clip(TNumber val, TNumber lowerBound, TNumber upperBound)
81 : {
82 0 : if(val > upperBound)
83 : return upperBound;
84 0 : else if(val < lowerBound)
85 0 : return lowerBound;
86 : return val;
87 : }
88 :
89 : ////////////////////////////////////////////////////////////////////////
90 : template <class TNumber>
91 : inline TNumber sq(TNumber val)
92 : {
93 0 : return val * val;
94 : }
95 :
96 : ////////////////////////////////////////////////////////////////////////
97 : template <class vector_t>
98 0 : void CalculateCenter(vector_t& centerOut, const vector_t* pointSet,
99 : size_t numPoints)
100 : {
101 0 : for(size_t i = 0; i < centerOut.size(); ++i)
102 0 : centerOut[i] = 0;
103 :
104 0 : if(numPoints > 0){
105 0 : for(size_t i = 0; i < numPoints; ++i)
106 0 : VecAdd(centerOut, centerOut, pointSet[i]);
107 :
108 0 : VecScale(centerOut, centerOut, 1. / (number)numPoints);
109 : }
110 0 : }
111 :
112 : template <class vector_t>
113 : vector_t
114 : TriangleBarycenter(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 :
121 : ////////////////////////////////////////////////////////////////////////
122 : template <class vector_t>
123 0 : number 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 0 : 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 0 : if(fabs(d2) > SMALL)
136 : {
137 : // calculate the projection p'
138 0 : 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 0 : return 0;
146 : }
147 :
148 : ////////////////////////////////////////////////////////////////////////
149 : template <class vector_t>
150 : vector_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 :
158 : ////////////////////////////////////////////////////////////////////////
159 : template <class vector_t>
160 0 : number 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 0 : if(fabs(d2) > SMALL)
171 : {
172 : // calculate the projection p'
173 0 : number s = d1/d2;
174 : VecScale(tmpDir, dir, s);
175 : VecAdd(vOut, from, tmpDir);
176 0 : return s;
177 : }
178 : else{
179 : vOut = from;
180 : }
181 0 : return 0;
182 : }
183 :
184 : ////////////////////////////////////////////////////////////////////////
185 : template <class vector_t>
186 : number 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 :
194 : ////////////////////////////////////////////////////////////////////////
195 : template <class vector_t>
196 : inline
197 : number DistancePointToLine(const vector_t& v, const vector_t& v1,
198 : const vector_t& v2)
199 : {
200 : number t;
201 0 : return DistancePointToLine(t, v, v1, v2);
202 : }
203 :
204 : template <class vector_t>
205 0 : number DistancePointToLine(number& tOut, const vector_t& v,
206 : const vector_t& v1, const vector_t& v2)
207 : {
208 : vector_t tmp;
209 0 : tOut = DropAPerpendicular(tmp, v, v1, v2);
210 0 : if(tOut > 1){
211 0 : tOut = 1;
212 0 : return VecDistance(v, v2);
213 : }
214 0 : else if(tOut < 0){
215 0 : tOut = 0;
216 0 : return VecDistance(v, v1);
217 : }
218 : else
219 0 : return VecDistance(v, tmp);
220 : }
221 :
222 : ////////////////////////////////////////////////////////////////////////
223 : template <class vector_t>
224 : inline
225 0 : number DistancePointToRay(const vector_t& v, const vector_t& from,
226 : const vector_t& dir)
227 : {
228 : vector_t tmp;
229 0 : ProjectPointToRay(tmp, v, from, dir);
230 0 : return VecDistance(v, tmp);
231 : }
232 :
233 : template <class vector_t>
234 : inline
235 0 : number DistancePointToRay(vector_t& vOut, number& tOut, const vector_t& v,
236 : const vector_t& from, const vector_t& dir)
237 : {
238 0 : tOut = ProjectPointToRay(vOut, v, from, dir);
239 0 : return VecDistance(v, vOut);
240 : }
241 :
242 : template <class vector_t>
243 0 : number 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 0 : if(RayTriangleIntersection(vOut, bc1Out, bc2Out, t, v1, v2, v3, p, n))
252 : { //min distance found
253 0 : 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 0 : d = DistancePointToRay(vOut, t, p, v1, vDir);
263 0 : bc1Out = t; bc2Out = 0;
264 :
265 : VecSubtract(vDir, v3, v1);
266 0 : tmpDist = DistancePointToRay(vTmp, tmpT, p, v1, vDir);
267 0 : if(tmpDist < d){
268 : bestIndex = 1;
269 : d = tmpDist;
270 0 : t = tmpT;
271 0 : bc1Out = 0; bc2Out = tmpT;
272 : vOut = vTmp;
273 : }
274 :
275 : VecSubtract(vDir, v3, v2);
276 0 : tmpDist = DistancePointToRay(vTmp, tmpT, p, v2, vDir);
277 0 : if(tmpDist < d){
278 : bestIndex = 2;
279 : d = tmpDist;
280 0 : t = tmpT;
281 0 : 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 0 : if(t > 0 && t < 1){
288 : return d;
289 : }
290 : else{
291 0 : switch(bestIndex){
292 0 : case 0:
293 0 : if(t < 0.5){
294 : vOut = v1;
295 0 : bc1Out = 0; bc2Out = 0;
296 : }
297 : else{
298 : vOut = v2;
299 0 : bc1Out = 1; bc2Out = 0;
300 : }
301 : break;
302 :
303 0 : case 1:
304 0 : if(t < 0.5){
305 : vOut = v1;
306 0 : bc1Out = 0; bc2Out = 0;
307 : }
308 : else{
309 : vOut = v3;
310 0 : bc1Out = 0; bc2Out = 1;
311 : }
312 : break;
313 0 : case 2:
314 0 : if(t < 0.5){
315 : vOut = v2;
316 0 : bc1Out = 1; bc2Out = 0;
317 : }
318 : else{
319 : vOut = v3;
320 0 : bc1Out = 0; bc2Out = 1;
321 : }
322 : break;
323 : }
324 : }
325 :
326 : // return the distance
327 0 : return VecDistance(p, vOut);
328 : }
329 :
330 : ////////////////////////////////////////////////////////////////////////
331 : template <class vector_t>
332 0 : number DistancePointToPlane(const vector_t& v, const vector_t& p,
333 : const vector_t& n)
334 : {
335 : vector_t vTmp;
336 0 : ProjectPointToPlane(vTmp, v, p, n);
337 0 : return VecDistance(vTmp, v);
338 : }
339 :
340 : ////////////////////////////////////////////////////////////////////////
341 : template <class vector_t>
342 0 : void 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 0 : 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 0 : }
357 :
358 : ////////////////////////////////////////////////////////////////////////
359 : template <class vector_t>
360 0 : bool 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 0 : if(fabs(denom) < SMALL_SQ)
367 : return false;
368 :
369 : // calculate intersection parameter
370 : vector_t v;
371 : VecSubtract(v, p, rayFrom);
372 0 : 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 :
381 : ////////////////////////////////////////////////////////////////////////
382 : template <class vector_t>
383 137376 : bool 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 137376 : m[0][0] = dir0[0]; m[0][1] = -1 * dir1[0];
398 137376 : 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 137376 : 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 137376 : t0Out = v[0];
417 137376 : t1Out = v[1];
418 :
419 : // compute intersection point
420 : vOut = p0;
421 137376 : VecScaleAppend(vOut, t0Out, dir0);
422 :
423 : return true;
424 : }
425 :
426 : ////////////////////////////////////////////////////////////////////////
427 : template <class vector_t>
428 0 : bool 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 0 : if(RayRayIntersection2d(vOut, bcOut, tOut, p0, dir0, vFrom, vDir)){
436 0 : if(bcOut >= -sml && bcOut <= 1.+sml)
437 0 : return true;
438 : }
439 : return false;
440 : }
441 :
442 : template <class vector_t>
443 137376 : bool 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 137376 : if(RayRayIntersection2d(vOut, t0Out, t1Out, from0, dir0, from1, dir1)){
452 137376 : if((t0Out >= -threshold) && (t0Out <= (1. + threshold))
453 52608 : && (t1Out >= -threshold) && (t1Out <= (1. + threshold)))
454 : {
455 32000 : return true;
456 : }
457 : }
458 : return false;
459 : }
460 :
461 : template <class vector_t>
462 : bool 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 :
487 : template <class vector_t>
488 : bool 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 :
504 : ////////////////////////////////////////////////////////////////////////
505 : template <class vector_t>
506 0 : bool 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 0 : 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 0 : 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 0 : 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 0 : bc1Out = 0;
549 0 : bc2Out = 0;
550 0 : tOut = 0;
551 :
552 0 : number dBestEntry = fabs(m[0][0]);
553 : i1 = 0;
554 0 : fac = fabs(m[1][0]);
555 0 : if(fac > dBestEntry){
556 : dBestEntry = fac;
557 : i1 = 1;
558 : }
559 :
560 0 : if(fabs(m[2][0]) > dBestEntry)
561 : i1 = 2;
562 :
563 :
564 0 : if(m[i1][0]){
565 0 : for(i2 = 0; i2 < 3; ++i2){
566 0 : if(i2!=i1){
567 0 : if(m[i2][0]){
568 0 : fac = -m[i2][0]/m[i1][0];
569 0 : for(j = 0; j < 3; ++j)
570 0 : m[i2][j] = m[i2][j] + fac*m[i1][j];
571 0 : b[i2] = b[i2] + fac * b[i1];
572 : }
573 : }
574 : }
575 :
576 0 : i2 = (i1 + 1) % 3;
577 0 : i3 = (i1 + 2) % 3;
578 0 : if(fabs(m[i2][1]) < fabs(m[i3][1])){
579 : int ti = i2;
580 : i2 = i3;
581 : i3 = ti;
582 : }
583 :
584 0 : if((m[i2][1]!=0) && (m[i3][1]!=0)){
585 0 : fac = -m[i3][1]/m[i2][1];
586 0 : for(j = 1; j < 3; ++j)
587 0 : m[i3][j] = m[i3][j] + fac*m[i2][j];
588 0 : b[i3] = b[i3] + fac * b[i2];
589 : }
590 :
591 : //calculate tOut (t)
592 0 : if(m[i3][2])
593 0 : tOut = b[i3] / m[i3][2];
594 0 : else if(b[i3] != 0)
595 : return false;
596 :
597 : //calculate bc2Out (s)
598 0 : b[i2] -= tOut*m[i2][2];
599 0 : if(m[i2][1])
600 0 : bc2Out = b[i2] / m[i2][1];
601 0 : else if(b[i2] != 0)
602 : return false;
603 :
604 : //calculate bc1Out (r)
605 0 : b[i1] -= (tOut*m[i1][2] + bc2Out*m[i1][1]);
606 0 : if(m[i1][0])
607 0 : bc1Out = b[i1] / m[i1][0];
608 0 : else if(b[i1] != 0)
609 : return false;
610 :
611 0 : if((bc1Out >=-small ) && (bc2Out >= -small ) && ((bc1Out + bc2Out) <= 1.+small))
612 : {
613 0 : vOut.x() = vFrom.x() + tOut*vDir.x();
614 0 : vOut.y() = vFrom.y() + tOut*vDir.y();
615 0 : vOut.z() = vFrom.z() + tOut*vDir.z();
616 0 : return true;
617 : }
618 : }
619 : return false;
620 : }
621 :
622 : template <class vector_t> inline
623 : bool 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 :
752 : ////////////////////////////////////////////////////////////////////////
753 : template <class vector_t>
754 0 : bool 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 0 : number tMin = 0, tMax = 0;// initialized only to avoid mislead compiler warnings...
759 : number t1, t2;
760 : bool bMinMaxSet = false;
761 :
762 0 : for(size_t i = 0; i < vector_t::Size; ++i){
763 0 : if(fabs(rayDir[i]) > SMALL)
764 : {
765 : //get near and far
766 0 : t1 = (boxMin[i] - rayFrom[i]) / rayDir[i];
767 0 : t2 = (boxMax[i] - rayFrom[i]) / rayDir[i];
768 0 : if(t1 > t2)
769 : std::swap(t1, t2);
770 0 : if(bMinMaxSet)
771 : {
772 0 : if((t1 <= tMax) && (t2 >= tMin))
773 : {
774 0 : tMin = std::max(t1, tMin);
775 0 : tMax = std::min(t2, tMax);
776 : }
777 : else
778 : return false;
779 : }
780 : else
781 : {
782 0 : tMin = t1;
783 0 : tMax = t2;
784 : }
785 : bMinMaxSet = true;
786 : }
787 0 : else if((rayFrom[i] < boxMin[i]) || (rayFrom[i] > boxMax[i]))
788 : return false;
789 : }
790 :
791 0 : if(bMinMaxSet)
792 : {
793 : // the ray intersects the box
794 : // lets calculate tNear and tFar and return true
795 0 : if(tMin > tMax)
796 : std::swap(tMin, tMax);
797 0 : if(tMinOut)
798 0 : *tMinOut = tMin;
799 0 : if(tMaxOut)
800 0 : *tMaxOut = tMax;
801 0 : return true;
802 : }
803 : else
804 : {
805 : // the ray has no direction -> we'll check if the From-point lies inside the box
806 0 : if(BoxBoundProbe(rayFrom, boxMin, boxMax)){
807 :
808 0 : if(tMinOut)
809 0 : *tMinOut = 0;
810 0 : if(tMaxOut)
811 0 : *tMaxOut = 0;
812 0 : return true;
813 : }
814 : else{
815 0 : if(tMinOut)
816 0 : *tMinOut = 0;
817 0 : if(tMaxOut)
818 0 : *tMaxOut = 0;
819 0 : return false;
820 : }
821 : }
822 : }
823 :
824 : ////////////////////////////////////////////////////////////////////////
825 : template <class vector_t>
826 0 : bool 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 0 : if(RayBoxIntersection(v1, vDir, boxMin, boxMax, &tmin, &tmax))
834 : {
835 0 : return ((tmin * tmax < 0) || (tmin >= 0 && tmin <= 1.));
836 : }
837 :
838 : return false;
839 : }
840 :
841 : ////////////////////////////////////////////////////////////////////////
842 : template <class vector_t>
843 0 : int RaySphereIntersection(number& s1Out, number& s2Out,
844 : const vector_t& v, const vector_t& dir,
845 : const vector_t& center, number radius)
846 : {
847 : vector_t p;
848 0 : number s = ProjectPointToRay(p, center, v, dir);
849 0 : number h = VecDistance(p, center);
850 0 : if(h > radius)
851 : return 0;
852 0 : if(h > radius - SMALL){
853 0 : s1Out = s;
854 0 : s2Out = s;
855 0 : return 1;
856 : }
857 :
858 : number dirLen = VecLength(dir);
859 0 : if(dirLen == 0){
860 0 : s1Out = s2Out = 0;
861 0 : return 0;
862 : }
863 :
864 0 : number a = sqrt(radius * radius - h * h);
865 :
866 0 : number sa = a / dirLen;
867 0 : s1Out = s + sa;
868 0 : s2Out = s - sa;
869 0 : return 2;
870 : }
871 :
872 : template <class vector_t>
873 : int LineSphereIntersection(number& s1Out, number& s2Out,
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 :
899 : ////////////////////////////////////////////////////////////////////////
900 : template <class vector_t>
901 : bool BoxBoxIntersection(const vector_t& box1Min, const vector_t& box1Max,
902 : const vector_t& box2Min, const vector_t& box2Max)
903 : {
904 0 : for(size_t i = 0; i < box1Min.size(); ++i){
905 0 : if(box1Min[i] > box2Max[i] || box1Max[i] < box2Min[i])
906 : return false;
907 : }
908 :
909 : return true;
910 : }
911 :
912 : ////////////////////////////////////////////////////////////////////////
913 : template <class vector_t>
914 0 : number TriangleArea(const vector_t& p1, const vector_t& p2, const vector_t& p3)
915 : {
916 0 : vector_t e[3];
917 : VecSubtract(e[0], p2, p1);
918 : VecSubtract(e[1], p3, p1);
919 0 : GenVecCross(e[2], e[0], e[1]);
920 0 : return 0.5 * VecLength(e[2]);
921 : }
922 :
923 : template <class vector_t>
924 : number 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 :
930 : ////////////////////////////////////////////////////////////////////////
931 : template <class vector_t>
932 0 : number GeometricApproximationDegree(vector_t& n1, vector_t& n2, vector_t& n3,
933 : vector_t& tn)
934 : {
935 0 : return std::min(VecDot(n1, tn), std::min(VecDot(n2, tn), VecDot(n3, tn)));
936 : }
937 :
938 : ////////////////////////////////////////////////////////////////////////
939 : template <class vector_t>
940 0 : number TriangleQuality_Area(const vector_t& p1, const vector_t& p2,
941 : const vector_t& p3)
942 : {
943 0 : number edgeSum = VecDistanceSq(p1, p2) + VecDistanceSq(p2, p3)
944 0 : + VecDistanceSq(p1, p3);
945 0 : if(edgeSum > SMALL)
946 : {
947 : // 4*sqrt(3) = 6.9282032
948 0 : 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 :
955 : ////////////////////////////////////////////////////////////////////////
956 : // BoxBoundProbe
957 : template <class vector_t>
958 : bool BoxBoundProbe(const vector_t& v, const vector_t& boxMin,
959 : const vector_t& boxMax)
960 : {
961 0 : for(size_t i = 0; i < v.size(); ++i){
962 0 : if(v[i] < boxMin[i] || v[i] > boxMax[i])
963 : return false;
964 : }
965 : return true;
966 : }
967 :
968 : ////////////////////////////////////////////////////////////////////////
969 : // PointIsInsideTriangle
970 : template <class vector_t>
971 320000000 : bool 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 320000000 : locSmall = locSmall * locSmall * SMALL;
987 : // const number locSmall = VecTwoNorm(e) * SMALL;
988 :
989 : //VecSubtract(e, v1, v0);
990 320000000 : edgeNorm.x() = e.y();
991 320000000 : edgeNorm.y() = -e.x();
992 : VecSubtract(tv1, v2, v0);
993 : VecSubtract(tv2, v, v0);
994 320000000 : if(VecDot(tv1, edgeNorm) * VecDot(tv2, edgeNorm) < -locSmall)
995 : return false;
996 :
997 : VecSubtract(e, v2, v1);
998 198009536 : edgeNorm.x() = e.y();
999 198009536 : edgeNorm.y() = -e.x();
1000 : VecSubtract(tv1, v0, v1);
1001 : VecSubtract(tv2, v, v1);
1002 198009536 : if(VecDot(tv1, edgeNorm) * VecDot(tv2, edgeNorm) < -locSmall)
1003 : return false;
1004 :
1005 : VecSubtract(e, v0, v2);
1006 101631488 : edgeNorm.x() = e.y();
1007 101631488 : edgeNorm.y() = -e.x();
1008 : VecSubtract(tv1, v1, v2);
1009 : VecSubtract(tv2, v, v2);
1010 101631488 : 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!
1019 : template <>
1020 0 : inline bool PointIsInsideTriangle(const MathVector<3>& v, const MathVector<3>& v0,
1021 : const MathVector<3>& v1, const MathVector<3>& v2)
1022 : {
1023 0 : UG_THROW("Not implemented for 3D!");
1024 : return false;
1025 : }
1026 :
1027 :
1028 : ////////////////////////////////////////////////////////////////////////
1029 : // PointIsInsideTriangle_HighAcc
1030 : template <class vector_t>
1031 : bool 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 :
1037 : ////////////////////////////////////////////////////////////////////////
1038 : // PointIsInsideQuadrilateral (for convex quads only)
1039 : template <class vector_t>
1040 320000000 : bool 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 320000000 : locSmall = locSmall * locSmall * SMALL;
1057 :
1058 : //VecSubtract(e, v1, v0);
1059 320000000 : edgeNorm.x() = e.y();
1060 320000000 : edgeNorm.y() = -e.x();
1061 : VecSubtract(tv1, v2, v0);
1062 : VecSubtract(tv2, v, v0);
1063 320000000 : if(VecDot(tv1, edgeNorm) * VecDot(tv2, edgeNorm) < -locSmall)
1064 : return false;
1065 :
1066 : VecSubtract(e, v2, v1);
1067 222560704 : edgeNorm.x() = e.y();
1068 222560704 : edgeNorm.y() = -e.x();
1069 : VecSubtract(tv1, v3, v1);
1070 : VecSubtract(tv2, v, v1);
1071 222560704 : if(VecDot(tv1, edgeNorm) * VecDot(tv2, edgeNorm) < -locSmall)
1072 : return false;
1073 :
1074 : VecSubtract(e, v3, v2);
1075 153289120 : edgeNorm.x() = e.y();
1076 153289120 : edgeNorm.y() = -e.x();
1077 : VecSubtract(tv1, v0, v2);
1078 : VecSubtract(tv2, v, v2);
1079 153289120 : if(VecDot(tv1, edgeNorm) * VecDot(tv2, edgeNorm) < -locSmall)
1080 : return false;
1081 :
1082 : VecSubtract(e, v0, v3);
1083 90135456 : edgeNorm.x() = e.y();
1084 90135456 : edgeNorm.y() = -e.x();
1085 : VecSubtract(tv1, v1, v3);
1086 : VecSubtract(tv2, v, v3);
1087 90135456 : 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!
1097 : template <>
1098 0 : inline bool PointIsInsideQuadrilateral(const MathVector<3>& v, const MathVector<3>& v0,
1099 : const MathVector<3>& v1, const MathVector<3>& v2,
1100 : const MathVector<3>& v3)
1101 : {
1102 0 : UG_THROW("Not implemented for 3D!");
1103 : return false;
1104 : }
1105 :
1106 :
1107 : ////////////////////////////////////////////////////////////////////////
1108 : // PointIsInsideTetrahedron
1109 : template <class vector_t>
1110 320000000 : bool 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 320000000 : locSmall = locSmall * locSmall * locSmall * SMALL;
1126 :
1127 : // check side 0, 2, 1
1128 : VecSubtract(e1, v2, v0);
1129 : //VecSubtract(e2, v1, v0);
1130 320000000 : VecCross(n, e1, e2);
1131 : pn = VecDot(v0, n);
1132 640000000 : 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 188222528 : VecCross(n, e1, e2);
1139 : pn = VecDot(v0, n);
1140 376445056 : 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 93249760 : VecCross(n, e1, e2);
1147 : pn = VecDot(v1, n);
1148 186499520 : 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 33298208 : VecCross(n, e1, e2);
1155 : pn = VecDot(v0, n);
1156 66596416 : 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 :
1163 : ////////////////////////////////////////////////////////////////////////
1164 : // ReflectVectorAtPlane
1165 : template <class vector_t>
1166 : void 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
|
