//============================================================================== int sos_simplex_intersection3d(int k, int pri0, const double* x0, int pri1, const double* x1, int pri2, const double* x2, int pri3, const double* x3, int pri4, const double* x4, double* alpha0, double* alpha1, double* alpha2, double* alpha3, double* alpha4) { assert(1<=k && k<=4); double sum1, sum2; switch(k){ case 1: // point vs. tetrahedron *alpha1=-sos_orientation3d(pri0, x0, pri2, x2, pri3, x3, pri4, x4); *alpha2= sos_orientation3d(pri0, x0, pri1, x1, pri3, x3, pri4, x4); if(!same_sign(*alpha1, *alpha2)) return 0; *alpha3=-sos_orientation3d(pri0, x0, pri1, x1, pri2, x2, pri4, x4); if(!same_sign(*alpha1, *alpha3)) return 0; *alpha4= sos_orientation3d(pri0, x0, pri1, x1, pri2, x2, pri3, x3); if(!same_sign(*alpha1, *alpha4)) return 0; *alpha0=1; sum2=*alpha1+*alpha2+*alpha3+*alpha4; *alpha1/=sum2; *alpha2/=sum2; *alpha3/=sum2; *alpha4/=sum2; return 1; case 2: // segment vs. triangle *alpha0= sos_orientation3d(pri1, x1, pri2, x2, pri3, x3, pri4, x4); *alpha1=-sos_orientation3d(pri0, x0, pri2, x2, pri3, x3, pri4, x4); if(!same_sign(*alpha0, *alpha1)) return 0; *alpha2= sos_orientation3d(pri0, x0, pri1, x1, pri3, x3, pri4, x4); *alpha3=-sos_orientation3d(pri0, x0, pri1, x1, pri2, x2, pri4, x4); if(!same_sign(*alpha2, *alpha3)) return 0; *alpha4= sos_orientation3d(pri0, x0, pri1, x1, pri2, x2, pri3, x3); if(!same_sign(*alpha2, *alpha4)) return 0; sum1=*alpha0+*alpha1; *alpha0/=sum1; *alpha1/=sum1; sum2=*alpha2+*alpha3+*alpha4; *alpha2/=sum2; *alpha3/=sum2; *alpha4/=sum2; return 1; case 3: // triangle vs. segment case 4: // tetrahedron vs. point return sos_simplex_intersection3d(5-k, pri4, x4, pri3, x3, pri2, x2, pri1, x1, pri0, x0, alpha4, alpha3, alpha2, alpha1, alpha0); default: return -1; // should never get here } }
//============================================================================== double sos_orientation4d(int priority0, const double* x0, int priority1, const double* x1, int priority2, const double* x2, int priority3, const double* x3, int priority4, const double* x4) { assert(priority0!=priority1 && priority0!=priority2 && priority0!=priority3); assert(priority0!=priority4 && priority1!=priority2 && priority1!=priority3); assert(priority1!=priority4 && priority2!=priority3 && priority2!=priority4); assert(priority3!=priority4); double d=orientation4d(x0, x1, x2, x3, x4); if(d) return d; // If we have an exact zero, use SoS to decide the sign. // Sort by priority first, keeping track of sign of permutation. // (we could do a better job with this, but this is easy to code...) double sign=1; sort_points(priority0, x0, priority1, x1, sign); sort_points(priority2, x2, priority3, x3, sign); sort_points(priority0, x0, priority2, x2, sign); sort_points(priority1, x1, priority3, x3, sign); sort_points(priority1, x1, priority2, x2, sign); sort_points(priority0, x0, priority4, x4, sign); sort_points(priority1, x1, priority4, x4, sign); sort_points(priority2, x2, priority4, x4, sign); sort_points(priority3, x3, priority4, x4, sign); // Evaluate SoS terms one by one, looking for the first nonzero. // (We skip a few that must be zero if the preceding are zero, and stop // at the first term which must always be nonzero.) // row 0 d= orientation3d(x1+1, x2+1, x3+1, x4+1); if(d<0) return sign*dbl_min; else if(d>0) return -sign*dbl_min; d=-orientation3d(x0+1, x2+1, x3+1, x4+1); if(d<0) return sign*dbl_min; else if(d>0) return -sign*dbl_min; d= orientation3d(x0+1, x1+1, x3+1, x4+1); if(d<0) return sign*dbl_min; else if(d>0) return -sign*dbl_min; d=-orientation3d(x0+1, x1+1, x2+1, x4+1); if(d<0) return sign*dbl_min; else if(d>0) return -sign*dbl_min; d= orientation3d(x0+1, x1+1, x2+1, x3+1); if(d<0) return sign*dbl_min; else if(d>0) return -sign*dbl_min; // row 1 double c0[3]={x0[0], x0[2], x0[3]}, c1[3]={x1[0], x1[2], x1[3]}, c2[3]={x2[0], x2[2], x2[3]}, c3[3]={x3[0], x3[2], x3[3]}, c4[3]={x4[0], x4[2], x4[3]}; d=-orientation3d(c1, c2, c3, c4); if(d<0) return sign*dbl_min; else if(d>0) return -sign*dbl_min; d=-orientation2d(c2+1, c3+1, c4+1); if(d<0) return sign*dbl_min; else if(d>0) return -sign*dbl_min; d= orientation2d(c1+1, c3+1, c4+1); if(d<0) return sign*dbl_min; else if(d>0) return -sign*dbl_min; d=-orientation2d(c1+1, c2+1, c4+1); if(d<0) return sign*dbl_min; else if(d>0) return -sign*dbl_min; d= orientation3d(c0, c2, c3, c4); if(d<0) return sign*dbl_min; else if(d>0) return -sign*dbl_min; d=-orientation2d(c0+1, c3+1, c4+1); if(d<0) return sign*dbl_min; else if(d>0) return -sign*dbl_min; d= orientation2d(c0+1, c2+1, c4+1); if(d<0) return sign*dbl_min; else if(d>0) return -sign*dbl_min; d=-orientation3d(c0, c1, c3, c4); if(d<0) return sign*dbl_min; else if(d>0) return -sign*dbl_min; d=-orientation2d(c0+1, c1+1, c4+1); if(d<0) return sign*dbl_min; else if(d>0) return -sign*dbl_min; d= orientation3d(c0, c1, c2, c4); if(d<0) return sign*dbl_min; else if(d>0) return -sign*dbl_min; d=-orientation3d(c0, c1, c2, c3); if(d<0) return sign*dbl_min; else if(d>0) return -sign*dbl_min; // row 2 c0[1]=x0[1]; c1[1]=x1[1]; c2[1]=x2[1]; c3[1]=x3[1]; c4[1]=x4[1]; d= orientation3d(c1, c2, c3, c4); if(d<0) return sign*dbl_min; else if(d>0) return -sign*dbl_min; d= orientation2d(c2+1, c3+1, c4+1); if(d<0) return sign*dbl_min; else if(d>0) return -sign*dbl_min; d=-orientation2d(c1+1, c3+1, c4+1); if(d<0) return sign*dbl_min; else if(d>0) return -sign*dbl_min; d= orientation2d(c1+1, c2+1, c4+1); if(d<0) return sign*dbl_min; else if(d>0) return -sign*dbl_min; double b0[2]={x0[0], x0[3]}, b1[2]={x1[0], x1[3]}, b2[2]={x2[0], x2[3]}, b3[2]={x3[0], x3[3]}, b4[2]={x4[0], x4[3]}; d=-orientation2d(b2, b3, b4); if(d<0) return sign*dbl_min; else if(d>0) return -sign*dbl_min; d=-orientation1d(b3+1, b4+1); if(d<0) return sign*dbl_min; else if(d>0) return -sign*dbl_min; d= orientation1d(b2+1, b4+1); if(d<0) return sign*dbl_min; else if(d>0) return -sign*dbl_min; d= orientation2d(b1, b3, b4); if(d<0) return sign*dbl_min; else if(d>0) return -sign*dbl_min; d=-orientation1d(b1+1, b4+1); if(d<0) return sign*dbl_min; else if(d>0) return -sign*dbl_min; d=-orientation2d(b1, b2, b4); if(d<0) return sign*dbl_min; else if(d>0) return -sign*dbl_min; d=-orientation3d(c0, c2, c3, c4); if(d<0) return sign*dbl_min; else if(d>0) return -sign*dbl_min; d= orientation2d(c0+1, c3+1, c4+1); if(d<0) return sign*dbl_min; else if(d>0) return -sign*dbl_min; d=-orientation2d(c0+1, c2+1, c4+1); if(d<0) return sign*dbl_min; else if(d>0) return -sign*dbl_min; d=-orientation2d(b0, b3, b4); if(d<0) return sign*dbl_min; else if(d>0) return -sign*dbl_min; d= orientation1d(b0+1, b4+1); if(d<0) return sign*dbl_min; else if(d>0) return -sign*dbl_min; d= orientation3d(c0, c1, c3, c4); if(d<0) return sign*dbl_min; else if(d>0) return -sign*dbl_min; d= orientation2d(c0+1, c1+1, c4+1); if(d<0) return sign*dbl_min; else if(d>0) return -sign*dbl_min; d=-orientation2d(b0, b1, b4); if(d<0) return sign*dbl_min; else if(d>0) return -sign*dbl_min; d=-orientation3d(c0, c1, c2, c4); if(d<0) return sign*dbl_min; else if(d>0) return -sign*dbl_min; d= orientation3d(c0, c1, c2, c3); if(d<0) return sign*dbl_min; else if(d>0) return -sign*dbl_min; // row 3 return sign*sos_orientation3d(priority1, x1, priority2, x2, priority3, x3, priority4, x4); }