36 |
|
convex_angle(v0,v1,v2) |
37 |
|
FVECT v0,v1,v2; |
38 |
|
{ |
39 |
< |
FVECT cp,cp01,cp12,v10,v02; |
40 |
< |
double dp; |
39 |
> |
double dp,dp1; |
40 |
> |
int test,test1; |
41 |
> |
FVECT nv0,nv1,nv2; |
42 |
> |
FVECT cp01,cp12,cp; |
43 |
|
|
44 |
|
/* test sign of (v0Xv1)X(v1Xv2). v1 */ |
45 |
< |
VCROSS(cp01,v0,v1); |
46 |
< |
VCROSS(cp12,v1,v2); |
45 |
> |
/* un-Simplified: */ |
46 |
> |
VCOPY(nv0,v0); |
47 |
> |
normalize(nv0); |
48 |
> |
VCOPY(nv1,v1); |
49 |
> |
normalize(nv1); |
50 |
> |
VCOPY(nv2,v2); |
51 |
> |
normalize(nv2); |
52 |
> |
|
53 |
> |
VCROSS(cp01,nv0,nv1); |
54 |
> |
VCROSS(cp12,nv1,nv2); |
55 |
|
VCROSS(cp,cp01,cp12); |
56 |
< |
|
57 |
< |
dp = DOT(cp,v1); |
58 |
< |
#if 0 |
59 |
< |
VCOPY(Norm[Ncnt++],cp01); |
60 |
< |
VCOPY(Norm[Ncnt++],cp12); |
61 |
< |
VCOPY(Norm[Ncnt++],cp); |
62 |
< |
#endif |
63 |
< |
if(ZERO(dp) || dp < 0.0) |
64 |
< |
return(FALSE); |
65 |
< |
return(TRUE); |
56 |
> |
normalize(cp); |
57 |
> |
dp1 = DOT(cp,nv1); |
58 |
> |
if(dp1 <= 1e-8 || dp1 >= (1-1e-8)) /* Test if on other side,or colinear*/ |
59 |
> |
test1 = FALSE; |
60 |
> |
else |
61 |
> |
test1 = TRUE; |
62 |
> |
|
63 |
> |
dp = nv0[2]*nv1[0]*nv2[1] - nv0[2]*nv1[1]*nv2[0] - nv0[0]*nv1[2]*nv2[1] |
64 |
> |
+ nv0[0]*nv1[1]*nv2[2] + nv0[1]*nv1[2]*nv2[0] - nv0[1]*nv1[0]*nv2[2]; |
65 |
> |
|
66 |
> |
if(dp <= 1e-8 || dp1 >= (1-1e-8)) |
67 |
> |
test = FALSE; |
68 |
> |
else |
69 |
> |
test = TRUE; |
70 |
> |
|
71 |
> |
if(test != test1) |
72 |
> |
fprintf(stderr,"test %f simplified %f\n",dp1,dp); |
73 |
> |
return(test1); |
74 |
|
} |
75 |
|
|
76 |
|
/* calculates the normal of a face contour using Newell's formula. e |
108 |
|
|
109 |
|
|
110 |
|
tri_plane_equation(v0,v1,v2,peqptr,norm) |
111 |
< |
FVECT v0,v1,v2; |
111 |
> |
FVECT v0,v1,v2; |
112 |
|
FPEQ *peqptr; |
113 |
|
int norm; |
114 |
|
{ |
117 |
|
FP_D(*peqptr) = -(DOT(FP_N(*peqptr),v0)); |
118 |
|
} |
119 |
|
|
102 |
– |
/* From quad_edge-code */ |
103 |
– |
int |
104 |
– |
point_in_circle_thru_origin(p,p0,p1) |
105 |
– |
FVECT p; |
106 |
– |
FVECT p0,p1; |
107 |
– |
{ |
120 |
|
|
109 |
– |
double dp0,dp1; |
110 |
– |
double dp,det; |
111 |
– |
|
112 |
– |
dp0 = DOT_VEC2(p0,p0); |
113 |
– |
dp1 = DOT_VEC2(p1,p1); |
114 |
– |
dp = DOT_VEC2(p,p); |
115 |
– |
|
116 |
– |
det = -dp0*CROSS_VEC2(p1,p) + dp1*CROSS_VEC2(p0,p) - dp*CROSS_VEC2(p0,p1); |
117 |
– |
|
118 |
– |
return (det > (1e-10)); |
119 |
– |
} |
120 |
– |
|
121 |
– |
|
122 |
– |
double |
123 |
– |
point_on_sphere(ps,p,c) |
124 |
– |
FVECT ps,p,c; |
125 |
– |
{ |
126 |
– |
double d; |
127 |
– |
VSUB(ps,p,c); |
128 |
– |
d= normalize(ps); |
129 |
– |
return(d); |
130 |
– |
} |
131 |
– |
|
132 |
– |
|
121 |
|
/* returns TRUE if ray from origin in direction v intersects plane defined |
122 |
|
* by normal plane_n, and plane_d. If plane is not parallel- returns |
123 |
|
* intersection point if r != NULL. If tptr!= NULL returns value of |
289 |
|
return(hit); |
290 |
|
} |
291 |
|
|
304 |
– |
|
292 |
|
int |
306 |
– |
point_in_cone(p,p0,p1,p2) |
307 |
– |
FVECT p; |
308 |
– |
FVECT p0,p1,p2; |
309 |
– |
{ |
310 |
– |
FVECT np,x_axis,y_axis; |
311 |
– |
double d1,d2; |
312 |
– |
FPEQ peq; |
313 |
– |
|
314 |
– |
/* Find the equation of the circle defined by the intersection |
315 |
– |
of the cone with the plane defined by p1,p2,p3- project p into |
316 |
– |
that plane and do an in-circle test in the plane |
317 |
– |
*/ |
318 |
– |
|
319 |
– |
/* find the equation of the plane defined by p0-p2 */ |
320 |
– |
tri_plane_equation(p0,p1,p2,&peq,FALSE); |
321 |
– |
|
322 |
– |
/* define a coordinate system on the plane: the x axis is in |
323 |
– |
the direction of np2-np1, and the y axis is calculated from |
324 |
– |
n cross x-axis |
325 |
– |
*/ |
326 |
– |
/* Project p onto the plane */ |
327 |
– |
/* NOTE: check this: does sideness check?*/ |
328 |
– |
if(!intersect_vector_plane(p,peq,NULL,np)) |
329 |
– |
return(FALSE); |
330 |
– |
|
331 |
– |
/* create coordinate system on plane: p1-p0 defines the x_axis*/ |
332 |
– |
VSUB(x_axis,p1,p0); |
333 |
– |
normalize(x_axis); |
334 |
– |
/* The y axis is */ |
335 |
– |
VCROSS(y_axis,FP_N(peq),x_axis); |
336 |
– |
normalize(y_axis); |
337 |
– |
|
338 |
– |
VSUB(p1,p1,p0); |
339 |
– |
VSUB(p2,p2,p0); |
340 |
– |
VSUB(np,np,p0); |
341 |
– |
|
342 |
– |
p1[0] = DOT(p1,x_axis); |
343 |
– |
p1[1] = 0; |
344 |
– |
|
345 |
– |
d1 = DOT(p2,x_axis); |
346 |
– |
d2 = DOT(p2,y_axis); |
347 |
– |
p2[0] = d1; |
348 |
– |
p2[1] = d2; |
349 |
– |
|
350 |
– |
d1 = DOT(np,x_axis); |
351 |
– |
d2 = DOT(np,y_axis); |
352 |
– |
np[0] = d1; |
353 |
– |
np[1] = d2; |
354 |
– |
|
355 |
– |
/* perform the in-circle test in the new coordinate system */ |
356 |
– |
return(point_in_circle_thru_origin(np,p1,p2)); |
357 |
– |
} |
358 |
– |
|
359 |
– |
int |
293 |
|
point_set_in_stri(v0,v1,v2,p,n,nset,sides) |
294 |
|
FVECT v0,v1,v2,p; |
295 |
|
FVECT n[3]; |
353 |
|
|
354 |
|
|
355 |
|
|
423 |
– |
|
424 |
– |
int |
425 |
– |
point_in_stri(v0,v1,v2,p) |
426 |
– |
FVECT v0,v1,v2,p; |
427 |
– |
{ |
428 |
– |
double d; |
429 |
– |
FVECT n; |
430 |
– |
|
431 |
– |
VCROSS(n,v0,v1); |
432 |
– |
/* Test the point for sidedness */ |
433 |
– |
d = DOT(n,p); |
434 |
– |
if(d > 0.0) |
435 |
– |
return(FALSE); |
436 |
– |
|
437 |
– |
/* Test next edge */ |
438 |
– |
VCROSS(n,v1,v2); |
439 |
– |
/* Test the point for sidedness */ |
440 |
– |
d = DOT(n,p); |
441 |
– |
if(d > 0.0) |
442 |
– |
return(FALSE); |
443 |
– |
|
444 |
– |
/* Test next edge */ |
445 |
– |
VCROSS(n,v2,v0); |
446 |
– |
/* Test the point for sidedness */ |
447 |
– |
d = DOT(n,p); |
448 |
– |
if(d > 0.0) |
449 |
– |
return(FALSE); |
450 |
– |
/* Must be interior to the pyramid */ |
451 |
– |
return(GT_INTERIOR); |
452 |
– |
} |
453 |
– |
|
454 |
– |
int |
455 |
– |
vertices_in_stri(t0,t1,t2,p0,p1,p2,nset,n,avg,pt_sides) |
456 |
– |
FVECT t0,t1,t2,p0,p1,p2; |
457 |
– |
int *nset; |
458 |
– |
FVECT n[3]; |
459 |
– |
FVECT avg; |
460 |
– |
int pt_sides[3][3]; |
461 |
– |
|
462 |
– |
{ |
463 |
– |
int below_plane[3],test; |
464 |
– |
|
465 |
– |
SUM_3VEC3(avg,t0,t1,t2); |
466 |
– |
*nset = 0; |
467 |
– |
/* Test vertex v[i] against triangle j*/ |
468 |
– |
/* Check if v[i] lies below plane defined by avg of 3 vectors |
469 |
– |
defining triangle |
470 |
– |
*/ |
471 |
– |
|
472 |
– |
/* test point 0 */ |
473 |
– |
if(DOT(avg,p0) < 0.0) |
474 |
– |
below_plane[0] = 1; |
475 |
– |
else |
476 |
– |
below_plane[0] = 0; |
477 |
– |
/* Test if b[i] lies in or on triangle a */ |
478 |
– |
test = point_set_in_stri(t0,t1,t2,p0,n,nset,pt_sides[0]); |
479 |
– |
/* If pts[i] is interior: done */ |
480 |
– |
if(!below_plane[0]) |
481 |
– |
{ |
482 |
– |
if(test == GT_INTERIOR) |
483 |
– |
return(TRUE); |
484 |
– |
} |
485 |
– |
/* Now test point 1*/ |
486 |
– |
|
487 |
– |
if(DOT(avg,p1) < 0.0) |
488 |
– |
below_plane[1] = 1; |
489 |
– |
else |
490 |
– |
below_plane[1]=0; |
491 |
– |
/* Test if b[i] lies in or on triangle a */ |
492 |
– |
test = point_set_in_stri(t0,t1,t2,p1,n,nset,pt_sides[1]); |
493 |
– |
/* If pts[i] is interior: done */ |
494 |
– |
if(!below_plane[1]) |
495 |
– |
{ |
496 |
– |
if(test == GT_INTERIOR) |
497 |
– |
return(TRUE); |
498 |
– |
} |
499 |
– |
|
500 |
– |
/* Now test point 2 */ |
501 |
– |
if(DOT(avg,p2) < 0.0) |
502 |
– |
below_plane[2] = 1; |
503 |
– |
else |
504 |
– |
below_plane[2] = 0; |
505 |
– |
/* Test if b[i] lies in or on triangle a */ |
506 |
– |
test = point_set_in_stri(t0,t1,t2,p2,n,nset,pt_sides[2]); |
507 |
– |
|
508 |
– |
/* If pts[i] is interior: done */ |
509 |
– |
if(!below_plane[2]) |
510 |
– |
{ |
511 |
– |
if(test == GT_INTERIOR) |
512 |
– |
return(TRUE); |
513 |
– |
} |
514 |
– |
|
515 |
– |
/* If all three points below separating plane: trivial reject */ |
516 |
– |
if(below_plane[0] && below_plane[1] && below_plane[2]) |
517 |
– |
return(FALSE); |
518 |
– |
/* Now check vertices in a against triangle b */ |
519 |
– |
return(FALSE); |
520 |
– |
} |
521 |
– |
|
522 |
– |
|
356 |
|
set_sidedness_tests(t0,t1,t2,p0,p1,p2,test,sides,nset,n) |
357 |
|
FVECT t0,t1,t2,p0,p1,p2; |
358 |
|
int test[3]; |
492 |
|
SET_NTH_BIT(test[2],2); |
493 |
|
} |
494 |
|
|
495 |
+ |
double |
496 |
+ |
point_on_sphere(ps,p,c) |
497 |
+ |
FVECT ps,p,c; |
498 |
+ |
{ |
499 |
+ |
double d; |
500 |
+ |
VSUB(ps,p,c); |
501 |
+ |
d= normalize(ps); |
502 |
+ |
return(d); |
503 |
+ |
} |
504 |
|
|
505 |
|
int |
506 |
< |
stri_intersect(a1,a2,a3,b1,b2,b3) |
507 |
< |
FVECT a1,a2,a3,b1,b2,b3; |
506 |
> |
point_in_stri(v0,v1,v2,p) |
507 |
> |
FVECT v0,v1,v2,p; |
508 |
|
{ |
509 |
< |
int which,test,n_set[2]; |
510 |
< |
int sides[2][3][3],i,j,inext,jnext; |
669 |
< |
int tests[2][3]; |
670 |
< |
FVECT n[2][3],p,avg[2],t1,t2,t3; |
671 |
< |
|
672 |
< |
#ifdef DEBUG |
673 |
< |
tri_normal(b1,b2,b3,p,FALSE); |
674 |
< |
if(DOT(p,b1) > 0) |
675 |
< |
{ |
676 |
< |
VCOPY(t3,b1); |
677 |
< |
VCOPY(t2,b2); |
678 |
< |
VCOPY(t1,b3); |
679 |
< |
} |
680 |
< |
else |
681 |
< |
#endif |
682 |
< |
{ |
683 |
< |
VCOPY(t1,b1); |
684 |
< |
VCOPY(t2,b2); |
685 |
< |
VCOPY(t3,b3); |
686 |
< |
} |
509 |
> |
double d; |
510 |
> |
FVECT n; |
511 |
|
|
512 |
< |
/* Test the vertices of triangle a against the pyramid formed by triangle |
513 |
< |
b and the origin. If any vertex of a is interior to triangle b, or |
514 |
< |
if all 3 vertices of a are ON the edges of b,return TRUE. Remember |
515 |
< |
the results of the edge normal and sidedness tests for later. |
516 |
< |
*/ |
693 |
< |
if(vertices_in_stri(a1,a2,a3,t1,t2,t3,&(n_set[0]),n[0],avg[0],sides[1])) |
694 |
< |
return(TRUE); |
695 |
< |
|
696 |
< |
if(vertices_in_stri(t1,t2,t3,a1,a2,a3,&(n_set[1]),n[1],avg[1],sides[0])) |
697 |
< |
return(TRUE); |
512 |
> |
VCROSS(n,v0,v1); |
513 |
> |
/* Test the point for sidedness */ |
514 |
> |
d = DOT(n,p); |
515 |
> |
if(d > 0.0) |
516 |
> |
return(FALSE); |
517 |
|
|
518 |
+ |
/* Test next edge */ |
519 |
+ |
VCROSS(n,v1,v2); |
520 |
+ |
/* Test the point for sidedness */ |
521 |
+ |
d = DOT(n,p); |
522 |
+ |
if(d > 0.0) |
523 |
+ |
return(FALSE); |
524 |
|
|
525 |
< |
set_sidedness_tests(t1,t2,t3,a1,a2,a3,tests[0],sides[0],n_set[1],n[1]); |
526 |
< |
if(tests[0][0]&tests[0][1]&tests[0][2]) |
527 |
< |
return(FALSE); |
528 |
< |
|
529 |
< |
set_sidedness_tests(a1,a2,a3,t1,t2,t3,tests[1],sides[1],n_set[0],n[0]); |
530 |
< |
if(tests[1][0]&tests[1][1]&tests[1][2]) |
531 |
< |
return(FALSE); |
532 |
< |
|
708 |
< |
for(j=0; j < 3;j++) |
709 |
< |
{ |
710 |
< |
jnext = (j+1)%3; |
711 |
< |
/* IF edge b doesnt cross any great circles of a, punt */ |
712 |
< |
if(tests[1][j] & tests[1][jnext]) |
713 |
< |
continue; |
714 |
< |
for(i=0;i<3;i++) |
715 |
< |
{ |
716 |
< |
inext = (i+1)%3; |
717 |
< |
/* IF edge a doesnt cross any great circles of b, punt */ |
718 |
< |
if(tests[0][i] & tests[0][inext]) |
719 |
< |
continue; |
720 |
< |
/* Now find the great circles that cross and test */ |
721 |
< |
if((NTH_BIT(tests[0][i],j)^(NTH_BIT(tests[0][inext],j))) |
722 |
< |
&& (NTH_BIT(tests[1][j],i)^NTH_BIT(tests[1][jnext],i))) |
723 |
< |
{ |
724 |
< |
VCROSS(p,n[0][i],n[1][j]); |
725 |
< |
|
726 |
< |
/* If zero cp= done */ |
727 |
< |
if(ZERO_VEC3(p)) |
728 |
< |
continue; |
729 |
< |
/* check above both planes */ |
730 |
< |
if(DOT(avg[0],p) < 0 || DOT(avg[1],p) < 0) |
731 |
< |
{ |
732 |
< |
NEGATE_VEC3(p); |
733 |
< |
if(DOT(avg[0],p) < 0 || DOT(avg[1],p) < 0) |
734 |
< |
continue; |
735 |
< |
} |
736 |
< |
return(TRUE); |
737 |
< |
} |
738 |
< |
} |
739 |
< |
} |
740 |
< |
return(FALSE); |
525 |
> |
/* Test next edge */ |
526 |
> |
VCROSS(n,v2,v0); |
527 |
> |
/* Test the point for sidedness */ |
528 |
> |
d = DOT(n,p); |
529 |
> |
if(d > 0.0) |
530 |
> |
return(FALSE); |
531 |
> |
/* Must be interior to the pyramid */ |
532 |
> |
return(GT_INTERIOR); |
533 |
|
} |
534 |
|
|
535 |
+ |
|
536 |
|
int |
537 |
|
ray_intersect_tri(orig,dir,v0,v1,v2,pt) |
538 |
|
FVECT orig,dir; |
651 |
|
} |
652 |
|
return(i); |
653 |
|
} |
861 |
– |
|
862 |
– |
|
863 |
– |
int |
864 |
– |
sedge_intersect(a0,a1,b0,b1) |
865 |
– |
FVECT a0,a1,b0,b1; |
866 |
– |
{ |
867 |
– |
FVECT na,nb,avga,avgb,p; |
868 |
– |
double d; |
869 |
– |
int sb0,sb1,sa0,sa1; |
870 |
– |
|
871 |
– |
/* First test if edge b straddles great circle of a */ |
872 |
– |
VCROSS(na,a0,a1); |
873 |
– |
d = DOT(na,b0); |
874 |
– |
sb0 = ZERO(d)?0:(d<0)? -1: 1; |
875 |
– |
d = DOT(na,b1); |
876 |
– |
sb1 = ZERO(d)?0:(d<0)? -1: 1; |
877 |
– |
/* edge b entirely on one side of great circle a: edges cannot intersect*/ |
878 |
– |
if(sb0*sb1 > 0) |
879 |
– |
return(FALSE); |
880 |
– |
/* test if edge a straddles great circle of b */ |
881 |
– |
VCROSS(nb,b0,b1); |
882 |
– |
d = DOT(nb,a0); |
883 |
– |
sa0 = ZERO(d)?0:(d<0)? -1: 1; |
884 |
– |
d = DOT(nb,a1); |
885 |
– |
sa1 = ZERO(d)?0:(d<0)? -1: 1; |
886 |
– |
/* edge a entirely on one side of great circle b: edges cannot intersect*/ |
887 |
– |
if(sa0*sa1 > 0) |
888 |
– |
return(FALSE); |
889 |
– |
|
890 |
– |
/* Find one of intersection points of the great circles */ |
891 |
– |
VCROSS(p,na,nb); |
892 |
– |
/* If they lie on same great circle: call an intersection */ |
893 |
– |
if(ZERO_VEC3(p)) |
894 |
– |
return(TRUE); |
895 |
– |
|
896 |
– |
VADD(avga,a0,a1); |
897 |
– |
VADD(avgb,b0,b1); |
898 |
– |
if(DOT(avga,p) < 0 || DOT(avgb,p) < 0) |
899 |
– |
{ |
900 |
– |
NEGATE_VEC3(p); |
901 |
– |
if(DOT(avga,p) < 0 || DOT(avgb,p) < 0) |
902 |
– |
return(FALSE); |
903 |
– |
} |
904 |
– |
if((!sb0 || !sb1) && (!sa0 || !sa1)) |
905 |
– |
return(FALSE); |
906 |
– |
return(TRUE); |
907 |
– |
} |
908 |
– |
|
654 |
|
|
655 |
|
/* Find the normalized barycentric coordinates of p relative to |
656 |
|
* triangle v0,v1,v2. Return result in coord |