/**************** C | Cu Cu | C **************/
static double c_cucu_c(double x , double y , double phi , double radius , double *t , double *u , double *v)
{
double a , b , u1 , theta , alpha , long_rs , va;
double toto;
a = x + radius*sin(phi);
b = y - radius*(cos(phi) + 1.0);
if (FEQ(a, 0.,EPS4)&&FEQ(b , 0.,EPS4)) return(infini);
u1 = sqrt(a*a + b*b);
if (u1 > 6.0 * radius) return(infini);
else{
theta = atan2(b , a);
va = 1.25 - (u1*u1) / (16.0 * radius * radius);
if ((va<0.)||(va>1.)) return(infini);
else{
*u = acos(va);
toto = sin(*u);
if (FEQ(toto ,0.0 ,EPS3)) toto = 0.;
alpha = asin(radius * toto * 2.0/u1);
*t = mod2pi(M_PI_2 + theta + alpha);
*v = mod2pi(*t - phi);
long_rs = radius*(2.0 *(*u) + *t + *v);
return(long_rs);
}
}
}
/**************** C C | C ****************/
static double cc_c(double x , double y , double phi , double radius , double *t , double *u , double *v)
{
double a , b , u1 , theta , alpha , long_rs , va;
static double toto;
a = x - radius*sin(phi);
b = y + radius*(cos(phi) -1.0);
if (FEQ(a, 0.,EPS4)&&FEQ(b , 0.,EPS4)) return(infini);
u1 = sqrt(a*a + b*b);
va = 4.0 * radius;
if (u1 > va) return(infini);
else{
theta = atan2(b , a);
*u = acos(1.0 - (2.0 * u1 * u1)/(va*va));
toto = sin(*u);
if (FEQ(toto ,0.0 ,EPS3)) toto = 0.0;
/*BMIC*/
if (FEQ(toto ,0.0 ,EPS3) &&FEQ(u1 ,0.0 ,EPS3)) return(infini);
alpha = asin((2.0 * radius *toto)/(u1));
/*EMIC*/
*t = mod2pi(M_PI_2 + theta - alpha);
*v = mod2pi(*t - *u - phi);
long_rs = radius*(*t + *u + *v);
return(long_rs);
}
}
/*** C Cu | Cu C ***/
static double ccu_cuc(double x , double y , double phi , double radius , double *t , double *u ,double* v)
{
double a , b , u1 , theta , alpha , long_rs , va;
a = x + radius*sin(phi);
b = y - radius*(cos(phi) + 1.0);
if (FEQ(a, 0.,EPS4)&&FEQ(b , 0.,EPS4)) return(infini);
u1 = sqrt(a*a + b*b);
va = 4.0 * radius;
if (u1 > va) return(infini);
else{
theta = atan2(b , a);
if (u1 > (2.0 *radius)) {
alpha = acos(u1/(4.0 * radius) - 0.5);
*t = mod2pi(M_PI_2 + theta - alpha);
*u = mod2pi(M_PI - alpha);
*v = mod2pi(phi - *t + 2.0 * (*u));
}
else{
alpha = acos(0.5 + u1/(4.0 * radius));
*t = mod2pi(M_PI_2 + theta + alpha);
*u = mod2pi(alpha);
*v = mod2pi(phi - *t + 2.0 * (*u));
}
long_rs = radius * (2.0 *(*u) + *t + *v);
return(long_rs);
}
}
int
dev_view( /* assign new driver view */
VIEW *nv
)
{
if (nv->type == VT_PAR || /* check view legality */
nv->horiz > 160. || nv->vert > 160.) {
error(COMMAND, "illegal view type/angle");
nv->type = odev.v.type;
nv->horiz = odev.v.horiz;
nv->vert = odev.v.vert;
return(0);
}
if (nv->vfore > FTINY) {
error(COMMAND, "cannot handle fore clipping");
nv->vfore = 0.;
return(0);
}
if (nv != &odev.v) {
if (!FEQ(nv->horiz,odev.v.horiz) || /* resize window? */
!FEQ(nv->vert,odev.v.vert)) {
int dw = DisplayWidth(ourdisplay,ourscreen);
int dh = DisplayHeight(ourdisplay,ourscreen);
dw -= 25; /* for window frame */
dh -= 50;
odev.hres = 2.*VIEWDIST/pwidth *
tan(PI/180./2.*nv->horiz);
odev.vres = 2.*VIEWDIST/pheight *
tan(PI/180./2.*nv->vert);
if (odev.hres > dw) {
odev.vres = dw * odev.vres / odev.hres;
odev.hres = dw;
}
if (odev.vres > dh) {
odev.hres = dh * odev.hres / odev.vres;
odev.vres = dh;
}
XResizeWindow(ourdisplay, gwind, odev.hres, odev.vres);
dev_input(); /* get resize event */
}
odev.v = *nv;
}
if (nxtzmax > FTINY) {
curzmax = nxtzmax;
nxtzmax = 0.;
}
glClear(GL_DEPTH_BUFFER_BIT);
qtReplant();
return(1);
}
bool MxQuadric3::optimize(Vec3& v, const Vec3& v1,
const Vec3& v2, const Vec3& v3) const
{
Vec3 d13 = v1 - v3;
Vec3 d23 = v2 - v3;
Mat3 A = tensor();
Vec3 B = vector();
Vec3 Ad13 = A*d13;
Vec3 Ad23 = A*d23;
Vec3 Av3 = A*v3;
double d13_d23 = (d13*Ad23) + (d23*Ad13);
double v3_d13 = (d13*Av3) + (v3*Ad13);
double v3_d23 = (d23*Av3) + (v3*Ad23);
double d23Ad23 = d23*Ad23;
double d13Ad13 = d13*Ad13;
double denom = d13Ad13*d23Ad23 - 2*d13_d23;
if( FEQ(denom, 0.0, 1e-12) )
return false;
double a = ( d23Ad23*(2*(B*d13) + v3_d13) -
d13_d23*(2*(B*d23) + v3_d23) ) / -denom;
double b = ( d13Ad13*(2*(B*d23) + v3_d23) -
d13_d23*(2*(B*d13) + v3_d13) ) / -denom;
if( a<0.0 ) a=0.0; else if( a>1.0 ) a=1.0;
if( b<0.0 ) b=0.0; else if( b>1.0 ) b=1.0;
v = a*d13 + b*d23 + v3;
return true;
}
bool Matrix::operator==(const Matrix &m)const{
// m1 == m2
if(this->m_unit != m.m_unit || this->m_mirrored != m.m_mirrored) return false;
for(int i = 0; i < 16; i++)
if(FEQ(this->e[i], m.e[i], TIGHT_TOLERANCE) == false) return false;
return true;
}
bool quadrix_find_best_fit(const Mat4& Q, Vec3& candidate)
{
Mat4 K = Q;
K(3,0) = K(3,1) = K(3,2) = 0.0; K(3,3) = 1;
Mat4 M;
real det = K.inverse(M);
if( FEQ(det, 0.0, 1e-12) )
return false;
#ifdef SAFETY
//
// The homogeneous division SHOULDN'T be necessary.
// But, when we're being SAFE, we do it anyway just in case.
//
candidate[X] = M(0,3)/M(3,3);
candidate[Y] = M(1,3)/M(3,3);
candidate[Z] = M(2,3)/M(3,3);
#else
candidate[X] = M(0,3);
candidate[Y] = M(1,3);
candidate[Z] = M(2,3);
#endif
return true;
}
/**** C | C C *****/
static double c_cc(double x , double y , double phi , double radius , double *t , double *u , double *v)
{
double a , b , u1 , theta , alpha , long_rs , va;
a = x - radius*sin(phi);
b = y + radius*(cos(phi) - 1.0);
if (FEQ(a, 0.,EPS4)&&FEQ(b , 0.,EPS4)) return(infini);
u1 = sqrt(a*a + b*b);
if (u1 > 4*radius) return(infini);
else{
theta = atan2(b , a);
alpha = acos(u1/(4.0 * radius));
va = M_PI_2 + alpha;
*t = mod2pi(va + theta);
*u = mod2pi(2.0 * (M_PI - va));
*v = mod2pi(*t + *u - phi);
long_rs = radius*(*t + *u + *v);
return(long_rs);
}
}
static KF1(jtcvt2bit){I c,i,m,q,r,r1,wr,*ws,*wv;UC k,*zv=(UC*)yv;
wv=AV(w); wr=AR(w); ws=AS(w);
c=wr?ws[wr-1]:1; m=c?AN(w)/c:0; q=c/BB; r=c%BB; r1=c%BW?(BW-c%BW)/BB:0;
switch(AT(w)){
default: R 0;
case B01: TOBIT(B, 1 ); break;
case INT: TOBIT(I, 1==x ); break;
case FL: TOBIT(D, FEQ(1.0,x)); break;
}
R 1;
}
bool MxQuadric3::optimize(Vec3& v) const
{
Mat3 Ainv;
double det = invert(Ainv, tensor());
if( FEQ(det, 0.0, 1e-12) )
return false;
v = -(Ainv*vector());
return true;
}
char *
viewopt( /* translate to minimal view string */
VIEW *vp
)
{
static char vwstr[128];
char *cp = vwstr;
*cp = '\0';
if (vp->type != stdview.type) {
sprintf(cp, " -vt%c", vp->type);
cp += strlen(cp);
}
if (!VEQ(vp->vp,stdview.vp)) {
sprintf(cp, " -vp %.6g %.6g %.6g",
vp->vp[0], vp->vp[1], vp->vp[2]);
cp += strlen(cp);
}
if (!FEQ(vp->vdist,stdview.vdist) || !VEQ(vp->vdir,stdview.vdir)) {
sprintf(cp, " -vd %.6g %.6g %.6g",
vp->vdir[0]*vp->vdist,
vp->vdir[1]*vp->vdist,
vp->vdir[2]*vp->vdist);
cp += strlen(cp);
}
if (!VEQ(vp->vup,stdview.vup)) {
sprintf(cp, " -vu %.6g %.6g %.6g",
vp->vup[0], vp->vup[1], vp->vup[2]);
cp += strlen(cp);
}
if (!FEQ(vp->horiz,stdview.horiz)) {
sprintf(cp, " -vh %.6g", vp->horiz);
cp += strlen(cp);
}
if (!FEQ(vp->vert,stdview.vert)) {
sprintf(cp, " -vv %.6g", vp->vert);
cp += strlen(cp);
}
if (!FEQ(vp->vfore,stdview.vfore)) {
sprintf(cp, " -vo %.6g", vp->vfore);
cp += strlen(cp);
}
if (!FEQ(vp->vaft,stdview.vaft)) {
sprintf(cp, " -va %.6g", vp->vaft);
cp += strlen(cp);
}
if (!FEQ(vp->hoff,stdview.hoff)) {
sprintf(cp, " -vs %.6g", vp->hoff);
cp += strlen(cp);
}
if (!FEQ(vp->voff,stdview.voff)) {
sprintf(cp, " -vl %.6g", vp->voff);
cp += strlen(cp);
}
return(vwstr);
}
int
addrot( /* compute rotation (x,y,z) => (xp,yp,zp) */
register char *xf,
FVECT xp,
FVECT yp,
FVECT zp
)
{
int n;
double theta;
if (yp[2]*yp[2] + zp[2]*zp[2] < 2.*FTINY*FTINY) {
/* Special case for X' along Z-axis */
theta = -atan2(yp[0], yp[1]);
sprintf(xf, " -ry %f -rz %f",
xp[2] < 0.0 ? 90.0 : -90.0,
theta*(180./PI));
return(4);
}
n = 0;
theta = atan2(yp[2], zp[2]);
if (!FEQ(theta,0.0)) {
sprintf(xf, " -rx %f", theta*(180./PI));
while (*xf) ++xf;
n += 2;
}
theta = asin(-xp[2]);
if (!FEQ(theta,0.0)) {
sprintf(xf, " -ry %f", theta*(180./PI));
while (*xf) ++xf;
n += 2;
}
theta = atan2(xp[1], xp[0]);
if (!FEQ(theta,0.0)) {
sprintf(xf, " -rz %f", theta*(180./PI));
/* while (*xf) ++xf; */
n += 2;
}
return(n);
}
bool MxQuadric::optimize(MxVector& v) const
{
MxMatrix Ainv(A.dim());
double det = A.invert(Ainv);
if( FEQ(det, 0.0, 1e-12) )
return false;
v = (Ainv * b);
mxv_neg(v, v.dim());
return true;
}
bool MxQuadric3::optimize(Vec3& v, const Vec3& v1, const Vec3& v2) const
{
Vec3 d = v1 - v2;
Mat3 A = tensor();
Vec3 Av2 = A*v2;
Vec3 Ad = A*d;
double denom = 2.0*d*Ad;
if( FEQ(denom, 0.0, 1e-12) )
return false;
double a = ( -2*(vector()*d) - (d*Av2) - (v2*Ad) ) / ( 2*(d*Ad) );
if( a<0.0 ) a=0.0; else if( a>1.0 ) a=1.0;
v = a*d + v2;
return true;
}
static double mod2pi(double a)
{
return((double)((a=fmod(a, M_2PI))>0.0)?a:(FEQ(a,0.0,EPSILON)?0.0:(a + M_2PI)));
}
int
getparam( /* get variable from user */
char *str,
char *dsc,
int typ,
void *p
)
{
MyUptr ptr = (MyUptr)p;
int i0;
double d0, d1, d2;
char buf[48];
switch (typ) {
case 'i': /* integer */
if (sscanf(str, "%d", &i0) != 1) {
(*dev->comout)(dsc);
sprintf(buf, " (%d): ", ptr->i);
(*dev->comout)(buf);
(*dev->comin)(buf, NULL);
if (sscanf(buf, "%d", &i0) != 1)
return(0);
}
if (ptr->i == i0)
return(0);
ptr->i = i0;
break;
case 'r': /* real */
if (sscanf(str, "%lf", &d0) != 1) {
(*dev->comout)(dsc);
sprintf(buf, " (%.6g): ", ptr->d);
(*dev->comout)(buf);
(*dev->comin)(buf, NULL);
if (sscanf(buf, "%lf", &d0) != 1)
return(0);
}
if (FEQ(ptr->d, d0))
return(0);
ptr->d = d0;
break;
case 'b': /* boolean */
if (sscanf(str, "%1s", buf) != 1) {
(*dev->comout)(dsc);
sprintf(buf, "? (%c): ", ptr->i ? 'y' : 'n');
(*dev->comout)(buf);
(*dev->comin)(buf, NULL);
if (buf[0] == '\0')
return(0);
}
if (strchr("yY+1tTnN-0fF", buf[0]) == NULL)
return(0);
i0 = strchr("yY+1tT", buf[0]) != NULL;
if (ptr->i == i0)
return(0);
ptr->i = i0;
break;
case 'C': /* color */
if (sscanf(str, "%lf %lf %lf", &d0, &d1, &d2) != 3) {
(*dev->comout)(dsc);
sprintf(buf, " (%.6g %.6g %.6g): ",
colval(ptr->C,RED),
colval(ptr->C,GRN),
colval(ptr->C,BLU));
(*dev->comout)(buf);
(*dev->comin)(buf, NULL);
if (sscanf(buf, "%lf %lf %lf", &d0, &d1, &d2) != 3)
return(0);
}
if (FEQ(colval(ptr->C,RED), d0) &&
FEQ(colval(ptr->C,GRN), d1) &&
FEQ(colval(ptr->C,BLU), d2))
return(0);
setcolor(ptr->C, d0, d1, d2);
break;
default:
return(0); /* shouldn't happen */
}
newparam++;
return(1);
}
double reed_shepp(Stconfig *c1 , Stconfig *c2, double radius , int* numero , double *t_r , double *u_r , double *v_r)
{
double x , y , phi;
double t , u , v , t1 , u1 , v1;
int num;
double var , var_d, theta , alpha , dx , dy , r, longueur;
/* Changement de repere,les courbes sont toujours calculees
de (0 0 0)--> (x , y , phi) */
dx = (double)(c2->x - c1->x);
dy = (double)(c2->y - c1->y);
var = (double)(c2->z -c1->z);
r = (double)radius;
theta = atan2(dy , dx);
alpha = theta - (double)(c1->z);
var_d = sqrt(dx*dx + dy*dy);
x = cos(alpha)*var_d;
y = sin(alpha)*var_d;
t1 = u1 = v1 = 0.0;
if (fabs(var) <= M_PI) phi = var;
else {
if (c2->z >= c1->z) phi = var - M_2PI;
else phi = mod2pi(var);}
if(FEQ(r,0.0,EPS4)) {
longueur = csca(x , y , phi , r , &t1 , &u1 , &v1); /* l+ s+ l+ */
var = t1+v1; num = 9; t = t1; u = u1; v = v1;
csca(x ,-y , -phi , r , &t1 , &u1 , &v1); /* r+ s+ r+ */
if((t1+v1) < (t+v)){num = 10;t = t1; u = u1; v = v1;}
csca(-x , y , -phi , r , &t1 , &u1 , &v1); /* l- s- l- */
if((t1+v1) < (t+v)){num = 11;t = t1; u = u1; v = v1;}
csca(-x ,-y , phi , r , &t1 , &u1 , &v1); /* r- s- r- */
if((t1+v1) < (t+v)){num = 12;t = t1; u = u1; v = v1;}
cscb(x , y , phi , r , &t1 , &u1 , &v1); /* l+ s+ r+ */
if((t1+v1) < (t+v)){num = 13;t = t1; u = u1; v = v1;}
cscb(x ,-y , -phi , r , &t1 , &u1 , &v1); /* r+ s+ l+ */
if((t1+v1) < (t+v)){num = 14;t = t1; u = u1; v = v1;}
cscb(-x , y , -phi , r , &t1 , &u1 , &v1); /* l- s- r- */
if((t1+v1) < (t+v)){num = 15;t = t1; u = u1; v = v1;}
cscb(-x ,-y , phi , r , &t1 , &u1 , &v1); /* r- s- l- */
if((t1+v1) < (t+v)){num = 16;t = t1; u = u1; v = v1;}
*t_r = (double)t; *u_r = (double)u; *v_r = (double)v;
*numero = num;
if(longueur == infini) PrintInfo(("infini0\n"));
return((double)longueur);
}
/**** C | C | C ***/
longueur = c_c_c(x , y , phi , r , &t1 , &u1 , &v1); /* l+ r- l+ */
num = 1; t = t1; u = u1; v = v1;
var = c_c_c(-x , y , -phi , r , &t1 , &u1 , &v1); /* l- r+ l- */
if (var < longueur){longueur = var; num = 2; t = t1; u = u1; v = v1;}
var = c_c_c(x ,-y , -phi , r , &t1 , &u1 , &v1); /* r+ l- r+ */
if (var < longueur){longueur = var; num = 3; t = t1; u = u1; v = v1;}
var = c_c_c(-x ,-y , phi , r , &t1 , &u1 , &v1); /* r- l+ r- */
if (var < longueur){longueur = var; num = 4; t = t1; u = u1; v = v1;}
/**** C | C C ***/
var = c_cc(x , y , phi , r , &t1 , &u1 , &v1); /* l+ r- l- */
if (var < longueur){longueur = var; num = 5; t = t1; u = u1; v = v1;}
var = c_cc(-x , y , -phi , r , &t1 , &u1 , &v1); /* l- r+ l+ */
if (var < longueur){longueur = var; num = 6; t = t1; u = u1; v = v1;}
var = c_cc(x ,-y , -phi , r , &t1 , &u1 , &v1); /* r+ l- r- */
if (var < longueur){longueur = var; num = 7; t = t1; u = u1; v = v1;}
var = c_cc(-x ,-y , phi , r , &t1 , &u1 , &v1); /* r- l+ r+ */
if (var < longueur){longueur = var; num = 8; t = t1; u = u1; v = v1;}
/**** C S C ****/
var = csca(x , y , phi , r , &t1 , &u1 , &v1); /* l+ s+ l+ */
if (var < longueur){longueur = var; num = 9; t = t1; u = u1; v = v1;}
var = csca(x ,-y , -phi , r , &t1 , &u1 , &v1); /* r+ s+ r+ */
if (var < longueur){longueur = var; num = 10;t = t1; u = u1; v = v1;}
var = csca(-x , y , -phi , r , &t1 , &u1 , &v1); /* l- s- l- */
if (var < longueur){longueur = var; num = 11;t = t1; u = u1; v = v1;}
var = csca(-x ,-y , phi , r , &t1 , &u1 , &v1); /* r- s- r- */
if (var < longueur){longueur = var; num = 12;t = t1; u = u1; v = v1;}
var = cscb(x , y , phi , r , &t1 , &u1 , &v1); /* l+ s+ r+ */
if (var < longueur){longueur = var; num = 13;t = t1; u = u1; v = v1;}
var = cscb(x ,-y , -phi , r , &t1 , &u1 , &v1); /* r+ s+ l+ */
if (var < longueur){longueur = var; num = 14;t = t1; u = u1; v = v1;}
var = cscb(-x , y , -phi , r , &t1 , &u1 , &v1); /* l- s- r- */
if (var < longueur){longueur = var; num = 15;t = t1; u = u1; v = v1;}
var = cscb(-x ,-y , phi , r , &t1 , &u1 , &v1); /* r- s- l- */
if (var < longueur){longueur = var; num = 16;t = t1; u = u1; v = v1;}
/*** C Cu | Cu C ***/
var = ccu_cuc(x , y , phi , r , &t1 , &u1 , &v1); /* l+ r+ l- r- */
if (var < longueur){longueur = var; num = 17;t = t1; u = u1; v = v1;}
var = ccu_cuc(x ,-y , -phi , r , &t1 , &u1 , &v1); /* r+ l+ r- l- */
if (var < longueur){longueur = var; num = 18;t = t1; u = u1; v = v1;}
var = ccu_cuc(-x , y , -phi , r , &t1 , &u1 , &v1); /* l- r- l+ r+ */
if (var < longueur){longueur = var; num = 19;t = t1; u = u1; v = v1;}
var = ccu_cuc(-x ,-y , phi , r , &t1 , &u1 , &v1); /* r- l- r+ l+ */
if (var < longueur){longueur = var; num = 20;t = t1; u = u1; v = v1;}
/*** C | Cu Cu | C ***/
var = c_cucu_c(x , y , phi , r , &t1 , &u1 , &v1); /* l+ r- l- r+ */
if (var < longueur){longueur = var;num = 21; t = t1; u = u1; v = v1;}
var = c_cucu_c(x ,-y , -phi , r , &t1 , &u1 , &v1); /* r+ l- r- l+ */
if (var < longueur){longueur = var; num = 22;t = t1; u = u1; v = v1;}
var = c_cucu_c(-x , y , -phi , r , &t1 , &u1 , &v1); /* l- r+ l+ r- */
if (var < longueur){longueur = var;num = 23; t = t1; u = u1; v = v1;}
var = c_cucu_c(-x ,-y , phi , r , &t1 , &u1 , &v1); /* r- l+ r+ l- */
if (var < longueur){longueur = var;num = 24; t = t1; u = u1; v = v1;}
/*** C | C2 S C ***/
var = c_c2sca(x , y , phi , r , &t1 , &u1 , &v1); /* l+ r- s- l- */
if (var < longueur){longueur = var;num = 25; t = t1; u = u1; v = v1;}
var = c_c2sca(x ,-y , -phi , r , &t1 , &u1 , &v1); /* r+ l- s- r- */
if (var < longueur){longueur = var;num = 26; t = t1; u = u1; v = v1;}
var = c_c2sca(-x , y , -phi , r , &t1 , &u1 , &v1); /* l- r+ s+ l+ */
if (var < longueur){longueur = var;num = 27; t = t1; u = u1; v = v1;}
var = c_c2sca(-x ,-y , phi , r , &t1 , &u1 , &v1); /* r- l+ s+ r+ */
if (var < longueur){longueur = var;num = 28; t = t1; u = u1; v = v1;}
var = c_c2scb(x , y , phi , r , &t1 , &u1 , &v1); /* l+ r- s- r- */
if (var < longueur){longueur = var; num = 29; t = t1; u = u1; v = v1;}
var = c_c2scb(x ,-y , -phi , r , &t1 , &u1 , &v1); /* r+ l- s- l- */
if (var < longueur){longueur = var; num = 30; t = t1; u = u1; v = v1;}
var = c_c2scb(-x , y , -phi , r , &t1 , &u1 , &v1); /* l- r+ s+ r+ */
if (var < longueur){longueur = var; num = 31; t = t1; u = u1; v = v1;}
var = c_c2scb(-x ,-y , phi , r , &t1 , &u1 , &v1); /* r- l+ s+ l+ */
if (var < longueur){longueur = var; num = 32; t = t1; u = u1; v = v1;}
/*** C | C2 S C2 | C ***/
var = c_c2sc2_c(x , y , phi , r , &t1 , &u1 , &v1); /* l+ r- s- l- r+ */
if (var < longueur){longueur = var; num = 33; t = t1; u = u1; v = v1;}
var = c_c2sc2_c(x ,-y , -phi , r , &t1 , &u1 , &v1); /* r+ l- s- r- l+ */
if (var < longueur){longueur = var; num = 34; t = t1; u = u1; v = v1;}
var = c_c2sc2_c(-x , y , -phi , r , &t1 , &u1 , &v1); /* l- r+ s+ l+ r- */
if (var < longueur){longueur = var; num = 35; t = t1; u = u1; v = v1;}
var = c_c2sc2_c(-x ,-y , phi , r , &t1 , &u1 , &v1); /* r- l+ s+ r+ l- */
if (var < longueur){longueur = var; num = 36; t = t1; u = u1; v = v1;}
/*** C C | C ****/
var = cc_c(x , y , phi , r , &t1 , &u1 , &v1); /* l+ r+ l- */
if (var < longueur){longueur = var; num = 37; t = t1; u = u1; v = v1;}
var = cc_c(x ,-y , -phi , r , &t1 , &u1 , &v1); /* r+ l+ r- */
if (var < longueur){longueur = var; num = 38; t = t1; u = u1; v = v1;}
var = cc_c(-x , y , -phi , r , &t1 , &u1 , &v1); /* l- r- l+ */
if (var < longueur){longueur = var; num = 39; t = t1; u = u1; v = v1;}
var = cc_c(-x ,-y , phi , r , &t1 , &u1 , &v1); /* r- l- r+ */
if (var < longueur){longueur = var; num = 40; t = t1; u = u1; v = v1;}
/*** C S C2 | C ***/
var = csc2_ca(x , y , phi , r , &t1 , &u1 , &v1); /* l+ s+ r+ l- */
if (var < longueur){longueur = var; num = 41; t = t1; u = u1; v = v1;}
var = csc2_ca(x ,-y , -phi , r , &t1 , &u1 , &v1); /* r+ s+ l+ r- */
if (var < longueur){longueur = var; num = 42; t = t1; u = u1; v = v1;}
var = csc2_ca(-x , y , -phi , r , &t1 , &u1 , &v1); /* l- s- r- l+ */
if (var < longueur){longueur = var; num = 43; t = t1; u = u1; v = v1;}
var = csc2_ca(-x ,-y , phi , r , &t1 , &u1 , &v1); /* r- s- l- r+ */
if (var < longueur){longueur = var; num = 44; t = t1; u = u1; v = v1;}
var = csc2_cb(x , y , phi , r , &t1 , &u1 , &v1); /* l+ s+ l+ r- */
if (var < longueur){longueur = var; num = 45; t = t1; u = u1; v = v1;}
var = csc2_cb(x ,-y , -phi , r , &t1 , &u1 , &v1); /* r+ s+ r+ l- */
if (var < longueur){longueur = var; num = 46; t = t1; u = u1; v = v1;}
var = csc2_cb(-x , y , -phi , r , &t1 , &u1 , &v1); /* l- s- l- r+ */
if (var < longueur){longueur = var; num = 47; t = t1; u = u1; v = v1;}
var = csc2_cb(-x ,-y , phi , r , &t1 , &u1 , &v1); /* r- s- r- l+ */
if (var < longueur){longueur = var; num = 48; t = t1; u = u1; v = v1;}
*t_r = (double)t; *u_r = (double)u; *v_r = (double)v;
*numero = num;
return((double)longueur);
}
bool Vector2d::operator==(const Vector2d &v)const {
return FEQ(dx, v.getx(), 1.0e-06) && FEQ(dy, v.gety(), 1.0e-06);
}
double dubins(Stconfig *c1 , Stconfig *c2, double radius , int* numero , double *t_r , double *u_r , double *v_r)
{
double x , y , phi;
double t , u , v , t1 , u1 , v1;
int num;
double var , var_d, theta , alpha , dx , dy , r, longueur;
/* Changement de repere,les courbes sont toujours calculees
de (0 0 0)--> (x , y , phi) */
dx = (double)(c2->x - c1->x);
dy = (double)(c2->y - c1->y);
var = (double)(c2->z -c1->z);
r = (double)radius;
theta = atan2(dy , dx);
alpha = theta - (double)(c1->z);
var_d = sqrt(dx*dx + dy*dy);
x = cos(alpha)*var_d;
y = sin(alpha)*var_d;
t1 = u1 = v1 = 0.0;
if (fabs(var) <= M_PI) phi = var;
else {
if (c2->z >= c1->z) phi = var - M_2PI;
else phi = mod2pi(var);}
if(FEQ(r,0.0,EPS4)) {
longueur = csca(x , y , phi , r , &t1 , &u1 , &v1); /* l+ s+ l+ */
var = t1+v1; num = 9; t = t1; u = u1; v = v1;
csca(x ,-y , -phi , r , &t1 , &u1 , &v1); /* r+ s+ r+ */
if((t1+v1) < (t+v)){num = 10;t = t1; u = u1; v = v1;}
cscb(x , y , phi , r , &t1 , &u1 , &v1); /* l+ s+ r+ */
if((t1+v1) < (t+v)){num = 13;t = t1; u = u1; v = v1;}
cscb(x ,-y , -phi , r , &t1 , &u1 , &v1); /* r+ s+ l+ */
if((t1+v1) < (t+v)){num = 14;t = t1; u = u1; v = v1;}
*t_r = (double)t; *u_r = (double)u; *v_r = (double)v;
*numero = num;
if(longueur == infini) PrintInfo(("infini0\n"));
return((double)longueur);
}
/**** C S C ****/
longueur = csca(x , y , phi , r , &t1 , &u1 , &v1); /* l+ s+ l+ */
num = 9; t = t1; u = u1; v = v1;
var = csca(x ,-y , -phi , r , &t1 , &u1 , &v1); /* r+ s+ r+ */
if (var < longueur){longueur = var; num = 10;t = t1; u = u1; v = v1;}
var = cscb(x , y , phi , r , &t1 , &u1 , &v1); /* l+ s+ r+ */
if (var < longueur){longueur = var; num = 13;t = t1; u = u1; v = v1;}
var = cscb(x ,-y , -phi , r , &t1 , &u1 , &v1); /* r+ s+ l+ */
if (var < longueur){longueur = var; num = 14;t = t1; u = u1; v = v1;}
*t_r = (double)t; *u_r = (double)u; *v_r = (double)v;
*numero = num;
return((double)longueur);
}
/*
* dooper() will execute a constant operation in a node and return
* the node to be the result of the operation.
*/
static EXPR *dooper P1 (EXPR *, ep)
{
EXPRTYPE type = ep->nodetype;
ep->nodetype = ep->v.p[0]->nodetype;
switch (ep->v.p[0]->nodetype) {
#ifdef FLOAT_SUPPORT
#ifndef FLOAT_BOOTSTRAP
RVAL f;
#endif /* FLOAT_BOOTSTRAP */
case en_fcon:
#ifndef FLOAT_BOOTSTRAP
FASSIGN (f, ep->v.p[0]->v.f);
switch (type) {
case en_uminus:
FASSIGN (ep->v.f, f);
FNEG (ep->v.f);
break;
case en_test:
ep->v.i = FTST (f) ? (IVAL) 1 : (IVAL) 0;
ep->nodetype = en_icon;
break;
case en_not:
ep->v.i = FTST (f) ? (IVAL) 0 : (IVAL) 1;
ep->nodetype = en_icon;
break;
case en_cast:
if (is_real_floating_type (ep->etp)) {
ep->v.f = f;
} else if (is_bool (ep->etp)) {
ep->v.u = (UVAL) (f ? 1 : 0);
ep->nodetype = en_icon;
} else {
FTOL (ep->v.i, f);
ep->v.i = strip_icon (ep->v.i, ep->etp);
ep->nodetype = en_icon;
}
break;
case en_add:
FADD3 (ep->v.f, f, ep->v.p[1]->v.f);
break;
case en_sub:
FSUB3 (ep->v.f, f, ep->v.p[1]->v.f);
break;
case en_mul:
FMUL3 (ep->v.f, f, ep->v.p[1]->v.f);
break;
case en_div:
if (FTST (ep->v.p[1]->v.f)) {
FDIV3 (ep->v.f, f, ep->v.p[1]->v.f);
} else {
ep->nodetype = en_div;
}
break;
case en_eq:
ep->v.i = (IVAL) FEQ (f, ep->v.p[1]->v.f);
ep->nodetype = en_icon;
break;
case en_ne:
ep->v.i = (IVAL) FNE (f, ep->v.p[1]->v.f);
ep->nodetype = en_icon;
break;
case en_land:
ep->v.i = (IVAL) (FTST (f) && FTST (ep->v.p[1]->v.f));
ep->nodetype = en_icon;
break;
case en_lor:
ep->v.i = (IVAL) (FTST (f) || FTST (ep->v.p[1]->v.f));
ep->nodetype = en_icon;
break;
case en_lt:
ep->v.i = (IVAL) FLT (f, ep->v.p[1]->v.f);
ep->nodetype = en_icon;
break;
case en_le:
ep->v.i = (IVAL) FLE (f, ep->v.p[1]->v.f);
ep->nodetype = en_icon;
break;
case en_gt:
ep->v.i = (IVAL) FGT (f, ep->v.p[1]->v.f);
ep->nodetype = en_icon;
break;
case en_ge:
ep->v.i = (IVAL) FGE (f, ep->v.p[1]->v.f);
ep->nodetype = en_icon;
break;
default:
CANNOT_REACH_HERE ();
break;
}
break;
#endif /* FLOAT_BOOTSTRAP */
#endif /* FLOAT_SUPPORT */
case en_icon:
if (is_unsigned_type (ep->v.p[0]->etp)) {
UVAL u = ep->v.p[0]->v.u;
switch (type) {
case en_uminus:
/*
* unary minus on an unsigned is normally a mistake so we must
* fool the compiler into not giving a warning.
*/
ep->v.u = (UVAL) (-(IVAL) u);
break;
case en_test:
ep->v.u = (u ? (UVAL) 1 : (UVAL) 0);
break;
case en_not:
ep->v.u = (u ? (UVAL) 0 : (UVAL) 1);
break;
case en_compl:
ep->v.u = (UVAL) strip_icon ((IVAL) ~u, ep->etp);
break;
case en_cast:
if (is_bool (ep->etp)) {
ep->v.u = (UVAL) (u ? 1 : 0);
#ifdef FLOAT_SUPPORT
} else if (is_real_floating_type (ep->etp)) {
ep->nodetype = en_fcon;
UTOF (ep->v.f, u);
break;
#endif /* FLOAT_SUPPORT */
} else {
ep->v.u = (UVAL) strip_icon ((IVAL) u, ep->etp);
}
break;
case en_add:
ep->v.u = u + ep->v.p[1]->v.u;
break;
case en_sub:
ep->v.u = u - ep->v.p[1]->v.u;
break;
case en_mul:
ep->v.u = u * ep->v.p[1]->v.u;
break;
case en_div:
if (ep->v.p[1]->v.u == (UVAL) 0) {
ep->nodetype = en_div;
} else {
ep->v.u = u / ep->v.p[1]->v.u;
}
break;
case en_mod:
if (ep->v.p[1]->v.u == (UVAL) 0) {
ep->nodetype = en_mod;
} else {
ep->v.u = u % ep->v.p[1]->v.u;
}
break;
case en_and:
ep->v.u = u & ep->v.p[1]->v.u;
break;
case en_or:
ep->v.u = u | ep->v.p[1]->v.u;
break;
case en_xor:
ep->v.u = u ^ ep->v.p[1]->v.u;
break;
case en_eq:
ep->v.u = (UVAL) (u == ep->v.p[1]->v.u);
break;
case en_ne:
ep->v.u = (UVAL) (u != ep->v.p[1]->v.u);
break;
case en_land:
ep->v.u = (UVAL) (u && ep->v.p[1]->v.u);
break;
case en_lor:
ep->v.u = (UVAL) (u || ep->v.p[1]->v.u);
break;
case en_lt:
ep->v.u = (UVAL) (u < ep->v.p[1]->v.u);
break;
case en_le:
ep->v.u = (UVAL) (u <= ep->v.p[1]->v.u);
break;
case en_gt:
ep->v.u = (UVAL) (u > ep->v.p[1]->v.u);
break;
case en_ge:
ep->v.u = (UVAL) (u >= ep->v.p[1]->v.u);
break;
case en_lsh:
ep->v.u = u << ep->v.p[1]->v.u;
break;
case en_rsh:
ep->v.u = u >> ep->v.p[1]->v.u;
break;
default:
CANNOT_REACH_HERE ();
break;
}
} else {
