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 : #include <vector>
34 : #include "math_util.h"
35 : #include "../ugmath.h"
36 : #include "../ugmath_types.h"
37 : #include "lineintersect_utils.h"
38 :
39 :
40 : using namespace std;
41 :
42 : namespace ug
43 : {
44 :
45 0 : bool TriangleCircumcenter(vector2& centerOut, const vector2& p1,
46 : const vector2& p2, const vector2& p3)
47 : {
48 : using std::swap;
49 :
50 : number d12 = VecDistanceSq(p1, p2);
51 : number d23 = VecDistanceSq(p2, p3);
52 : number d13 = VecDistanceSq(p1, p3);
53 : /*
54 : // if any of the sides is too short, then we have to abort
55 : if(d12 < SMALL || d23 < SMALL || d13 < SMALL){
56 : return false;
57 : }
58 : */
59 : // centers of sides and side-normals
60 : vector2 c1, c2, n1, n2;
61 :
62 : // for maximal accuracy, we'll choose the two longest sides
63 0 : if(d12 >= d23){
64 : VecScaleAdd(c1, 0.5, p1, 0.5, p2);
65 : VecSubtract(n1, p2, p1);// perform swapping later
66 0 : if(d23 >= d13){
67 : VecScaleAdd(c2, 0.5, p2, 0.5, p3);
68 : VecSubtract(n2, p3, p2);// perform swapping later
69 : }
70 : else{
71 : VecScaleAdd(c2, 0.5, p1, 0.5, p3);
72 : VecSubtract(n2, p3, p1);// perform swapping later
73 : }
74 : }
75 : else{
76 : VecScaleAdd(c1, 0.5, p2, 0.5, p3);
77 : VecSubtract(n1, p3, p2);// perform swapping later
78 0 : if(d12 >= d13){
79 : VecScaleAdd(c2, 0.5, p1, 0.5, p2);
80 : VecSubtract(n2, p2, p1);// perform swapping later
81 : }
82 : else{
83 : VecScaleAdd(c2, 0.5, p1, 0.5, p3);
84 : VecSubtract(n2, p3, p1);// perform swapping later
85 : }
86 : }
87 :
88 : // swap normal-coefficients
89 : swap(n1.x(), n1.y());
90 0 : n1.x() *= -1.;
91 : swap(n2.x(), n2.y());
92 0 : n2.x() *= -1.;
93 :
94 : // calculate intersection of the two lines
95 : number t0, t1;
96 0 : if(!RayRayIntersection2d(centerOut, t0, t1, c1, n1, c2, n2)){
97 : // line-line intersection failed. return the barycenter instead.
98 : return false;
99 : }
100 :
101 : return true;
102 : }
103 :
104 0 : bool TriangleCircumcenter(vector3& centerOut, const vector3& p1,
105 : const vector3& p2, const vector3& p3)
106 : {
107 0 : number d12 = VecDistanceSq(p1, p2);
108 0 : number d13 = VecDistanceSq(p1, p3);
109 0 : number d23 = VecDistanceSq(p2, p3);
110 :
111 : // sort p1, p2 and p3 into v1, v2, v3, so that v1 is the vertex at which the
112 : // two shorter edges meet
113 : vector3 v1, v2, v3;
114 0 : if(d12 < d13){
115 0 : if(d13 < d23){
116 : // d23 is biggest
117 : v1 = p1; v2 = p2; v3 = p3;
118 : }
119 : else{
120 : // d13 is biggest
121 : v1 = p2; v2 = p3; v3 = p1;
122 : }
123 : }
124 : else{
125 0 : if(d12 < d23){
126 : // d23 is biggest
127 : v1 = p1; v2 = p2; v3 = p3;
128 : }
129 : else{
130 : // d12 is biggest
131 : v1 = p3; v2 = p1; v3 = p2;
132 : }
133 : }
134 :
135 : vector3 dir12, dir13, dir23;
136 : VecSubtract(dir12, v2, v1);
137 : VecSubtract(dir13, v3, v1);
138 : VecSubtract(dir23, v3, v2);
139 :
140 : // we'll construct a line through the center of v1, v2, with a direction
141 : // normal to dir12, which lies in the triangles plane.
142 : vector3 proj;
143 0 : ProjectPointToRay(proj, v1, v2, dir23);
144 : VecSubtract(proj, proj, v1);
145 :
146 : number a = VecDot(dir12, dir12);
147 0 : if(fabs(a) < SMALL){
148 : //UG_LOG("a == 0: " << v1 << v2 << v3 << endl);
149 : return false;
150 : }
151 :
152 : number b = VecDot(dir12, proj);
153 0 : if(fabs(b) < SMALL){
154 : //UG_LOG("b == 0: " << v1 << v2 << v3 << endl);
155 : return false;
156 : }
157 :
158 : vector3 n1;
159 0 : VecScaleAdd(n1, -b / a, dir12, 1., proj);
160 :
161 : vector3 c1;
162 : VecScaleAdd(c1, 0.5, v1, 0.5, v2);
163 :
164 : // we also have to construct a plane. The planes normal is dir13.
165 : vector3 c2;
166 : VecScaleAdd(c2, 0.5, v1, 0.5, v3);
167 :
168 : // finally calculate the intersection of the line with the plane.
169 : number t;
170 0 : bool retVal = RayPlaneIntersection(centerOut, t, c1, n1, c2, dir13);
171 : /*
172 : number dist1 = fabs(VecDistanceSq(centerOut, v1) - VecDistanceSq(centerOut, v2));
173 : number dist2 = fabs(VecDistanceSq(centerOut, v1) - VecDistanceSq(centerOut, v3));
174 : if( retVal &&
175 : (dist1 > SMALL || dist2 > SMALL))
176 : {
177 : UG_LOG("Center distance mismatch!!!\n");
178 : UG_LOG("v: " << v1 << v2 << v3 << endl);
179 : UG_LOG("|v2-v1| = " << dist1 <<", |v3-v1| = " << dist2 << endl);
180 : }*/
181 0 : return retVal;
182 : }
183 :
184 0 : bool FindNormal(vector3& normOut, const vector3& v)
185 : {
186 : // normalize v to avoid problems with oversized vectors.
187 : vector3 n;
188 0 : VecNormalize(n, v);
189 :
190 : //TODO: check whether 0.7 is a good threshold
191 : const number dotThreshold = 0.7;
192 :
193 : // try projections of the unit-normals onto the
194 : // plane which is defined by normOut and the origin.
195 0 : for(int i = 0; i < 3; ++i){
196 : vector3 e(0, 0, 0);
197 0 : e[i] = 1;
198 : number d = VecDot(e, n);
199 0 : if(fabs(d) < dotThreshold){
200 : // the projection will be sufficient to calculate a normal.
201 : VecScale(n, n, d);
202 : VecSubtract(normOut, e, n);
203 0 : VecNormalize(normOut, normOut);
204 0 : return true;
205 : }
206 : }
207 :
208 : return false;
209 : }
210 :
211 0 : bool ConstructOrthonormalSystem(matrix33& matOut, const vector3& v,
212 : size_t vColInd)
213 : {
214 : // the first expression has been always true for unsigned int (AV)
215 0 : if(/*vColInd < 0 ||*/ vColInd > 2)
216 : return false;
217 :
218 0 : vector3 newCols[2];
219 : vector3 n;
220 :
221 : // normalize v
222 0 : VecNormalize(n, v);
223 :
224 : // find a normal to n
225 0 : if(!FindNormal(newCols[0], n))
226 : return false;
227 :
228 : // calculate the last col
229 0 : VecCross(newCols[1], n, newCols[0]);
230 :
231 : // copy cols to matOut
232 : int nColCount = 0;
233 0 : for(size_t j = 0; j < 3; ++j){
234 0 : if(j == vColInd){
235 0 : for(size_t i = 0; i < 3; ++ i)
236 0 : matOut[i][j] = n[i];
237 : }
238 : else{
239 0 : for(size_t i = 0; i < 3; ++ i)
240 0 : matOut[i][j] = newCols[nColCount][i];
241 0 : ++nColCount;
242 : }
243 : }
244 :
245 : return true;
246 : }
247 :
248 0 : void CalculateCovarianceMatrix(matrix33& matOut, const vector3* pointSet,
249 : const vector3& center, size_t numPoints)
250 : {
251 : // set all matrix entries to 0
252 0 : for(size_t i = 0; i < 3; ++i){
253 0 : for(size_t j = 0; j < 3; ++j)
254 0 : matOut[i][j] = 0;
255 : }
256 :
257 : // sum the vector-products
258 0 : for(size_t pInd = 0; pInd < numPoints; ++pInd){
259 0 : for(size_t i = 0; i < 3; ++i){
260 0 : for(size_t j = 0; j < 3; ++j){
261 0 : matOut[i][j] += (pointSet[pInd][i] - center[i]) *
262 0 : (pointSet[pInd][j] - center[j]);
263 : }
264 : }
265 : }
266 0 : }
267 :
268 0 : bool FindClosestPlane(vector3& centerOut, vector3& normalOut,
269 : const vector3* pointSet, size_t numPoints)
270 : {
271 : // calculate the center of the point set
272 : vector3 center;
273 0 : CalculateCenter(center, pointSet, numPoints);
274 :
275 : // calculate the covariance matrix of the point set
276 : matrix33 matCo;
277 0 : CalculateCovarianceMatrix(matCo, pointSet, center, numPoints);
278 :
279 : // find the eigenvector of smallest eigenvalue of the covariance matrix
280 : number lambda[3];
281 0 : vector3 ev[3];
282 0 : if(!CalculateEigenvalues(matCo, lambda[0], lambda[1], lambda[2],
283 : ev[0], ev[1], ev[2]))
284 : {
285 : return false;
286 : }
287 :
288 0 : VecNormalize(normalOut, ev[0]);
289 : centerOut = center;
290 :
291 0 : return true;
292 : }
293 :
294 0 : bool TransformPointSetTo2D(vector2* pointSetOut, const vector3* pointSet,
295 : size_t numPoints)
296 : {
297 : // calculate the center of the point set
298 : vector3 center;
299 : vector3 normal;
300 :
301 0 : if(!FindClosestPlane(center, normal, pointSet, numPoints))
302 : return false;
303 :
304 : // lambda[0] is the smallest (absolute) eigenvalue.
305 : // ev[0] can be regarded as the normal of a plane through center.
306 : // we now have to find the matrix that transforms this plane
307 : // to a plane parallel to the x-y-plane.
308 : matrix33 matOrtho;
309 0 : if(!ConstructOrthonormalSystem(matOrtho, normal, 2))
310 : return false;
311 :
312 : // we need the inverse of this matrix. Since it is a orthonormal
313 : // matrix, the inverse corresponds to the transposed matrix.
314 : // move the point set and rotate it afterwards.
315 0 : for(size_t i = 0; i < numPoints; ++i){
316 : vector3 vTmpIn;
317 : vector3 vTmpOut;
318 0 : VecSubtract(vTmpIn, pointSet[i], center);
319 : TransposedMatVecMult(vTmpOut, matOrtho, vTmpIn);
320 :
321 : // trivial projection
322 0 : pointSetOut[i].x() = vTmpOut.x();
323 0 : pointSetOut[i].y() = vTmpOut.y();
324 : }
325 :
326 : // done. return true.
327 : return true;
328 : }
329 :
330 : ////////////////////////////////////////////////////////////////////////
331 0 : bool RayRayIntersection3d(vector3& aOut, vector3& bOut,
332 : const vector3& p0, const vector3& dir0,
333 : const vector3& p1, const vector3& dir1)
334 : {
335 : vector3 vNear, vAB;
336 : bool trueIntersection;
337 : vector3 q0, q1;
338 : VecAdd(q0, p0, dir0);
339 : VecAdd(q1, p1, dir1);
340 :
341 0 : IntersectLineSegments(p0.x(), p0.y(), p0.z(), q0.x(), q0.y(), q0.z(),
342 : p1.x(), p1.y(), p1.z(), q1.x(), q1.y(), q1.z(),
343 : true, SMALL,
344 : aOut.x(), aOut.y(), aOut.z(), bOut.x(), bOut.y(), bOut.z(),
345 : vNear.x(), vNear.y(), vNear.z(), vAB.x(), vAB.y(), vAB.z(),
346 : trueIntersection);
347 :
348 0 : return trueIntersection;
349 : }
350 :
351 : ////////////////////////////////////////////////////////////////////////
352 0 : bool LineLineIntersection3d(vector3& aOut, vector3& bOut,
353 : const vector3& a1, const vector3& a2,
354 : const vector3& b1, const vector3& b2)
355 : {
356 : vector3 vNear, vAB;
357 : bool trueIntersection;
358 :
359 0 : IntersectLineSegments(a1.x(), a1.y(), a1.z(), a2.x(), a2.y(), a2.z(),
360 : b1.x(), b1.y(), b1.z(), b2.x(), b2.y(), b2.z(),
361 : false, SMALL,
362 : aOut.x(), aOut.y(), aOut.z(), bOut.x(), bOut.y(), bOut.z(),
363 : vNear.x(), vNear.y(), vNear.z(), vAB.x(), vAB.y(), vAB.z(),
364 : trueIntersection);
365 :
366 0 : return trueIntersection;
367 : }
368 :
369 0 : number DistanceLineToLine(const vector3& a1, const vector3& a2,
370 : const vector3& b1, const vector3& b2)
371 : {
372 : vector3 vA, vB, vNear, vAB;
373 : bool trueIntersection;
374 :
375 0 : IntersectLineSegments(a1.x(), a1.y(), a1.z(), a2.x(), a2.y(), a2.z(),
376 : b1.x(), b1.y(), b1.z(), b2.x(), b2.y(), b2.z(),
377 : false, SMALL,
378 : vA.x(), vA.y(), vA.z(), vB.x(), vB.y(), vB.z(),
379 : vNear.x(), vNear.y(), vNear.z(), vAB.x(), vAB.y(), vAB.z(),
380 : trueIntersection);
381 :
382 0 : return VecLength(vAB);
383 : }
384 :
385 :
386 0 : bool RayCylinderIntersection(number& tMinOut, number& tMaxOut, const vector3& rayFrom,
387 : const vector3& rayDir, const vector3& cylCenter,
388 : const vector3& cylAxis, number cylRadius)
389 : {
390 : // find the closest points on the ray and the cylinder axis
391 : vector3 vr, va;
392 0 : RayRayIntersection3d(vr, va, rayFrom, rayDir, cylCenter, cylAxis);
393 :
394 : number rayAxisDist = VecDistance(vr, va);
395 :
396 0 : if(rayAxisDist > cylRadius)
397 : return false;
398 :
399 : // parallel and normalized orthogonal component of rayDir regarding cylAxis
400 : vector3 dirParallel, dirOrtho;
401 : vector3 axisNormized;
402 0 : VecNormalize(axisNormized, cylAxis);
403 : VecScale(dirParallel, cylAxis, VecDot(axisNormized, rayDir));
404 : VecSubtract(dirOrtho, rayDir, dirParallel);
405 :
406 : // calculate the distance from rayFrom, where rayFrom+t*dirOrtho intersects the cylinder
407 0 : number orthoIntersectionDist = sqrt(cylRadius*cylRadius - rayAxisDist*rayAxisDist);
408 :
409 : number orthoLen = VecLength(dirOrtho);
410 0 : if(orthoLen < SMALL)
411 : return false;
412 :
413 : // the factor by which rayDir has to be scaled to reach the intersection point
414 : // starting from vr.
415 0 : number scaleFac = orthoIntersectionDist / orthoLen;
416 :
417 : // get the parameter at which vr lies on the given ray
418 : number t = 0;
419 0 : if(fabs(rayDir.x()) >= fabs(rayDir.y())){
420 0 : if(fabs(rayDir.x()) >= fabs(rayDir.z())){
421 0 : if(rayDir.x() == 0)
422 : return false;
423 0 : t = (vr.x() - rayFrom.x()) / rayDir.x();
424 : }
425 : else
426 0 : t = (vr.z() - rayFrom.z()) / rayDir.z();
427 : }
428 0 : else if(fabs(rayDir.y()) >= fabs(rayDir.z()))
429 0 : t = (vr.y() - rayFrom.y()) / rayDir.y();
430 : else
431 0 : t = (vr.z() - rayFrom.z()) / rayDir.z();
432 :
433 0 : tMinOut = t - scaleFac;
434 0 : tMaxOut = t + scaleFac;
435 0 : return true;
436 : }
437 :
438 :
439 : ////////////////////////////////////////////////////////////////////////////////
440 0 : number CalculateTetrahedronVolume(const vector3& a, const vector3& b,
441 : const vector3& c, const vector3& d)
442 : {
443 : //
444 : // Assume a tetrahedron with vertices a, b, c, d, then the volume is given by
445 : //
446 : // V = 1/6 * |VecDot( (a-d) , VecCross((b-d), (c-d)) )|
447 : //
448 : number tetrahedronVolume;
449 : vector3 ad;
450 : vector3 bd;
451 : vector3 cd;
452 : vector3 cross;
453 :
454 : VecSubtract(ad, a, d);
455 : VecSubtract(bd, b, d);
456 : VecSubtract(cd, c, d);
457 :
458 0 : VecCross(cross, bd, cd);
459 :
460 0 : tetrahedronVolume = abs(VecDot(ad, cross) / 6);
461 :
462 0 : return tetrahedronVolume;
463 : }
464 :
465 0 : number CalculatePyramidVolume(const vector3& a, const vector3& b,
466 : const vector3& c, const vector3& d, const vector3& e)
467 : {
468 : vector3 center, h_, h, c1, c2, da, ba, cb, cd;
469 : VecSubtract(da, d, a);
470 : VecSubtract(ba, b, a);
471 : VecSubtract(cb, c, b);
472 : VecSubtract(cd, c, d);
473 0 : VecCross(c1, da, ba);
474 0 : VecCross(c2, cb, cd);
475 0 : number A = 0.5 * VecLength(c1) + 0.5 * VecLength(c2);
476 :
477 : vector3 arr[] = { a, b, c, d };
478 0 : CalculateCenter(center, arr, 4);
479 :
480 0 : number height = DistancePointToPlane(e, center, c1);
481 :
482 0 : return 1.0 / 3.0 * A * height;
483 : }
484 :
485 0 : number CalculatePrismVolume(const vector3& a, const vector3& b,
486 : const vector3& c, const vector3& d, const vector3& e,
487 : const vector3& f) {
488 : number result = 0;
489 : vector3 center;
490 : vector3 arr[] = { a, b, c, d, e, f };
491 0 : CalculateCenter(center, arr, 6);
492 :
493 0 : result += CalculateTetrahedronVolume(a, b, c, center);
494 0 : result += CalculateTetrahedronVolume(d, e, f, center);
495 :
496 0 : result +=CalculatePyramidVolume(a, b, e, d, center);
497 0 : result +=CalculatePyramidVolume(b, c, f, e, center);
498 0 : result +=CalculatePyramidVolume(c, a, d, f, center);
499 :
500 0 : return result;
501 : }
502 :
503 0 : number CalculateHexahedronVolume(const vector3& a, const vector3& b,
504 : const vector3& c, const vector3& d, const vector3& e, const vector3& f,
505 : const vector3& g, const vector3& h) {
506 : number result = 0;
507 : vector3 arr[] = { a, b, c, d, e, f, g, h };
508 : vector3 center;
509 0 : CalculateCenter(center, arr, 8);
510 :
511 : // top and bottom
512 0 : result += CalculatePyramidVolume(a, b, c, d, center);
513 0 : result += CalculatePyramidVolume(e, f, g, h, center);
514 :
515 : // sides
516 0 : result += CalculatePyramidVolume(a, b, f, e, center);
517 0 : result += CalculatePyramidVolume(b, c, g, f, center);
518 :
519 0 : result += CalculatePyramidVolume(c, d, g, h, center);
520 0 : result += CalculatePyramidVolume(a, d, h, e, center);
521 :
522 0 : return result;
523 : }
524 :
525 0 : number CalculateOctahedronVolume(const vector3& a, const vector3& b,
526 : const vector3& c, const vector3& d, const vector3& e,
527 : const vector3& f) {
528 : number result = 0;
529 :
530 : MathVector<3> x31, x42, x51, n;
531 : MathVector<3> x01;
532 :
533 : VecSubtract(x31, d, b);
534 : VecSubtract(x42, e, c);
535 : VecSubtract(x51, f, b);
536 0 : VecCross(n, x31, x42);
537 :
538 : //VecSubtract(x31, d, b);
539 : //VecSubtract(x42, e, c);
540 : VecSubtract(x01, a, b);
541 : //VecCross(n, x31, x42);
542 :
543 0 : number volTopPyr = (1./6.) * fabs(VecDot(n, x51));
544 0 : number volBottomPyr = (1./6.) * fabs(VecDot(n, x01));
545 :
546 0 : result += volTopPyr + volBottomPyr;
547 :
548 0 : return result;
549 : }
550 :
551 : ////////////////////////////////////////////////////////////////////////////////
552 : // Returns the BinomialCoefficient
553 25 : int BinomCoeff(int n, int k)
554 : {
555 : // if n == 0, we define result to be always zero iff k != 0
556 29 : if(!n&&k) return 0;
557 :
558 : // if denominator is greater than nominator: flip
559 29 : if(n-k>k) return BinomCoeff(n,n-k);
560 :
561 : // if equal binomCoeff is always one
562 25 : if(n==k) return 1;
563 :
564 : // do recursion
565 16 : return BinomCoeff(n-1,k)*n/(n-k);
566 : }
567 :
568 : }// end of namespace
|
