//==============================================================================
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);
}
