1
// Released into the public domain by Robert Bridson, 2009.
8
//==============================================================================
10
same_sign(double a, double b)
12
return (a<=0 && b<=0) || (a>=0 && b>=0);
15
//==============================================================================
17
simplex_intersection1d(int k,
26
assert(alpha0 && alpha1 && alpha2);
29
if(x0[0]<x1[0]) return 0;
30
else if(x0[0]>x2[0]) return 0;
32
*alpha1=(x2[0]-x0[0])/(x2[0]-x1[0]);
33
*alpha2=(x0[0]-x1[0])/(x2[0]-x1[0]);
35
}else if(x1[0]>x2[0]){
36
if(x0[0]<x2[0]) return 0;
37
else if(x0[0]>x1[0]) return 0;
39
*alpha1=(x2[0]-x0[0])/(x2[0]-x1[0]);
40
*alpha2=(x0[0]-x1[0])/(x2[0]-x1[0]);
42
}else{ // x1[0]==x2[0]
43
if(x0[0]!=x1[0]) return 0;
50
return simplex_intersection1d(1, x2, x1, 0, alpha2, alpha1, alpha0);
53
//==============================================================================
54
// degenerate test in 2d - assumes three points lie on the same line
56
simplex_intersection2d(int k,
65
// try projecting each coordinate out in turn
67
if(!simplex_intersection1d(1, x0+1, x1+1, x2+1, &ax0, &ax1, &ax2)) return 0;
69
if(!simplex_intersection1d(1, x0, x1, x2, &ay0, &ay1, &ay2)) return 0;
70
// decide which solution is more accurate for barycentric coordinates
71
double checkx=std::fabs(-ax0*x0[0]+ax1*x1[0]+ax2*x2[0])
72
+std::fabs(-ax0*x0[1]+ax1*x1[1]+ax2*x2[1]);
73
double checky=std::fabs(-ay0*x0[0]+ay1*x1[0]+ay2*x2[0])
74
+std::fabs(-ay0*x0[1]+ay1*x1[1]+ay2*x2[1]);
87
//==============================================================================
89
simplex_intersection2d(int k,
102
case 1: // point vs. triangle
103
*alpha1=-orientation2d(x0, x2, x3);
104
*alpha2= orientation2d(x0, x1, x3);
105
if(!same_sign(*alpha1, *alpha2)) return 0;
106
*alpha3=-orientation2d(x0, x1, x2);
107
if(!same_sign(*alpha1, *alpha3)) return 0;
108
if(!same_sign(*alpha2, *alpha3)) return 0;
109
sum2=*alpha1+*alpha2+*alpha3;
110
if(sum2){ // triangle not degenerate?
116
}else{ // triangle is degenerate and point lies on same line
117
if(simplex_intersection2d(1, x0, x1, x2, alpha0, alpha1, alpha2)){
121
if(simplex_intersection2d(1, x0, x1, x3, alpha0, alpha1, alpha3)){
125
if(simplex_intersection2d(1, x0, x2, x3, alpha0, alpha2, alpha3)){
132
case 2: // segment vs. segment
133
*alpha0= orientation2d(x1, x2, x3);
134
*alpha1=-orientation2d(x0, x2, x3);
135
if(!same_sign(*alpha0, *alpha1)) return 0;
136
*alpha2= orientation2d(x0, x1, x3);
137
*alpha3=-orientation2d(x0, x1, x2);
138
if(!same_sign(*alpha2, *alpha3)) return 0;
139
sum1=*alpha0+*alpha1;
140
sum2=*alpha2+*alpha3;
147
}else{ // degenerate: segments lie on the same line
148
if(simplex_intersection2d(1, x0, x2, x3, alpha0, alpha2, alpha3)){
152
if(simplex_intersection2d(1, x1, x2, x3, alpha1, alpha2, alpha3)){
156
if(simplex_intersection2d(1, x2, x0, x1, alpha2, alpha0, alpha1)){
160
if(simplex_intersection2d(1, x3, x0, x1, alpha3, alpha0, alpha1)){
166
case 3: // triangle vs. point
167
return simplex_intersection2d(1, x3, x2, x1, x0,
168
alpha3, alpha2, alpha1, alpha0);
170
return -1; // should never get here
174
//==============================================================================
175
// degenerate test in 3d - assumes four points lie on the same plane
177
simplex_intersection3d(int k,
188
// try projecting each coordinate out in turn
189
double ax0, ax1, ax2, ax3;
190
if(!simplex_intersection2d(k, x0+1, x1+1, x2+1, x3+1, &ax0, &ax1, &ax2,&ax3))
192
double ay0, ay1, ay2, ay3;
193
double p0[2]={x0[0], x0[2]}, p1[2]={x1[0], x1[2]},
194
p2[2]={x2[0], x2[2]}, p3[2]={x3[0], x3[2]};
195
if(!simplex_intersection2d(k, p0, p1, p2, p3, &ay0, &ay1, &ay2, &ay3))
197
double az0, az1, az2, az3;
198
if(!simplex_intersection2d(k, x0, x1, x2, x3, &az0, &az1, &az2, &az3))
200
// decide which solution is more accurate for barycentric coordinates
201
double checkx, checky, checkz;
203
checkx=std::fabs(-ax0*x0[0]+ax1*x1[0]+ax2*x2[0]+ax3*x3[0])
204
+std::fabs(-ax0*x0[1]+ax1*x1[1]+ax2*x2[1]+ax3*x3[1])
205
+std::fabs(-ax0*x0[2]+ax1*x1[2]+ax2*x2[2]+ax3*x3[2]);
206
checky=std::fabs(-ay0*x0[0]+ay1*x1[0]+ay2*x2[0]+ay3*x3[0])
207
+std::fabs(-ay0*x0[1]+ay1*x1[1]+ay2*x2[1]+ay3*x3[1])
208
+std::fabs(-ay0*x0[2]+ay1*x1[2]+ay2*x2[2]+ay3*x3[2]);
209
checkz=std::fabs(-az0*x0[0]+az1*x1[0]+az2*x2[0]+az3*x3[0])
210
+std::fabs(-az0*x0[1]+az1*x1[1]+az2*x2[1]+az3*x3[1])
211
+std::fabs(-az0*x0[2]+az1*x1[2]+az2*x2[2]+az3*x3[2]);
213
checkx=std::fabs(-ax0*x0[0]-ax1*x1[0]+ax2*x2[0]+ax3*x3[0])
214
+std::fabs(-ax0*x0[1]-ax1*x1[1]+ax2*x2[1]+ax3*x3[1])
215
+std::fabs(-ax0*x0[2]-ax1*x1[2]+ax2*x2[2]+ax3*x3[2]);
216
checky=std::fabs(-ay0*x0[0]-ay1*x1[0]+ay2*x2[0]+ay3*x3[0])
217
+std::fabs(-ay0*x0[1]-ay1*x1[1]+ay2*x2[1]+ay3*x3[1])
218
+std::fabs(-ay0*x0[2]-ay1*x1[2]+ay2*x2[2]+ay3*x3[2]);
219
checkz=std::fabs(-az0*x0[0]-az1*x1[0]+az2*x2[0]+az3*x3[0])
220
+std::fabs(-az0*x0[1]-az1*x1[1]+az2*x2[1]+az3*x3[1])
221
+std::fabs(-az0*x0[2]-az1*x1[2]+az2*x2[2]+az3*x3[2]);
223
if(checkx<=checky && checkx<=checkz){
228
}else if(checky<=checkz){
242
//==============================================================================
244
simplex_intersection3d(int k,
256
assert(1<=k && k<=4);
259
case 1: // point vs. tetrahedron
260
*alpha1=-orientation3d(x0, x2, x3, x4);
261
*alpha2= orientation3d(x0, x1, x3, x4);
262
if(!same_sign(*alpha1, *alpha2)) return 0;
263
*alpha3=-orientation3d(x0, x1, x2, x4);
264
if(!same_sign(*alpha1, *alpha3)) return 0;
265
if(!same_sign(*alpha2, *alpha3)) return 0;
266
*alpha4= orientation3d(x0, x1, x2, x3);
267
if(!same_sign(*alpha1, *alpha4)) return 0;
268
if(!same_sign(*alpha2, *alpha4)) return 0;
269
if(!same_sign(*alpha3, *alpha4)) return 0;
271
sum2=*alpha1+*alpha2+*alpha3+*alpha4;
278
}else{ // degenerate: point and tetrahedron in same plane
279
if(simplex_intersection3d(1, x0, x2, x3, x4,
280
alpha0, alpha2, alpha3, alpha4)){
284
if(simplex_intersection3d(1, x0, x1, x3, x4,
285
alpha0, alpha1, alpha3, alpha4)){
289
if(simplex_intersection3d(1, x0, x1, x2, x4,
290
alpha0, alpha1, alpha2, alpha4)){
294
if(simplex_intersection3d(1, x0, x1, x2, x3,
295
alpha0, alpha1, alpha2, alpha3)){
302
case 2: // segment vs. triangle
303
*alpha0= orientation3d(x1, x2, x3, x4);
304
*alpha1=-orientation3d(x0, x2, x3, x4);
305
if(!same_sign(*alpha0, *alpha1)) return 0;
306
*alpha2= orientation3d(x0, x1, x3, x4);
307
*alpha3=-orientation3d(x0, x1, x2, x4);
308
if(!same_sign(*alpha2, *alpha3)) return 0;
309
*alpha4= orientation3d(x0, x1, x2, x3);
310
if(!same_sign(*alpha2, *alpha4)) return 0;
311
if(!same_sign(*alpha3, *alpha4)) return 0;
312
sum1=*alpha0+*alpha1;
313
sum2=*alpha2+*alpha3+*alpha4;
322
}else{ // degenerate: segment and triangle in same plane
323
if(simplex_intersection3d(1, x1, x2, x3, x4,
324
alpha1, alpha2, alpha3, alpha4)){
328
if(simplex_intersection3d(1, x0, x2, x3, x4,
329
alpha0, alpha2, alpha3, alpha4)){
333
if(simplex_intersection3d(2, x0, x1, x3, x4,
334
alpha0, alpha1, alpha3, alpha4)){
338
if(simplex_intersection3d(2, x0, x1, x2, x4,
339
alpha0, alpha1, alpha2, alpha4)){
343
if(simplex_intersection3d(2, x0, x1, x2, x3,
344
alpha0, alpha1, alpha2, alpha3)){
351
case 3: // triangle vs. segment
352
case 4: // tetrahedron vs. point
353
return simplex_intersection3d(5-k, x4, x3, x2, x1, x0,
354
alpha4, alpha3, alpha2, alpha1, alpha0);
356
return -1; // should never get here
360
//==============================================================================
361
// degenerate test in 3d+time - assumes five points lie on the same hyper-plane
363
simplex_intersection_time3d(int k,
364
const double* x0, int time0,
365
const double* x1, int time1,
366
const double* x2, int time2,
367
const double* x3, int time3,
368
const double* x4, int time4,
376
assert(time0==0 || time0==1);
377
assert(time1==0 || time1==1);
378
assert(time2==0 || time2==1);
379
assert(time3==0 || time3==1);
380
assert(time4==0 || time4==1);
381
// try projecting each coordinate out in turn
382
double ax0, ax1, ax2, ax3, ax4;
383
double r0[3]={x0[0], x0[2], time0}, r1[3]={x1[0], x1[2], time1},
384
r2[3]={x2[0], x2[2], time2}, r3[3]={x3[0], x3[2], time3},
385
r4[3]={x4[0], x4[2], time4};
386
if(!simplex_intersection3d(k, r0, r1, r2, r3, r4,
387
&ax0, &ax1, &ax2, &ax3, &ax4)) return 0;
388
double ay0, ay1, ay2, ay3, ay4;
389
double p0[3]={x0[0], x0[2], time0}, p1[3]={x1[0], x1[2], time1},
390
p2[3]={x2[0], x2[2], time2}, p3[3]={x3[0], x3[2], time3},
391
p4[3]={x4[0], x4[2], time4};
392
if(!simplex_intersection3d(k, p0, p1, p2, p3, p4,
393
&ay0, &ay1, &ay2, &ay3, &ay4)) return 0;
394
double az0, az1, az2, az3, az4;
395
double q0[3]={x0[0], x0[1], time0}, q1[3]={x1[0], x1[1], time1},
396
q2[3]={x2[0], x2[1], time2}, q3[3]={x3[0], x3[1], time3},
397
q4[3]={x4[0], x4[1], time4};
398
if(!simplex_intersection3d(k, q0, q1, q2, q3, q4,
399
&az0, &az1, &az2, &az3, &az4)) return 0;
400
double at0, at1, at2, at3, at4;
401
if(!simplex_intersection3d(k, x0, x1, x2, x3, x4,
402
&at0, &at1, &at2, &at3, &at4)) return 0;
403
// decide which solution is more accurate for barycentric coordinates
404
double checkx, checky, checkz, checkt;
406
checkx=std::fabs(-ax0*x0[0]+ax1*x1[0]+ax2*x2[0]+ax3*x3[0]+ax4*x4[0])
407
+std::fabs(-ax0*x0[1]+ax1*x1[1]+ax2*x2[1]+ax3*x3[1]+ax4*x4[1])
408
+std::fabs(-ax0*x0[2]+ax1*x1[2]+ax2*x2[2]+ax3*x3[2]+ax4*x4[2])
409
+std::fabs(-ax0*time0+ax1*time1+ax2*time2+ax3*time3+ax4*time4);
410
checky=std::fabs(-ay0*x0[0]+ay1*x1[0]+ay2*x2[0]+ay3*x3[0]+ay4*x4[0])
411
+std::fabs(-ay0*x0[1]+ay1*x1[1]+ay2*x2[1]+ay3*x3[1]+ay4*x4[1])
412
+std::fabs(-ay0*x0[2]+ay1*x1[2]+ay2*x2[2]+ay3*x3[2]+ay4*x4[2])
413
+std::fabs(-ay0*time0+ay1*time1+ay2*time2+ay3*time3+ay4*time4);
414
checkz=std::fabs(-az0*x0[0]+az1*x1[0]+az2*x2[0]+az3*x3[0]+az4*x4[0])
415
+std::fabs(-az0*x0[1]+az1*x1[1]+az2*x2[1]+az3*x3[1]+az4*x4[1])
416
+std::fabs(-az0*x0[2]+az1*x1[2]+az2*x2[2]+az3*x3[2]+az4*x4[2])
417
+std::fabs(-az0*time0+az1*time1+az2*time2+az3*time3+az4*time4);
418
checkt=std::fabs(-at0*x0[0]+at1*x1[0]+at2*x2[0]+at3*x3[0]+at4*x4[0])
419
+std::fabs(-at0*x0[1]+at1*x1[1]+at2*x2[1]+at3*x3[1]+at4*x4[1])
420
+std::fabs(-at0*x0[2]+at1*x1[2]+at2*x2[2]+at3*x3[2]+at4*x4[2])
421
+std::fabs(-at0*time0+at1*time1+at2*time2+at3*time3+at4*time4);
423
checkx=std::fabs(-ax0*x0[0]-ax1*x1[0]+ax2*x2[0]+ax3*x3[0]+ax4*x4[0])
424
+std::fabs(-ax0*x0[1]-ax1*x1[1]+ax2*x2[1]+ax3*x3[1]+ax4*x4[1])
425
+std::fabs(-ax0*x0[2]-ax1*x1[2]+ax2*x2[2]+ax3*x3[2]+ax4*x4[2])
426
+std::fabs(-ax0*time0-ax1*time1+ax2*time2+ax3*time3+ax4*time4);
427
checky=std::fabs(-ay0*x0[0]-ay1*x1[0]+ay2*x2[0]+ay3*x3[0]+ay4*x4[0])
428
+std::fabs(-ay0*x0[1]-ay1*x1[1]+ay2*x2[1]+ay3*x3[1]+ay4*x4[1])
429
+std::fabs(-ay0*x0[2]-ay1*x1[2]+ay2*x2[2]+ay3*x3[2]+ay4*x4[2])
430
+std::fabs(-ay0*time0-ay1*time1+ay2*time2+ay3*time3+ay4*time4);
431
checkz=std::fabs(-az0*x0[0]-az1*x1[0]+az2*x2[0]+az3*x3[0]+az4*x4[0])
432
+std::fabs(-az0*x0[1]-az1*x1[1]+az2*x2[1]+az3*x3[1]+az4*x4[1])
433
+std::fabs(-az0*x0[2]-az1*x1[2]+az2*x2[2]+az3*x3[2]+az4*x4[2])
434
+std::fabs(-az0*time0-az1*time1+az2*time2+az3*time3+az4*time4);
435
checkt=std::fabs(-at0*x0[0]-at1*x1[0]+at2*x2[0]+at3*x3[0]+at4*x4[0])
436
+std::fabs(-at0*x0[1]-at1*x1[1]+at2*x2[1]+at3*x3[1]+at4*x4[1])
437
+std::fabs(-at0*x0[2]-at1*x1[2]+at2*x2[2]+at3*x3[2]+at4*x4[2])
438
+std::fabs(-at0*time0-at1*time1+at2*time2+at3*time3+at4*time4);
440
if(checkx<=checky && checkx<=checkz && checkx<=checkt){
445
}else if(checky<=checkz && checky<=checkt){
450
}else if(checkz<=checkt){
464
//==============================================================================
466
simplex_intersection_time3d(int k,
467
const double* x0, int t0,
468
const double* x1, int t1,
469
const double* x2, int t2,
470
const double* x3, int t3,
471
const double* x4, int t4,
472
const double* x5, int t5,
480
assert(1<=k && k<=5);
481
assert(t0==0 || t0==1);
482
assert(t1==0 || t1==1);
483
assert(t2==0 || t2==1);
484
assert(t3==0 || t3==1);
485
assert(t4==0 || t4==1);
486
assert(t5==0 || t5==1);
489
case 1: // point vs. pentachoron
490
*alpha1=-orientation_time3d(x0, t0, x2, t2, x3, t3, x4, t4, x5, t5);
491
*alpha2= orientation_time3d(x0, t0, x1, t1, x3, t3, x4, t4, x5, t5);
492
if(!same_sign(*alpha1, *alpha2)) return 0;
493
*alpha3=-orientation_time3d(x0, t0, x1, t1, x2, t3, x4, t4, x5, t5);
494
if(!same_sign(*alpha1, *alpha3)) return 0;
495
*alpha4= orientation_time3d(x0, t0, x1, t1, x2, t2, x3, t3, x5, t5);
496
if(!same_sign(*alpha1, *alpha4)) return 0;
497
*alpha5=-orientation_time3d(x0, t0, x1, t1, x2, t2, x3, t3, x4, t4);
498
if(!same_sign(*alpha1, *alpha5)) return 0;
499
sum2=*alpha1+*alpha2+*alpha3+*alpha4+*alpha5;
509
if(simplex_intersection_time3d(1, x0, t0, x2, t2, x3, t3, x4, t4,
510
x5, t5, alpha0, alpha2, alpha3, alpha4, alpha5)){
514
if(simplex_intersection_time3d(1, x0, t0, x1, t1, x3, t3, x4, t4,
515
x5, t5, alpha0, alpha1, alpha3, alpha4, alpha5)){
519
if(simplex_intersection_time3d(1, x0, t0, x1, t1, x2, t2, x4, t4,
520
x5, t5, alpha0, alpha1, alpha2, alpha4, alpha5)){
524
if(simplex_intersection_time3d(1, x0, t0, x1, t1, x2, t2, x3, t3,
525
x5, t5, alpha0, alpha1, alpha2, alpha3, alpha5)){
529
if(simplex_intersection_time3d(1, x0, t0, x1, t1, x2, t2, x3, t3,
530
x4, t4, alpha0, alpha1, alpha2, alpha3, alpha4)){
537
case 2: // segment vs. tetrahedron
538
*alpha0= orientation_time3d(x1, t1, x2, t2, x3, t3, x4, t4, x5, t5);
539
*alpha1=-orientation_time3d(x0, t0, x2, t2, x3, t3, x4, t4, x5, t5);
540
if(!same_sign(*alpha0, *alpha1)) return 0;
541
*alpha2= orientation_time3d(x0, t0, x1, t1, x3, t3, x4, t4, x5, t5);
542
*alpha3=-orientation_time3d(x0, t0, x1, t1, x2, t2, x4, t4, x5, t5);
543
if(!same_sign(*alpha2, *alpha3)) return 0;
544
*alpha4= orientation_time3d(x0, t0, x1, t1, x2, t2, x3, t3, x5, t5);
545
if(!same_sign(*alpha2, *alpha4)) return 0;
546
*alpha5=-orientation_time3d(x0, t0, x1, t1, x2, t2, x3, t3, x4, t4);
547
if(!same_sign(*alpha2, *alpha5)) return 0;
548
sum1=*alpha0+*alpha1;
549
sum2=*alpha2+*alpha3+*alpha4+*alpha5;
559
if(simplex_intersection_time3d(1, x1, t1, x2, t2, x3, t3, x4, t4,
560
x5, t5, alpha1, alpha2, alpha3, alpha4, alpha5)){
564
if(simplex_intersection_time3d(1, x0, t0, x2, t2, x3, t3, x4, t4,
565
x5, t5, alpha0, alpha2, alpha3, alpha4, alpha5)){
569
if(simplex_intersection_time3d(2, x0, t0, x1, t1, x3, t3, x4, t4,
570
x5, t5, alpha0, alpha1, alpha3, alpha4, alpha5)){
574
if(simplex_intersection_time3d(2, x0, t0, x1, t1, x2, t2, x4, t4,
575
x5, t5, alpha0, alpha1, alpha2, alpha4, alpha5)){
579
if(simplex_intersection_time3d(2, x0, t0, x1, t1, x2, t2, x3, t3,
580
x5, t5, alpha0, alpha1, alpha2, alpha3, alpha5)){
584
if(simplex_intersection_time3d(2, x0, t0, x1, t1, x2, t2, x3, t3,
585
x4, t4, alpha0, alpha1, alpha2, alpha3, alpha4)){
592
case 3: // triangle vs. triangle
593
*alpha0= orientation_time3d(x1, t1, x2, t2, x3, t3, x4, t4, x5, t5);
594
*alpha1=-orientation_time3d(x0, t0, x2, t2, x3, t3, x4, t4, x5, t5);
595
if(!same_sign(*alpha0, *alpha1)) return 0;
596
*alpha2= orientation_time3d(x0, t0, x1, t1, x3, t3, x4, t4, x5, t5);
597
if(!same_sign(*alpha0, *alpha2)) return 0;
598
*alpha3=-orientation_time3d(x0, t0, x1, t1, x2, t2, x4, t4, x5, t5);
599
*alpha4= orientation_time3d(x0, t0, x1, t1, x2, t2, x3, t3, x5, t5);
600
if(!same_sign(*alpha3, *alpha4)) return 0;
601
*alpha5=-orientation_time3d(x0, t0, x1, t1, x2, t2, x3, t3, x4, t4);
602
if(!same_sign(*alpha3, *alpha5)) return 0;
603
sum1=*alpha0+*alpha1+*alpha2;
604
sum2=*alpha3+*alpha4+*alpha5;
614
if(simplex_intersection_time3d(2, x1, t1, x2, t2, x3, t3, x4, t4,
615
x5, t5, alpha1, alpha2, alpha3, alpha4, alpha5)){
619
if(simplex_intersection_time3d(2, x0, t0, x2, t2, x3, t3, x4, t4,
620
x5, t5, alpha0, alpha2, alpha3, alpha4, alpha5)){
624
if(simplex_intersection_time3d(2, x0, t0, x1, t1, x3, t3, x4, t4,
625
x5, t5, alpha0, alpha1, alpha3, alpha4, alpha5)){
629
if(simplex_intersection_time3d(2, x4, t4, x5, t5, x0, t0, x1, t1,
630
x2, t2, alpha4, alpha5, alpha0, alpha1, alpha2)){
634
if(simplex_intersection_time3d(2, x3, t3, x5, t5, x0, t0, x1, t1,
635
x2, t2, alpha3, alpha5, alpha0, alpha1, alpha2)){
639
if(simplex_intersection_time3d(2, x3, t3, x4, t4, x0, t0, x1, t1,
640
x2, t2, alpha3, alpha4, alpha0, alpha1, alpha2)){
647
case 4: // tetrahedron vs. segment
648
case 5: // pentachoron vs. point
649
return simplex_intersection_time3d(6-k, x5, t5, x4, t4, x3, t3, x2, t2,
650
x1, t1, x0, t0, alpha5, alpha4, alpha3, alpha2, alpha1, alpha0);
652
return -1; // should never get here
656
//==============================================================================
657
// degenerate test in 4d - assumes five points lie on the same hyper-plane
659
simplex_intersection4d(int k,
672
// try projecting each coordinate out in turn
673
double ax0, ax1, ax2, ax3, ax4;
674
if(!simplex_intersection3d(k, x0+1, x1+1, x2+1, x3+1, x4+1,
675
&ax0, &ax1, &ax2, &ax3, &ax4)) return 0;
676
double ay0, ay1, ay2, ay3, ay4;
677
double p0[3]={x0[0], x0[2], x0[3]}, p1[3]={x1[0], x1[2], x1[3]},
678
p2[3]={x2[0], x2[2], x2[3]}, p3[3]={x3[0], x3[2], x3[3]},
679
p4[3]={x4[0], x4[2], x4[3]};
680
if(!simplex_intersection3d(k, p0, p1, p2, p3, p4,
681
&ay0, &ay1, &ay2, &ay3, &ay4)) return 0;
682
double az0, az1, az2, az3, az4;
683
double q0[3]={x0[0], x0[1], x0[3]}, q1[3]={x1[0], x1[1], x1[3]},
684
q2[3]={x2[0], x2[1], x2[3]}, q3[3]={x3[0], x3[1], x3[3]},
685
q4[3]={x4[0], x4[1], x4[3]};
686
if(!simplex_intersection3d(k, q0, q1, q2, q3, q4,
687
&az0, &az1, &az2, &az3, &az4)) return 0;
688
double at0, at1, at2, at3, at4;
689
if(!simplex_intersection3d(k, x0, x1, x2, x3, x4,
690
&at0, &at1, &at2, &at3, &at4)) return 0;
691
// decide which solution is more accurate for barycentric coordinates
692
double checkx, checky, checkz, checkt;
694
checkx=std::fabs(-ax0*x0[0]+ax1*x1[0]+ax2*x2[0]+ax3*x3[0]+ax4*x4[0])
695
+std::fabs(-ax0*x0[1]+ax1*x1[1]+ax2*x2[1]+ax3*x3[1]+ax4*x4[1])
696
+std::fabs(-ax0*x0[2]+ax1*x1[2]+ax2*x2[2]+ax3*x3[2]+ax4*x4[2])
697
+std::fabs(-ax0*x0[3]+ax1*x1[3]+ax2*x2[3]+ax3*x3[3]+ax4*x4[3]);
698
checky=std::fabs(-ay0*x0[0]+ay1*x1[0]+ay2*x2[0]+ay3*x3[0]+ay4*x4[0])
699
+std::fabs(-ay0*x0[1]+ay1*x1[1]+ay2*x2[1]+ay3*x3[1]+ay4*x4[1])
700
+std::fabs(-ay0*x0[2]+ay1*x1[2]+ay2*x2[2]+ay3*x3[2]+ay4*x4[2])
701
+std::fabs(-ay0*x0[3]+ay1*x1[3]+ay2*x2[3]+ay3*x3[3]+ay4*x4[3]);
702
checkz=std::fabs(-az0*x0[0]+az1*x1[0]+az2*x2[0]+az3*x3[0]+az4*x4[0])
703
+std::fabs(-az0*x0[1]+az1*x1[1]+az2*x2[1]+az3*x3[1]+az4*x4[1])
704
+std::fabs(-az0*x0[2]+az1*x1[2]+az2*x2[2]+az3*x3[2]+az4*x4[2])
705
+std::fabs(-az0*x0[3]+az1*x1[3]+az2*x2[3]+az3*x3[3]+az4*x4[3]);
706
checkt=std::fabs(-at0*x0[0]+at1*x1[0]+at2*x2[0]+at3*x3[0]+at4*x4[0])
707
+std::fabs(-at0*x0[1]+at1*x1[1]+at2*x2[1]+at3*x3[1]+at4*x4[1])
708
+std::fabs(-at0*x0[2]+at1*x1[2]+at2*x2[2]+at3*x3[2]+at4*x4[2])
709
+std::fabs(-at0*x0[3]+at1*x1[3]+at2*x2[3]+at3*x3[3]+at4*x4[3]);
711
checkx=std::fabs(-ax0*x0[0]-ax1*x1[0]+ax2*x2[0]+ax3*x3[0]+ax4*x4[0])
712
+std::fabs(-ax0*x0[1]-ax1*x1[1]+ax2*x2[1]+ax3*x3[1]+ax4*x4[1])
713
+std::fabs(-ax0*x0[2]-ax1*x1[2]+ax2*x2[2]+ax3*x3[2]+ax4*x4[2])
714
+std::fabs(-ax0*x0[3]-ax1*x1[3]+ax2*x2[3]+ax3*x3[3]+ax4*x4[3]);
715
checky=std::fabs(-ay0*x0[0]-ay1*x1[0]+ay2*x2[0]+ay3*x3[0]+ay4*x4[0])
716
+std::fabs(-ay0*x0[1]-ay1*x1[1]+ay2*x2[1]+ay3*x3[1]+ay4*x4[1])
717
+std::fabs(-ay0*x0[2]-ay1*x1[2]+ay2*x2[2]+ay3*x3[2]+ay4*x4[2])
718
+std::fabs(-ay0*x0[3]-ay1*x1[3]+ay2*x2[3]+ay3*x3[3]+ay4*x4[3]);
719
checkz=std::fabs(-az0*x0[0]-az1*x1[0]+az2*x2[0]+az3*x3[0]+az4*x4[0])
720
+std::fabs(-az0*x0[1]-az1*x1[1]+az2*x2[1]+az3*x3[1]+az4*x4[1])
721
+std::fabs(-az0*x0[2]-az1*x1[2]+az2*x2[2]+az3*x3[2]+az4*x4[2])
722
+std::fabs(-az0*x0[3]-az1*x1[3]+az2*x2[3]+az3*x3[3]+az4*x4[3]);
723
checkt=std::fabs(-at0*x0[0]-at1*x1[0]+at2*x2[0]+at3*x3[0]+at4*x4[0])
724
+std::fabs(-at0*x0[1]-at1*x1[1]+at2*x2[1]+at3*x3[1]+at4*x4[1])
725
+std::fabs(-at0*x0[2]-at1*x1[2]+at2*x2[2]+at3*x3[2]+at4*x4[2])
726
+std::fabs(-at0*x0[3]-at1*x1[3]+at2*x2[3]+at3*x3[3]+at4*x4[3]);
728
if(checkx<=checky && checkx<=checkz && checkx<=checkt){
733
}else if(checky<=checkz && checky<=checkt){
738
}else if(checkz<=checkt){
752
//==============================================================================
754
simplex_intersection4d(int k,
768
assert(1<=k && k<=5);
771
case 1: // point vs. pentachoron
772
*alpha1=-orientation4d(x0, x2, x3, x4, x5);
773
*alpha2= orientation4d(x0, x1, x3, x4, x5);
774
if(!same_sign(*alpha1, *alpha2)) return 0;
775
*alpha3=-orientation4d(x0, x1, x2, x4, x5);
776
if(!same_sign(*alpha1, *alpha3)) return 0;
777
if(!same_sign(*alpha2, *alpha3)) return 0;
778
*alpha4= orientation4d(x0, x1, x2, x3, x5);
779
if(!same_sign(*alpha1, *alpha4)) return 0;
780
if(!same_sign(*alpha2, *alpha4)) return 0;
781
if(!same_sign(*alpha3, *alpha4)) return 0;
782
*alpha5=-orientation4d(x0, x1, x2, x3, x4);
783
if(!same_sign(*alpha1, *alpha5)) return 0;
784
if(!same_sign(*alpha2, *alpha5)) return 0;
785
if(!same_sign(*alpha3, *alpha5)) return 0;
786
if(!same_sign(*alpha4, *alpha5)) return 0;
789
sum2=*alpha1+*alpha2+*alpha3+*alpha4+*alpha5;
799
if(simplex_intersection4d(1, x0, x2, x3, x4, x5,
800
alpha0, alpha2, alpha3, alpha4, alpha5)){
804
if(simplex_intersection4d(1, x0, x1, x3, x4, x5,
805
alpha0, alpha1, alpha3, alpha4, alpha5)){
809
if(simplex_intersection4d(1, x0, x1, x2, x4, x5,
810
alpha0, alpha1, alpha2, alpha4, alpha5)){
814
if(simplex_intersection4d(1, x0, x1, x2, x3, x5,
815
alpha0, alpha1, alpha2, alpha3, alpha5)){
819
if(simplex_intersection4d(1, x0, x1, x2, x3, x4,
820
alpha0, alpha1, alpha2, alpha3, alpha4)){
827
case 2: // segment vs. tetrahedron
828
*alpha0= orientation4d(x1, x2, x3, x4, x5);
829
*alpha1=-orientation4d(x0, x2, x3, x4, x5);
830
if(!same_sign(*alpha0, *alpha1)) return 0;
831
*alpha2= orientation4d(x0, x1, x3, x4, x5);
832
*alpha3=-orientation4d(x0, x1, x2, x4, x5);
833
if(!same_sign(*alpha2, *alpha3)) return 0;
834
*alpha4= orientation4d(x0, x1, x2, x3, x5);
835
if(!same_sign(*alpha2, *alpha4)) return 0;
836
if(!same_sign(*alpha3, *alpha4)) return 0;
837
*alpha5=-orientation4d(x0, x1, x2, x3, x4);
838
if(!same_sign(*alpha2, *alpha5)) return 0;
839
if(!same_sign(*alpha3, *alpha5)) return 0;
840
if(!same_sign(*alpha4, *alpha5)) return 0;
842
sum1=*alpha0+*alpha1;
843
sum2=*alpha2+*alpha3+*alpha4+*alpha5;
853
if(simplex_intersection4d(1, x1, x2, x3, x4, x5,
854
alpha1, alpha2, alpha3, alpha4, alpha5)){
858
if(simplex_intersection4d(1, x0, x2, x3, x4, x5,
859
alpha0, alpha2, alpha3, alpha4, alpha5)){
863
if(simplex_intersection4d(2, x0, x1, x3, x4, x5,
864
alpha0, alpha1, alpha3, alpha4, alpha5)){
868
if(simplex_intersection4d(2, x0, x1, x2, x4, x5,
869
alpha0, alpha1, alpha2, alpha4, alpha5)){
873
if(simplex_intersection4d(2, x0, x1, x2, x3, x5,
874
alpha0, alpha1, alpha2, alpha3, alpha5)){
878
if(simplex_intersection4d(2, x0, x1, x2, x3, x4,
879
alpha0, alpha1, alpha2, alpha3, alpha4)){
886
case 3: // triangle vs. triangle
887
*alpha0= orientation4d(x1, x2, x3, x4, x5);
888
*alpha1=-orientation4d(x0, x2, x3, x4, x5);
889
if(!same_sign(*alpha0, *alpha1)) return 0;
890
*alpha2= orientation4d(x0, x1, x3, x4, x5);
891
if(!same_sign(*alpha0, *alpha2)) return 0;
892
if(!same_sign(*alpha1, *alpha2)) return 0;
893
*alpha3=-orientation4d(x0, x1, x2, x4, x5);
894
*alpha4= orientation4d(x0, x1, x2, x3, x5);
895
if(!same_sign(*alpha3, *alpha4)) return 0;
896
*alpha5=-orientation4d(x0, x1, x2, x3, x4);
897
if(!same_sign(*alpha3, *alpha5)) return 0;
898
if(!same_sign(*alpha4, *alpha5)) return 0;
900
sum1=*alpha0+*alpha1+*alpha2;
901
sum2=*alpha3+*alpha4+*alpha5;
911
if(simplex_intersection4d(2, x1, x2, x3, x4, x5,
912
alpha1, alpha2, alpha3, alpha4, alpha5)){
916
if(simplex_intersection4d(2, x0, x2, x3, x4, x5,
917
alpha0, alpha2, alpha3, alpha4, alpha5)){
921
if(simplex_intersection4d(2, x0, x1, x3, x4, x5,
922
alpha0, alpha1, alpha3, alpha4, alpha5)){
926
if(simplex_intersection4d(2, x4, x5, x0, x1, x2,
927
alpha4, alpha5, alpha0, alpha1, alpha2)){
931
if(simplex_intersection4d(2, x3, x5, x0, x1, x2,
932
alpha3, alpha5, alpha0, alpha1, alpha2)){
936
if(simplex_intersection4d(2, x3, x4, x0, x1, x2,
937
alpha3, alpha4, alpha0, alpha1, alpha2)){
944
case 4: // tetrahedron vs. segment
945
case 5: // pentachoron vs. point
946
return simplex_intersection4d(6-k, x5, x4, x3, x2, x1, x0,
947
alpha5, alpha4, alpha3, alpha2, alpha1, alpha0);
949
return -1; // should never get here
added
removed
extern/eltopo/common/tunicate/intersection.cpp
