PeriDyno 1.0.0
Loading...
Searching...
No Matches
ComputeGeometry.inl
Go to the documentation of this file.
1#include "Platform.h"
2
3#include "Matrix.h"
4#include "Vector/Vector2D.h"
5#include "Vector.h"
6#include <algorithm>
7#include <cassert>
8#include <cmath>
9
10namespace dyno
11{
12 namespace cgeo
13 {
14 #define REAL_infinity 1.0e30
15 #define REAL_ZERO 1.0e-5
16 #define REAL_EPS 1e-4
17 #define REAL_EQUAL(a,b) (((a < b + REAL_EPS) && (a > b - REAL_EPS)) ? true : false)
18 #define REAL_GREAT(a,b) ((a > REAL_EPS + b)? true: false)
19 #define REAL_LESS(a,b) ((a + REAL_EPS < b)? true: false)
20
21 template<typename T>
22 DYN_FUNC void Swap(T& a, T& b) { T c = a; a = b; b = c;}
23 DYN_FUNC float Dot(Vec3f const& U, Vec3f const& V)
24 {
25 float dot = U[0] * V[0] + U[1] * V[1] + U[2] * V[2];
26 return dot;
27 }
28
29 DYN_FUNC float Dot(Vec2f const& U, Vec2f const& V)
30 {
31 float dot = U[0] * V[0] + U[1] * V[1];
32 return dot;
33 }
34
35 DYN_FUNC Vec3f Cross(Vec3f const& U, Vec3f const& V)
36 {
37 Vec3f cross =
38 {
39 U[1] * V[2] - U[2] * V[1],
40 U[2] * V[0] - U[0] * V[2],
41 U[0] * V[1] - U[1] * V[0]
42 };
43 return cross;
44 }
45
46 // U (V x W)
47 DYN_FUNC float DotCross(Vec3f const& U, Vec3f const& V,
48 Vec3f const& W)
49 {
50 return Dot(U, Cross(V, W));
51 }
52
53 DYN_FUNC Vec2f Perp(Vec2f const& v)
54 {
55 return Vec2f(v[1], -v[0]);
56 }
57
58 // 2D Cross
59 // Dot((x0,x1),Perp(y0,y1)) = x0*y1 - x1*y0
60 DYN_FUNC float DotPerp(Vec2f const& v0, Vec2f const& v1)
61 {
62 return Dot(v0, Perp(v1));
63 }
64
65 DYN_FUNC bool Sign(Vec3f const& n, Vec3f const& p0, Vec3f const& p1, Vec3f const& p2)
66 {
67 return REAL_GREAT(Dot(n, Cross(p1 - p0, p2 - p0)), 0.f);
68 }
69
70 DYN_FUNC bool isOverLap(float& c0, float& c1, float a0, float a1, float b0, float b1)
71 {
72 c0 = fmax(a0, b0);
73 c1 = fmin(a1, b1);
74 if (REAL_LESS(c1, c0)) return false;
75 return true;
76 }
77
78 // get closest point v from point p to triangle a0a1a2 of minimum distance
79 DYN_FUNC Vec3f getProjectionVF(Vec3f p,
80 Vec3f a0, Vec3f a1, Vec3f a2)
81 {
82 //triangle base: a0, e0(a0,a1), e1(a0,a2)
83 //point base: p
84 //projection base: v (para0, para1, para2)
85
86 Vec3f d = a0 - p;
87 Vec3f e0 = a1 - a0;
88 Vec3f e1 = a2 - a0;
89 float a00 = e0.dot(e0);
90 float a01 = e0.dot(e1);
91 float a11 = e1.dot(e1);
92 float b0 = e0.dot(d);
93 float b1 = e1.dot(d);
94 float f = d.dot(d);
95 float det = fmax(a00 * a11 - a01 * a01, 0.f);
96 float s = a01 * b1 - a11 * b0;
97 float t = a01 * b0 - a00 * b1;
98 if (s + t <= det) {
99 if (REAL_LESS(s, 0.f)) {
100 if (REAL_LESS(t, 0.f)) {
101 //region 4
102 if (REAL_LESS(b0,0.f)){
103 t = 0.f;
104 if (-b0 >= a00){
105 s = 1.f;
106 }
107 else{
108 s = -b0 / a00;
109 }
110 }
111 else{
112 s = 0.f;
113 if (REAL_GREAT(b1,0.f)||REAL_EQUAL(b1,0.f)){
114 t = 0.f;
115 }
116 else if (REAL_GREAT(-b1,a11)|| REAL_EQUAL(-b1,a11)){
117 t = 1.f;
118 }
119 else{
120 t = -b1 / a11;
121 }
122 }
123 }
124 else {
125 //region 3
126 s = 0.f;
127 if (REAL_GREAT(b1,0.f)||REAL_EQUAL(b1,0.f)){
128 t = 0.f;
129 }
130 else if (REAL_GREAT(-b1,a11)||REAL_EQUAL(-b1,a11)){
131 t = 1.f;
132 }
133 else{
134 t = -b1 / a11;
135 }
136 }
137 }
138 else if (REAL_LESS(t, 0.f)) {
139 //region 5
140 t = 0.f;
141 if (REAL_GREAT(b0,0.f)||REAL_EQUAL(b0,0.f)){
142 s = 0.f;
143 }
144 else if (REAL_GREAT(-b0,a00)||REAL_EQUAL(-b0,a00)){
145 s = 1.f;
146 }
147 else{
148 s = -b0 / a00;
149 }
150 }
151 else {
152 //region 0, minimum at interior point
153 s /= det;
154 t /= det;
155 }
156 }
157 else {
158 float tmp0 = 0.f; float tmp1 = 0.f; float numer = 0.f; float denom = 0.f;
159 if (REAL_LESS(s, 0.f)) {
160 //region 2
161 tmp0 = a01 + b0;
162 tmp1 = a11 + b1;
163 if (REAL_GREAT(tmp1,tmp0)){
164 numer = tmp1 - tmp0;
165 denom = a00 - 2.f * a01 + a11;
166 if (REAL_GREAT(numer,denom)||REAL_EQUAL(numer,denom)){
167 s = 1.f;
168 t = 0.f;
169 }
170 else{
171 s = numer / denom;
172 t = 1.f - s;
173 }
174 }
175 else
176 {
177 s = 0.f;
178 if (REAL_LESS(tmp1,0.f)||REAL_EQUAL(tmp1,0.f)){
179 t = 1.f;
180 }
181 else if (REAL_GREAT(b1,0.f)||REAL_EQUAL(b1,0.f)){
182 t = 0.f;
183 }
184 else{
185 t = -b1 / a11;
186 }
187 }
188 }
189 else if (REAL_LESS(t, 0.f)) {
190 //region 6
191 tmp0 = a01 + b1;
192 tmp1 = a00 + b0;
193 if (REAL_GREAT(tmp1,tmp0)){
194 numer = tmp1 - tmp0;
195 denom = a00 - 2.f * a01 + a11;
196 if (REAL_GREAT(numer,denom)||REAL_EQUAL(numer,denom)){
197 t = 1.f;
198 s = 0.f;
199 }
200 else
201 {
202 t = numer / denom;
203 s = 1.f - t;
204 }
205 }
206 else{
207 t = 1.f;
208 if (REAL_LESS(tmp1,0.f)||REAL_EQUAL(tmp1,0.f)){
209 s = 1.f;
210 }
211 else if (REAL_GREAT(b0,0.f)||REAL_EQUAL(b0,0.f)){
212 s = 0.f;
213 }
214 else{
215 s = -b0 / a00;
216 }
217 }
218 }
219 else {
220 //region 1
221 numer = a11 + b1 - a01 - b0;
222 if (REAL_LESS(numer,0.f)||REAL_EQUAL(numer,0.f)){
223 s = 0.f;
224 t = 1.f;
225 }
226 else{
227 denom = a00 - 2.f * a01 + a11;
228 if (REAL_GREAT(numer,denom)||REAL_EQUAL(numer,denom)){
229 s = 1.f;
230 t = 0.f;
231 }
232 else{
233 s = numer / denom;
234 t = 1.f - s;
235 }
236 }
237 }
238 }
239
240 Vec3f v = (a0 + s * e0 + t * e1);
241 // if (para != nullptr) {
242 // para[0] = 1.0;
243 // para[1] = 1.0-s-t;
244 // para[2] = s;
245 // para[3] = t;
246 // }
247 return v;
248 }
249
250 // get unit direction d from point p to triangle a0a1a2 of minimum distance
251 DYN_FUNC Vec3f getDirectionVF(Vec3f p,
252 Vec3f a0, Vec3f a1, Vec3f a2)
253 {
254 Vec3f d = getProjectionVF(p, a0, a1, a2) - p;
255 return d / d.norm();
256 }
257
258 // get minimum distance from point p to triangle a0a1a2
259 DYN_FUNC float getDistanceVF(Vec3f p,
260 Vec3f a0, Vec3f a1, Vec3f a2)
261 {
262 // return sqrt(Dot(a0 - p, a0 - p)); // DEBUG
263
264 Vec3f d = getProjectionVF(p, a0, a1, a2) - p;
265 return sqrt(Dot(d,d));
266 }
267
268 // check tet b0b1b2b3 whether in narrow band of triangle a0a1a2 (Dis < Band Width)
269 // But, this function is an approximate method, only check center point of tet.
270 DYN_FUNC bool isInNarrowBand(Vec3f b0, Vec3f b1, Vec3f b2, Vec3f b3,
271 Vec3f a0, Vec3f a1, Vec3f a2, float d)
272 {
273 return REAL_LESS(
274 getDistanceVF((b0 + b1 + b2 + b3) / 4.f, a0, a1, a2)
275 , d);
276 }
277
278 // Test for intersectino of two Convex coplanar Polygon. (2D Separating Axis)
279 inline DYN_FUNC bool isConPolyOverLap2D(Vec3f face_n, int n_a, Vec3f* a,
280 int n_b, Vec3f* b)
281 {
282 if (n_a == 0 || n_b == 0) return false;
283 auto project = [&](Vec3f axis, Vec3f* p, int n, float& pmin, float& pmax)
284 {
285 pmin = REAL_infinity;
286 pmax = -REAL_infinity;
287 for (int i = 0; i < n; i++)
288 {
289 float dot = Dot(axis, p[i] - Vec3f(0.0));
290 pmin = fmin(pmin, dot);
291 pmax = fmax(pmax, dot);
292 }
293 };
294 auto check = [&](Vec3f axis)
295 {
296 float amin, amax, bmin, bmax, cmin, cmax;
297 project(axis, a, n_a, amin, amax);
298 project(axis, b, n_b, bmin, bmax);
299 if(!isOverLap(cmin, cmax, amin, amax, bmin, bmax))
300 {
301 return false;
302 }
303 return true;
304 };
305 int j;
306 // Check each edge noraml
307 for (int i = 0; i < n_a; i++)
308 {
309 j = (i + 1 == n_a) ? 0 : i + 1;
310 Vec3f axis = Cross(face_n, a[j] - a[i]);
311 if(!check(axis))
312 {
313 // printf("[geo] [%d]Polygon((%f, %f, %f), (%f, %f, %f), (%f, %f, %f))\n [%d] Polygon((%f, %f, %f),(%f, %f, %f),(%f, %f, %f),(%f, %f, %f))\n",
314 // n_a, a[0][0], a[0][1], a[0][2],
315 // a[1][0], a[1][1], a[1][2],
316 // a[2][0], a[2][1], a[2][2],
317 // n_b, b[0][0], b[0][1], b[0][2],
318 // b[1][0], b[1][1], b[1][2],
319 // b[2][0], b[2][1], b[2][2],
320 // b[3][0], b[3][1], b[3][2]);
321 return false;
322 }
323 }
324
325 for (int i = 0; i < n_b; i++)
326 {
327 j = (i + 1 == n_b) ? 0 : i + 1;
328 Vec3f axis = Cross(face_n, b[j] - b[i]);
329 if(!check(axis))
330 {
331 // printf("[geo] [%d]Polygon((%f, %f, %f), (%f, %f, %f), (%f, %f, %f))\n [%d] Polygon((%f, %f, %f),(%f, %f, %f),(%f, %f, %f),(%f, %f, %f))\n",
332 // n_a, a[0][0], a[0][1], a[0][2],
333 // a[1][0], a[1][1], a[1][2],
334 // a[2][0], a[2][1], a[2][2],
335 // n_b, b[0][0], b[0][1], b[0][2],
336 // b[1][0], b[1][1], b[1][2],
337 // b[2][0], b[2][1], b[2][2],
338 // b[3][0], b[3][1], b[3][2]);
339 return false;
340 }
341 }
342 // printf("[geo] [%d]Polygon((%f, %f, %f), (%f, %f, %f), (%f, %f, %f))\n [%d] Polygon((%f, %f, %f),(%f, %f, %f),(%f, %f, %f),(%f, %f, %f))\n",
343 // n_a, a[0][0] * 10, a[0][1] * 10, a[0][2] * 10,
344 // a[1][0] * 10, a[1][1] * 10, a[1][2] * 10,
345 // a[2][0] * 10, a[2][1] * 10, a[2][2] * 10,
346 // n_b, b[0][0] * 10, b[0][1] * 10, b[0][2] * 10,
347 // b[1][0] * 10, b[1][1] * 10, b[1][2] * 10,
348 // b[2][0] * 10, b[2][1] * 10, b[2][2] * 10,
349 // b[3][0] * 10, b[3][1] * 10, b[3][2] * 10);
350 return true;
351 }
352
353 inline DYN_FUNC float getOverLapBoxAreaInPoly2D(Vec3f face_n, int n_a, Vec3f* a,
354 int n_b, Vec3f* b)
355 {
356 float res = REAL_infinity;
357 if (n_a == 0 || n_b == 0) return 0.f;
358 auto project = [&](Vec3f axis, Vec3f* p, int n, float& pmin, float& pmax)
359 {
360 pmin = REAL_infinity;
361 pmax = -REAL_infinity;
362 for (int i = 0; i < n; i++)
363 {
364 float dot = Dot(axis, p[i] - Vec3f(0.0));
365 pmin = fmin(pmin, dot);
366 pmax = fmax(pmax, dot);
367 }
368 };
369
370 auto cal_box_area = [&](Vec3f axis)
371 {
372 float amin, amax, bmin, bmax, cmin, cmax;
373 project(axis, a, n_a, amin, amax);
374 project(axis, b, n_b, bmin, bmax);
375 if(!isOverLap(cmin, cmax, amin, amax, bmin, bmax))
376 {
377 return 0.f;
378 }
379
380 float dx = cmax - cmin;
381 Vec3f axis_y = Cross(face_n, axis);
382 axis_y.normalize();
383 project(axis_y, a, n_a, amin, amax);
384 project(axis_y, b, n_b, bmin, bmax);
385 if(!isOverLap(cmin, cmax, amin, amax, bmin, bmax))
386 {
387 return 0.f;
388 }
389 float dy = cmax - cmin;
390 // printf("[geo] a %f\n", dx * dy);
391 return dx * dy;
392 };
393
394 int j;
395 // Check each edge noraml
396 for (int i = 0; i < n_a; i++)
397 {
398 j = (i + 1 == n_a) ? 0 : i + 1;
399 Vec3f axis = Cross(face_n, a[j] - a[i]);
400 axis.normalize();
401 float area = cal_box_area(axis);
402 res = fmin(res, area);
403 if(REAL_EQUAL(area, 0.f))
404 {
405 // printf("[geo] [%d]Polygon((%f, %f, %f), (%f, %f, %f), (%f, %f, %f))\n [%d] Polygon((%f, %f, %f),(%f, %f, %f),(%f, %f, %f),(%f, %f, %f))\n",
406 // n_a, a[0][0], a[0][1], a[0][2],
407 // a[1][0], a[1][1], a[1][2],
408 // a[2][0], a[2][1], a[2][2],
409 // n_b, b[0][0], b[0][1], b[0][2],
410 // b[1][0], b[1][1], b[1][2],
411 // b[2][0], b[2][1], b[2][2],
412 // b[3][0], b[3][1], b[3][2]);
413 return 0.f;
414 }
415 }
416
417 for (int i = 0; i < n_b; i++)
418 {
419 j = (i + 1 == n_b) ? 0 : i + 1;
420 Vec3f axis = Cross(face_n, b[j] - b[i]);
421 axis.normalize();
422 float area = cal_box_area(axis);
423 res = fmin(res, area);
424 if(REAL_EQUAL(area, 0.f))
425 {
426 // printf("[geo] [%d]Polygon((%f, %f, %f), (%f, %f, %f), (%f, %f, %f))\n [%d] Polygon((%f, %f, %f),(%f, %f, %f),(%f, %f, %f),(%f, %f, %f))\n",
427 // n_a, a[0][0], a[0][1], a[0][2],
428 // a[1][0], a[1][1], a[1][2],
429 // a[2][0], a[2][1], a[2][2],
430 // n_b, b[0][0], b[0][1], b[0][2],
431 // b[1][0], b[1][1], b[1][2],
432 // b[2][0], b[2][1], b[2][2],
433 // b[3][0], b[3][1], b[3][2]);
434 return 0.f;
435 }
436 }
437
438 // printf("[geo] Area %f\n", res);
439 return res;
440 }
441
442 // Test for intersectino of a triangle and a tetrahedron that have a shared vertex.
443 inline DYN_FUNC bool isIntrTri2Tet(Vec3f a0, Vec3f a1, Vec3f a2,
444 Vec3f b1, Vec3f b2, Vec3f b3) // b0 = a0
445 {
446
447 // Intersection with Plane A -> <= 4 point Convex Polygon A
448 Vec3f nA = (a1 - a0).cross(a2- a0); // face normal
449 nA.normalize();
450 Vec3f oA = a0;
451 Vec3f t[3] = {b1, b2, b3};
452 Vec3f p[3];
453 int p_num = 0;
454
455 float d[3]; int s[3];
456 for (int i = 0; i < 3; i++) {
457 d[i] = Dot(nA, t[i] - oA);
458 s[i] = REAL_EQUAL(d[i], 0.f) ? 0 : (REAL_LESS(d[i], 0.f) ? -1 : 1);
459 }
460
461 // Tet degrade to tri
462 if (s[0] == 0 && s[1] == 0 && s[2] == 0) return false;
463
464 for (int i = 0; i < 3; i++)
465 {
466 if(s[i] == 0) p[p_num++] = t[i];
467 else
468 {
469 for (int j = i + 1; j < 3; j++)
470 if (s[i] * s[j] < 0) p[p_num++] = t[j] + (t[i] - t[j]) * d[j] / (d[j] - d[i]);
471 }
472 }
473 if (p_num > 2) printf("[geo] Error: p_num > 2 %d %d %d %f %f %f\n", s[0], s[1], s[2], d[0], d[1], d[2]);
474 if (p_num != 2) return false;
475
476 Vec3f vec_a1 = (a1 - oA);
477 Vec3f vec_a2 = (a2 - oA);
478 Vec3f vec_p1 = (p[0] - oA);
479 Vec3f vec_p2 = (p[1] - oA);
480 vec_a1.normalize();
481 vec_a2.normalize();
482 Vec3f mid_a = (vec_a1 + vec_a2) * 0.5f;
483 mid_a.normalize();
484 Real cos_a = Dot(vec_a1, mid_a);
485 Real cos_p1 = Dot(vec_p1, mid_a);
486 Real cos_p2 = Dot(vec_p2, mid_a);
487 if(REAL_LESS(cos_a, cos_p1) || REAL_LESS(cos_a, cos_p2))
488 {
489 // printf("[CSR] No Intr Polygon((%f, %f, %f), (%f, %f, %f), (%f, %f, %f))\nPyramid(Polygon((%f, %f, %f),(%f, %f, %f),(%f, %f, %f)),(%f, %f, %f))\n",
490 // a0[0] * 10, a0[1] * 10, a0[2] * 10,
491 // a1[0] * 10, a1[1] * 10, a1[2] * 10,
492 // a2[0] * 10, a2[1] * 10, a2[2] * 10,
493 // a0[0] * 10, a0[1] * 10, a0[2] * 10,
494 // b1[0] * 10, b1[1] * 10, b1[2] * 10,
495 // b2[0] * 10, b2[1] * 10, b2[2] * 10,
496 // b3[0] * 10, b3[1] * 10, b3[2] * 10);
497 return true;
498 }
499 return false;
500 }
501
502 // Test for intersectino of a triangle and a tetrahedron.
503 inline DYN_FUNC bool isIntrTri2Tet(Vec3f a0, Vec3f a1, Vec3f a2,
504 Vec3f b0, Vec3f b1, Vec3f b2, Vec3f b3)
505 {
506 // Intersection with Plane A -> <= 4 point Convex Polygon A
507 Vec3f nA = (a1 - a0).cross(a2 - a0); // face normal
508 nA.normalize();
509 Vec3f oA = a0;
510 Vec3f t[4] = {b0, b1, b2, b3};
511 Vec3f p[4];
512 int p_num = 0;
513
514 float d[4]; int s[4];
515 for (int i = 0; i < 4; i++) {
516 d[i] = Dot(nA, t[i] - oA);
517 if(d[i] != d[i])
518 printf("[geo] Error d nan nA(%f %f %f) oA(%f %f %f) t(%f %f %f)\n", nA[0], nA[1], nA[2], oA[0], oA[1], oA[2], t[i][0], t[i][1], t[i][2]);
519 s[i] = REAL_EQUAL(d[i], 0.f) ? 0 : (REAL_LESS(d[i], 0.f) ? -1 : 1);
520 }
521 for (int i = 0; i < 4; i++)
522 {
523 if(s[i] == 0) p[p_num++] = t[i];
524 else
525 {
526 for (int j = i + 1; j < 4; j++)
527 if (s[i] * s[j] < 0) {
528 if (REAL_EQUAL(d[j] - d[i], 0.f))
529 p[p_num++] = t[i];
530 else
531 p[p_num++] = t[j] + (t[i] - t[j]) * d[j] / (d[j] - d[i]);
532 }
533 }
534 }
535 // if(p_num == 2)
536 // {
537 // printf("[geo] d0(%f) d1(%f) d2(%f) d3(%f)\n", d[0],d[1],d[2],d[3]);
538 // }
539 if (p_num > 4) printf("[geo] Error: p_num > 4 [%d %d %d %d]\n", s[0], s[1], s[2], s[3]);
540 // set order.
541 if (p_num == 4)
542 {
543 bool s0 = Sign(nA, p[0], p[1], p[2]);
544 bool s1 = Sign(nA, p[0], p[1], p[3]);
545 if(s1 != s0) Swap(p[0], p[3]);
546 else{
547 bool s2 = Sign(nA, p[0], p[2], p[3]);
548 if(s2 != s0) Swap(p[2], p[3]);
549 }
550 // if(Sign(nA, p[0], p[1], p[2]) != Sign(nA, p[0], p[2], p[3])) printf("[geo] Error in p order\n");
551 }
552
553 if (p_num > 0)
554 {
555 for(int i = 0; i < p_num; i++)
556 {
557 // if(!isInTet(p[i], b0, b1, b2, b3))
558 // {
559 // printf("[geo] Error outside tet\n");
560 // }
561 if(!(REAL_GREAT(Dot(nA, p[i] - oA), -REAL_ZERO) && REAL_LESS(Dot(nA, p[i] - oA), REAL_ZERO) ))
562 {
563 printf("[geo] (isIntrTri2Tet) Error outside plane %f (%f %f %f) s[%d %d %d %d] d(%f %f %f %f)\n", Dot(nA, p[i] - oA), nA[0], p[i][0], oA[0], s[0], s[1], s[2], s[3], d[0], d[1], d[2], d[3]);
564 }
565 }
566 }
567 // printf("[geo] p_num: %d\n", p_num);
568 // Check if Triangle A is overlap with Polygon A
569 Vec3f a[3] = {a0, a1, a2};
570 return isConPolyOverLap2D(nA, 3, a, p_num, p);
571 }
572
573 // Test for containment of a point by a tetrahedron.
574 DYN_FUNC bool isInTet(Vec3f p,
575 Vec3f b0, Vec3f b1, Vec3f b2, Vec3f b3)
576 {
577 Vec3f edge20, edge10, edge30, edge21, edge31;
578 Vec3f diffP0, diffP1;
579 float tsp = 0.f, zero = 0.f;
580 float tps0 = 0.f;
581
582 // face <0,2,1>
583 edge20 = b2 - b0;
584 edge10 = b1 - b0;
585 edge30 = b3 - b0;
586 tps0 = DotCross(edge30, edge10, edge20); // Two cases: +[0,1,2,3] -[0,2,1,3]
587
588 diffP0 = p - b0;
589 tsp = DotCross(diffP0, edge10, edge20);
590 if (REAL_LESS(tsp * tps0, zero))
591 {
592 return false;
593 }
594
595 // face <0,1,3>
596 tsp = DotCross(diffP0, edge30, edge10);
597 if (REAL_LESS(tsp * tps0, zero))
598 {
599 return false;
600 }
601
602 // face <0,3,2>
603 tsp = DotCross(diffP0, edge20, edge30);
604 if (REAL_LESS(tsp * tps0, zero))
605 {
606 return false;
607 }
608
609 // face<1,2,3>
610 edge21 = b2 - b1;
611 edge31 = b3 - b1;
612 diffP1 = p - b1;
613 tsp = DotCross(diffP1, edge31, edge21);
614 if (REAL_LESS(tsp * tps0, zero))
615 {
616 return false;
617 }
618 // printf("[Geo]Tet Pyramid(Polygon((%f, %f, %f),(%f, %f, %f),(%f, %f, %f)),(%f, %f, %f)) , (%f, %f, %f)\n",
619 // b0[0] * 10, b0[1] * 10, b0[2] * 10,
620 // b1[0] * 10, b1[1] * 10, b1[2] * 10,
621 // b2[0] * 10, b2[1] * 10, b2[2] * 10,
622 // b3[0] * 10, b3[1] * 10, b3[2] * 10,
623 // p[0] * 10, p[1] * 10, p[2] * 10);
624 return true;
625 }
626
627 DYN_FUNC float getVolume(Vec3f b0, Vec3f b1, Vec3f b2, Vec3f b3)
628 {
629 Vec3f edge20, edge10, edge30;
630 edge20 = b2 - b0;
631 edge10 = b1 - b0;
632 edge30 = b3 - b0;
633 return DotCross(edge30, edge10, edge20) / 6.f;
634 }
635
636 DYN_FUNC bool getBarycentric(Vec3f & bary,Vec3f p,
637 Vec3f b0, Vec3f b1, Vec3f b2, Vec3f b3)
638 {
639 // Compute the vectors relative to V3 of the tetrahedron.
640 Vec3f diff[4] = { b0 - b3, b1 - b3, b2 - b3, p - b3 };
641 float det = DotCross(diff[0], diff[1], diff[2]);
642 if (det < -EPSILON || det > EPSILON) // check if tet is not degenerate
643 {
644 bary[0] = DotCross(diff[3], diff[1], diff[2]) / det;
645 bary[1] = DotCross(diff[3], diff[2], diff[0]) / det;
646 bary[2] = DotCross(diff[3], diff[0], diff[1]) / det;
647 // outside
648 if(REAL_LESS(bary[0], -REAL_ZERO) || REAL_LESS(bary[1], -REAL_ZERO) || REAL_LESS(bary[2], -REAL_ZERO))
649 {
650 // printf("[geo] outside (%f %f %f)\n", bary[0], bary[1], bary[2]);
651 return false;
652 }
653 return true;
654 }
655 return false;
656 }
657
658 // Compute the intersection of a line and a triangle in 2D [return num of intersection: 0, 1, 2]
659 DYN_FUNC int getIntersection(float& t0, float& t1,
660 Vec2f a0, Vec2f a1,
661 Vec2f b0, Vec2f b1, Vec2f b2)
662 {
663 float const zero = 0.f;
664 int res = 0;
665 Vec3f s( zero, zero, zero );
666 int numPositive = 0, numNegative = 0, numZero = 0;
667 Vec2f v[3] = { b0, b1, b2 };
668 Vec2f diff[3] = { b0 - a0, b1 - a0, b2 - a0 };
669 Vec2f direction = a1 - a0;
670 Vec2f origin = a0;
671 for (size_t i = 0; i < 3; ++i)
672 {
673 s[i] = DotPerp(direction, diff[i]);
674 if (s[i] > zero)
675 {
676 ++numPositive;
677 }
678 else if (s[i] < zero)
679 {
680 ++numNegative;
681 }
682 else
683 {
684 ++numZero;
685 }
686 }
687
688 if (numZero == 0 && numPositive > 0 && numNegative > 0)
689 {
690 // (n,p,z) is (1,2,0) or (2,1,0).
691 // result.intersect = true;
692 // result.numIntersections = 2;
693 res = 2;
694
695 // sign is +1 when (n,p) is (2,1) or -1 when (n,p) is (1,2).
696 float sign = (3 > numPositive * 2 ? 1.f : -1.f);
697 for (size_t i0 = 1, i1 = 2, i2 = 0; i2 < 3; i0 = i1, i1 = i2++)
698 {
699 if (sign * s[i2] > zero)
700 {
701 Vec2f diffVi0P0 = v[i0] - origin;
702 Vec2f diffVi2Vi0 = v[i2] - v[i0];
703 float lambda0 = s[i0] / (s[i0] - s[i2]);
704 Vec2f q0 = diffVi0P0 + lambda0 * diffVi2Vi0;
705 t0 = Dot(direction, q0);
706 Vec2f diffVi1P0 = v[i1] - origin;
707 Vec2f diffVi2Vi1 = v[i2] - v[i1];
708 float lambda1 = s[i1] / (s[i1] - s[i2]);
709 Vec2f q1 = diffVi1P0 + lambda1 * diffVi2Vi1;
710 t1 = Dot(direction, q1);
711 break;
712 }
713 }
714 }
715 else if (numZero == 1)
716 {
717 // (n,p,z) is (1,1,1), (2,0,1) or (0,2,1).
718 for (size_t i0 = 1, i1 = 2, i2 = 0; i2 < 3; i0 = i1, i1 = i2++)
719 {
720 if (s[i2] == zero)
721 {
722 Vec2f diffVi2P0 = v[i2] - origin;
723 t0 = Dot(direction, diffVi2P0);
724 if (numPositive == 2 || numNegative == 2)
725 {
726 // (n,p,z) is (2,0,1) or (0,2,1).
727 res = 1;
728 t1 = t0;
729 }
730 else
731 {
732 // (n,p,z) is (1,1,1).
733 res = 2;
734 Vec2f diffVi0P0 = v[i0] - origin;
735 Vec2f diffVi1Vi0 = v[i1] - v[i0];
736 float lambda0 = s[i0] / (s[i0] - s[i1]);
737 Vec2f q = diffVi0P0 + lambda0 * diffVi1Vi0;
738 t1 = Dot(direction, q);
739 }
740 break;
741 }
742 }
743 }
744 else if (numZero == 2)
745 {
746 // (n,p,z) is (1,0,2) or (0,1,2).
747 res = 2;
748 for (size_t i0 = 1, i1 = 2, i2 = 0; i2 < 3; i0 = i1, i1 = i2++)
749 {
750 if (s[i2] != zero)
751 {
752 Vec2f diffVi0P0 = v[i0] - origin;
753 t0 = Dot(direction, diffVi0P0);
754 Vec2f diffVi1P0 = v[i1] - origin;
755 t1 = Dot(direction, diffVi1P0);
756 break;
757 }
758 }
759 }
760 // else: (n,p,z) is (3,0,0), (0,3,0) or (0,0,3). The constructor
761 // for Result initializes all members to zero, so no additional
762 // assignments are needed for 'result'.
763
764 if (res > 0)
765 {
766 float directionSqrLength = Dot(direction, direction);
767 t0 /= directionSqrLength;
768 t1 /= directionSqrLength;
769 if (t0 > t1)
770 {
771 Swap(t0, t1);
772 }
773 }
774 return res;
775 }
776
777 // Compute the intersection of two triangle [return num of intersection: 0, 1, 2]
778 DYN_FUNC int getIntersection(Vec3f& p0, Vec3f& p1,
779 Vec3f a0, Vec3f a1, Vec3f a2,
780 Vec3f b0, Vec3f b1, Vec3f b2)
781 {
782 int res = 0;
783 int seg_res = 0;
784 Vec3f q[2];
785
786 // face A
787 Vec3f nA = Cross(a1 - a0, a2 - a0); nA.normalize();
788 Vec3f nB = Cross(b1 - b0, b2 - b0); nB.normalize();
789 Vec3f oA = a0;
790
791 // if two triangle are coplanar,
792 // whose penetration deep is 0, return 0
793 // if two triangle are parallel, return 0
794 if (!REAL_LESS(fabs(Dot(nA, nB)), 1.f))
795 {
796 return 0;
797 }
798
799 // check the intersection of three segments and one plane
800 float d0 = Dot(nA, b0 - oA);
801 float d1 = Dot(nA, b1 - oA);
802 float d2 = Dot(nA, b2 - oA);
803
804 int s0 = REAL_EQUAL(d0, 0.f) ? 0 : (REAL_LESS(d0, 0.f) ? -1 : 1);
805 int s1 = REAL_EQUAL(d1, 0.f) ? 0 : (REAL_LESS(d1, 0.f) ? -1 : 1);
806 int s2 = REAL_EQUAL(d2, 0.f) ? 0 : (REAL_LESS(d2, 0.f) ? -1 : 1);
807
808 // two triangle are coplanar
809 if (s0 == 0 && s1 == 0 && s2 == 0) return 0;
810
811 // check if three points in plane
812 if (s0 == 0) q[seg_res++] = b0;
813 if (s1 == 0) q[seg_res++] = b1;
814 if (s2 == 0) q[seg_res++] = b2;
815
816 // check if three segments intersect with plane A
817 if (s0 * s1 < 0 && (!REAL_EQUAL(d0 - d1, 0.f))) q[seg_res++] = b0 + (b1 - b0) * d0 / (d0 - d1);
818 if (s1 * s2 < 0 && (!REAL_EQUAL(d1 - d2, 0.f))) q[seg_res++] = b1 + (b2 - b1) * d1 / (d1 - d2);
819 if (s0 * s2 < 0 && (!REAL_EQUAL(d2 - d0, 0.f))) q[seg_res++] = b2 + (b0 - b2) * d2 / (d2 - d0);
820
821 if (seg_res > 2) printf("[geo] Error: res > 2 %d %d %d %f %f %f\n", s0, s1, s2, d0, d1, d2);
822 for (int _ = 0; _ < seg_res; ++_)
823 if (q[_] != q[_]) printf("[geo] Error in nan\n");
824 // check if one segments(q[0], q[1]) intersect with triangle A
825 if (seg_res == 2)
826 {
827 // printf("[geo] seg Polygon((%f, %f, %f), (%f, %f, %f))\n", q[0][0] * 10, q[0][1] * 10, q[0][2] * 10, q[1][0] * 10, q[1][1] * 10, q[1][2] * 10);
828 // 3D -> 2D (Project onto the one of xy- xz- yz- plane)
829 int maxIndex = 0;
830 float cmax = std::fabs(nA[0]);
831 float cvalue = std::fabs(nA[1]);
832 if (cvalue > cmax)
833 {
834 maxIndex = 1;
835 cmax = cvalue;
836 }
837 cvalue = std::fabs(nA[2]);
838 if (cvalue > cmax)
839 {
840 maxIndex = 2;
841 }
842
843 Vec3u lookup;
844 if (maxIndex == 0)
845 {
846 // Project onto the yz-plane.
847 lookup = { 1, 2, 0 };
848 }
849 else if (maxIndex == 1)
850 {
851 // Project onto the xz-plane.
852 lookup = { 0, 2, 1 };
853 }
854 else // maxIndex = 2
855 {
856 // Project onto the xy-plane.
857 lookup = { 0, 1, 2 };
858 }
859
860 // Project
861 Vec2f a2d[3];
862 Vec3f ao[3] = {a0 - oA, a1 - oA, a2 - oA};
863 for (size_t i = 0; i < 3; ++i)
864 {
865 a2d[i][0] = ao[i][lookup[0]];
866 a2d[i][1] = ao[i][lookup[1]];
867 }
868
869 Vec2f q2d[2];
870 Vec3f qo[2] ={q[0] - oA, q[1] - oA};
871 for (size_t i = 0; i < 2; ++i)
872 {
873 q2d[i][0] = qo[i][lookup[0]];
874 q2d[i][1] = qo[i][lookup[1]];
875 }
876
877 float t0 = 0.f, t1 = 0.f;
878 int res2d = getIntersection(t0, t1, q2d[0], q2d[1], a2d[0], a2d[1], a2d[2]);
879 if (res2d == 1)
880 {
881 if (REAL_LESS(t0, 1.f) && REAL_GREAT(t0, 0.f))
882 {
883 res = 1;
884 p0 = q[0] + (q[1] - q[0]) * t0;
885 }
886 }
887 if (res2d == 2)
888 {
889 // [0, 1] overlap [t0, t1]
890 if (REAL_LESS(t1, 0.f) || REAL_GREAT(t0, 1.f))
891 return 0;
892 if (REAL_LESS(t0, 0.f)) t0 = 0.f;
893 if (REAL_GREAT(t1, 1.f)) t1 = 1.f;
894 res = 2;
895 p0 = q[0] + (q[1] - q[0]) * t0;
896 p1 = q[0] + (q[1] - q[0]) * t1;
897 }
898 }
899 // check if one intersection point (q[0]) inside triangle A
900 else if (seg_res == 1)
901 {
902 // printf("[geo] p Point(%f, %f, %f)\n", q[0][0], q[0][1], q[0][2]);
903
904 int numPositive = 0, numNegative = 0, numZero = 0;
905 float s[3] = {Dot(a0 - q[0], a1 - q[0]), Dot(a1 - q[0], a2 - q[0]), Dot(a2 - q[0], a0 - q[0])};
906 for (size_t i = 0; i < 3; ++i)
907 {
908 if (REAL_GREAT(s[i], 0.f))
909 {
910 ++numPositive;
911 }
912 else if (REAL_LESS(s[i], 0.f))
913 {
914 ++numNegative;
915 }
916 else
917 {
918 ++numZero;
919 }
920 }
921 if (!(numPositive > 0 && numNegative > 0))
922 {
923 res = 1;
924 p0 = q[0];
925 }
926 }
927 return res;
928 }
929
930 // get the area of a triangle in a tetrahedron
931 DYN_FUNC float getTriBoxAreaInTet(Vec3f a0, Vec3f a1, Vec3f a2,
932 Vec3f b0, Vec3f b1, Vec3f b2, Vec3f b3)
933 {
934 float res = 0.f;
935
936 // Intersection with Plane A -> <= 4 point Convex Polygon A
937 Vec3f nA = (a1 - a0).cross(a2- a0); // face normal
938 nA.normalize();
939 Vec3f oA = a0;
940 Vec3f t[4] = {b0, b1, b2, b3};
941 Vec3f p[4];
942 int p_num = 0;
943
944 float d[4]; int s[4];
945 for (int i = 0; i < 4; i++) {
946 d[i] = Dot(nA, t[i] - oA);
947 s[i] = REAL_EQUAL(d[i], 0.f) ? 0 : (REAL_LESS(d[i], 0.f) ? -1 : 1);
948 }
949 for (int i = 0; i < 4; i++)
950 {
951 if(s[i] == 0) p[p_num++] = t[i];
952 else
953 {
954 for (int j = i + 1; j < 4; j++)
955 if (s[i] * s[j] < 0) p[p_num++] = t[j] + (t[i] - t[j]) * d[j] / (d[j] - d[i]);
956 }
957 }
958 if (p_num > 4) printf("[geo] Error: p_num > 4\n");
959 // set order.
960 if (p_num == 4)
961 {
962 bool s0 = Sign(nA, p[0], p[1], p[2]);
963 bool s1 = Sign(nA, p[0], p[1], p[3]);
964 if(s1 != s0) Swap(p[0], p[3]);
965 else{
966 bool s2 = Sign(nA, p[0], p[2], p[3]);
967 if(s2 != s0) Swap(p[2], p[3]);
968 }
969 // if(Sign(nA, p[0], p[1], p[2]) != Sign(nA, p[0], p[2], p[3])) printf("[geo] Error in p order\n");
970 }
971
972 // Check if Triangle A is overlap with Polygon A
973 Vec3f a[3] = {a0, a1, a2};
974 if (p_num <= 2) res = 0.f;
975 else {
976 res = getOverLapBoxAreaInPoly2D(nA, 3, a, p_num, p);
977 }
978 if (REAL_LESS(res, 0.f))
979 {
980 res = 0.f;
981 // printf("[geo] Error: res < 0\n");
982 }
983 return res;
984 }
985
986 inline DYN_FUNC Vec2f projectWithParall(Vec3f p, Vec3f a0, Vec3f a1, Vec3f a2, Vec3f a3)
987 {
988 Vec3f a = a0;
989 Vec3f d0 = a1 - a0;
990 Vec3f d1 = a3 - a2;
991
992 float A = Dot(d0, d0);
993 float B = Dot(d0, d1);
994 float C = Dot(d1, d1);
995 float D0 = Dot(d0, p);
996 float D1 = Dot(d1, p);
997
998 float delta = A * C - B * B;
999 if (REAL_EQUAL(delta, 0.f)) return Vec2f(0.f,0.f);
1000 float u = (C * D0 - B * D1) / delta;
1001 float v = (A * D1 - B * D0) / delta;
1002 return Vec2f(u, v);
1003 }
1004
1005
1006 inline DYN_FUNC int intrSegWithPlane(Vec3f* q,
1007 Vec3f oA, Vec3f nA,
1008 Vec3f b0, Vec3f b1)
1009 {
1010 int res = 0;
1011 // check the intersection of one segments and one plane
1012 float d0 = Dot(nA, b0 - oA);
1013 float d1 = Dot(nA, b1 - oA);
1014
1015 int s0 = REAL_EQUAL(d0, 0.f) ? 0 : (REAL_LESS(d0, 0.f) ? -1 : 1);
1016 int s1 = REAL_EQUAL(d1, 0.f) ? 0 : (REAL_LESS(d1, 0.f) ? -1 : 1);
1017
1018 // check if two points in plane
1019 if (s0 == 0) q[res++] = b0;
1020 if (s1 == 0) q[res++] = b1;
1021
1022 // check if one segments intersect with plane A
1023 if (s0 * s1 < 0 && (!REAL_EQUAL(d0 - d1, 0.f))) q[res++] = b0 + (b1 - b0) * d0 / (d0 - d1);
1024
1025 assert(res <= 2);
1026 return res;
1027 }
1028
1029 inline DYN_FUNC int intrTriWithPlane(Vec3f* q,
1030 Vec3f oA, Vec3f nA,
1031 Vec3f b0, Vec3f b1, Vec3f b2)
1032 {
1033 int res = 0;
1034 float d0 = Dot(nA, b0 - oA);
1035 float d1 = Dot(nA, b1 - oA);
1036 float d2 = Dot(nA, b2 - oA);
1037
1038 int s0 = REAL_EQUAL(d0, 0.f) ? 0 : (REAL_LESS(d0, 0.f) ? -1 : 1);
1039 int s1 = REAL_EQUAL(d1, 0.f) ? 0 : (REAL_LESS(d1, 0.f) ? -1 : 1);
1040 int s2 = REAL_EQUAL(d2, 0.f) ? 0 : (REAL_LESS(d2, 0.f) ? -1 : 1);
1041
1042 // check if three points in plane
1043 if (s0 == 0) q[res++] = b0;
1044 if (s1 == 0) q[res++] = b1;
1045 if (s2 == 0) q[res++] = b2;
1046
1047 // check if three segments intersect with plane A
1048 if (s0 * s1 < 0 && (!REAL_EQUAL(d0 - d1, 0.f))) q[res++] = b0 + (b1 - b0) * d0 / (d0 - d1);
1049 if (s1 * s2 < 0 && (!REAL_EQUAL(d1 - d2, 0.f))) q[res++] = b1 + (b2 - b1) * d1 / (d1 - d2);
1050 if (s0 * s2 < 0 && (!REAL_EQUAL(d2 - d0, 0.f))) q[res++] = b2 + (b0 - b2) * d2 / (d2 - d0);
1051
1052 assert(res <= 3);
1053 return res;
1054 }
1055
1056 inline DYN_FUNC int intrTetWithPlane(Vec3f* q,
1057 Vec3f oA, Vec3f nA,
1058 Vec3f b0, Vec3f b1, Vec3f b2, Vec3f b3)
1059 {
1060 int res = 0;
1061
1062 float d0 = Dot(nA, b0 - oA);
1063 float d1 = Dot(nA, b1 - oA);
1064 float d2 = Dot(nA, b2 - oA);
1065 float d3 = Dot(nA, b3 - oA);
1066
1067 int s0 = REAL_EQUAL(d0, 0.f) ? 0 : (REAL_LESS(d0, 0.f) ? -1 : 1);
1068 int s1 = REAL_EQUAL(d1, 0.f) ? 0 : (REAL_LESS(d1, 0.f) ? -1 : 1);
1069 int s2 = REAL_EQUAL(d2, 0.f) ? 0 : (REAL_LESS(d2, 0.f) ? -1 : 1);
1070 int s3 = REAL_EQUAL(d3, 0.f) ? 0 : (REAL_LESS(d3, 0.f) ? -1 : 1);
1071
1072 // check if four points in plane
1073 if (s0 == 0) q[res++] = b0;
1074 if (s1 == 0) q[res++] = b1;
1075 if (s2 == 0) q[res++] = b2;
1076 if (s3 == 0) q[res++] = b3;
1077
1078 // check if six segments intersect with plane A
1079 if (s0 * s1 < 0 && (!REAL_EQUAL(d0 - d1, 0.f))) q[res++] = b0 + (b1 - b0) * d0 / (d0 - d1);
1080 if (s0 * s2 < 0 && (!REAL_EQUAL(d0 - d2, 0.f))) q[res++] = b0 + (b2 - b0) * d0 / (d0 - d2);
1081 if (s0 * s3 < 0 && (!REAL_EQUAL(d0 - d3, 0.f))) q[res++] = b0 + (b3 - b0) * d0 / (d0 - d3);
1082 if (s1 * s2 < 0 && (!REAL_EQUAL(d1 - d2, 0.f))) q[res++] = b1 + (b2 - b1) * d1 / (d1 - d2);
1083 if (s1 * s3 < 0 && (!REAL_EQUAL(d1 - d3, 0.f))) q[res++] = b1 + (b3 - b1) * d1 / (d1 - d3);
1084 if (s2 * s3 < 0 && (!REAL_EQUAL(d2 - d3, 0.f))) q[res++] = b2 + (b3 - b2) * d2 / (d2 - d3);
1085
1086 assert(res <= 4);
1087 if (res == 4)
1088 {
1089 Vec3f q01 = q[1] - q[0];
1090 Vec3f q02 = q[2] - q[0];
1091 Vec3f q03 = q[3] - q[0];
1092 if (REAL_GREAT(Dot(q01.cross(q02), q01.cross(q03)), 0.f)) Swap(q[2], q[3]);
1093 }
1094 return res;
1095 }
1096
1097 // 6-------7
1098 // /| /|
1099 // / | / |
1100 // 4------5 |
1101 // | 2 ---|--3
1102 // | / | / W V
1103 // 0-------1 |/_U
1104 inline DYN_FUNC int intrBoxWithPlane(Vec3f* q,
1105 Vec3f oA, Vec3f nA,
1106 Vec3f center, Vec3f halfU, Vec3f halfV, Vec3f halfW)
1107 {
1108 int res = 0;
1109 Vec3f b0 = center - halfU - halfV - halfW;
1110 Vec3f b1 = center + halfU - halfV - halfW;
1111 Vec3f b2 = center - halfU + halfV - halfW;
1112 Vec3f b3 = center + halfU + halfV - halfW;
1113 Vec3f b4 = center - halfU - halfV + halfW;
1114 Vec3f b5 = center + halfU - halfV + halfW;
1115 Vec3f b6 = center - halfU + halfV + halfW;
1116 Vec3f b7 = center + halfU + halfV + halfW;
1117
1118 float d0 = Dot(nA, b0 - oA);
1119 float d1 = Dot(nA, b1 - oA);
1120 float d2 = Dot(nA, b2 - oA);
1121 float d3 = Dot(nA, b3 - oA);
1122 float d4 = Dot(nA, b4 - oA);
1123 float d5 = Dot(nA, b5 - oA);
1124 float d6 = Dot(nA, b6 - oA);
1125 float d7 = Dot(nA, b7 - oA);
1126
1127 int s0 = REAL_EQUAL(d0, 0.f) ? 0 : (REAL_LESS(d0, 0.f) ? -1 : 1);
1128 int s1 = REAL_EQUAL(d1, 0.f) ? 0 : (REAL_LESS(d1, 0.f) ? -1 : 1);
1129 int s2 = REAL_EQUAL(d2, 0.f) ? 0 : (REAL_LESS(d2, 0.f) ? -1 : 1);
1130 int s3 = REAL_EQUAL(d3, 0.f) ? 0 : (REAL_LESS(d3, 0.f) ? -1 : 1);
1131 int s4 = REAL_EQUAL(d4, 0.f) ? 0 : (REAL_LESS(d4, 0.f) ? -1 : 1);
1132 int s5 = REAL_EQUAL(d5, 0.f) ? 0 : (REAL_LESS(d5, 0.f) ? -1 : 1);
1133 int s6 = REAL_EQUAL(d6, 0.f) ? 0 : (REAL_LESS(d6, 0.f) ? -1 : 1);
1134 int s7 = REAL_EQUAL(d7, 0.f) ? 0 : (REAL_LESS(d7, 0.f) ? -1 : 1);
1135
1136 // check if eight points in plane
1137 if (s0 == 0) q[res++] = b0;
1138 if (s1 == 0) q[res++] = b1;
1139 if (s2 == 0) q[res++] = b2;
1140 if (s3 == 0) q[res++] = b3;
1141 if (s4 == 0) q[res++] = b4;
1142 if (s5 == 0) q[res++] = b5;
1143 if (s6 == 0) q[res++] = b6;
1144 if (s7 == 0) q[res++] = b7;
1145
1146 // check if twelve segments intersect with plane A
1147 if (s0 * s1 < 0 && (!REAL_EQUAL(d0 - d1, 0.f))) q[res++] = b0 + (b1 - b0) * d0 / (d0 - d1);
1148 if (s0 * s2 < 0 && (!REAL_EQUAL(d0 - d2, 0.f))) q[res++] = b0 + (b2 - b0) * d0 / (d0 - d2);
1149 if (s0 * s4 < 0 && (!REAL_EQUAL(d0 - d4, 0.f))) q[res++] = b0 + (b4 - b0) * d0 / (d0 - d4);
1150
1151 if (s1 * s3 < 0 && (!REAL_EQUAL(d1 - d3, 0.f))) q[res++] = b1 + (b3 - b1) * d1 / (d1 - d3);
1152 if (s1 * s5 < 0 && (!REAL_EQUAL(d1 - d5, 0.f))) q[res++] = b1 + (b5 - b1) * d1 / (d1 - d5);
1153
1154 if (s2 * s3 < 0 && (!REAL_EQUAL(d2 - d3, 0.f))) q[res++] = b2 + (b3 - b2) * d2 / (d2 - d3);
1155 if (s2 * s6 < 0 && (!REAL_EQUAL(d2 - d6, 0.f))) q[res++] = b2 + (b6 - b2) * d2 / (d2 - d6);
1156
1157 if (s3 * s7 < 0 && (!REAL_EQUAL(d3 - d7, 0.f))) q[res++] = b3 + (b7 - b3) * d3 / (d3 - d7);
1158
1159 if (s4 * s5 < 0 && (!REAL_EQUAL(d4 - d5, 0.f))) q[res++] = b4 + (b5 - b4) * d4 / (d4 - d5);
1160 if (s4 * s6 < 0 && (!REAL_EQUAL(d4 - d6, 0.f))) q[res++] = b4 + (b6 - b4) * d4 / (d4 - d6);
1161
1162 if (s5 * s7 < 0 && (!REAL_EQUAL(d5 - d7, 0.f))) q[res++] = b5 + (b7 - b5) * d5 / (d5 - d7);
1163
1164 if (s6 * s7 < 0 && (!REAL_EQUAL(d6 - d7, 0.f))) q[res++] = b6 + (b7 - b6) * d6 / (d6 - d7);
1165
1166 assert(res <= 4);
1167 if (res == 4)
1168 {
1169 Vec3f q01 = q[1] - q[0];
1170 Vec3f q02 = q[2] - q[0];
1171 Vec3f q03 = q[3] - q[0];
1172 if (REAL_GREAT(Dot(q01.cross(q02), q01.cross(q03)), 0.f)) Swap(q[2], q[3]);
1173 }
1174
1175 return res;
1176 }
1177
1178 inline DYN_FUNC int intrPolyWithLine(float* t,
1179 int n, Vec2f* p,
1180 Vec2f a0, Vec2f a1)
1181 {
1182 Vec2f oA = a0;
1183 Vec2f dA = a1 - a0;
1184 int res = 0;
1185
1186 assert(n >= 3);
1187
1188 for (int i = 0; i < n; i++)
1189 {
1190 int ni = (i == n - 1) ? 0 : i + 1;
1191 Vec2f oB = p[i];
1192 Vec2f dB = p[ni] - oB;
1193 float tA, tB; // tB \in [0, 1]
1194 float d = DotPerp(dA, dB);
1195 if (REAL_EQUAL(d, 0.f)) continue;
1196 tA = DotPerp(dB, oA - oB) / d;
1197 // if (REAL_EQUAL(tA, 1.f)) continue;
1198 tB = DotPerp(dA, oA - oB) / d;
1199 if (REAL_LESS(tB, 0.f) || REAL_GREAT(tB, 1.f)) continue;
1200 if(REAL_EQUAL(tB, 1.f))
1201 {
1202 int nni = (ni == n - 1) ? 0 : ni + 1;
1203 Vec2f ndB = p[nni] - p[ni];
1204 float d2 = DotPerp(dA, ndB);
1205 if (REAL_GREAT(d * d2 , 0.f)) continue; // up endpoint
1206 }
1207 t[res++] = tA;
1208 }
1209 if (res > 2)
1210 {
1211 for (int i = 0; i < n; i++)
1212 {
1213 int ni = (i == n - 1) ? 0 : i + 1;
1214 Vec2f oB = p[i];
1215 Vec2f dB = p[ni] - oB;
1216 float tA, tB; // tB \in [0, 1]
1217 float d = DotPerp(dA, dB);
1218 if (REAL_EQUAL(d, 0.f)) continue;
1219 tA = DotPerp(dB, oA - oB) / d;
1220 // if (REAL_EQUAL(tA, 1.f)) continue;
1221 tB = DotPerp(dA, oA - oB) / d;
1222 printf("tA tB :%f %f\n", tA, tB);
1223 if (REAL_LESS(tB, 0.f) || REAL_GREAT(tB, 1.f)) continue;
1224 if (REAL_EQUAL(tB, 1.f))
1225 {
1226 int nni = (ni == n - 1) ? 0 : ni + 1;
1227 Vec2f ndB = p[nni] - p[ni];
1228 float d2 = DotPerp(dA, ndB);
1229 printf("dd %f %f\n", d, d2);
1230 if (REAL_GREAT(d * d2, 0.f)) continue; // up endpoint
1231 }
1232 }
1233 for (int i = 0; i < res; ++i)
1234 {
1235 Vec2f b = a0 + t[i] * (a1 - a0);
1236 printf(" t %f p: %f %f\n", t[i], b[0], b[1]);
1237 }
1238 for (int i = 0; i < n; ++i)
1239 printf("p: %f %f\n", p[i][0], p[i][1]);
1240 printf("a0 %f %f\n", a0[0], a0[1]);
1241 printf("a1 %f %f\n", a1[0], a1[1]);
1242 }
1243 assert(res <= 2);
1244 return res;
1245 }
1246
1247
1248 inline DYN_FUNC int intrPolyWithTri(Vec3f* q,
1249 int n, Vec3f* p,
1250 Vec3f a0, Vec3f a1, Vec3f a2)
1251 {
1252 int res = 0;
1253 Vec3f nA = Cross(a1 - a0, a2 - a0); nA.normalize();
1254
1255 int maxIndex = 0;
1256 float cmax = std::fabs(nA[0]);
1257 float cvalue = std::fabs(nA[1]);
1258 if (cvalue > cmax)
1259 {
1260 maxIndex = 1;
1261 cmax = cvalue;
1262 }
1263 cvalue = std::fabs(nA[2]);
1264 if (cvalue > cmax) maxIndex = 2;
1265
1266 Vec3u lookup;
1267 if (maxIndex == 0) lookup = { 1, 2, 0 };// Project onto the yz-plane.
1268 else if (maxIndex == 1) lookup = { 0, 2, 1 }; // Project onto the xz-plane.
1269 else lookup = { 0, 1, 2 }; // Project onto the xy-plane.
1270
1271 // Proj
1272 Vec2f a2d[3]; Vec2f p2d[4];
1273 a2d[0] = Vec2f(a0[lookup[0]], a0[lookup[1]]);
1274 a2d[1] = Vec2f(a1[lookup[0]], a1[lookup[1]]);
1275 a2d[2] = Vec2f(a2[lookup[0]], a2[lookup[1]]);
1276 for (int i = 0; i < n; ++i)
1277 p2d[i] = Vec2f(p[i][lookup[0]], p[i][lookup[1]]);
1278
1279 if (n == 1)
1280 {
1281 // Check Point inside Rect
1282 Vec2f center = (a2d[0] + a2d[1] + a2d[2]) / 3.0f;
1283 float t[2];
1284 int num_intr = intrPolyWithLine(t, 3, a2d, p2d[0], center);
1285 int num_inside = 0;
1286 for (int j = 0; j < num_intr; ++j)
1287 {
1288 if (REAL_GREAT(t[j], 0.f)) num_inside++;
1289 }
1290 if (num_inside == 1) q[res++] = p[0];
1291 return res;
1292 }
1293
1294 // Check Polygon Edge Intr with Tri
1295 for (int i = 0; i < n; ++i)
1296 {
1297 int ni = (i == n - 1) ? 0 : i + 1;
1298 float t[2];
1299 int num_intr = intrPolyWithLine(t, 3, a2d, p2d[i], p2d[ni]);
1300 int num_inside = 0;
1301 for (int j = 0; j < num_intr; ++j)
1302 {
1303 if (REAL_GREAT(t[j], 0.f))
1304 {
1305 num_inside++;
1306 if (REAL_LESS(t[j], 1.f) )
1307 q[res++] = p[i] + (p[ni] - p[i]) * t[j];
1308 }
1309 }
1310 if (num_inside == 1) q[res++] = p[i];
1311 }
1312
1313 // Check Tri Point inside Polygon
1314 if (n > 2)
1315 {
1316 for (int i = 0; i < 3; ++i)
1317 {
1318 int ni = (i == 2) ? 0 : i + 1;
1319 float t[2];
1320 int num_intr = intrPolyWithLine(t, n, p2d, a2d[i], a2d[ni]);
1321 int num_inside = 0;
1322 for (int j = 0; j < num_intr; ++j)
1323 {
1324 if (REAL_GREAT(t[j], 0.f))
1325 num_inside++;
1326 }
1327 if (num_inside == 1) q[res++] = (i == 0) ? a0 : ((i == 1) ? a1 : a2);
1328 }
1329 }
1330
1331 assert(res <= 6);
1332 return res;
1333 }
1334
1335 inline DYN_FUNC int intrPolyWithRect(Vec3f* q,
1336 int n, Vec3f* p,
1337 Vec3f a0, Vec3f a1, Vec3f a2, Vec3f a3)
1338 {
1339 int res = 0;
1340 Vec3f nA = Cross(a1 - a0, a2 - a0); nA.normalize();
1341
1342 int maxIndex = 0;
1343 float cmax = std::fabs(nA[0]);
1344 float cvalue = std::fabs(nA[1]);
1345 if (cvalue > cmax)
1346 {
1347 maxIndex = 1;
1348 cmax = cvalue;
1349 }
1350 cvalue = std::fabs(nA[2]);
1351 if (cvalue > cmax) maxIndex = 2;
1352
1353 Vec3u lookup;
1354 if (maxIndex == 0) lookup = { 1, 2, 0 };// Project onto the yz-plane.
1355 else if (maxIndex == 1) lookup = { 0, 2, 1 }; // Project onto the xz-plane.
1356 else lookup = { 0, 1, 2 }; // Project onto the xy-plane.
1357
1358 // Proj
1359 Vec3f a[4]; Vec2f a2d[4]; Vec2f p2d[4];
1360 a[0] = a0; a[1] = a1; a[2] = a2; a[3] = a3;
1361 for (int i = 0; i < 4; ++i) a2d[i] = Vec2f(a[i][lookup[0]], a[i][lookup[1]]);
1362 for (int i = 0; i < n; ++i) p2d[i] = Vec2f(p[i][lookup[0]], p[i][lookup[1]]);
1363
1364 if (n == 1)
1365 {
1366 // Check Point inside Rect
1367 Vec2f center = (a2d[0] + a2d[1] + a2d[2] + a2d[3]) * 0.25f;
1368 float t[2];
1369 int num_intr = intrPolyWithLine(t, 4, a2d, p2d[0], center);
1370 int num_inside = 0;
1371 for (int j = 0; j < num_intr; ++j)
1372 {
1373 if (REAL_GREAT(t[j], 0.f)) num_inside++;
1374 }
1375 if (num_inside == 1) q[res++] = p[0];
1376 return res;
1377 }
1378 // Check Polygon Edge Intr with Rect
1379 for (int i = 0; i < n; ++i)
1380 {
1381 int ni = (i == n - 1) ? 0 : i + 1;
1382 float t[2];
1383 int num_intr = intrPolyWithLine(t, 4, a2d, p2d[i], p2d[ni]);
1384 int num_inside = 0;
1385 for (int j = 0; j < num_intr; ++j)
1386 {
1387 if (REAL_GREAT(t[j], 0.f))
1388 {
1389 num_inside++;
1390 if (REAL_LESS(t[j], 1.f) && (ni != 0 || i != 1) ) // same edge
1391 q[res++] = p[i] + (p[ni] - p[i]) * t[j];
1392 }
1393 }
1394 if (num_inside == 1) q[res++] = p[i];
1395 }
1396
1397 // Check Tri Point inside Polygon
1398 if (n > 2)
1399 {
1400 for (int i = 0; i < 4; ++i)
1401 {
1402 int ni = (i == 3) ? 0 : i + 1;
1403 float t[2];
1404 int num_intr = intrPolyWithLine(t, n, p2d, a2d[i], a2d[ni]);
1405 int num_inside = 0;
1406 for (int j = 0; j < num_intr; ++j)
1407 {
1408 if (REAL_GREAT(t[j], 0.f))
1409 num_inside++;
1410 }
1411 if (num_inside == 1) q[res++] = a[i];
1412 }
1413 }
1414
1415 assert(res <= 8);
1416 return res;
1417 }
1418
1419 };
1420};
REAL_GREAT
#define REAL_GREAT(a, b)
Definition AdditiveCCD.inl:7
REAL_infinity
#define REAL_infinity
Definition AdditiveCCD.inl:5
REAL_LESS
#define REAL_LESS(a, b)
Definition AdditiveCCD.inl:8
REAL_EQUAL
#define REAL_EQUAL(a, b)
Definition AdditiveCCD.inl:6
REAL_ZERO
#define REAL_ZERO
Definition ComputeGeometry.inl:15
check
void check(T result, char const *const func, const char *const file, int const line)
Definition ImageEncoder.cpp:6
Real
double Real
Definition Typedef.inl:23
assert
assert(queueCount >=1)
V
#define V(a, b, c)
T
#define T(t)
dyno::cgeo::intrSegWithPlane
DYN_FUNC int intrSegWithPlane(Vec3f *q, Vec3f oA, Vec3f nA, Vec3f b0, Vec3f b1)
Definition ComputeGeometry.inl:1006
dyno::cgeo::isInNarrowBand
DYN_FUNC bool isInNarrowBand(Vec3f b0, Vec3f b1, Vec3f b2, Vec3f b3, Vec3f a0, Vec3f a1, Vec3f a2, float d)
Definition ComputeGeometry.inl:270
dyno::cgeo::projectWithParall
DYN_FUNC Vec2f projectWithParall(Vec3f p, Vec3f a0, Vec3f a1, Vec3f a2, Vec3f a3)
Definition ComputeGeometry.inl:986
dyno::cgeo::getDistanceVF
DYN_FUNC float getDistanceVF(Vec3f p, Vec3f a0, Vec3f a1, Vec3f a2)
Definition ComputeGeometry.inl:259
dyno::cgeo::DotPerp
DYN_FUNC float DotPerp(Vec2f const &v0, Vec2f const &v1)
Definition ComputeGeometry.inl:60
dyno::cgeo::intrBoxWithPlane
DYN_FUNC int intrBoxWithPlane(Vec3f *q, Vec3f oA, Vec3f nA, Vec3f center, Vec3f halfU, Vec3f halfV, Vec3f halfW)
Definition ComputeGeometry.inl:1104
dyno::cgeo::isConPolyOverLap2D
DYN_FUNC bool isConPolyOverLap2D(Vec3f face_n, int n_a, Vec3f *a, int n_b, Vec3f *b)
Definition ComputeGeometry.inl:279
dyno::cgeo::DotCross
DYN_FUNC float DotCross(Vec3f const &U, Vec3f const &V, Vec3f const &W)
Definition ComputeGeometry.inl:47
dyno::cgeo::intrPolyWithRect
DYN_FUNC int intrPolyWithRect(Vec3f *q, int n, Vec3f *p, Vec3f a0, Vec3f a1, Vec3f a2, Vec3f a3)
Definition ComputeGeometry.inl:1335
dyno::cgeo::getProjectionVF
DYN_FUNC Vec3f getProjectionVF(Vec3f p, Vec3f a0, Vec3f a1, Vec3f a2)
Definition ComputeGeometry.inl:79
dyno::cgeo::intrTetWithPlane
DYN_FUNC int intrTetWithPlane(Vec3f *q, Vec3f oA, Vec3f nA, Vec3f b0, Vec3f b1, Vec3f b2, Vec3f b3)
Definition ComputeGeometry.inl:1056
dyno::cgeo::getTriBoxAreaInTet
DYN_FUNC float getTriBoxAreaInTet(Vec3f a0, Vec3f a1, Vec3f a2, Vec3f b0, Vec3f b1, Vec3f b2, Vec3f b3)
Definition ComputeGeometry.inl:931
dyno::cgeo::isInTet
DYN_FUNC bool isInTet(Vec3f p, Vec3f b0, Vec3f b1, Vec3f b2, Vec3f b3)
Definition ComputeGeometry.inl:574
dyno::cgeo::Swap
DYN_FUNC void Swap(T &a, T &b)
Definition ComputeGeometry.inl:22
dyno::cgeo::isIntrTri2Tet
DYN_FUNC bool isIntrTri2Tet(Vec3f a0, Vec3f a1, Vec3f a2, Vec3f b0, Vec3f b1, Vec3f b2, Vec3f b3)
Definition ComputeGeometry.inl:503
dyno::cgeo::intrPolyWithLine
DYN_FUNC int intrPolyWithLine(float *t, int n, Vec2f *p, Vec2f a0, Vec2f a1)
Definition ComputeGeometry.inl:1178
dyno::cgeo::intrPolyWithTri
DYN_FUNC int intrPolyWithTri(Vec3f *q, int n, Vec3f *p, Vec3f a0, Vec3f a1, Vec3f a2)
Definition ComputeGeometry.inl:1248
dyno::cgeo::Cross
DYN_FUNC Vec3f Cross(Vec3f const &U, Vec3f const &V)
Definition ComputeGeometry.inl:35
dyno::cgeo::intrTriWithPlane
DYN_FUNC int intrTriWithPlane(Vec3f *q, Vec3f oA, Vec3f nA, Vec3f b0, Vec3f b1, Vec3f b2)
Definition ComputeGeometry.inl:1029
dyno::cgeo::getIntersection
DYN_FUNC int getIntersection(float &t0, float &t1, Vec2f a0, Vec2f a1, Vec2f b0, Vec2f b1, Vec2f b2)
Definition ComputeGeometry.inl:659
dyno::cgeo::isOverLap
DYN_FUNC bool isOverLap(float &c0, float &c1, float a0, float a1, float b0, float b1)
Definition ComputeGeometry.inl:70
dyno::cgeo::getOverLapBoxAreaInPoly2D
DYN_FUNC float getOverLapBoxAreaInPoly2D(Vec3f face_n, int n_a, Vec3f *a, int n_b, Vec3f *b)
Definition ComputeGeometry.inl:353
dyno::cgeo::Sign
DYN_FUNC bool Sign(Vec3f const &n, Vec3f const &p0, Vec3f const &p1, Vec3f const &p2)
Definition ComputeGeometry.inl:65
dyno::cgeo::Perp
DYN_FUNC Vec2f Perp(Vec2f const &v)
Definition ComputeGeometry.inl:53
dyno::cgeo::getDirectionVF
DYN_FUNC Vec3f getDirectionVF(Vec3f p, Vec3f a0, Vec3f a1, Vec3f a2)
Definition ComputeGeometry.inl:251
dyno::cgeo::Dot
DYN_FUNC float Dot(Vec3f const &U, Vec3f const &V)
Definition ComputeGeometry.inl:23
dyno::cgeo::getVolume
DYN_FUNC float getVolume(Vec3f b0, Vec3f b1, Vec3f b2, Vec3f b3)
Definition ComputeGeometry.inl:627
dyno::cgeo::getBarycentric
DYN_FUNC bool getBarycentric(Vec3f &bary, Vec3f p, Vec3f b0, Vec3f b1, Vec3f b2, Vec3f b3)
Definition ComputeGeometry.inl:636
dyno
This is an implementation of AdditiveCCD based on peridyno.
Definition Array.h:25
dyno::sign
DYN_FUNC int sign(T const &a, T const EPS=EPSILON)
Definition SimpleMath.h:39
dyno::Vec3u
Vector< uint, 3 > Vec3u
Definition Vector3D.h:96
dyno::cross
DYN_FUNC Vector< T, 3 > cross(Vector< T, 3 > const &U, Vector< T, 3 > const &V)
Definition SimpleMath.h:211
dyno::dot
DYN_FUNC T dot(Vector< T, 2 > const &U, Vector< T, 2 > const &V)
Definition SimpleMath.h:199
dyno::Vec2f
Vector< float, 2 > Vec2f
Definition Vector2D.h:81
dyno::EPSILON
constexpr Real EPSILON
Definition Typedef.inl:42
dyno::Vec3f
Vector< float, 3 > Vec3f
Definition Vector3D.h:93
dyno::sqrt
DYN_FUNC Complex< Real > sqrt(const Complex< Real > &)
Definition Complex.inl:321
Generated by doxygen 1.13.2