PeriDyno 1.0.0
Loading...
Searching...
No Matches
AdditiveCCD.inl
Go to the documentation of this file.
1#include "Vector.h"
2
3namespace dyno
4{
5 #define REAL_infinity 1.0e30
6 #define REAL_EQUAL(a,b) (((a < b + EPSILON) && (a > b - EPSILON)) ? true : false)
7 #define REAL_GREAT(a,b) ((a > EPSILON + b)? true: false)
8 #define REAL_LESS(a,b) ((a + EPSILON < b)? true: false)
9 #define MAX_ITE (500)
10
11
12
13 template<typename T>
14 DYN_FUNC T getPoint2SegmentDistance( const Vector<T,3> &p, const Vector<T,3>& v0, const Vector<T,3>& v1){
15 T d0 = (p - v0).norm();
16 T d1 = (p - v1).norm();
17 T dv = min(d0, d1);
18 if ((v1 - v0).norm() < 1e-7) return dv; //v0 = v1
19
20 Vector<T, 3> dir = (v0 - v1)/(v0-v1).norm();
21 if ((p-v0).dot(dir) * (p-v1).dot(dir) <= 0.0) { //project in line
22 Vector<T, 3> prox = (v0 - p) - (v0 - p).dot(dir) * dir;
23 T dp = prox.norm();
24 return dp;
25 }
26 else
27 return dv;
28
29 }
30
31 template<typename T>
32 DYN_FUNC bool inTri(const Vector<T, 3>& p,
33 const Vector<T, 3>& v0, const Vector<T, 3>& v1, const Vector<T, 3>& v2) {
34 auto n = (v1 - v0).cross(v2 - v0);
35 if (n.norm() < 1e-6)// in line
36 return false;
37 n.normalize();
38 // On this board, assuming that p is in plane of triangle, so one have to project the point in plane first.
39 auto d0 = (p - v0).cross(v1 - v0);
40 auto d1 = (p - v1).cross(v2 - v1);
41 auto d2 = (p - v2).cross(v0 - v2);
42 return (REAL_LESS(d0.dot(n),0.0) && REAL_LESS(d1.dot(n),0.0) && REAL_LESS(d2.dot(n),0.0)) ||
43 (REAL_GREAT(d0.dot(n), 0.0) && REAL_GREAT(d1.dot(n), 0.0) && REAL_GREAT(d2.dot(n), 0.0));
44 }
45
46 template<typename T>
47 DYN_FUNC T AdditiveCCD<T>::DistanceVF(
48 const Vector<T, 3>& x,
49 const Vector<T, 3>& y0,
50 const Vector<T, 3>& y1,
51 const Vector<T, 3>& y2)
52 {
53 Vector<T, 3> y01 = y1 - y0;
54 Vector<T, 3> y12 = y2 - y1;
55 Vector<T, 3> n = y01.cross(y12); //norm of plane
56 T minLine = min(min(getPoint2SegmentDistance(x, y0, y1), getPoint2SegmentDistance(x, y1, y2)),
57 getPoint2SegmentDistance(x, y2, y0));
58 if (n.norm() < 1e-6) // in line
59 return minLine;
60
61 n.normalize();
62 //projection point to plane;
63 Vector<T, 3> p_ortho = (x - y0).dot(n) * n;
64 Vector<T, 3> p_inPlane = x - p_ortho;
65 if (inTri(p_inPlane, y0, y1, y2)) {
66 return p_ortho.norm();
67 }
68 return minLine;
69 }
70
71 template<typename T>
72 DYN_FUNC Vector<T, 3> AdditiveCCD<T>::DistanceVF_v(
73 const Vector<T, 3>& x,
74 const Vector<T, 3>& y0,
75 const Vector<T, 3>& y1,
76 const Vector<T, 3>& y2,
77 T* para)
78 {
79 //triangle base: y0, e0(y0,y1), e1(y0,y2)
80 //point base: x0
81
82 Vector<T, 3> d = y0 - x;
83 Vector<T, 3> e0 = y1 - y0;
84 Vector<T, 3> e1 = y2 - y0;
85 T a00 = e0.dot(e0);
86 T a01 = e0.dot(e1);
87 T a11 = e1.dot(e1);
88 T b0 = e0.dot(d);
89 T b1 = e1.dot(d);
90 T f = d.dot(d);
91 T det = max(a00 * a11 - a01 * a01, 0.0);
92 T s = a01 * b1 - a11 * b0;
93 T t = a01 * b0 - a00 * b1;
94 if (s + t <= det) {
95 if (REAL_LESS(s, 0.0)) {
96 if (REAL_LESS(t, 0.0)) {
97 //region 4
98 if (REAL_LESS(b0,0.0)){
99 t = 0.0;
100 if (-b0 >= a00){
101 s = 1.0;
102 }
103 else{
104 s = -b0 / a00;
105 }
106 }
107 else{
108 s = 0.0;
109 if (REAL_GREAT(b1,0.0)||REAL_EQUAL(b1,0.0)){
110 t = 0.0;
111 }
112 else if (REAL_GREAT(-b1,a11)|| REAL_EQUAL(-b1,a11)){
113 t = 1.0;
114 }
115 else{
116 t = -b1 / a11;
117 }
118 }
119 }
120 else {
121 //region 3
122 s = 0.0;
123 if (REAL_GREAT(b1,0.0)||REAL_EQUAL(b1,0.0)){
124 t = 0.0;
125 }
126 else if (REAL_GREAT(-b1,a11)||REAL_EQUAL(-b1,a11)){
127 t = 1.0;
128 }
129 else{
130 t = -b1 / a11;
131 }
132 }
133 }
134 else if (REAL_LESS(t, 0.0)) {
135 //region 5
136 t = 0.0;
137 if (REAL_GREAT(b0,0.0)||REAL_EQUAL(b0,0.0)){
138 s = 0.0;
139 }
140 else if (REAL_GREAT(-b0,a00)||REAL_EQUAL(-b0,a00)){
141 s = 1.0;
142 }
143 else{
144 s = -b0 / a00;
145 }
146 }
147 else {
148 //region 0, minimum at interior point
149 s /= det;
150 t /= det;
151 }
152 }
153 else {
154 T tmp0 = 0.0; T tmp1 = 0.0; T numer = 0.0; T denom = 0.0;
155 if (REAL_LESS(s, 0.0)) {
156 //region 2
157 tmp0 = a01 + b0;
158 tmp1 = a11 + b1;
159 if (REAL_GREAT(tmp1,tmp0)){
160 numer = tmp1 - tmp0;
161 denom = a00 - 2.0 * a01 + a11;
162 if (REAL_GREAT(numer,denom)||REAL_EQUAL(numer,denom)){
163 s = 1.0;
164 t = 0.0;
165 }
166 else{
167 s = numer / denom;
168 t = 1.0 - s;
169 }
170 }
171 else
172 {
173 s = 0.0;
174 if (REAL_LESS(tmp1,0.0)||REAL_EQUAL(tmp1,0.0)){
175 t = 1.0;
176 }
177 else if (REAL_GREAT(b1,0.0)||REAL_EQUAL(b1,0.0)){
178 t = 0.0;
179 }
180 else{
181 t = -b1 / a11;
182 }
183 }
184 }
185 else if (REAL_LESS(t, 0.0)) {
186 //region 6
187 tmp0 = a01 + b1;
188 tmp1 = a00 + b0;
189 if (REAL_GREAT(tmp1,tmp0)){
190 numer = tmp1 - tmp0;
191 denom = a00 - 2.0 * a01 + a11;
192 if (REAL_GREAT(numer,denom)||REAL_EQUAL(numer,denom)){
193 t = 1.0;
194 s = 0.0;
195 }
196 else
197 {
198 t = numer / denom;
199 s = 1.0 - t;
200 }
201 }
202 else{
203 t = 1.0;
204 if (REAL_LESS(tmp1,0.0)||REAL_EQUAL(tmp1,0.0)){
205 s = 1.0;
206 }
207 else if (REAL_GREAT(b0,0.0)||REAL_EQUAL(b0,0.0)){
208 s = 0.0;
209 }
210 else{
211 s = -b0 / a00;
212 }
213 }
214 }
215 else {
216 //region 1
217 numer = a11 + b1 - a01 - b0;
218 if (REAL_LESS(numer,0.0)||REAL_EQUAL(numer,0.0)){
219 s = 0.0;
220 t = 1.0;
221 }
222 else{
223 denom = a00 - 2.0 * a01 + a11;
224 if (REAL_GREAT(numer,denom)||REAL_EQUAL(numer,denom)){
225 s = 1.0;
226 t = 0.0;
227 }
228 else{
229 s = numer / denom;
230 t = 1.0 - s;
231 }
232 }
233 }
234 }
235
236 Vector<T, 3> v = x - (y0 + s * e0 + t * e1);
237 if (para != nullptr) {
238 para[0] = 1.0;
239 para[1] = 1.0-s-t;
240 para[2] = s;
241 para[3] = t;
242 }
243
244 return v;
245 }
246
247
248
249 template<typename T>
250 DYN_FUNC Vector<T, 3> AdditiveCCD<T>::DistanceEE(
251 const Vector<T, 3>& x0, const Vector<T, 3>& x1,
252 const Vector<T, 3>& y0, const Vector<T, 3>& y1,
253 T* para)
254 {
255 //segment s:(x0, x1); base: x0, dir: x1-x0
256 //segment t:(y0, y1); base: y0, dir: y1-y0
257
258 Vector<T, 3> P1mP0 = x1 - x0;
259 Vector<T, 3> Q1mQ0 = y1 - y0;
260 Vector<T, 3> P0mQ0 = x0 - y0;
261 T a = P1mP0.dot(P1mP0);
262 T b = P1mP0.dot(Q1mQ0);
263 T c = Q1mQ0.dot(Q1mQ0);
264 T d = P1mP0.dot(P0mQ0);
265 T e = Q1mQ0.dot(P0mQ0);
266 T det = a * c - b * b;
267 T s, t, nd, bmd, bte, ctd, bpe, ate, btd;
268
269 if (REAL_GREAT(det, 0.0)) {
270 bte = b * e;
271 ctd = c * d;
272 if (REAL_LESS(bte, ctd) || REAL_EQUAL(bte, ctd)) { //s<=0
273 s = 0.0;
274 if (REAL_LESS(e, 0.0) || REAL_EQUAL(e, 0.0)) {//t<=0
275 //reigen 6
276 t = 0.0;
277 nd = -d;
278 if (REAL_GREAT(nd, a) || REAL_EQUAL(nd, a)) {
279 s = 1.0;
280 }
281 else if (REAL_GREAT(nd, 0.0)) {
282 s = nd / a;
283 }
284 }
285 else if (REAL_LESS(e, c))//0<t<1
286 {
287 //reigon 5
288 t = e / c;
289 }
290 else {//t >=1
291 //reigon 4
292 t = 1.0;
293 bmd = b - d;
294 if (REAL_GREAT(bmd, a) || REAL_EQUAL(bmd, a)) {
295 s = 1.0;
296 }
297 else if (REAL_GREAT(bmd, 0.0)) {
298 s = bmd / a;
299 }
300 }
301 }
302 else { //s>0
303 s = bte - ctd;
304 if (REAL_GREAT(s, det) || REAL_EQUAL(s, det)) { //s>=1
305 s = 1;
306 bpe = b + e;
307 if (REAL_LESS(bpe, 0.0)||REAL_EQUAL(bpe,0.0)) { //t<=0
308 //reigon 8
309 t = 0.0;
310 nd = -d;
311 if (REAL_LESS(nd, 0.0) || REAL_EQUAL(nd, 0.0)) {
312 s = 0.0;
313 }
314 else if (REAL_LESS(nd, a)) {
315 s = nd / a;
316 }
317 }
318 else if (REAL_LESS(bpe, c)) {//0<t<1
319 //reigon 1
320 t = bpe / c;
321 }
322 else { //t>1
323 //reigon 2
324 t = 1.0;
325 bmd = b - d;
326 if (REAL_LESS(bmd, 0.0) || REAL_EQUAL(bmd, 0.0)) {
327 s = 0.0;
328 }
329 else if (REAL_LESS(bmd, a)) {
330 s = bmd / a;
331 }
332
333 }
334 }
335 else { //0<s<1
336 ate = a * e;
337 btd = b * d;
338 if (REAL_LESS(ate, btd) || REAL_EQUAL(ate, btd)) { //t<0
339 //reigon 7
340 t = 0.0;
341 nd = -d;
342 if (REAL_LESS(nd, 0.0) || REAL_EQUAL(nd, 0.0)) {
343 s = 0.0;
344 }
345 else if(REAL_GREAT(nd,a)||REAL_EQUAL(nd,a)){
346 s = 1.0;
347 }
348 else {
349 s = nd / a;
350 }
351 }
352 else {//t >0
353 t = ate - btd;
354 if (REAL_GREAT(t, det) || REAL_EQUAL(t, det)) {
355 //region 3
356 t = 1.0;
357 bmd = b - d;
358 if (REAL_LESS(bmd, 0.0) || REAL_EQUAL(bmd, 0.0)) {
359 s = 0.0;
360 }
361 else if (REAL_GREAT(bmd, a) || REAL_EQUAL(bmd, a)) {
362 s = 1.0;
363 }else{
364 s = bmd / a;
365 }
366 }
367 else { //0<t<1
368 //reigon 0
369 s /= det;
370 t /= det;
371 }
372 }
373 }
374 }
375
376 }
377 else {
378 //parallel
379
380 if (REAL_LESS(e,0.0)||REAL_EQUAL(e,0.0)) // t <= 0
381 {
382 // Now solve a*s - b*t + d = 0 for t = 0 (s = -d/a).
383 t = 0.0;
384 nd = -d;
385 if (REAL_LESS(nd, 0.0) || REAL_EQUAL(nd, 0.0)) // s <= 0
386 {
387 // region 6
388 s = 0.0;
389 }
390 else if (REAL_GREAT(nd,a)||REAL_EQUAL(nd,a)) // s >= 1
391 {
392 // region 8
393 s = 1.0;
394 }
395 else // 0 < s < 1
396 {
397 // region 7
398 s = nd / a;
399 }
400 }
401 else if (REAL_GREAT(e,c)||REAL_EQUAL(e,c)) // t >= 1
402 {
403 // Now solve a*s - b*t + d = 0 for t = 1 (s = (b-d)/a).
404 t = 1.0;
405 bmd = b - d;
406 if (REAL_LESS(bmd,0.0)||REAL_EQUAL(bmd,0.0)) // s <= 0
407 {
408 // region 4
409 s = 0.0;
410 }
411 else if (REAL_GREAT(bmd,a)||REAL_EQUAL(bmd,a)) // s >= 1
412 {
413 // region 2
414 s = 1.0;
415 }
416 else // 0 < s < 1
417 {
418 // region 3
419 s = bmd / a;
420 }
421 }
422 else // 0 < t < 1
423 {
424 // The point (0,e/c) is on the line and domain, so we have
425 // one point at which R is a minimum.
426 s = 0.0;
427 t = e / c;
428 }
429 }
430
431 Vector<T, 3> v = P0mQ0 + s * P1mP0 - (t*Q1mQ0);
432 if (para!=nullptr) {
433 para[0] = 1.0 - s;;
434 para[1] = s;
435 para[2] = 1.0-t;
436 para[3] = t ;
437 }
438 return v;
439
440
441 }
442
443
444 //edge(x0,x1), edge(x2,x3)
445 template<typename T>
446 DYN_FUNC T AdditiveCCD<T>::SquareDistanceEE(
447 const Vector<T, 3>& x0, const Vector<T, 3>& x1,
448 const Vector<T, 3>& x2, const Vector<T, 3>& x3) {
449
450 Vector<T, 3> signedD = DistanceEE(x0, x1, x2, x3, nullptr);
451 T d = signedD.normSquared();
452 return d;
453 }
454
455 //tri(x0,x1,x2), vex(x3)
456 template<typename T>
457 DYN_FUNC T AdditiveCCD<T>::SquareDistanceVF(const Vector<T, 3>& x0, const Vector<T, 3>& x1, const Vector<T, 3>& x2,
458 const Vector<T, 3>& x3) {
459
460 Vector<T, 3> signedD = DistanceVF_v(x3, x0, x1, x2, nullptr);
461 T d = signedD.normSquared();
462
463 // T d = DistanceVF(x3, x0, x1, x2);
464
465 return d;
466 }
467
468 template<typename T>
469 DYN_FUNC bool AdditiveCCD<T>::VertexFaceCCD(
470 const Vector<T, 3>& x0, const Vector<T, 3>& x1, const Vector<T, 3>& x2, const Vector<T, 3>& x3,
471 const Vector<T, 3>& y0, const Vector<T, 3>& y1, const Vector<T, 3>& y2, const Vector<T, 3>& y3,
472 T& time, T invL) {
473 //denote p as the transform vector from t0 to t1
474 Vector<T, 3> p[4];
475 p[0] = y0 - x0;
476 p[1] = y1 - x1;
477 p[2] = y2 - x2;
478 p[3] = y3 - x3;
479
480 Vector<T, 3> x[4];
481 x[0] = x0, x[1] = x1; x[2] = x2; x[3] = x3;
482
483 Vector<T, 3> p_bar = (p[0] + p[1] + p[2] + p[3]) / 4.0;
484 for (int i = 0; i < 4; ++i) {
485 p[i] -= p_bar;
486 }
487 T lp = max(max(p[0].norm(), p[1].norm()), p[2].norm()) + p[3].norm();
488 if (lp == 0.0) return false;
489 T dsqr = SquareDistanceVF(x[0], x[1], x[2], x[3]);
490 //printf("dsqrVF:%f\n", sqrt(dsqr));
491
492 T g = this->s * (dsqr - glm::pow(this->xi * invL, 2)) / (glm::sqrt(dsqr) + this->xi * invL);
493 time = 0.0;
494 T tL = (1 - this->s) * (dsqr - glm::pow(this->xi * invL, 2)) / ((glm::sqrt(dsqr) + this->xi* invL) * lp);
495
496 if (tL<0.0){
497 time = 0.0;
498 return true;
499 }if (tL >= 1.0)
500 return false;
501
502 int ite = 0;
503 while (ite<MAX_ITE) {
504 tL = (1 - this->s) * (dsqr - glm::pow(this->xi * invL, 2)) / ((glm::sqrt(dsqr) + this->xi * invL) * lp);
505 for (int i = 0; i < 4; ++i)
506 x[i] += tL * p[i];
507
508 dsqr = SquareDistanceVF(x[0], x[1], x[2], x[3]);
509 if (time > 0.0 && ((dsqr - glm::pow(this->xi * invL, 2)) / (glm::sqrt(dsqr) + this->xi * invL) <= g))
510 break;
511 time += tL;
512 if (REAL_GREAT(time, this->tc))
513 return false;
514 ++ite;
515 /*if (tL <= 0.0)
516 printf("VF, int %d, dsqr-xi^2: %f, lp: %f, tL: %f, time: %f\n", ite, dsqr - pow(this->xi, 2), lp, tL,time);
517 */
518 }
519
520 //printf("max VF ite:%d\n", ite);
521 return true;
522
523 }
524
525 template<typename T>
526 DYN_FUNC bool AdditiveCCD<T>::EdgeEdgeCCD(
527 const Vector<T, 3>& x0, const Vector<T, 3>& x1, const Vector<T, 3>& x2, const Vector<T, 3>& x3,
528 const Vector<T, 3>& y0, const Vector<T, 3>& y1, const Vector<T, 3>& y2, const Vector<T, 3>& y3,
529 T& time, T invL)
530 {
531 //denote p as the transform vector from t0 to t1
532 Vector<T, 3> p[4];
533 p[0]= y0 - x0;
534 p[1] = y1 - x1;
535 p[2] = y2 - x2;
536 p[3] = y3 - x3;
537 Vector<T, 3> x[4] = { x0,x1,x2,x3 };
538 auto mLength = [](Vector<T, 3>p1, Vector<T, 3> p2, Vector<T, 3>q1, Vector<T, 3> q2) {
539 T L1 = pow((p1 - q1).norm(),2);
540 T L2 = pow((p1 - q2).norm(),2);
541 T L3 = pow((p2 - q1).norm(), 2);
542 T L4 = pow((p2 - q2).norm(), 2);
543 return minimum(minimum(L1, L2),minimum(L3, L4));
544 };
545
546 Vector<T, 3> p_bar = (p[0] + p[1] + p[2] + p[3]) / 4.0;
547 for (int i = 0; i < 4; ++i) {
548 p[i] -= p_bar;
549 }
550
551 T lp = max(p[0].norm(), p[1].norm()) +
552 max(p[2].norm(), p[3].norm());
553
554 if (lp ==0.0) return false;
555
556 T dsqr = SquareDistanceEE(x[0], x[1], x[2], x[3]);
557
558 /*
559 if (dsqr-pow(this->xi,2) <= 0.0)
560 dsqr = mLength(x[0], x[1], x[2], x[3]);
561 */
562 //printf("dsqrEE:%f\n",sqrt(dsqr));
563
564 T g = this->s * (dsqr - pow(this->xi * invL, 2)) / (sqrt(dsqr) + this->xi * invL);
565 time = 0.0;
566 T tL = (1 - this->s) * (dsqr - pow(this->xi * invL, 2)) / ((sqrt(dsqr) + this->xi * invL) * lp);
567 if (tL < 0.0) {
568 time = 0.0;
569 return true;
570 }if (tL >= 1.0)
571 return false;
572
573 int ite = 0;
574 while (ite<MAX_ITE) {
575 tL = (1 - this->s) * (dsqr - pow(this->xi * invL, 2)) / ((sqrt(dsqr) + this->xi * invL) * lp);
576
577 for (int i = 0; i < 4; ++i) {
578 x[i] += tL * p[i];
579 }
580
581 dsqr = SquareDistanceEE(x[0], x[1], x[2], x[3]);
582 /*
583 if (dsqr - pow(this->xi, 2) <= 0.0)
584 dsqr = mLength(x[0], x[1], x[2], x[3]);
585 */
586 auto r = (dsqr - pow(this->xi * invL, 2)) / (sqrt(dsqr) + this->xi * invL);
587 if (time >0.0 && r<g)
588 break;
589 time += tL;
590 if (REAL_GREAT(time, this->tc))
591 return false;
592
593 ++ite;
594
595 }
596
597 return true;
598 }
599
600 template<typename T>
601 DYN_FUNC bool AdditiveCCD<T>::TriangleCCD(TTriangle3D<Real>& s0, TTriangle3D<Real>& s1, TTriangle3D<Real>& t0, TTriangle3D<Real>& t1, Real& toi)
602 {
603 Real l0 = s0.maximumEdgeLength();
604 Real l1 = s1.maximumEdgeLength();
605 Real l2 = t0.maximumEdgeLength();
606 Real l3 = t1.maximumEdgeLength();
607
608 Real lmax = maximum(maximum(l0, l1), maximum(l2, l3));
609 if (lmax < REAL_EPSILON)
610 return false;
611
612 Real invL = 1 / lmax;
613
614 Vector<Real, 3> p[3];
615 p[0] = invL * s0.v[0];
616 p[1] = invL * s0.v[1];
617 p[2] = invL * s0.v[2];
618
619 Vector<Real, 3> pp[3];
620 pp[0] = invL * s1.v[0];
621 pp[1] = invL * s1.v[1];
622 pp[2] = invL * s1.v[2];
623
624 Vector<Real, 3> q[3];
625 q[0] = invL * t0.v[0];
626 q[1] = invL * t0.v[1];
627 q[2] = invL * t0.v[2];
628
629 Vector<Real, 3> qq[3];
630 qq[0] = invL * t1.v[0];
631 qq[1] = invL * t1.v[1];
632 qq[2] = invL * t1.v[2];
633
635 //VF
636 bool ret = false;
637 for (int st = 0; st < 3; st++)
638 {
639 Real t = Real(1);
640 bool collided = this->VertexFaceCCD(
641 q[0], q[1], q[2],p[st],
642 qq[0], qq[1], qq[2],pp[st],
643 t, invL);
644
645
646 toi = collided ? minimum(t, toi) : toi;
647 ret |= collided;
648 }
649
650 //VF
651 for (int st = 0; st < 3; st++)
652 {
653 Real t = Real(1);
654 bool collided = this->VertexFaceCCD(
655 p[0], p[1], p[2],q[st],
656 pp[0], pp[1], pp[2],qq[st],
657 t, invL);
658 toi = collided ? minimum(t, toi) : toi;
659 ret |= collided;
660 }
661
662 //EE
663 for (int st = 0; st < 3; st++)
664 {
665 int ind0 = st;
666 int ind1 = (st + 1) % 3;
667 for (int ss = 0; ss < 3; ss++)
668 {
669 int ind2 = ss;
670 int ind3 = (ss + 1) % 3;
671
672 Real t = Real(1);
673 bool collided = this->EdgeEdgeCCD(
674 p[ind0], p[ind1], q[ind2], q[ind3],
675 pp[ind0], pp[ind1], qq[ind2], qq[ind3],
676 t, invL);
677
678 toi = collided ? minimum(t, toi) : toi;
679 ret |= collided;
680 }
681 }
682 toi = max(toi, 0.0);
683 toi = min(toi, 1.0);
684 return ret;
685 }
686
687
688 template<typename T>
689 DYN_FUNC void AdditiveCCD<T>::projectClosePoint(
690 const TTriangle3D<Real>& s, const TTriangle3D<Real>& t,
691 Vector<T, 3>& first, Vector<T, 3>& second) {
692
693 Real l0 = s.maximumEdgeLength();
694 Real l1 = t.maximumEdgeLength();
695
696
697 Real lmax = maximum(l0, l1);
698 if (lmax < REAL_EPSILON) { //triangles are as small as points.
699 first[0] = 1.0, first[1] = 0.0, first[2] = 0.0,
700 second[0] = 1.0, second[1] = 0.0, second[2] = 0.0;
701 return;
702 }
703
704 Real invL = 1 / lmax;
705
706 Vector<Real, 3> p[3];
707 p[0] = invL * s.v[0];
708 p[1] = invL * s.v[1];
709 p[2] = invL * s.v[2];
710
711 Vector<Real, 3> q[3];
712 q[0] = invL * t.v[0];
713 q[1] = invL * t.v[1];
714 q[2] = invL * t.v[2];
715
716 //VF
717 Real D = REAL_infinity;
718
719 for (int st = 0; st < 3; st++)
720 {
721 T para[4];
722 auto disVector = this->DistanceVF_v(
723 p[st], q[0], q[1], q[2], para);
724 auto dis = disVector.norm();
725 if (dis < D) {
726 D = dis;
727 first[st] = para[0];
728 first[(st + 1) % 3] = 0.0;
729 first[(st + 2) % 3] = 0.0;
730 second[0] = para[1];
731 second[1] = para[2];
732 second[2] = para[3];
733 }
734 }
735
736 //VF
737 for (int st = 0; st < 3; st++)
738 {
739 T para[4];
740 auto disVector = this->DistanceVF_v(
741 q[st], p[0], p[1], p[2], para);
742 auto dis = disVector.norm();
743 if (dis < D) {
744 D = dis;
745 second[st] = para[0];
746 second[(st + 1) % 3] = 0.0;
747 second[(st + 2) % 3] = 0.0;
748 first[0] = para[1];
749 first[1] = para[2];
750 first[2] = para[3];
751 }
752 }
753 //EE
754 for (int st = 0; st < 3; st++)
755 {
756 int ind0 = st;
757 int ind1 = (st + 1) % 3;
758 for (int ss = 0; ss < 3; ss++)
759 {
760 int ind2 = ss;
761 int ind3 = (ss + 1) % 3;
762
763 T para[4];
764 auto disVector = this->DistanceEE(
765 p[ind0], p[ind1], q[ind2], q[ind3], para);
766 auto dis = disVector.norm();
767 if (dis <= (D+EPSILON)) {
768 D = dis;
769 first[ind0] = para[0];
770 first[ind1] = para[1];
771 first[(ind1 + 1) % 3] = 0.0;
772 second[ind2] = para[2];
773 second[ind3] = para[3];
774 second[(ind3 + 1) % 3] = 0.0;
775 }
776 }
777 }
778
779 }//end function
780
781
782
783
784}//end inl
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
MAX_ITE
#define MAX_ITE
Definition AdditiveCCD.inl:9
Real
double Real
Definition Typedef.inl:23
dyno::AdditiveCCD::DistanceVF_v
DYN_FUNC Vector< T, 3 > DistanceVF_v(const Vector< T, 3 > &x, const Vector< T, 3 > &y0, const Vector< T, 3 > &y1, const Vector< T, 3 > &y2, T *para)
Definition AdditiveCCD.inl:72
dyno::AdditiveCCD::DistanceVF
DYN_FUNC T DistanceVF(const Vector< T, 3 > &x, const Vector< T, 3 > &y0, const Vector< T, 3 > &y1, const Vector< T, 3 > &y2)
Definition AdditiveCCD.inl:47
dyno::AdditiveCCD::VertexFaceCCD
DYN_FUNC bool VertexFaceCCD(const Vector< T, 3 > &x0, const Vector< T, 3 > &x1, const Vector< T, 3 > &x2, const Vector< T, 3 > &x3, const Vector< T, 3 > &y0, const Vector< T, 3 > &y1, const Vector< T, 3 > &y2, const Vector< T, 3 > &y3, T &time, T invL)
Do a continuous collision detection between a vertex and a triangle.
Definition AdditiveCCD.inl:469
dyno::AdditiveCCD::DistanceEE
DYN_FUNC Vector< T, 3 > DistanceEE(const Vector< T, 3 > &x0, const Vector< T, 3 > &x1, const Vector< T, 3 > &y0, const Vector< T, 3 > &y1, T *para)
Definition AdditiveCCD.inl:250
dyno::AdditiveCCD::TriangleCCD
DYN_FUNC bool TriangleCCD(TTriangle3D< Real > &s0, TTriangle3D< Real > &s1, TTriangle3D< Real > &t0, TTriangle3D< Real > &t1, Real &toi)
Do a continuous collision detection between two triangles.
Definition AdditiveCCD.inl:601
dyno::AdditiveCCD::EdgeEdgeCCD
DYN_FUNC bool EdgeEdgeCCD(const Vector< T, 3 > &x0, const Vector< T, 3 > &x1, const Vector< T, 3 > &x2, const Vector< T, 3 > &x3, const Vector< T, 3 > &y0, const Vector< T, 3 > &y1, const Vector< T, 3 > &y2, const Vector< T, 3 > &y3, T &time, T invL)
Do a continuous collision detection between two edges.
Definition AdditiveCCD.inl:526
dyno::AdditiveCCD::projectClosePoint
DYN_FUNC void projectClosePoint(const TTriangle3D< Real > &s, const TTriangle3D< Real > &t, Vector< T, 3 > &first, Vector< T, 3 > &second)
find the close point between two triangles, store their barycentric coordinates ordered as vertex.
Definition AdditiveCCD.inl:689
dyno::AdditiveCCD::SquareDistanceEE
DYN_FUNC T SquareDistanceEE(const Vector< T, 3 > &x0, const Vector< T, 3 > &x1, const Vector< T, 3 > &x2, const Vector< T, 3 > &x3)
Definition AdditiveCCD.inl:446
dyno::AdditiveCCD::SquareDistanceVF
DYN_FUNC T SquareDistanceVF(const Vector< T, 3 > &x0, const Vector< T, 3 > &x1, const Vector< T, 3 > &x2, const Vector< T, 3 > &x3)
Definition AdditiveCCD.inl:457
dyno::TTriangle3D::maximumEdgeLength
DYN_FUNC Real maximumEdgeLength() const
Definition Primitive3D.inl:2968
T
#define T(t)
dyno
This is an implementation of AdditiveCCD based on peridyno.
Definition Array.h:25
dyno::getPoint2SegmentDistance
DYN_FUNC T getPoint2SegmentDistance(const Vector< T, 3 > &p, const Vector< T, 3 > &v0, const Vector< T, 3 > &v1)
Definition AdditiveCCD.inl:14
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::pow
DYN_FUNC Complex< Real > pow(const Complex< Real > &, const Real &)
Definition Complex.inl:370
dyno::EPSILON
constexpr Real EPSILON
Definition Typedef.inl:42
dyno::inTri
DYN_FUNC bool inTri(const Vector< T, 3 > &p, const Vector< T, 3 > &v0, const Vector< T, 3 > &v1, const Vector< T, 3 > &v2)
Definition AdditiveCCD.inl:32
dyno::minimum
DYN_FUNC T minimum(const T &v0, const T &v1)
Definition SimpleMath.h:120
dyno::REAL_EPSILON
constexpr Real REAL_EPSILON
Definition Typedef.inl:43
dyno::sqrt
DYN_FUNC Complex< Real > sqrt(const Complex< Real > &)
Definition Complex.inl:321
dyno::maximum
DYN_FUNC T maximum(const T &v0, const T &v1)
Definition SimpleMath.h:160
max
#define max(x, y)
Definition svd3_cuda.h:41
Generated by doxygen 1.13.2