draw12.cc
0000#include <math.h>
0001#include <stdio.h>
0002#include "draw12.h"
0003
0004
0005#define d2r 0.01745329252
0006#define r2d 57.295779513
0007
0008int outstr(char *buf); /* for zero-terminated string */
0009int flush();
0010char msg[200];
0011
0012CThreeD thd;
0013
0014/*********************************************************************
0015
0016 DegreeToTAE
0017
0018 Put angles in degree into TransAngleEuler structure
0019
0020*********************************************************************/
0021void DegreeToTAE(double theta,double phi,double psi,TransAngleEuler *dest)
0022{
0023 dest->theta=d2r*theta;
0024 dest->phi=d2r*phi;
0025 dest->psi=d2r*psi;
0026}
0027
0028/*********************************************************************
0029
0030 DegreeToAE
0031
0032 Put angles in degree into AngleEuler structure
0033
0034*********************************************************************/
0035void DegreeToAE(double theta,double phi,double psi,AngleEuler *dest)
0036{
0037 dest->theta=d2r*theta;
0038 dest->phi=d2r*phi;
0039 dest->psi=d2r*psi;
0040}
0041
0042
0043/*********************************************************************
0044
0045 LinesToTAM
0046
0047 Construct TransAngleMatrix parameter from 2 Line3D parameters
0048
0049 a line segment on one of the coordinate axis
0050 b line segment on one of the coordinate axis
0051 w a and b are yzAxis, zxAxis or xyAxis
0052 dest TransAngleMatrix constructed
0053*********************************************************************/
0054void LinesToTAM(Line3d *a,Line3d *b,int w,TransAngleMatrix *dest)
0055{
0056 Point3d x,y,z;
0057 double d;
0058
0059 dest->x=a->sx;
0060 dest->y=a->sy;
0061 dest->z=a->sz;
0062
0063 switch (w){
0064 case yzAxis:
0065 NormLine3d(a,&y,&d);
0066 NormLine3d(b,&z,&d);
0067 x.x=y.y*z.z-y.z*z.y;
0068 x.y=y.z*z.x-y.x*z.z;
0069 x.z=y.x*z.y-y.y*z.x;
0070 break;
0071 case zxAxis:
0072 NormLine3d(a,&z,&d);
0073 NormLine3d(b,&x,&d);
0074 y.x=z.y*x.z-z.z*x.y;
0075 y.y=z.z*x.x-z.x*x.z;
0076 y.z=z.x*x.y-z.y*x.x;
0077 break;
0078 case xyAxis:
0079 NormLine3d(a,&x,&d);
0080 NormLine3d(b,&y,&d);
0081 z.x=x.y*y.z-x.z*y.y;
0082 z.y=x.z*y.x-x.x*y.z;
0083 z.z=x.x*y.y-x.y*y.x;
0084 break;
0085 }
0086 dest->c[0][0]=x.x;
0087 dest->c[1][0]=x.y;
0088 dest->c[2][0]=x.z;
0089 dest->c[0][1]=y.x;
0090 dest->c[1][1]=y.y;
0091 dest->c[2][1]=y.z;
0092 dest->c[0][2]=z.x;
0093 dest->c[1][2]=z.y;
0094 dest->c[2][2]=z.z;
0095}
0096
0097/*********************************************************************
0098
0099 LinesToTAE
0100
0101 Construct TransAngleEuler parameter from 2 Line3D parameters
0102
0103 a line segment on one of the coordinate axis
0104 b line segment on one of the coordinate axis
0105 w a and b are yzAxis, zxAxis or xyAxis
0106 dest TransAngleEuler constructed
0107*********************************************************************/
0108void LinesToTAE(Line3d *a,Line3d *b,int w,TransAngleEuler *dest)
0109{
0110 TransAngleMatrix tmp;
0111
0112 LinesToTAM(a,b,w,&tmp);
0113 dest->x=tmp.x;
0114 dest->y=tmp.y;
0115 dest->z=tmp.z;
0116
0117 if(tmp.c[0][2]==0.&&tmp.c[1][2]==0.){
0118 dest->theta=0.;
0119 dest->phi=0.;
0120 dest->psi=atan2(tmp.c[1][0],tmp.c[0][0]);
0121 }
0122 else{
0123 double x1,y1;
0124 dest->theta=acos(tmp.c[2][2]);
0125 dest->phi=atan2(tmp.c[1][2],tmp.c[0][2])+1.5707963268;
0126 x1=cos(dest->phi); // ascending node
0127 y1=sin(dest->phi);
0128
0129// (y1*x.z-z1*x.y)*z.x+(z1*x.x-x1*x.z)*z.y+(x1*x.y-y1*x.x)*z.z
0130// x1*x.x+y1*x.y+z1*x.z
0131// dest->psi=atan2(y1*x.z*z.x-x1*x.z*z.y+(x1*x.y-y1*x.x)*z.z,x1*x.x+y1*x.y);
0132
0133 dest->psi=atan2(y1*tmp.c[2][0]*tmp.c[0][2]-x1*tmp.c[2][0]*tmp.c[2][1]
0134 +(x1*tmp.c[1][0]-y1*tmp.c[0][0])*tmp.c[2][2],
0135 x1*tmp.c[0][0]+y1*tmp.c[1][0]);
0136 }
0137}
0138
0139
0140/*********************************************************************
0141
0142 PointsToAM
0143
0144 Construct AngleMatrix parameter from 2 Point3D parameters
0145
0146 a point on one of the coordinate axis
0147 b point on one of the coordinate axis
0148 w a and b are yzAxis, zxAxis or xyAxis
0149 dest AngleMatrix constructed
0150*********************************************************************/
0151void PointsToAM(Point3d *a,Point3d *b,int w,AngleMatrix *dest)
0152{
0153 Point3d x,y,z;
0154 double d;
0155
0156
0157 switch (w){
0158 case yzAxis:
0159 NormPoint3d(a,&y,&d);
0160 NormPoint3d(b,&z,&d);
0161 x.x=y.y*z.z-y.z*z.y;
0162 x.y=y.z*z.x-y.x*z.z;
0163 x.z=y.x*z.y-y.y*z.x;
0164 break;
0165 case zxAxis:
0166 NormPoint3d(a,&z,&d);
0167 NormPoint3d(b,&x,&d);
0168 y.x=z.y*x.z-z.z*x.y;
0169 y.y=z.z*x.x-z.x*x.z;
0170 y.z=z.x*x.y-z.y*x.x;
0171 break;
0172 case xyAxis:
0173 NormPoint3d(a,&x,&d);
0174 NormPoint3d(b,&y,&d);
0175 z.x=x.y*y.z-x.z*y.y;
0176 z.y=x.z*y.x-x.x*y.z;
0177 z.z=x.x*y.y-x.y*y.x;
0178 break;
0179 }
0180 dest->c[0][0]=x.x;
0181 dest->c[1][0]=x.y;
0182 dest->c[2][0]=x.z;
0183 dest->c[0][1]=y.x;
0184 dest->c[1][1]=y.y;
0185 dest->c[2][1]=y.z;
0186 dest->c[0][2]=z.x;
0187 dest->c[1][2]=z.y;
0188 dest->c[2][2]=z.z;
0189}
0190
0191/*********************************************************************
0192
0193 PointsToAE
0194
0195 Construct AngleEuler parameter from 2 Point3D parameters
0196
0197 a point on one of the coordinate axis
0198 b point on one of the coordinate axis
0199 w a and b are yzAxis, zxAxis or xyAxis
0200 dest TransAngleEuler constructed
0201*********************************************************************/
0202void PointsToAE(Point3d *a,Point3d *b,int w,AngleEuler *dest)
0203{
0204 AngleMatrix tmp;
0205
0206 PointsToAM(a,b,w,&tmp);
0207
0208 if(tmp.c[0][2]==0.&&tmp.c[1][2]==0.){
0209 dest->theta=0.;
0210 dest->phi=0.;
0211 dest->psi=atan2(tmp.c[1][0],tmp.c[0][0]);
0212 }
0213 else{
0214 double x1,y1;
0215 dest->theta=acos(tmp.c[2][2]);
0216 dest->phi=atan2(tmp.c[1][2],tmp.c[0][2])+1.5707963268;
0217 x1=cos(dest->phi); // ascending node
0218 y1=sin(dest->phi);
0219
0220// (y1*x.z-z1*x.y)*z.x+(z1*x.x-x1*x.z)*z.y+(x1*x.y-y1*x.x)*z.z
0221// x1*x.x+y1*x.y+z1*x.z
0222// dest->psi=atan2(y1*x.z*z.x-x1*x.z*z.y+(x1*x.y-y1*x.x)*z.z,x1*x.x+y1*x.y);
0223
0224 dest->psi=atan2(y1*tmp.c[2][0]*tmp.c[0][2]-x1*tmp.c[2][0]*tmp.c[2][1]
0225 +(x1*tmp.c[1][0]-y1*tmp.c[0][0])*tmp.c[2][2],
0226 x1*tmp.c[0][0]+y1*tmp.c[1][0]);
0227 }
0228}
0229
0230
0231/*********************************************************************
0232 NormLine3d
0233
0234 normalize a vector
0235
0236 source line segment to be normalized
0237
0238 dest normalized vector
0239 norm length of the vector
0240
0241*********************************************************************/
0242void NormLine3d(Line3d *source,Point3d *dest,double *norm)
0243{
0244 double x,y,z,d;
0245
0246 x=source->ex-source->sx;
0247 y=source->ey-source->sy;
0248 z=source->ez-source->sz;
0249
0250 d=sqrt(x*x+y*y+z*z);
0251 dest->x=x/d;
0252 dest->y=y/d;
0253 dest->z=z/d;
0254 *norm=d;
0255}
0256
0257/*********************************************************************
0258 NormPoint3d
0259
0260 normalize a vector
0261
0262 source vector to be normalized
0263
0264 dest normalized vector
0265 norm length of the vector
0266
0267*********************************************************************/
0268void NormPoint3d(Point3d *source,Point3d *dest,double *norm)
0269{
0270 double x,y,z,d;
0271
0272 x=source->x;
0273 y=source->y;
0274 z=source->z;
0275
0276 d=sqrt(x*x+y*y+z*z);
0277 dest->x=x/d;
0278 dest->y=y/d;
0279 dest->z=z/d;
0280 *norm=d;
0281}
0282
0283/*********************************************************************
0284 NormLine2d
0285
0286 normalize a vector
0287
0288 source line segment to be normalized
0289
0290 dest normalized vector
0291 norm length of the vector
0292
0293*********************************************************************/
0294void NormLine2d(Line2d *source,Point2d *dest,double *norm)
0295{
0296 double x,y,d;
0297
0298 x=source->ex-source->sx;
0299 y=source->ey-source->sy;
0300
0301 d=sqrt(x*x+y*y);
0302 dest->x=x/d;
0303 dest->y=y/d;
0304 *norm=d;
0305}
0306
0307/*********************************************************************
0308 NormPoint2d
0309
0310 normalize a vector
0311
0312 source vector to be normalized
0313
0314 dest normalized vector
0315 norm length of the vector
0316
0317*********************************************************************/
0318void NormPoint2d(Point2d *source,Point2d *dest,double *norm)
0319{
0320 double x,y,d;
0321
0322 x=source->x;
0323 y=source->y;
0324
0325 d=sqrt(x*x+y*y);
0326 dest->x=x/d;
0327 dest->y=y/d;
0328 *norm=d;
0329}
0330
0331
0332/*********************************************************************
0333 TAEtoTAM
0334
0335 convert TransAngleEuler to TransAngleMatrix
0336
0337 source TransAngleEuler
0338
0339 dest TransAngleMatrix
0340
0341*********************************************************************/
0342void TAEtoTAM(TransAngleEuler *source,TransAngleMatrix *dest)
0343{
0344 double sth,cth,sph,cph,sps,cps;
0345
0346 dest->x=source->x;
0347 dest->y=source->y;
0348 dest->z=source->z;
0349 sth=sin(source->theta);
0350 cth=cos(source->theta);
0351 sph=sin(source->phi);
0352 cph=cos(source->phi);
0353 sps=sin(source->psi);
0354 cps=cos(source->psi);
0355 dest->c[0][0]=cps*cph-sps*cth*sph; // x component of x'
0356 dest->c[0][1]=-sps*cph-cps*cth*sph;
0357 dest->c[0][2]=sth*sph;
0358 dest->c[1][0]=cps*sph+sps*cth*cph; // y component of x'
0359 dest->c[1][1]=-sps*sph+cps*cth*cph;
0360 dest->c[1][2]=-sth*cph;
0361 dest->c[2][0]=sps*sth; // z component of x'
0362 dest->c[2][1]=cps*sth;
0363 dest->c[2][2]=cth;
0364}
0365
0366
0367/*********************************************************************
0368 TAMtoTAE
0369
0370 convert TransAngleMatrix to TransAngleEuler
0371
0372 source TransAngleMatrix
0373
0374 dest TransAngleEuler
0375
0376*********************************************************************/
0377void TAMtoTAE(TransAngleMatrix *source,TransAngleEuler *dest)
0378{
0379 double a,ca,sa;
0380
0381 dest->x=source->x;
0382 dest->y=source->y;
0383 dest->z=source->z;
0384
0385 if(source->c[2][2]==1.){
0386 dest->theta=0.;
0387 dest->phi=0.;
0388 dest->psi=atan2(source->c[0][1],source->c[0][0]);
0389 return;
0390 }
0391 dest->theta=acos(source->c[2][2]);
0392 dest->phi=atan2(source->c[1][2],source->c[0][2])+1.5707963268;
0393 a=dest->phi;
0394 ca=cos(a);
0395 sa=sin(a);
0396 dest->psi=-atan2(source->c[0][1]*ca+source->c[1][1]*sa,
0397 source->c[0][0]*ca+source->c[1][0]*sa);
0398}
0399
0400
0401/*********************************************************************
0402 InverseTAM
0403
0404 Inverse body-frame order
0405
0406 source TransAngleMatrix
0407
0408 dest TransAngleMatrix
0409
0410*********************************************************************/
0411void InverseTAM(TransAngleMatrix *source,TransAngleMatrix *dest)
0412{
0413 Point3d a;
0414
0415 a.x=-source->x;
0416 a.y=-source->y;
0417 a.z=-source->z;
0418
0419 dest->x=source->c[0][0]*a.x+source->c[1][0]*a.y+source->c[2][0]*a.z;
0420 dest->y=source->c[0][1]*a.x+source->c[1][1]*a.y+source->c[2][1]*a.z;
0421 dest->z=source->c[0][2]*a.x+source->c[1][2]*a.y+source->c[2][2]*a.z;
0422
0423 dest->c[0][0]=source->c[0][0];
0424 dest->c[0][1]=source->c[1][0];
0425 dest->c[0][2]=source->c[2][0];
0426 dest->c[1][0]=source->c[0][1];
0427 dest->c[1][1]=source->c[1][1];
0428 dest->c[1][2]=source->c[2][1];
0429 dest->c[2][0]=source->c[0][2];
0430 dest->c[2][1]=source->c[1][2];
0431 dest->c[2][2]=source->c[2][2];
0432
0433}
0434
0435
0436
0437/*********************************************************************
0438 BodyToFrameTAM
0439
0440 convert Body coordinate to Frame coordinate
0441
0442 source Point3d
0443 m TransAngleMatrix
0444
0445 dest Point3d
0446
0447*********************************************************************/
0448void BodyToFrameTAM(Point3d *source,TransAngleMatrix *m,Point3d *dest)
0449{
0450 dest->x=m->x+m->c[0][0]*source->x+m->c[0][1]*source->y+m->c[0][2]*source->z;
0451 dest->y=m->y+m->c[1][0]*source->x+m->c[1][1]*source->y+m->c[1][2]*source->z;
0452 dest->z=m->z+m->c[2][0]*source->x+m->c[2][1]*source->y+m->c[2][2]*source->z;
0453}
0454
0455/*********************************************************************
0456 FrameToBodyTAM
0457
0458 convert Frame coordinate to Body coordinate
0459
0460 source Point3d
0461 m TransAngleMatrix
0462
0463 dest Point3d
0464
0465*********************************************************************/
0466void FrameToBodyTAM(Point3d *source,TransAngleMatrix *m,Point3d *dest)
0467{
0468 dest->x=m->c[0][0]*(source->x-m->x)
0469 +m->c[1][0]*(source->y-m->y)
0470 +m->c[2][0]*(source->z-m->z);
0471 dest->y=m->c[0][1]*(source->x-m->x)
0472 +m->c[1][1]*(source->y-m->y)
0473 +m->c[2][1]*(source->z-m->z);
0474 dest->z=m->c[0][2]*(source->x-m->x)
0475 +m->c[1][2]*(source->y-m->y)
0476 +m->c[2][2]*(source->z-m->z);
0477}
0478
0479
0480/*********************************************************************
0481 MoveTwiceTAE
0482
0483 obtain single conversion parameters from double consecutive
0484 coordinate conversion parameters
0485
0486 src1 TransAngleEuler cood1 - frame cood2 - body
0487 src2 TransAngleEuler cood2 - frame cood3 - body
0488
0489 dest TransAngleEuler cood1 - frame cood3 - body
0490
0491********************************************************************/
0492void MoveTwiceTAE(TransAngleEuler *src1,TransAngleEuler *src2,TransAngleEuler *dest)
0493{
0494 double x1,y1,z1,theta1,phi1,psi1;
0495 double x2,y2,z2,theta2,phi2,psi2;
0496 double x3,y3,z3,theta3,phi3,psi3;
0497
0498 double a[3][3],b[3][3],c[3],d[3],e[3];
0499 double cost,sint,cosf,sinf,cosp,sinp;
0500 bool locked;
0501
0502 x1=src1->x;
0503 y1=src1->y;
0504 z1=src1->z;
0505 theta1=src1->theta;
0506 phi1=src1->phi;
0507 psi1=src1->psi;
0508 x2=src2->x;
0509 y2=src2->y;
0510 z2=src2->z;
0511 theta2=src2->theta;
0512 phi2=src2->phi;
0513 psi2=src2->psi;
0514
0515 cost=cos(theta1);
0516 sint=sin(theta1);
0517 cosf=cos(phi1);
0518 sinf=sin(phi1);
0519 cosp=cos(psi1);
0520 sinp=sin(psi1);
0521// if d[i] is a vector in cood2, c[j]=a[j][i]*d[i] is the vector in cood1
0522 a[0][0]=cosp*cosf-sinp*cost*sinf;
0523 a[0][1]=-sinp*cosf-cosp*cost*sinf;
0524 a[0][2]=sint*sinf;
0525
0526 a[1][0]=cosp*sinf+sinp*cost*cosf;
0527 a[1][1]=-sinp*sinf+cosp*cost*cosf;
0528 a[1][2]=-sint*cosf;
0529
0530 a[2][0]=sinp*sint;
0531 a[2][1]=cosp*sint;
0532 a[2][2]=cost;
0533
0534 cost=cos(theta2);
0535 sint=sin(theta2);
0536 cosf=cos(phi2);
0537 sinf=sin(phi2);
0538 cosp=cos(psi2);
0539 sinp=sin(psi2);
0540
0541// if d[i] is a vector in cood3, c[j]=b[j][i]*d[i] is the vector in cood2
0542 b[0][0]=cosp*cosf-sinp*cost*sinf;
0543 b[0][1]=-sinp*cosf-cosp*cost*sinf;
0544 b[0][2]=sint*sinf;
0545
0546 b[1][0]=cosp*sinf+sinp*cost*cosf;
0547 b[1][1]=-sinp*sinf+cosp*cost*cosf;
0548 b[1][2]=-sint*cosf;
0549
0550 b[2][0]=sinp*sint;
0551 b[2][1]=cosp*sint;
0552 b[2][2]=cost;
0553
0554 x3=x1+a[0][0]*x2+a[0][1]*y2+a[0][2]*z2;
0555 y3=y1+a[1][0]*x2+a[1][1]*y2+a[1][2]*z2;
0556 z3=z1+a[2][0]*x2+a[2][1]*y2+a[2][2]*z2;
0557
0558// if c is a unit z vector in coord 3, d=b*c is the vector in coord 2
0559// and a*d is the vector in coord 1
0560 c[0]=0.;
0561 c[1]=0.;
0562 c[2]=1.;
0563 d[0]=b[0][0]*c[0]+b[0][1]*c[1]+b[0][2]*c[2];
0564 d[1]=b[1][0]*c[0]+b[1][1]*c[1]+b[1][2]*c[2];
0565 d[2]=b[2][0]*c[0]+b[2][1]*c[1]+b[2][2]*c[2];
0566 e[0]=a[0][0]*d[0]+a[0][1]*d[1]+a[0][2]*d[2];
0567 e[1]=a[1][0]*d[0]+a[1][1]*d[1]+a[1][2]*d[2];
0568 e[2]=a[2][0]*d[0]+a[2][1]*d[1]+a[2][2]*d[2];
0569
0570// Euler angles of coord3 with reference to coord1 are to be derived
0571// theta is the angle between z vector in coord3 and z vector in coord1
0572// cosine of the theta is e[2] in the above expression
0573
0574 theta3=acos(e[2]);
0575 if(theta3<0.0005) {
0576 phi3=0.; // the case when theta = 0
0577 locked=true;
0578 }
0579 else {
0580 phi3=atan2(e[0],-e[1]);
0581 locked=false;
0582 }
0583
0584// if c is a unit x vector in coord 3, d=b*c is the vector in coord 2
0585// and a*d is the vector in coord 1
0586 c[0]=1.;
0587 c[1]=0.;
0588 c[2]=0.;
0589 d[0]=b[0][0]*c[0]+b[0][1]*c[1]+b[0][2]*c[2];
0590 d[1]=b[1][0]*c[0]+b[1][1]*c[1]+b[1][2]*c[2];
0591 d[2]=b[2][0]*c[0]+b[2][1]*c[1]+b[2][2]*c[2];
0592 e[0]=a[0][0]*d[0]+a[0][1]*d[1]+a[0][2]*d[2];
0593 e[1]=a[1][0]*d[0]+a[1][1]*d[1]+a[1][2]*d[2];
0594 e[2]=a[2][0]*d[0]+a[2][1]*d[1]+a[2][2]*d[2];
0595// if c is a unit y vector in coord 3, d=b*c is the vector in coord 2
0596// and a*d is the vector in coord 1
0597 c[0]=0.;
0598 c[1]=1.;
0599 c[2]=0.;
0600 d[0]=b[0][0]*c[0]+b[0][1]*c[1]+b[0][2]*c[2];
0601 d[1]=b[1][0]*c[0]+b[1][1]*c[1]+b[1][2]*c[2];
0602 d[2]=b[2][0]*c[0]+b[2][1]*c[1]+b[2][2]*c[2];
0603 c[0]=a[0][0]*d[0]+a[0][1]*d[1]+a[0][2]*d[2];
0604 c[1]=a[1][0]*d[0]+a[1][1]*d[1]+a[1][2]*d[2];
0605 c[2]=a[2][0]*d[0]+a[2][1]*d[1]+a[2][2]*d[2];
0606
0607 if(locked) psi3=atan2(e[1],e[0]);
0608 else psi3=atan2(e[2],c[2]);
0609
0610 dest->x=x3;
0611 dest->y=y3;
0612 dest->z=z3;
0613 dest->theta=theta3;
0614 dest->phi=phi3;
0615 dest->psi=psi3;
0616}
0617
0618
0619/*********************************************************************
0620 AEtoAM
0621
0622 convert CoordEuler to CoordMatrix
0623
0624 source AngleEuler
0625
0626 dest AngleMatrix
0627
0628*********************************************************************/
0629void AEtoAM(AngleEuler *source,AngleMatrix *dest)
0630{
0631 double sth,cth,sph,cph,sps,cps;
0632
0633 sth=sin(source->theta);
0634 cth=cos(source->theta);
0635 sph=sin(source->phi);
0636 cph=cos(source->phi);
0637 sps=sin(source->psi);
0638 cps=cos(source->psi);
0639 dest->c[0][0]=cps*cph-sps*cth*sph;
0640 dest->c[0][1]=-sps*cph-cps*cth*sph;
0641 dest->c[0][2]=sth*sph;
0642 dest->c[1][0]=cps*sph+sps*cth*cph;
0643 dest->c[1][1]=-sps*sph+cps*cth*cph;
0644 dest->c[1][2]=-sth*cph;
0645 dest->c[2][0]=sps*sth;
0646 dest->c[2][1]=cps*sth;
0647 dest->c[2][2]=cth;
0648}
0649
0650/*********************************************************************
0651 BodyToFrameAM
0652
0653 convert Body coordinate to Frame coordinate
0654
0655 source Point3d - Cartesian
0656 m AngleMatrix
0657
0658 dest Point3d - Cartesian
0659
0660*********************************************************************/
0661void BodyToFrameAM(Point3d *source,AngleMatrix *m,Point3d *dest)
0662{
0663
0664 dest->x=m->c[0][0]*source->x+m->c[0][1]*source->y+m->c[0][2]*source->z;
0665 dest->y=m->c[1][0]*source->x+m->c[1][1]*source->y+m->c[1][2]*source->z;
0666 dest->z=m->c[2][0]*source->x+m->c[2][1]*source->y+m->c[2][2]*source->z;
0667
0668}
0669
0670/*********************************************************************
0671 FrameToBodyAM
0672
0673 convert Frame coordinate to Body coordinate
0674
0675 source Point3d - Cartesian
0676 m AngleMatrix
0677
0678 dest Point3d - Cartesian
0679
0680*********************************************************************/
0681void FrameToBodyAM(Point3d *source,AngleMatrix *m,Point3d *dest)
0682{
0683 dest->x=m->c[0][0]*source->x+m->c[1][0]*source->y+m->c[2][0]*source->z;
0684 dest->y=m->c[0][1]*source->x+m->c[1][1]*source->y+m->c[2][1]*source->z;
0685 dest->z=m->c[0][2]*source->x+m->c[1][2]*source->y+m->c[2][2]*source->z;
0686}
0687
0688/*********************************************************************
0689 BodyToFrameAMP
0690
0691 convert Body coordinate to Frame coordinate - Polar
0692
0693 source Point2d - azimuth and elevation (rad)
0694 m AngleMatrix
0695
0696 dest Point2d - azimuth and elevation (rad)
0697
0698*********************************************************************/
0699void BodyToFrameAMP(Point2d *source,AngleMatrix *m,Point2d *dest)
0700{
0701 Point3d s,d; // unit vector
0702
0703 ToCartesian3d(source,&s);
0704
0705 d.x=m->c[0][0]*s.x+m->c[0][1]*s.y+m->c[0][2]*s.z;
0706 d.y=m->c[1][0]*s.x+m->c[1][1]*s.y+m->c[1][2]*s.z;
0707 d.z=m->c[2][0]*s.x+m->c[2][1]*s.y+m->c[2][2]*s.z;
0708
0709 ToPolar3d(&d,dest);
0710
0711}
0712
0713/*********************************************************************
0714 FrameToBodyAMP
0715
0716 convert Frame coordinate to Body coordinate - Polar
0717
0718 source Point2d - azimuth and elevation (rad)
0719 m AngleMatrix
0720
0721 dest Point2d - azimuth and elevation (rad)
0722
0723*********************************************************************/
0724void FrameToBodyAMP(Point2d *source,AngleMatrix *m,Point2d *dest)
0725{
0726 Point3d s,d;
0727
0728 ToCartesian3d(source,&s);
0729
0730 d.x=m->c[0][0]*s.x+m->c[1][0]*s.y+m->c[2][0]*s.z;
0731 d.y=m->c[0][1]*s.x+m->c[1][1]*s.y+m->c[2][1]*s.z;
0732 d.z=m->c[0][2]*s.x+m->c[1][2]*s.y+m->c[2][2]*s.z;
0733
0734 ToPolar3d(&d,dest);
0735
0736}
0737
0738
0739/*********************************************************************
0740 ToPolar3d
0741
0742 convert Catesian coordinate to Polar coordinate
0743
0744 source Point3dDouble - Cartesian with unit length
0745
0746 dest Point2dDouble - azimuth and elevation (rad)
0747
0748*********************************************************************/
0749void ToPolar3d(Point3d *source,Point2d *dest)
0750{
0751 dest->y=asin(source->z);
0752 if(source->z==1.||source->z==-1.) dest->x=0.;
0753 else dest->x=atan2(source->y,source->x);
0754}
0755
0756/*********************************************************************
0757 ToCartesian3d
0758
0759 convert Polar coordinate to Cartesian coordinate
0760
0761 source Point2dDouble - azimuth and elevation (rad)
0762
0763 dest Point3dDouble - Cartesian with unit length
0764
0765*********************************************************************/
0766void ToCartesian3d(Point2d *source,Point3d *dest)
0767{
0768 double cel;
0769 cel=cos(source->y); // cosine elevation
0770 dest->x=cel*cos(source->x);
0771 dest->y=cel*sin(source->x);
0772 dest->z=sin(source->y);
0773}
0774
0775/*********************************************************************
0776 VectorProduct
0777
0778 Vector product
0779
0780 u input
0781 v input
0782 w output uxv
0783
0784*********************************************************************/
0785void VectorProduct(Point3d *u,Point3d *v,Point3d *w)
0786{
0787 w->x=u->y*v->z-u->z*v->y;
0788 w->y=u->z*v->x-u->x*v->z;
0789 w->z=u->x*v->y-u->y*v->x;
0790}
0791
0792/*********************************************************************
0793 AcuteAngle
0794
0795 Test if the angle u-v-w is an acute angle
0796
0797 u point1
0798 v point2 - center point
0799 w point3
0800
0801 returned true if the angle is acute angle
0802 false if the angle is obtuse or right angle
0803
0804*********************************************************************/
0805bool AcuteAngle(Point3d *u,Point3d *v,Point3d *w)
0806{
0807 if((u->x-v->x)*(w->x-v->x)+(u->y-v->y)*(w->y-v->y)+
0808 (u->z-v->z)*(w->z-v->z)>0.) return true;
0809 else return false;
0810}
0811
0812/**************************************************************************
0813 MakeThePolar
0814
0815 'polar' is a coordinate system with its origin at the viewer's eye.
0816 Its z axis is directed toward line of sight and x-z plane is pararel
0817 to the world z axis. This coordinate is for generating polar coordinate
0818 where the north pole is directed to the line of sight and the azimuth
0819 is measured crockwise from the direction pointing upward.
0820
0821 sight starts at the eye and ends at the target
0822
0823 polar with origin at the eye and z-axis toward the line of sight
0824**************************************************************************/
0825void MakeThePolar(Line3d *sight,TransAngleMatrix *polar)
0826{
0827 Point3d x,y,z;
0828 double d;
0829
0830 polar->x=sight->sx;
0831 polar->y=sight->sy;
0832 polar->z=sight->sz;
0833
0834 NormLine3d(sight,&z,&d);
0835
0836// z.x z.y z.z
0837// 0 0 1
0838
0839 y.x=z.y;
0840 y.y=-z.x;
0841 y.z=0.;
0842 NormPoint3d(&y,&y,&d);
0843
0844// z.y -z.x 0
0845// z.x z.y z.z
0846
0847 x.x=-z.x*z.z;
0848 x.y=-z.y*z.z;
0849 x.z=z.y*z.y+z.x*z.x;
0850 NormPoint3d(&x,&x,&d);
0851
0852 polar->c[0][0]=x.x;
0853 polar->c[0][1]=y.x;
0854 polar->c[0][2]=z.x;
0855 polar->c[1][0]=x.y;
0856 polar->c[1][1]=y.y;
0857 polar->c[1][2]=z.y;
0858 polar->c[2][0]=x.z;
0859 polar->c[2][1]=y.z;
0860 polar->c[2][2]=z.z;
0861}
0862
0863/**************************************************************************
0864 WorldToPolarCnv
0865
0866 'polar' is a coordinate system with its origin at the viewer's eye.
0867 Its z axis is directed toward line of sight and x-z plane is pararel
0868 to the world z axis. This coordinate is for generating polar coordinate
0869 where the north pole is directed to the line of sight and the azimuth
0870 is measured crockwise from the direction pointing upward.
0871
0872 polar with origin at the eye and z-axis toward the line of sight
0873 s point in World coordinate
0874
0875 d polar coordinate az(rad), el(rad), and distance
0876 el measured fron boresight
0877
0878**************************************************************************/
0879void WorldToPolarCnv(Point3d *s,TransAngleMatrix *polar,Point3d *d)
0880{
0881 Point3d a;
0882 double b;
0883
0884 FrameToBodyTAM(s,polar,d);
0885 NormPoint3d(d,&a,&b);
0886 if(a.z==1.){
0887 d->x=0.;
0888 d->y=0.;
0889 d->z=b;
0890 }
0891 else {
0892 d->x=atan2(a.y,a.x);
0893 d->y=acos(a.z);
0894 d->z=b;
0895 }
0896}
0897
0898/**************************************************************************
0899 WorldToPerspCnv
0900
0901 'polar' is a coordinate system with its origin at the viewer's eye.
0902 Its z axis is directed toward line of sight and x-z plane is pararel
0903 to the world z axis. This coordinate is for generating polar coordinate
0904 where the north pole is directed to the line of sight and the azimuth
0905 is measured crockwise from the direction pointing upward.
0906
0907 polar with origin at the eye and z-axis toward the line of sight
0908 s point in World coordinate
0909
0910 d polar coordinate az(rad), el(rad), and distance
0911 el measured fron boresight
0912
0913**************************************************************************/
0914void WorldToPerspCnv(Point3d *s,TransAngleMatrix *polar,PointPerspectve *d)
0915{
0916 Point3d a;
0917 double b;
0918
0919 WorldToPolarCnv(s,polar,&a);
0920 d->az=a.x;
0921 d->el=a.y;
0922 d->r=a.z;
0923 b=tan(a.y);
0924 d->h=b*sin(a.x); // right plus
0925 d->v=b*cos(a.x); // up plus
0926}
0927
0928/**************************************************************************
0929 ClipAngleMatrix
0930
0931 Represent a side of a clip rect by a polar coordinate
0932 and an angular segment in its equatorial plane.
0933
0934 Input parameters are in the polar coordinate, where z axis
0935 is directed to the boresight.
0936
0937 azi azimuth (rad)
0938 inc inclination (rad - measured from z axis)
0939
0940 m conversion matrix (AngleMatrix)
0941 ang azimuth 0 to ang is a clip arc
0942**************************************************************************/
0943void ClipAngleMatrix(double azi,double inc,AngleMatrix *m,double *ang)
0944{
0945 double rot,az,de;
0946 double pi2=1.5707963268;
0947 Point2d s;
0948 Point3d d;
0949
0950 de=pi2-inc; // declination
0951
0952 m->c[0][0]=cos(de)*cos(azi); // body x' axis
0953 m->c[1][0]=cos(de)*sin(azi);
0954 m->c[2][0]=sin(de);
0955
0956// rotate azimuth by an angle rot so that az falls 0 and 1.57079
0957 rot=0.;
0958 az=azi;
0959 while(az>pi2){
0960 az-=pi2;
0961 rot-=pi2;
0962 }
0963 while(az<0.){
0964 az+=pi2;
0965 rot+=pi2;
0966 }
0967
0968// obtain *ang as angle between 2 vectors with the same declination
0969 d.x=cos(de)*cos(az);
0970 d.y=cos(de)*sin(az);
0971 d.z=sin(de);
0972 *ang=acos(d.x*d.x-d.y*d.y+d.z*d.z); // d and another vector symmet with x-xis
0973
0974// now resume calculating z' axis
0975 s.x=-rot; // azimuth -rot
0976 s.y=-atan(tan(inc)*cos(az)); // elevation
0977 ToCartesian3d(&s,&d);
0978
0979 m->c[0][2]=d.x;
0980 m->c[1][2]=d.y;
0981 m->c[2][2]=d.z;
0982
0983// y' axis as a vector product of z' and x'
0984 m->c[0][1]=m->c[1][2]*m->c[2][0] -m->c[2][2]*m->c[1][0];
0985 m->c[1][1]=m->c[2][2]*m->c[0][0] -m->c[0][2]*m->c[2][0];
0986 m->c[2][1]=m->c[0][2]*m->c[1][0] -m->c[1][2]*m->c[0][0];
0987
0988
0989}
0990
0991
0992/**************************************************************************
0993 SurfthePolarTAM
0994
0995 Make a coordinate system which converts two dimensional surface points
0996 to two dimensional polar coordinates on the viewer's sphere
0997
0998 s surface coordinate
0999 p polar coordinate ( usually 'thePolar' )
1000
1001 sp TransAngleMatrix where the surface is the body,
1002 and the polar is the frame
1003**************************************************************************/
1004void SurfthePolarTAM(TransAngleEuler *s,TransAngleMatrix *p,TransAngleMatrix *sp)
1005{
1006 TransAngleMatrix m;
1007 TransAngleEuler e,d;
1008
1009 InverseTAM(p,&m); // inverse of thePolar
1010 TAMtoTAE(&m,&e);
1011 MoveTwiceTAE(&e,s,&d);
1012 TAEtoTAM(&d,sp);
1013}
1014
1015/**************************************************************************
1016 SurfToPolar
1017
1018 Converts a two dimensional surface point
1019 to a two dimensional polar coordinate on the viewer's sphere
1020
1021 s surface coordinate (x,y)
1022 sp TransAngleMatrix where the surface is the body,
1023 and the polar is the frame
1024
1025 p polar coordinate (alpha,delta)
1026**************************************************************************/
1027void SurfToPolar(Point2d *s,TransAngleMatrix *sp,Point2d *p)
1028{
1029 Point3d a;
1030 double d;
1031
1032 a.x=sp->x+sp->c[0][0]*s->x+sp->c[0][1]*s->y;
1033 a.y=sp->y+sp->c[1][0]*s->x+sp->c[1][1]*s->y;
1034 a.z=sp->z+sp->c[2][0]*s->x+sp->c[2][1]*s->y;
1035
1036 d=sqrt(a.x*a.x+a.y*a.y);
1037 p->y=atan2(a.z,d);
1038 if(d==0.){
1039 p->x=0.;
1040 return;
1041 }
1042 p->x=atan2(a.y,a.x);
1043}
1044
1045/**************************************************************************
1046 PolarToSurf
1047
1048 Converts a two dimensional polar coordinate on the viewer's sphere
1049 to a two dimensional surface point
1050
1051 p polar coordinate (alpha,delta)
1052 sp TransAngleMatrix where the surface is the body,
1053 and the polar is the frame
1054
1055 s surface coordinate (x,y)
1056**************************************************************************/
1057void PolarToSurf(Point2d *p,TransAngleMatrix *sp,Point2d *s)
1058{
1059 Point3d a,b,c;
1060 double d;
1061
1062 a.x=cos(p->y)*cos(p->x);
1063 a.y=cos(p->y)*sin(p->x);
1064 a.z=sin(p->y);
1065 FrameToBodyTAM(&a,sp,&b);
1066 c.x=0.;
1067 c.y=0.;
1068 c.z=0.;
1069 FrameToBodyTAM(&c,sp,&a);
1070 s->x=(b.z*a.x-a.z*b.x)/(b.z-a.z);
1071 s->y=(b.z*a.y-a.z*b.y)/(b.z-a.z);
1072}
1073
1074/**************************************************************************
1075 CrossClipSide
1076
1077 Tests whether an arc given by 2 points in plolar coordinate
1078 intersects a side of the clipping rect, returnes if the arc
1079 lies outside of the clipping arc, if the arc could be inside
1080 of the clipping arc, or if the arc intersects the clipping arc.
1081 In the lattermost case, polar coordinate of the intersecting
1082 point will be returned as the last parameter.
1083
1084 frame coordinate system where a side of the clipping rect
1085 lies on the x-y plane, and x-axis corresponds to the
1086 start point of the side
1087 arc angle between the starting point and the end point
1088 of the arc corresponding to the clipping side
1089 p1 starting point the arc to be tested
1090 p2 end point the arc to be tested
1091 p crossing point
1092
1093 returned int 0 - could be inside of the clipping rect
1094 1 - intersect (p1 out, p2 in)
1095 2 - intersect (p1 in, p2 out)
1096 3 - outside
1097
1098**************************************************************************/
1099int CrossClipSide(AngleMatrix *frame,double arc,Point2d *p1,
1100 Point2d *p2,Point2d *p,double *ang)
1101{
1102 Point2d p3,p4,p8;
1103 Point3d p5,p6,p7;
1104
1105 FrameToBodyAMP(p1,frame,&p3);
1106 FrameToBodyAMP(p2,frame,&p4);
1107 if(p3.y>0.&&p4.y>0.) return 3; // outside
1108 if(p3.y<0.&&p4.y<0.) return 0;
1109 ToCartesian3d(&p3,&p5);
1110 ToCartesian3d(&p4,&p6);
1111
1112 VectorProduct(&p5,&p6,&p7);
1113
1114 ToPolar3d(&p7,&p8);
1115 if(p3.y>0.) p8.x-=1.5707963268; // p1 out, p2 in
1116 else p8.x+=1.5707963268;
1117
1118 if(p8.x>6.2831853072) p8.x-=6.2831853072;
1119 if(p8.x<0.) p8.x+=6.2831853072;
1120
1121 if(p8.x>arc) return 0; // could intersect with another side
1122
1123 p8.y=0.;
1124 BodyToFrameAMP(&p8,frame,p);
1125 *ang=p8.x; // angle from the corner
1126 if(p3.y>0.) return 1;
1127 else return 2;
1128}
1129
1130/****************************************************************************
1131 compactFarNear
1132
1133****************************************************************************/
1134/*
1135void compactFarNear(StartPTE *farNear,int *n)
1136{
1137 StartPTE a;
1138 int i,j;
1139 bool recursFarNear(StartPTE *farNear,int n,int p1,int p2,int depth);
1140
1141 for(i=0;i<*n;i++){
1142 a=farNear[i];
1143 farNear[i].start=-1;
1144 farNear[i].pte=-1;
1145 if(recursFarNear(farNear,*n,a.start,a.pte,0)){
1146 for(j=i+1;j<*n;j++) farNear[j-1]=farNear[j];
1147 *n--;
1148 }
1149 else farNear[i]=a;
1150 }
1151}
1152*/
1153
1154
1155
1156/****************************************************************************
1157 inferFarNear
1158
1159 Given a set of far-near relationships, infers the far-near relationship
1160 of a given pair of items
1161
1162 farNear array of far-near relationhips - start:far, pte:near
1163 n number of data in array farNear
1164 p1 the first item
1165 p2 the second item
1166
1167 *near either p1 or p2, which is nearer
1168 returned if false, the relationship cannot be inferred
1169*****************************************************************************/
1170bool inferFarNear(StartPTE *farNear,long n,int p1,int p2,int *near)
1171{
1172
1173// test if p1 is farther
1174 *near=p2;
1175 if(recursFarNear(farNear,n,p1,p2,0)) return true;
1176// test if p2 is farther
1177 *near=p1;
1178 if(recursFarNear(farNear,n,p2,p1,0)) return true;
1179 return false;
1180}
1181
1182/****************************************************************************
1183 recursFarNear
1184
1185 finds recursively a path to the destination
1186
1187 farNear array of far-near relationhips - start:far, pte:near
1188 n number of data in array farNear
1189 p1 the first item
1190 p2 the second item
1191 depth depth of recursion - detect a loop condition and abort
1192
1193 returned if false, the relationship cannot be inferred
1194*****************************************************************************/
1195bool recursFarNear(StartPTE *farNear,long n,int p1,int p2,long depth)
1196{
1197 long i;
1198
1199 depth++;
1200 if(depth>n) return false; // escape loop
1201
1202 for(i=0;i<n;i++){
1203 if(farNear[i].start==p1){
1204 if(farNear[i].pte==p2) return true;
1205 if(recursFarNear(farNear,n,farNear[i].pte,p2,depth)) return true;
1206 }
1207 }
1208 return false;
1209}
1210
1211/****************************************************************************
1212 Overlap
1213
1214 tests if the given two surfaces are seen overlapped
1215
1216 p point array
1217 s1 indeces of points for a surface 1
1218 s2 indices of points for a surface 2
1219
1220 p0 typical overlapped point
1221
1222 returned if false, the surfaces are disjoint
1223*****************************************************************************/
1224bool Overlap(Point2d *p,StartPTE *s1,StartPTE *s2,Point2d *p0)
1225{
1226 Point2d s1max,s1min;
1227 Point2d s2max,s2min;
1228 int i,j;
1229 double x1,y1,x2,y2,x3,y3,x4,y4;
1230 double a,b,d;
1231 double l1,l2;
1232
1233// for(i=s1->start;i<s1->pte;i++){
1234// sprintf(msg,"Overlap s1 i= %3i p[i].x=%8.3f p[i].y=%8.3f\n",i,p[i].x,p[i].y);
1235// outstr(msg);
1236//}
1237//flush();
1238//for(i=s2->start;i<s2->pte;i++){
1239// sprintf(msg,"Overlap s2 i= %3i p[i].x=%8.3f p[i].y=%8.3f\n",i,p[i].x,p[i].y);
1240// outstr(msg);
1241//}
1242//flush();
1243
1244 s1max.x=-10000.;
1245 s1max.y=-10000.;
1246 s1min.x=10000.;
1247 s1min.y=10000.;
1248
1249 for(i=s1->start;i<s1->pte;i++){
1250 if(p[i].x>s1max.x) s1max.x=p[i].x;
1251 if(p[i].y>s1max.y) s1max.y=p[i].y;
1252 if(p[i].x<s1min.x) s1min.x=p[i].x;
1253 if(p[i].y<s1min.y) s1min.y=p[i].y;
1254 }
1255
1256 s2max.x=-10000.;
1257 s2max.y=-10000.;
1258 s2min.x=10000.;
1259 s2min.y=10000.;
1260
1261 for(i=s2->start;i<s2->pte;i++){
1262 if(p[i].x>s2max.x) s2max.x=p[i].x;
1263 if(p[i].y>s2max.y) s2max.y=p[i].y;
1264 if(p[i].x<s2min.x) s2min.x=p[i].x;
1265 if(p[i].y<s2min.y) s2min.y=p[i].y;
1266 }
1267
1268 if(s1max.x<s2min.x) return false;
1269 if(s2max.x<s1min.x) return false;
1270 if(s1max.y<s2min.y) return false;
1271 if(s2max.y<s1min.y) return false;
1272
1273 for(i=s1->start;i<s1->pte;i++){
1274 x1=p[i].x;
1275 y1=p[i].y;
1276 if(i==s1->pte-1){
1277 if(s1->pte-s1->start==2) break;
1278 x2=p[s1->start].x;
1279 y2=p[s1->start].y;
1280 }
1281 else{
1282 x2=p[i+1].x;
1283 y2=p[i+1].y;
1284 }
1285 l1=(x2-x1)*(x2-x1)+(y2-y1)*(y2-y1); // approximate l1*l2 by this
1286 for(j=s2->start;j<s2->pte;j++){
1287 x3=p[j].x;
1288 y3=p[j].y;
1289 if(j==s2->pte-1){
1290 if(s2->pte-s2->start==2) break;
1291 x4=p[s2->start].x;
1292 y4=p[s2->start].y;
1293 }
1294 else{
1295 x4=p[j+1].x;
1296 y4=p[j+1].y;
1297 }
1298// detect simply disjoint cases
1299 while(1){
1300 if(x1<=x2){
1301 if(x3<=x4){
1302 if(x2<x3) break;
1303 if(x4<x1) break;
1304 }
1305 else{
1306 if(x2<x4) break;
1307 if(x3<x1) break;
1308 }
1309 }
1310 else{
1311 if(x3<=x4){
1312 if(x1<x3) break;
1313 if(x4<x2) break;
1314 }
1315 else{
1316 if(x1<x4) break;
1317 if(x3<x2) break;
1318 }
1319 }
1320 if(y1<=y2){
1321 if(y3<=y4){
1322 if(y2<y3) break;
1323 if(y4<y1) break;
1324 }
1325 else{
1326 if(y2<y4) break;
1327 if(y3<y1) break;
1328 }
1329 }
1330 else{
1331 if(y3<=y4){
1332 if(y1<y3) break;
1333 if(y4<y2) break;
1334 }
1335 else{
1336 if(y1<y4) break;
1337 if(y3<y2) break;
1338 }
1339 }
1340// sprintf(msg,"Overlap x1=%8.3f y1=%8.3f x2=%8.3f y2=%8.3f x3=%8.3f y3=%8.3f x4=%8.3f y4=%8.3f \n",x1,y1,x2,y2,x3,y3,x4,y4);
1341// outstr(msg);
1342// flush();
1343 d=(x2-x1)*(y3-y4)-(x3-x4)*(y2-y1);
1344 if(fabs(d/l1)<0.001) break;
1345 a=((x3-x1)*(y3-y4)-(x3-x4)*(y3-y1))/d;
1346 if(a<-0.01||a>1.01) break;
1347 b=((x2-x1)*(y3-y1)-(x3-x1)*(y2-y1))/d;
1348 if(b<-0.01||b>1.01) break;
1349 if(a<0.01&&b<0.01) break;
1350 if(a<0.01&&b>0.99) break;
1351 if(a>0.99&&b<0.01) break;
1352 if(a>0.99&&b>0.99) break;
1353 p0->x=x1+(x2-x1)*a;
1354 p0->y=y1+(y2-y1)*a;
1355 return true;
1356 }
1357 }
1358 }
1359// test which bounding rect encloses the other bounding rect
1360 if((s1max.x-s1min.x)<=(s2max.x-s2min.x)&&
1361 (s1max.y-s1min.y)<=(s2max.y-s2min.y)){
1362 i=s1->start;
1363 x1=0.5*(p[i].x+p[i+2].x);
1364 y1=0.5*(p[i].y+p[i+2].y);
1365 d=0.;
1366 for(j=s2->start;j<s2->pte;j++){
1367 x3=p[j].x;
1368 y3=p[j].y;
1369 if(j==s2->pte-1){
1370 if(s2->pte-s2->start==2) break;
1371 x4=p[s2->start].x;
1372 y4=p[s2->start].y;
1373 }
1374 else{
1375 x4=p[j+1].x;
1376 y4=p[j+1].y;
1377 }
1378 d+=ArgDif(x1,y1,x3,y3,x4,y4);
1379 }
1380 if(d>6.){
1381 p0->x=x1;
1382 p0->y=y1;
1383 return true;
1384 }
1385
1386 }
1387
1388 if((s1max.x-s1min.x)>=(s2max.x-s2min.x)&&
1389 (s1max.y-s1min.y)>=(s2max.y-s2min.y)){
1390 i=s2->start;
1391 x1=0.5*(p[i].x+p[i+2].x);
1392 y1=0.5*(p[i].y+p[i+2].y);
1393 d=0.;
1394 for(j=s1->start;j<s1->pte;j++){
1395 x3=p[j].x;
1396 y3=p[j].y;
1397 if(j==s1->pte-1){
1398 if(s1->pte-s1->start==2) break;
1399 x4=p[s1->start].x;
1400 y4=p[s1->start].y;
1401 }
1402 else{
1403 x4=p[j+1].x;
1404 y4=p[j+1].y;
1405 }
1406 d+=ArgDif(x1,y1,x3,y3,x4,y4);
1407 }
1408 if(d>6.){
1409 p0->x=x1;
1410 p0->y=y1;
1411 return true;
1412 }
1413 }
1414 return false;
1415}
1416
1417/****************************************************************************
1418 FarNearDist
1419
1420 distance separating surface1 and surface2 for a given direction of view
1421
1422 s1 coordinate conversion (surface:body view:frame)
1423 s2 coordinate conversion (surface:body view:frame)
1424 p polar coordinate of the line of sight
1425
1426 d distance to s1 minus distance to s2
1427*****************************************************************************/
1428void FarNearDist(TransAngleMatrix *s1,TransAngleMatrix *s2,Point2d *p,double *d)
1429{
1430 Point2d srf1,srf2;
1431 Point3d a;
1432
1433 PolarToSurf(p,s1,&srf1);
1434 PolarToSurf(p,s2,&srf2);
1435
1436 a.x=s1->x+s1->c[0][0]*srf1.x+s1->c[0][1]*srf1.y;
1437 a.y=s1->y+s1->c[1][0]*srf1.x+s1->c[1][1]*srf1.y;
1438 a.z=s1->z+s1->c[2][0]*srf1.x+s1->c[2][1]*srf1.y;
1439
1440 *d=sqrt(a.x*a.x+a.y*a.y+a.z*a.z);
1441
1442 a.x=s2->x+s2->c[0][0]*srf2.x+s2->c[0][1]*srf2.y;
1443 a.y=s2->y+s2->c[1][0]*srf2.x+s2->c[1][1]*srf2.y;
1444 a.z=s2->z+s2->c[2][0]*srf2.x+s2->c[2][1]*srf2.y;
1445
1446 *d-=sqrt(a.x*a.x+a.y*a.y+a.z*a.z);
1447
1448}
1449
1450/****************************************************************************
1451 PolarToGnomo
1452
1453 Polar to Gnomonic Conversion
1454
1455 p polar coordinate (x:aximuth, y:elevation)
1456 g gnomonic coordinate (x:toward polar y, y:toward polar x)
1457*****************************************************************************/
1458void PolarToGnomo(Point2d *p,Point2d *g)
1459{
1460 double r;
1461
1462 r=tan(1.5707963267-p->y);
1463 g->x=r*sin(p->x);
1464 g->y=r*cos(p->x);
1465}
1466
1467
1468/****************************************************************************
1469 GnomoToPolar
1470
1471 Gnomonic to Polar Conversion
1472
1473 g gnomonic coordinate (x:toward polar y, y:toward polar x)
1474 p polar coordinate (x:aximuth, y:elevation)
1475*****************************************************************************/
1476void GnomoToPolar(Point2d *g,Point2d *p)
1477{
1478 double r;
1479
1480 r=sqrt(g->x*g->x+g->y*g->y);
1481 p->y=1.5707963267-atan(r);
1482 p->x=atan2(g->x,g->y);
1483}
1484
1485
1486/****************************************************************************
1487 ArgDif
1488
1489 argument difference between two vectors
1490
1491 (x0,y0) origin
1492 (x1,y1) vector1 tip
1493 (x2,y2) vector2 tip
1494
1495 returned argument of vector2 - argument of vector1
1496*****************************************************************************/
1497double ArgDif(double x0,double y0,double x1,double y1,double x2,double y2)
1498{
1499 double ang;
1500
1501 ang=atan2(y2-y0,x2-x0)-atan2(y1-y0,x1-x0);
1502 if(fabs(ang)<3.141592) return ang;
1503 if(ang<0.) ang+=6.283185;
1504 else ang-=6.283185;
1505 return ang;
1506}
1507
1508
1509/****************************************************************************
1510 SameSideOfSurface
1511
1512 Return true if the Two Points are in the Same Side of a Surface
1513
1514 s Surface
1515 p1 point 1
1516 p2 point 2
1517
1518 returned true if point 1 and 2 are in the same side of s
1519*****************************************************************************/
1520bool SameSideOfSurface(Surface *s,Point3d *p1,Point3d *p2)
1521{
1522 Point3d z,d1,d2;
1523
1524 z.x=sin(s->theta0)*sin(s->phi0); // axis normal to the plane
1525 z.y=-sin(s->theta0)*cos(s->phi0);
1526 z.z=cos(s->theta0);
1527 d1.x=p1->x-s->x0;
1528 d1.y=p1->y-s->y0;
1529 d1.z=p1->z-s->z0;
1530 d2.x=p2->x-s->x0;
1531 d2.y=p2->y-s->y0;
1532 d2.z=p2->z-s->z0;
1533 if((d1.x*z.x+d1.y*z.y+d1.z*z.z)*(d2.x*z.x+d2.y*z.y+d2.z*z.z)>0.)
1534 return true;
1535 else return false;
1536}
1537
1538
1539
1540/******************************************************************************
1541 CMPpart
1542
1543 part of the concept
1544 ******************************************************************************/
1545void CMPpart::setCood(double x,double y,double z,double theta,double phi,double psi)
1546{
1547 x0=x; /* local coordinate referred to */
1548 y0=y; /* refPart */
1549 z0=z;
1550 theta0=theta;
1551 phi0=phi;
1552 psi0=psi;
1553}
1554
1555void CMPpart::setColor(int i)
1556{
1557 color=i;
1558}
1559
1560void CMPpart::hide()
1561{
1562 showDrawing=false;
1563}
1564
1565
1566/******************************************************************************
1567 CMPpoint
1568
1569 part of the concept - point
1570 ******************************************************************************/
1571
1572
1573/******************************************************************************
1574 CMPlines
1575
1576 part of the concept - lines
1577 ******************************************************************************/
1578
1579
1580void CMPlines::SetPoint(double x,double y,double z)
1581{
1582 Point3d p;
1583 p.x=x;
1584 p.y=y;
1585 p.z=z;
1586 pt.push_back(p);
1587 npoint++;
1588}
1589
1590
1591/******************************************************************************
1592 GetSurface
1593
1594 Return a line segment
1595
1596 ******************************************************************************/
1597
1598void CMPlines::GetSurface(int m,TransAngleMatrix *po,Surface *surf,Point2d *surfPt)
1599{
1600 double x1,y1,z1,x2,y2,z2;
1601 TransAngleEuler tae1;
1602 TransAngleMatrix tam1;
1603 TransAngleEuler tae2;
1604 TransAngleEuler tae;
1605 Point3d s;
1606 Point3d e;
1607 Point3d f;
1608 Point3d g;
1609 double length,d;
1610 Point2d a;
1611
1612 s=pt[m];
1613 e.x=pt[m+1].x-s.x;
1614 e.y=pt[m+1].y-s.y;
1615 e.z=pt[m+1].z-s.z;
1616
1617 if(e.x==0.&&e.y==0.&&e.z==0.){
1618 surf->start=-1;
1619 surf->pte=-1;
1620 return;
1621 }
1622
1623 tae1.x=xg; // Translate Angle Euler for the part
1624 tae1.y=yg;
1625 tae1.z=zg;
1626 tae1.theta=thetag;
1627 tae1.phi=phig;
1628 tae1.psi=psig;
1629 TAEtoTAM(&tae1,&tam1);
1630 FrameToBodyTAM((Point3d *)po,&tam1,&g); // viewer by part body coordinate
1631 g.x-=s.x; // direction to the viewer
1632 g.y-=s.y;
1633 g.z-=s.z;
1634 NormPoint3d(&g,&g,&d);
1635 tam1.x=s.x; // Translate Angle Matrix for the line segment
1636 tam1.y=s.y;
1637 tam1.z=s.z;
1638 NormPoint3d(&e,&f,&length); // normalized y-axis
1639 if(fabs(f.x*g.x+f.y*g.y+f.z*g.z)>0.99996){ // if the line is directed to viewer
1640 ToPolar3d(&g,&a); // viewer's direction
1641 a.y-=0.0174532; // Offset 1 deg from the direction to the viewer
1642 f.x=cos(a.y)*cos(a.x); // updated line direction to avoid anomaly
1643 f.y=cos(a.y)*sin(a.x);
1644 f.z=sin(a.y);
1645 }
1646 tam1.c[0][1]=f.x;
1647 tam1.c[1][1]=f.y;
1648 tam1.c[2][1]=f.z;
1649 VectorProduct(&f,&g,&s); // direction to x-axis
1650 NormPoint3d(&s,&s,&d);
1651 tam1.c[0][0]=s.x;
1652 tam1.c[1][0]=s.y;
1653 tam1.c[2][0]=s.z;
1654 VectorProduct(&s,&f,&g); // direction to z-axis
1655 tam1.c[0][2]=g.x;
1656 tam1.c[1][2]=g.y;
1657 tam1.c[2][2]=g.z;
1658 TAMtoTAE(&tam1,&tae2);
1659 MoveTwiceTAE(&tae1,&tae2,&tae);
1660 surf->x0=tae.x;
1661 surf->y0=tae.y;
1662 surf->z0=tae.z;
1663 surf->theta0=tae.theta;
1664 surf->phi0=tae.phi;
1665 surf->psi0=tae.psi;
1666 surf->start=0;
1667 surf->pte=2;
1668 surfPt[0].x=0.;
1669 surfPt[0].y=0.;
1670 surfPt[1].x=0.;
1671 surfPt[1].y=length;
1672}
1673
1674/******************************************************************************
1675 GetDrawData
1676
1677 Return data for drawing
1678
1679 m line segment number (0-)
1680 po thePolar
1681
1682 seg array of line segments (clockwise angle from top, angle from line of sight)
1683 blk indices of segs which are to be drawn with lines
1684 cont indices of closed line segments. Inside of the closure
1685 will be painted by a color.
1686
1687 ******************************************************************************/
1688
1689void CMPlines::GetDrawData(int m,TransAngleMatrix *po,
1690 Line2d *seg,StartPTE *blk,StartPTE *cont)
1691{
1692 TransAngleEuler tae;
1693 TransAngleMatrix tam;
1694 Point3d s;
1695 Point3d e;
1696 Point3d f;
1697 double r;
1698 Point2d a;
1699
1700 s=pt[m];
1701 e=pt[m+1];
1702
1703 tae.x=xg; // Translate Angle Euler for the part
1704 tae.y=yg;
1705 tae.z=zg;
1706 tae.theta=thetag;
1707 tae.phi=phig;
1708 tae.psi=psig;
1709 TAEtoTAM(&tae,&tam);
1710 BodyToFrameTAM(&s,&tam,&f);
1711 FrameToBodyTAM(&f,po,&s);
1712 NormPoint3d(&s,&f,&r);
1713 ToPolar3d(&f,&a);
1714 a.y=1.5707963267-a.y;
1715 seg[0].sx=a.x;
1716 seg[0].sy=a.y;
1717 BodyToFrameTAM(&e,&tam,&f);
1718 FrameToBodyTAM(&f,po,&e);
1719 NormPoint3d(&e,&f,&r);
1720 ToPolar3d(&f,&a);
1721 a.y=1.5707963267-a.y;
1722 seg[0].ex=a.x;
1723 seg[0].ey=a.y;
1724 blk->start=0;
1725 blk->pte=1;
1726 cont->start=0;
1727 cont->pte=1;
1728
1729}
1730
1731
1732/******************************************************************************
1733 FieldOfView
1734
1735 Get horizontal and vertical field of view to cover the object
1736 ******************************************************************************/
1737
1738void CMPlines::FieldOfView(TransAngleMatrix *vu,Point2d *az,Point2d *el)
1739{
1740 TransAngleEuler tae;
1741 TransAngleMatrix tam;
1742 int i;
1743 Point3d p;
1744 Point3d s;
1745 Point3d f;
1746 Point2d a;
1747 double d;
1748
1749 tae.x=xg; // Translate Angle Euler for the part
1750 tae.y=yg;
1751 tae.z=zg;
1752 tae.theta=thetag;
1753 tae.phi=phig;
1754 tae.psi=psig;
1755 TAEtoTAM(&tae,&tam);
1756 az->x=10.;
1757 az->y=-10.;
1758 el->x=10.;
1759 el->y=-10.;
1760 for(i=0;i<npoint;i++){
1761 s=pt[i];
1762 BodyToFrameTAM(&s,&tam,&f);
1763 FrameToBodyTAM(&f,vu,&s);
1764 NormPoint3d(&s,&s,&d);
1765 ToPolar3d(&s,&a);
1766 if(a.x<az->x) az->x=a.x;
1767 if(a.x>az->y) az->y=a.x;
1768 if(a.y<el->x) el->x=a.y;
1769 if(a.y>el->y) el->y=a.y;
1770 }
1771}
1772
1773/******************************************************************************
1774 CMPplane
1775
1776 part of the concept - plane
1777 ******************************************************************************/
1778
1779void CMPplane::SetPoint(double x,double y)
1780{
1781 Point2d p;
1782 p.x=x;
1783 p.y=y;
1784 pt.push_back(p);
1785 npoint++;
1786}
1787
1788/******************************************************************************
1789 GetSurface
1790
1791 Return the surface data
1792
1793 The surface is assumed to be given by points that are arranged to
1794 move clock-wise looking toward the z vector.
1795
1796 Both sides of the surface are assumed to be visible. Therefore,
1797 the order of the point array is reversed, when the viewer's eye
1798 is in the -z side.
1799
1800 ******************************************************************************/
1801
1802void CMPplane::GetSurface(TransAngleMatrix *po,Surface *surf,Point2d *surfPt)
1803{
1804 TransAngleMatrix tam;
1805 Point3d d;
1806 int i,j;
1807 int np;
1808 double x1,y1;
1809
1810 surf->x0=xg;
1811 surf->y0=yg;
1812 surf->z0=zg;
1813 surf->theta0=thetag;
1814 surf->phi0=phig;
1815 surf->psi0=psig;
1816 surf->start=0;
1817 TAEtoTAM((TransAngleEuler *)surf,&tam);
1818 FrameToBodyTAM((Point3d *)po,&tam,&d); // if d>0 +z axis is facing the viewer
1819 np=0; // number of effective points
1820 if(d.z>0.){
1821 x1=pt[npoint-1].x;
1822 y1=pt[npoint-1].y;
1823 }
1824 else{
1825 x1=pt[0].x;
1826 y1=pt[0].y;
1827 }
1828 for(i=0;i<npoint;i++){
1829 if(d.z>0.) j=i;
1830 else j=npoint-i-1;
1831 surfPt[np].x=pt[j].x;
1832 surfPt[np].y=pt[j].y;
1833 if(x1!=surfPt[np].x||y1!=surfPt[np].y){
1834 x1=surfPt[np].x;
1835 y1=surfPt[np].y;
1836 if(np<47) np++; // taking into some allowance to max 50
1837 }
1838 }
1839 surf->pte=np;
1840}
1841
1842
1843/******************************************************************************
1844 GetDrawData
1845
1846 Return data for drawing
1847
1848 po thePolar
1849
1850 seg array of line segments (clockwise angle from top, angle from line of sight)
1851 blk indices of segs which are to be drawn with lines
1852 cont indices of closed line segments. Inside of the closure
1853 will be painted by a color.
1854
1855 ******************************************************************************/
1856
1857void CMPplane::GetDrawData(TransAngleMatrix *po,Line2d *seg,StartPTE *blk,StartPTE *cont)
1858{
1859 StartPTE blk1;
1860 StartPTE cont1;
1861 int i,j;
1862 double sx1,sy1,ex1,ey1;
1863 CMPplane *next;
1864
1865 cont->start=0;
1866 GetDrawData1(po,seg,blk,cont);
1867 if(otherHalf){
1868 bool face1,face2;
1869 face1=FacingViewer((Point3d *)po);
1870 next=otherHalf;
1871 while(next!=this){
1872 cont1.start=blk->pte;
1873 next->GetDrawData1(po,seg,&blk1,&cont1);
1874 face2=next->FacingViewer((Point3d *)po);
1875 for(i=blk->start;i<cont->pte;i++){
1876 if(face1==face2){
1877 sx1=seg[i].sx;
1878 sy1=seg[i].sy;
1879 ex1=seg[i].ex;
1880 ey1=seg[i].ey;
1881 }
1882 else{
1883 sx1=seg[i].ex;
1884 sy1=seg[i].ey;
1885 ex1=seg[i].sx;
1886 ey1=seg[i].sy;
1887 }
1888 for(j=cont1.start;j<cont1.pte;j++){
1889 double d0,d1,d2,d3;
1890 d0=seg[j].sx-ex1;
1891 d1=seg[j].sy-ey1;
1892 d2=seg[j].ex-sx1;
1893 d3=seg[j].ey-sy1;
1894 if(d0*d0+d1*d1+d2*d2+d3*d3<0.000001) break;
1895// if(seg[j].sx==ex1&&seg[j].sy==ey1&&seg[j].ex==sx1&&seg[j].ey==sy1)
1896// break;
1897 }
1898 if(j!=cont1.pte){
1899 for(j=i;j>0;j--) seg[j]=seg[j-1];
1900 seg[0].sx=sx1;
1901 seg[0].sy=sy1;
1902 seg[0].ex=ex1;
1903 seg[0].ey=ey1;
1904 blk->start++;
1905 }
1906 }
1907 next=next->otherHalf;
1908 }
1909 }
1910
1911
1912}
1913
1914void CMPplane::GetDrawData1(TransAngleMatrix *po,Line2d *seg,StartPTE *blk,StartPTE *cont)
1915{
1916 TransAngleEuler tae;
1917 TransAngleMatrix tam;
1918 Point3d a,b,c,d;
1919 Point2d e,f,g;
1920 double r;
1921 int i,j,k;
1922
1923 k=cont->start;
1924 tae.x=xg; // Translate Angle Euler for the part
1925 tae.y=yg;
1926 tae.z=zg;
1927 tae.theta=thetag;
1928 tae.phi=phig;
1929 tae.psi=psig;
1930 TAEtoTAM(&tae,&tam);
1931 a.x=pt[0].x;
1932 a.y=pt[0].y;
1933 a.z=0.;
1934 BodyToFrameTAM(&a,&tam,&b);
1935 FrameToBodyTAM(&b,po,&c);
1936 NormPoint3d(&c,&d,&r);
1937 ToPolar3d(&d,&e);
1938 e.y=1.5707963267-e.y;
1939 f=e;
1940 for(i=1;i<=npoint;i++){
1941 g=f;
1942 if(i<npoint){
1943 a.x=pt[i].x;
1944 a.y=pt[i].y;
1945 a.z=0.;
1946 BodyToFrameTAM(&a,&tam,&b);
1947 FrameToBodyTAM(&b,po,&c);
1948 NormPoint3d(&c,&d,&r);
1949 ToPolar3d(&d,&f);
1950 f.y=1.5707963267-f.y;
1951 }
1952 else f=e;
1953 seg[k].sx=g.x;
1954 seg[k].sy=g.y;
1955 seg[k].ex=f.x;
1956 seg[k].ey=f.y;
1957 k++;
1958
1959 }
1960 blk->start=cont->start;
1961 blk->pte=blk->start+npoint;
1962 cont->pte=cont->start+npoint;
1963}
1964
1965/******************************************************************************
1966 FieldOfView
1967
1968 Get horizontal and vertical field of view to cover the object
1969 ******************************************************************************/
1970
1971void CMPplane::FieldOfView(TransAngleMatrix *vu,Point2d *az,Point2d *el)
1972{
1973 TransAngleEuler tae;
1974 TransAngleMatrix tam;
1975 int i;
1976 Point2d p;
1977 Point3d s;
1978 Point3d f;
1979 Point2d a;
1980 double d;
1981
1982 tae.x=xg; // Translate Angle Euler for the part
1983 tae.y=yg;
1984 tae.z=zg;
1985 tae.theta=thetag;
1986 tae.phi=phig;
1987 tae.psi=psig;
1988 TAEtoTAM(&tae,&tam);
1989 az->x=10.;
1990 az->y=-10.;
1991 el->x=10.;
1992 el->y=-10.;
1993 for(i=0;i<npoint;i++){
1994 s.x=pt[i].x;
1995 s.y=pt[i].y;
1996 s.z=0.;
1997 BodyToFrameTAM(&s,&tam,&f);
1998 FrameToBodyTAM(&f,vu,&s);
1999 NormPoint3d(&s,&s,&d);
2000 ToPolar3d(&s,&a);
2001 if(a.x<az->x) az->x=a.x;
2002 if(a.x>az->y) az->y=a.x;
2003 if(a.y<el->x) el->x=a.y;
2004 if(a.y>el->y) el->y=a.y;
2005 }
2006}
2007
2008/*****************************************************************
2009 Return if the surface is facing the viewer
2010
2011 po thePolar casted to Point3d
2012*****************************************************************/
2013bool CMPplane::FacingViewer(Point3d *po)
2014{
2015 double nx,ny,nz;
2016
2017 ny=sin(thetag);
2018 nx=ny*cos(phig-1.5707963268);
2019 ny*=sin(phig-1.5707963268);
2020 nz=cos(thetag);
2021 return (po->x-xg)*nx+(po->y-yg)*ny+(po->z-zg)*nz>0.;
2022}
2023
2024
2025/******************************************************************************
2026 CMPbox
2027
2028 ******************************************************************************/
2029
2030void CMPbox::SetSize(double x,double y,double z)
2031{
2032 lx=x;
2033 ly=y;
2034 lz=z;
2035}
2036
2037
2038/******************************************************************************
2039 ClearVisSurface
2040
2041 Clear 'visibleSurface' instance variable so that new visibility
2042 from a new viewpoint can be obtained in GetSurface
2043 ******************************************************************************/
2044void CMPbox::ClearVisSurface(void)
2045{
2046 visibleSurface=0;
2047}
2048
2049/******************************************************************************
2050 GetSurface
2051
2052 Return a surface
2053
2054 Six surfaces are related to an index number in the following way
2055
2056 index face toward...
2057 0 +x
2058 1 +y
2059 2 +z
2060 3 -x
2061 4 -y
2062 5 -z
2063
2064
2065 The instance variable visibleSurface is used to map between
2066 the above index and the reference index passed as the first parameter.
2067
2068 The returned bool value is false if no more visible surface exists
2069 for the asked reference number.
2070 ******************************************************************************/
2071
2072bool CMPbox::GetSurface(int n,TransAngleMatrix *po,Surface *surf,Point2d *surfPt)
2073{
2074 double x1,y1,z1,x2,y2;
2075 TransAngleEuler tae1;
2076 TransAngleMatrix tam1;
2077 TransAngleEuler tae2;
2078 TransAngleEuler tae;
2079 Point3d p;
2080 Point3d s;
2081 Point3d e;
2082 Point3d f;
2083 double length;
2084 Point2d a;
2085 int i,j,m;
2086
2087 x1=0.5*lx;
2088 y1=0.5*ly;
2089 z1=0.5*lz;
2090 tae1.x=xg; // Translate Angle Euler for the part
2091 tae1.y=yg;
2092 tae1.z=zg;
2093 tae1.theta=thetag;
2094 tae1.phi=phig;
2095 tae1.psi=psig;
2096
2097 if(visibleSurface==0){
2098 TAEtoTAM(&tae1,&tam1);
2099 j=1;
2100 for(i=0;i<6;i++){
2101 s.x=((i>2)? -1.:1.)*((i%3==0)? x1:0.);
2102 s.y=((i>2)? -1.:1.)*((i%3==1)? y1:0.);
2103 s.z=((i>2)? -1.:1.)*((i%3==2)? z1:0.);
2104 BodyToFrameTAM(&s,&tam1,&e);
2105 if((xg-e.x)*(po->x-e.x)+(yg-e.y)*(po->y-e.y)
2106 +(zg-e.z)*(po->z-e.z)<0.) {
2107// DebugStr("\psurface");
2108 visibleSurface|=j;
2109 }
2110 j=j<<1;
2111 }
2112 }
2113 j=1;
2114 i=0;
2115 for(m=0;m<6;m++){
2116 if(visibleSurface&j){
2117 if(i==n) break;
2118 i++;
2119 }
2120 j=j<<1;
2121 }
2122 if(m==6) return false;
2123 switch (m){
2124 case 0: // +x
2125 tae2.x=x1; // Translate Angle Euler for the side
2126 tae2.y=0.;
2127 tae2.z=0.;
2128 tae2.theta=1.5707963268; // rotate 90 deg around +y axis
2129 tae2.phi=1.5707963268;
2130 tae2.psi=-1.5707963268;
2131 x2=z1;
2132 y2=y1;
2133 break;
2134 case 1: // +y
2135 tae2.x=0.; // Translate Angle Euler for the side
2136 tae2.y=y1;
2137 tae2.z=0.;
2138 tae2.theta=1.5707963268; // rotate 90 deg around -x axis
2139 tae2.phi=3.141592653589;
2140 tae2.psi=3.141592653589;
2141 x2=x1;
2142 y2=z1;
2143 break;
2144 case 2: // +z
2145 tae2.x=0.; // Translate Angle Euler for the side
2146 tae2.y=0.;
2147 tae2.z=z1;
2148 tae2.theta=0.; // no rotation
2149 tae2.phi=0.;
2150 tae2.psi=0.;
2151 x2=x1;
2152 y2=y1;
2153 break;
2154 case 3: // -x
2155 tae2.x=-x1; // Translate Angle Euler for the side
2156 tae2.y=0.;
2157 tae2.z=0.;
2158 tae2.theta=1.5707963268; // rotate 90 deg around -y axis
2159 tae2.phi=-1.5707963268;
2160 tae2.psi=1.5707963268;
2161 x2=z1;
2162 y2=y1;
2163 break;
2164 case 4: // -y
2165 tae2.x=0.; // Translate Angle Euler for the side
2166 tae2.y=-y1;
2167 tae2.z=0.;
2168 tae2.theta=1.5707963268; // rotate 90 deg around -x axis
2169 tae2.phi=0.;
2170 tae2.psi=0.;
2171 x2=x1;
2172 y2=z1;
2173 break;
2174 case 5: // -z
2175 tae2.x=0.; // Translate Angle Euler for the side
2176 tae2.y=0.;
2177 tae2.z=-z1;
2178 tae2.theta=3.1415926536; // no rotation
2179 tae2.phi=0.;
2180 tae2.psi=0.;
2181 x2=x1;
2182 y2=y1;
2183 break;
2184 }
2185 MoveTwiceTAE(&tae1,&tae2,&tae);
2186 surf->x0=tae.x;
2187 surf->y0=tae.y;
2188 surf->z0=tae.z;
2189 surf->theta0=tae.theta;
2190 surf->phi0=tae.phi;
2191 surf->psi0=tae.psi;
2192 surf->start=0;
2193 surf->pte=4;
2194 surfPt[0].x=x2;
2195 surfPt[0].y=y2;
2196 surfPt[1].x=-x2;
2197 surfPt[1].y=y2;
2198 surfPt[2].x=-x2;
2199 surfPt[2].y=-y2;
2200 surfPt[3].x=x2;
2201 surfPt[3].y=-y2;
2202 return true;
2203}
2204
2205/******************************************************************************
2206 GetCenter
2207
2208 Return the center point in frame coordinate
2209
2210 ******************************************************************************/
2211
2212void CMPbox::GetCenter(Point3d *p)
2213{
2214 p->x=xg;
2215 p->y=yg;
2216 p->z=zg;
2217}
2218
2219/******************************************************************************
2220 GetDrawData
2221
2222 Return data for drawing
2223
2224 po thePolar
2225
2226 seg array of line segments (clockwise angle from top, angle from line of sight)
2227 blk indices of segs which are to be drawn with lines
2228 cont indices of closed line segments. Inside of the closure
2229 will be painted by a color.
2230
2231 ******************************************************************************/
2232
2233void CMPbox::GetDrawData(TransAngleMatrix *po,Line2d *seg,StartPTE *blk,StartPTE *cont)
2234{
2235 double x,y,z,x1,y1,z1,x2,y2,z2,dx,dy,dz;
2236 int isen[3][10];
2237 int i,j,k,l,m,n,nsen,n1,brnch,lin,line;
2238 TransAngleEuler tae1;
2239 TransAngleMatrix tam1;
2240 TransAngleEuler tae2;
2241 TransAngleEuler tae;
2242 Point3d apex;
2243 Point3d ten[8]; /* coordinate of 8 apexes */
2244 Point3d s,d;
2245 double r;
2246 Point2d direc;
2247
2248
2249/* set conversion parameters */
2250 dx=lx;
2251 dy=ly;
2252 dz=lz;
2253
2254 tae1.x=xg; // Translate Angle Euler for the part
2255 tae1.y=yg;
2256 tae1.z=zg;
2257 tae1.theta=thetag;
2258 tae1.phi=phig;
2259 tae1.psi=psig;
2260 TAEtoTAM(&tae1,&tam1);
2261
2262/* fill coordinate of the 8 apexes */
2263/* l=0 ( 0, 0, 0) l=1 (dx, 0, 0) l=2 ( 0,dy, 0) l=3 (dx,dy, 0) */
2264/* l=4 ( 0, 0,dz) l=5 (dx, 0,dz) l=6 ( 0,dy,dz) l=7 (dx,dy,dz) */
2265 for(i=0;i<2;i++){
2266 apex.z=dz*i-0.5*dz; /* float */
2267 for(j=0;j<2;j++){
2268 apex.y=dy*j-0.5*dy; /* float */
2269 for(k=0;k<2;k++){
2270 apex.x=dx*k-0.5*dx; /* float */
2271 l=4*i+2*j+k; /* l=0-7 */
2272 BodyToFrameTAM(&apex,&tam1,&ten[l]);
2273 }
2274 }
2275 }
2276
2277/* fill line information */
2278 for(i=0;i<10;i++){
2279 for(j=0;j<3;j++) isen[j][i]=0;
2280 }
2281 nsen=0;
2282 for(i=0;i<3;i++){
2283 for(j=0;j<2;j++){
2284
2285/***********************************************************
2286 (j=0) (j=1)
2287 (i=0) k=0,l=4,m=2+4=6 0->4->6 k=1,l=3,m=7 1->3->7
2288 (i=1) k=0,l=1,m=4+1=5 0->1->5 k=2,l=6,m=7 2->6->7
2289 (i=2) k=0,l=2,m=1+2=3 0->2->3 k=4,l=5,m=7 4->5->7
2290
2291 surface i-j direction
2292 0-0 -x
2293 0-1 +x
2294 1-0 -y
2295 1-1 +y
2296 2-0 -z
2297 2-1 +z
2298***********************************************************/
2299
2300 k=(int)(j*pow(2,i));
2301 l=(int)(k+pow(2,(i+2-j)%3));
2302 m=(int)(k+pow(2,(i+1)%3)+pow(2,(i+2)%3));
2303 x1=ten[l].x-ten[k].x;
2304 y1=ten[l].y-ten[k].y;
2305 z1=ten[l].z-ten[k].z;
2306 x2=ten[m].x-ten[l].x;
2307 y2=ten[m].y-ten[l].y;
2308 z2=ten[m].z-ten[l].z;
2309
2310 if((y1*z2-z1*y2)*(po->x-ten[l].x)+(z1*x2-x1*z2)*(po->y
2311 -ten[l].y)+(x1*y2-y1*x2)*(po->z-ten[l].z)>=0.){
2312
2313/* if the plane is facing to the observer's eye do : */
2314 m=k; /* initialize present point */
2315 for(n1=0;n1<4;n1++){ // do for four sorrounding points
2316 l=m; /* define previous point */
2317
2318 /* redefine present point */
2319 if(n1==0) m=(int)(k+pow(2,(i+2-j)%3));
2320 if(n1==1) m=(int)(k+pow(2,(i+1)%3)+pow(2,(i+2)%3));
2321 if(n1==2) m=(int)(k+pow(2,(i+j+1)%3));
2322 if(n1==3) m=k;
2323 // now, the side start at point l and ends at point m
2324 if(nsen==0) brnch=1;
2325 else {
2326 brnch=1;
2327 for(n=0;n<nsen;n++){
2328 if(isen[1][n]==l && isen[0][n]==m){ // already there
2329 isen[2][n]=2; /* double line */
2330 brnch=0; // don't add this line
2331 break;
2332 }
2333 }
2334 }
2335 if(brnch==1){ // newly added line
2336 isen[0][nsen]=l; // start point
2337 isen[1][nsen]=m; // end point
2338 isen[2][nsen]=1; // single line
2339 nsen+=1;
2340 }
2341 } /* n1 */
2342 } /* facing this direction */
2343 } /* j */
2344 } /* i */
2345 blk->start=0; /* start line of a block */
2346 cont->start=0; /* start line of a contour */
2347
2348/* first, collect single lines and place in the first part */
2349 line=0;
2350 for(i=0;i<nsen;i++){
2351 if(isen[2][i]==1){ // single line
2352 for(j=0;j<2;j++){
2353 k=isen[j][i];
2354 s.x=ten[k].x;
2355 s.y=ten[k].y;
2356 s.z=ten[k].z;
2357 FrameToBodyTAM(&s,po,&d);
2358 NormPoint3d(&d,&s,&r);
2359 ToPolar3d(&s,&direc);
2360 if(j==0){
2361 seg[line].sx=direc.x;
2362 seg[line].sy=1.5707963267-direc.y;
2363 }
2364 else{
2365 seg[line].ex=direc.x;
2366 seg[line].ey=1.5707963267-direc.y;
2367 }
2368 }
2369 line+=1;
2370 }
2371 }
2372
2373 cont->pte=line; /* next-to-end line of the contour */
2374
2375/* then, collect double lines and place in the last part */
2376 for(i=0;i<nsen;i++){
2377 if(isen[2][i]==2){
2378 for(j=0;j<2;j++){
2379 k=isen[j][i];
2380 s.x=ten[k].x;
2381 s.y=ten[k].y;
2382 s.z=ten[k].z;
2383 FrameToBodyTAM(&s,po,&d);
2384 NormPoint3d(&d,&s,&r);
2385 ToPolar3d(&s,&direc);
2386 if(j==0){
2387 seg[line].sx=direc.x;
2388 seg[line].sy=1.5707963267-direc.y;
2389 }
2390 else{
2391 seg[line].ex=direc.x;
2392 seg[line].ey=1.5707963267-direc.y;
2393 }
2394 }
2395 line+=1;
2396 }
2397 }
2398 blk->pte=line; /* next-to-end line of the block */
2399}
2400
2401
2402/******************************************************************************
2403 FieldOfView
2404
2405 Get horizontal and vertical field of view to cover the object
2406 ******************************************************************************/
2407
2408void CMPbox::FieldOfView(TransAngleMatrix *vu,Point2d *az,Point2d *el)
2409{
2410 double x1,y1,z1,d;
2411 TransAngleEuler tae;
2412 TransAngleMatrix tam;
2413 int i;
2414 Point3d s;
2415 Point3d f;
2416 Point2d a;
2417
2418 x1=0.5*lx;
2419 y1=0.5*ly;
2420 z1=0.5*lz;
2421 tae.x=xg; // Translate Angle Euler for the part
2422 tae.y=yg;
2423 tae.z=zg;
2424 tae.theta=thetag;
2425 tae.phi=phig;
2426 tae.psi=psig;
2427 TAEtoTAM(&tae,&tam);
2428 az->x=10.;
2429 az->y=-10.;
2430 el->x=10.;
2431 el->y=-10.;
2432 for(i=0;i<8;i++){
2433 s.x=i&1? x1:-x1;
2434 s.y=i&2? y1:-y1;
2435 s.z=i&4? z1:-z1;
2436 BodyToFrameTAM(&s,&tam,&f);
2437 FrameToBodyTAM(&f,vu,&s);
2438 NormPoint3d(&s,&s,&d);
2439 ToPolar3d(&s,&a);
2440 if(a.x<az->x) az->x=a.x;
2441 if(a.x>az->y) az->y=a.x;
2442 if(a.y<el->x) el->x=a.y;
2443 if(a.y>el->y) el->y=a.y;
2444 }
2445}
2446
2447
2448/******************************************************************************
2449 CMPsphere - sphere
2450 ******************************************************************************/
2451
2452void CMPsphere::SetRadius(double x)
2453{
2454 r=x;
2455}
2456
2457
2458/******************************************************************************
2459 GetSurface
2460
2461 Return a line segment
2462
2463 ******************************************************************************/
2464
2465void CMPsphere::GetSurface(TransAngleMatrix *po,Surface *surf,Point2d *surfPt)
2466{
2467 Line3d eye;
2468 Point3d u;
2469 double d,ra,d1,r1;
2470 Point2d a;
2471 int i;
2472 double ang;
2473
2474 eye.sx=xg; // center of the sphere
2475 eye.sy=yg;
2476 eye.sz=zg;
2477 eye.ex=po->x; // thePolar of CThreeD - position of the viewer's eye
2478 eye.ey=po->y;
2479 eye.ez=po->z;
2480
2481 NormLine3d(&eye,&u,&d);
2482 ToPolar3d(&u,&a);
2483
2484 ra=r; // radius of the sphere
2485 d1=ra*ra/d; // distance to the center of tangent circle from sphere center
2486 r1=sqrt(ra*ra-d1*d1); // radius of the tangent circle
2487 r1*=1.03528; // r1/cos(15deg) to enable establishing the far-near
2488 // relationship with proximate parts.
2489
2490 surf->x0=xg+d1*u.x;
2491 surf->y0=yg+d1*u.y;
2492 surf->z0=zg+d1*u.z;
2493 surf->theta0=1.5707963268-a.y; // z axis toward viewer's eye
2494 surf->phi0=a.x+1.5707963268;
2495 surf->psi0=0.;
2496 surf->start=0;
2497 surf->pte=12;
2498 for(i=0;i<12;i++){
2499 ang=0.52359877559*i; // every 30 degree
2500 surfPt[i].x=r1*cos(ang);
2501 surfPt[i].y=r1*sin(ang);
2502 }
2503}
2504
2505
2506/******************************************************************************
2507 GetDrawData
2508
2509 Return data for drawing
2510
2511 po thePolar
2512
2513 seg array of line segments (clockwise angle from top, angle from line of sight)
2514 blk indices of segs which are to be drawn with lines
2515 cont indices of closed line segments. Inside of the closure
2516 will be painted by a color.
2517
2518 ******************************************************************************/
2519
2520void CMPsphere::GetDrawData(TransAngleMatrix *po,Line2d *seg,StartPTE *blk,StartPTE *cont)
2521{
2522 TransAngleEuler tae;
2523 TransAngleMatrix tam;
2524 Line3d eye;
2525 Point3d u,b,c;
2526 Point2d e,f,g;
2527 double d,ra,d1,r1;
2528 Point2d a;
2529 int i;
2530 double ang;
2531
2532 eye.sx=xg; // center of the sphere
2533 eye.sy=yg;
2534 eye.sz=zg;
2535 eye.ex=po->x; // thePolar of CThreeD - position of the viewer's eye
2536 eye.ey=po->y;
2537 eye.ez=po->z;
2538
2539 NormLine3d(&eye,&u,&d);
2540 ToPolar3d(&u,&a);
2541
2542 ra=r; // radius of the sphere
2543 d1=ra*ra/d; // distance to the center of tangent circle from sphere center
2544 r1=sqrt(ra*ra-d1*d1); // radius of the tangent circle
2545
2546 tae.x=xg+d1*u.x;
2547 tae.y=yg+d1*u.y;
2548 tae.z=zg+d1*u.z;
2549 tae.theta=1.5707963268-a.y; // z axis toward viewer's eye
2550 tae.phi=a.x+1.5707963268;
2551 tae.psi=0.;
2552 TAEtoTAM(&tae,&tam);
2553
2554 u.x=r1*cos(0.);
2555 u.y=r1*sin(0.);
2556 u.z=0.;
2557 BodyToFrameTAM(&u,&tam,&b);
2558 FrameToBodyTAM(&b,po,&u);
2559 NormPoint3d(&u,&b,&d);
2560 ToPolar3d(&b,&e);
2561 e.y=1.5707963267-e.y;
2562 f=e;
2563 for(i=0;i<90;i++){
2564 g=f;
2565 if(i<89){
2566 ang=.06981317008*(i+1); // every 4 degree
2567 u.x=r1*cos(ang);
2568 u.y=r1*sin(ang);
2569 u.z=0.;
2570 BodyToFrameTAM(&u,&tam,&b);
2571 FrameToBodyTAM(&b,po,&u);
2572 NormPoint3d(&u,&b,&d);
2573 ToPolar3d(&b,&f);
2574 f.y=1.5707963267-f.y;
2575 }
2576 else f=e;
2577 seg[i].sx=g.x;
2578 seg[i].sy=g.y;
2579 seg[i].ex=f.x;
2580 seg[i].ey=f.y;
2581
2582 }
2583 blk->start=0;
2584 blk->pte=90;
2585 cont->start=0;
2586 cont->pte=90;
2587}
2588
2589
2590
2591/******************************************************************************
2592 FieldOfView
2593
2594 Get horizontal and vertical field of view to cover the object
2595 ******************************************************************************/
2596
2597void CMPsphere::FieldOfView(TransAngleMatrix *vu,Point2d *az,Point2d *el)
2598{
2599 Point3d s;
2600 Point3d f;
2601 Point2d a;
2602 double b,d;
2603
2604 f.x=xg;
2605 f.y=yg;
2606 f.z=zg;
2607 FrameToBodyTAM(&f,vu,&s);
2608 NormPoint3d(&s,&s,&d);
2609 ToPolar3d(&s,&a);
2610 b=asin(r/d);
2611 az->x=a.x-b;
2612 az->y=a.x+b;
2613 el->x=a.y-b;
2614 el->y=a.y+b;
2615}
2616
2617/******************************************************************************
2618 CMPbodyOfRot - bodyOfRot
2619 ******************************************************************************/
2620
2621void CMPbodyOfRot::SetPoint(double x,double z)
2622{
2623 switch (npoint){
2624 case 0:
2625 p0.x=x;
2626 p0.y=z;
2627 npoint=1;
2628 break;
2629 case 1:
2630 p1.x=x;
2631 p1.y=z;
2632 npoint=2;
2633 break;
2634 default:
2635 break;
2636 }
2637}
2638
2639/******************************************************************************
2640 ClearVisSurface
2641
2642 Clear 'visibleSurface' instance variable so that new visibility
2643 from a new viewpoint can be obtained in GetSurface
2644 ******************************************************************************/
2645void CMPbodyOfRot::ClearVisSurface(void)
2646{
2647 visibleSurface=0;
2648}
2649
2650/******************************************************************************
2651 GetSurface
2652
2653 Return a surface
2654
2655 14 surfaces are related to an index number in the following way
2656
2657 index face toward...
2658 0 - 11 azimuth 30*index+15 (deg)
2659 12 +z
2660 13 -z
2661
2662 The instance variable visibleSurface is used to map between
2663 the above index and the reference index passed as the first parameter.
2664
2665 The returned bool value is false if no more visible surface exists
2666 for the asked reference number.
2667
2668 ******************************************************************************/
2669
2670bool CMPbodyOfRot::GetSurface(int n,bool nearside,TransAngleMatrix *po,
2671 Surface *surf,Point2d *surfPt)
2672{
2673 Point2d p2,p3;
2674 TransAngleEuler tae1;
2675 TransAngleMatrix tam1;
2676 double alpha,delta,r;
2677
2678 TransAngleEuler tae2;
2679 TransAngleEuler tae;
2680 Point3d s;
2681 Point3d e;
2682 int i,j,k,m;
2683
2684 p2.x=p0.x; // bottom radial point
2685 p2.y=p0.y;
2686 p3.x=p1.x; // top radial point
2687 p3.y=p1.y;
2688
2689 if(p2.x==0.) p2.x=p3.x*0.01; // cone
2690 if(p3.x==0.) p3.x=p2.x*0.01;
2691 if(p2.y==p3.y) { // didc
2692 if(p2.x>p3.x) p3.y+=0.01*p2.x;
2693 else p2.y+=0.01*p3.x;
2694 }
2695
2696 p2.x*=1.0178; // make the average radius same as the real radius
2697 p3.x*=1.0178;
2698
2699 tae1.x=xg; // Translate Angle Euler for the part
2700 tae1.y=yg;
2701 tae1.z=zg;
2702 tae1.theta=thetag;
2703 tae1.phi=phig;
2704 tae1.psi=psig;
2705
2706 if(visibleSurface==0){
2707 TAEtoTAM(&tae1,&tam1);
2708 j=1;
2709 delta=atan2((p2.x-p3.x)*cos(0.017453293*15.),p3.y-p2.y);
2710 r=-p2.y*(p3.x-p2.x)/(p3.y-p2.y)+p2.x;
2711 r*=cos(0.017453293*15.);
2712 s.z=r*cos(delta)*sin(delta);
2713 r=r*cos(delta)*cos(delta);
2714 for(i=0;i<12;i++){
2715 alpha=0.017453293*(30.*i+15.);
2716 s.x=r*cos(alpha);
2717 s.y=r*sin(alpha);
2718 BodyToFrameTAM(&s,&tam1,&e);
2719 if((xg-e.x)*(po->x-e.x)+(yg-e.y)*(po->y-e.y)
2720 +(zg-e.z)*(po->z-e.z)<0.) visibleSurface|=j;
2721 j=j<<1;
2722 }
2723 FrameToBodyTAM((Point3d *)po,&tam1,&s); // position of viewer
2724 if((s.z-p3.y)*(p2.y-p3.y)<0.) visibleSurface|=j; // upper surface
2725 j=j<<1;
2726 if((s.z-p2.y)*(p3.y-p2.y)<0.) visibleSurface|=j; // lower surface
2727
2728 // determine instance variable 'appear'
2729 j=1<<12;
2730 if(visibleSurface&j){
2731 j=1;
2732 k=0;
2733 for(i=0;i<12;i++){
2734 if(visibleSurface&j) k++;
2735 j=j<<1;
2736 }
2737 if(k==0) appear=1;
2738 else if(k==12) appear=3;
2739 else appear=2;
2740 }
2741 else{
2742 j=j<<1;
2743 if(visibleSurface&j){
2744 j=1;
2745 k=0;
2746 for(i=0;i<12;i++){
2747 if(visibleSurface&j) k++;
2748 j=j<<1;
2749 }
2750 if(k==0) appear=4;
2751 else if(k==12) appear=6;
2752 else appear=5;
2753 }
2754 else appear=0;
2755 }
2756 }
2757 if(isTube){
2758 if(appear==0||((appear==2||appear==5)&&nearside)){
2759 j=1;
2760 i=0;
2761 for(m=0;m<12;m++){
2762 if(visibleSurface&j){
2763 if(i==n) break;
2764 i++;
2765 }
2766 j=j<<1;
2767 }
2768 if(m==12) return false;
2769
2770 delta=atan2((p2.x-p3.x)*cos(0.017453293*15.),p3.y-p2.y);
2771 alpha=0.017453293*(30.*m+15.);
2772 r=p2.x*cos(0.017453293*15.); // radius to the center of the bottom segment
2773
2774 tae2.x=r*cos(alpha);
2775 tae2.y=r*sin(alpha);
2776 tae2.z=p2.y;
2777 tae2.theta=1.5707963268-delta;
2778 tae2.phi=alpha+1.5707963268;
2779 tae2.psi=0.;
2780
2781 surfPt[0].x=-r*tan(0.017453293*15.); // left-bottom
2782 surfPt[0].y=0.;
2783 surfPt[1].x=-surfPt[0].x; // right-bottom
2784 surfPt[1].y=0.;
2785 r=p3.x*cos(0.017453293*15.); // radius to the center of the top segment
2786 surfPt[2].x=r*tan(0.017453293*15.); // right-top
2787 surfPt[2].y=(p3.y-p2.y)/cos(delta);
2788 surfPt[3].x=-surfPt[2].x; // left-top
2789 surfPt[3].y=surfPt[2].y;
2790
2791 MoveTwiceTAE(&tae1,&tae2,&tae);
2792 surf->x0=tae.x;
2793 surf->y0=tae.y;
2794 surf->z0=tae.z;
2795 surf->theta0=tae.theta;
2796 surf->phi0=tae.phi;
2797 surf->psi0=tae.psi;
2798 surf->start=0;
2799 if(m<12) surf->pte=4;
2800 else surf->pte=12;
2801 return true;
2802 }
2803 if((appear==2||appear==5)&&!nearside){
2804 j=1;
2805 i=0;
2806 for(m=0;m<12;m++){
2807 if(!(visibleSurface&j)){
2808 if(i==n) break;
2809 i++;
2810 }
2811 j=j<<1;
2812 }
2813 if(m==12) return false;
2814
2815 delta=atan2((p2.x-p3.x)*cos(0.017453293*15.),p3.y-p2.y);
2816 alpha=0.017453293*(30.*m+15.);
2817 r=p2.x*cos(0.017453293*15.); // radius to the center of the bottom segment
2818
2819 tae2.x=r*cos(alpha);
2820 tae2.y=r*sin(alpha);
2821 tae2.z=p2.y;
2822 tae2.theta=1.5707963268-delta;
2823 tae2.phi=alpha+1.5707963268;
2824 tae2.psi=0.;
2825
2826 surfPt[0].x=r*tan(0.017453293*15.); // left-bottom
2827 surfPt[0].y=0.;
2828 surfPt[1].x=-surfPt[0].x; // right-bottom
2829 surfPt[1].y=0.;
2830 r=p3.x*cos(0.017453293*15.); // radius to the center of the top segment
2831 surfPt[2].x=-r*tan(0.017453293*15.); // right-top
2832 surfPt[2].y=(p3.y-p2.y)/cos(delta);
2833 surfPt[3].x=-surfPt[2].x; // left-top
2834 surfPt[3].y=surfPt[2].y;
2835
2836 MoveTwiceTAE(&tae1,&tae2,&tae);
2837 surf->x0=tae.x;
2838 surf->y0=tae.y;
2839 surf->z0=tae.z;
2840 surf->theta0=tae.theta;
2841 surf->phi0=tae.phi;
2842 surf->psi0=tae.psi;
2843 surf->start=0;
2844 if(m<12) surf->pte=4;
2845 else surf->pte=12;
2846 return true;
2847 }
2848 if(appear==1||appear==4||appear==3||appear==6){
2849 if(n>5) return false;
2850 if(nearside) m=n;
2851 else m=n+6;
2852
2853 delta=atan2((p2.x-p3.x)*cos(0.017453293*15.),p3.y-p2.y);
2854 alpha=0.017453293*(30.*m+15.);
2855 r=p2.x*cos(0.017453293*15.); // radius to the center of the bottom segment
2856 if(appear==1||appear==4){
2857 alpha-=3.14159265;
2858 delta=-delta;
2859 }
2860 tae2.x=r*cos(alpha);
2861 tae2.y=r*sin(alpha);
2862 tae2.z=p2.y;
2863 tae2.theta=1.5707963268-delta;
2864 tae2.phi=alpha+1.5707963268;
2865 tae2.psi=0.;
2866
2867 surfPt[0].x=-r*tan(0.017453293*15.); // left-bottom
2868 surfPt[0].y=0.;
2869 surfPt[1].x=-surfPt[0].x; // right-bottom
2870 surfPt[1].y=0.;
2871 r=p3.x*cos(0.017453293*15.); // radius to the center of the top segment
2872 surfPt[2].x=r*tan(0.017453293*15.); // right-top
2873 surfPt[2].y=(p3.y-p2.y)/cos(delta);
2874 surfPt[3].x=-surfPt[2].x; // left-top
2875 surfPt[3].y=surfPt[2].y;
2876
2877 MoveTwiceTAE(&tae1,&tae2,&tae);
2878 surf->x0=tae.x;
2879 surf->y0=tae.y;
2880 surf->z0=tae.z;
2881 surf->theta0=tae.theta;
2882 surf->phi0=tae.phi;
2883 surf->psi0=tae.psi;
2884 surf->start=0;
2885 if(m<12) surf->pte=4;
2886 else surf->pte=12;
2887 return true;
2888 }
2889 }
2890 else{ // solid
2891 j=1;
2892 i=0;
2893 for(m=0;m<14;m++){
2894 if(visibleSurface&j){
2895 if(i==n) break;
2896 i++;
2897 }
2898 j=j<<1;
2899 }
2900 if(m==14) return false;
2901
2902 if(m<12){
2903 delta=atan2((p2.x-p3.x)*cos(0.017453293*15.),p3.y-p2.y);
2904 alpha=0.017453293*(30.*m+15.);
2905 r=p2.x*cos(0.017453293*15.); // radius to the center of the bottom segment
2906
2907 tae2.x=r*cos(alpha);
2908 tae2.y=r*sin(alpha);
2909 tae2.z=p2.y;
2910 tae2.theta=1.5707963268-delta;
2911 tae2.phi=alpha+1.5707963268;
2912 tae2.psi=0.;
2913
2914 surfPt[0].x=-r*tan(0.017453293*15.); // left-bottom
2915 surfPt[0].y=0.;
2916 surfPt[1].x=-surfPt[0].x; // right-bottom
2917 surfPt[1].y=0.;
2918 r=p3.x*cos(0.017453293*15.); // radius to the center of the top segment
2919 surfPt[2].x=r*tan(0.017453293*15.); // right-top
2920 surfPt[2].y=(p3.y-p2.y)/cos(delta);
2921 surfPt[3].x=-surfPt[2].x; // left-top
2922 surfPt[3].y=surfPt[2].y;
2923 }
2924 if(m==12){
2925 tae2.x=0.;
2926 tae2.y=0.;
2927 tae2.z=p3.y;
2928 tae2.theta=0.;
2929 tae2.phi=0.;
2930 tae2.psi=0.;
2931
2932 r=p3.x;
2933 for(i=0;i<12;i++){
2934 alpha=0.017453293*30.*i;
2935 surfPt[i].x=r*cos(alpha);
2936 surfPt[i].y=r*sin(alpha);
2937 }
2938 }
2939 if(m==13){
2940 tae2.x=0.;
2941 tae2.y=0.;
2942 tae2.z=p2.y;
2943 tae2.theta=3.1415926536;
2944 tae2.phi=0.;
2945 tae2.psi=0.;
2946
2947 r=p2.x;
2948 for(i=0;i<12;i++){
2949 alpha=0.017453293*30.*i;
2950 surfPt[i].x=r*cos(alpha);
2951 surfPt[i].y=r*sin(alpha);
2952 }
2953 }
2954
2955 MoveTwiceTAE(&tae1,&tae2,&tae);
2956 surf->x0=tae.x;
2957 surf->y0=tae.y;
2958 surf->z0=tae.z;
2959 surf->theta0=tae.theta;
2960 surf->phi0=tae.phi;
2961 surf->psi0=tae.psi;
2962 surf->start=0;
2963 if(m<12) surf->pte=4;
2964 else surf->pte=12;
2965 return true;
2966 }
2967}
2968
2969/******************************************************************************
2970 GetCenter
2971
2972 Return the center point in frame coordinate
2973
2974 ******************************************************************************/
2975
2976void CMPbodyOfRot::GetCenter(Point3d *p)
2977{
2978 Point3d pt;
2979 TransAngleEuler tae;
2980 TransAngleMatrix tam;
2981
2982 pt.x=0.;
2983 pt.y=0.;
2984 pt.z=0.5*(p0.y+p1.y);
2985
2986 tae.x=xg;
2987 tae.y=yg;
2988 tae.z=zg;
2989 tae.theta=thetag;
2990 tae.phi=phig;
2991 tae.psi=psig;
2992
2993 TAEtoTAM(&tae,&tam);
2994 BodyToFrameTAM(&pt,&tam,p);
2995}
2996
2997/******************************************************************************
2998 GetDrawData
2999
3000 Return data for drawing
3001
3002 m segment number (0 - near side, 1 - far side)
3003 po thePolar
3004
3005 seg array of line segments (clockwise angle from top,
3006 angle from line of sight)
3007 blk indices of segs which are to be drawn with lines
3008 cont indices of closed line segments. Inside of the closure
3009 will be painted by a color.
3010
3011 ******************************************************************************/
3012
3013void CMPbodyOfRot::GetDrawData(int m,TransAngleMatrix *po,
3014 Line2d *seg,StartPTE *blk,StartPTE *cont)
3015{
3016 double eye[3];
3017 Point3d *ten0;
3018 Point3d *ten1;
3019 double x1,z1,x2,z2,y1,y2;
3020 TransAngleEuler tae;
3021 TransAngleMatrix tam1;
3022 int nseg,i,n0,n1,n2,j,k,is,is1,line,istart,iend,itb,ibt,n;
3023 double deld,a,co,si,r;
3024 Point3d q,s,d;
3025 Point2d direc;
3026
3027 if(oblong){
3028 GetDrawData1(po,seg,blk,cont);
3029 return;
3030 }
3031
3032 eye[0]=po->x;
3033 eye[1]=po->y;
3034 eye[2]=po->z;
3035
3036// lower surface ten0, upper surface ten1
3037// indices are from 0 to 90, 90th data being the same as 0th data
3038 ten0=new Point3d[91];
3039 ten1=new Point3d[91];
3040 x1=p0.x;
3041 z1=p0.y;
3042 x2=p1.x;
3043 z2=p1.y;
3044
3045 if(x1==0.) x1=x2*0.01; // cone
3046 if(x2==0.) x2=x1*0.01;
3047 if(z1==z2) { // didc
3048 if(x1>x2) z2+=0.01*x1;
3049 else z1-=0.01*x2;
3050 }
3051
3052// set conversion parameters
3053 tae.x=xg;
3054 tae.y=yg;
3055 tae.z=zg;
3056 tae.theta=thetag;
3057 tae.phi=phig;
3058 tae.psi=psig;
3059
3060 TAEtoTAM(&tae,&tam1);
3061
3062// At this point, initial conditions are set.
3063// tam1 defines a local coordinate.
3064// In the local coordinate, two points (x1,z1) and (x2,z2) are given.
3065// The body of rotation is given by rotating
3066// the line connecting the two points around z-axis.
3067// The viewer is given by eye[]
3068
3069/* fill coordinates of 2*nseg apexes */
3070
3071 nseg=90;
3072 deld=nseg;
3073 deld=2.*3.1415926536/deld;
3074 for(i=0;i<=nseg;i++){ // 91 data are set to each of the arrays
3075 a=i*deld;
3076 co=cos(a);
3077 si=sin(a);
3078 q.x=x1*co;
3079 q.y=x1*si;
3080 q.z=z1; /* lower surface */
3081 BodyToFrameTAM(&q,&tam1,&ten0[i]);
3082 q.x=x2*co;
3083 q.y=x2*si;
3084 q.z=z2; /* uppser surface */
3085 BodyToFrameTAM(&q,&tam1,&ten1[i]);
3086 }
3087
3088// the outline was replaced by two circles ten0[] and ten1[]
3089/* obtain side surface segments which are visible */
3090 n0=0;
3091 n1=0; // invisible-to-visible index
3092 n2=0; // visible-to-invisible index
3093
3094// In CMPbodyOfRot::GetSurface, we have already categorized
3095// into seven cases, which are inducated by the variable 'appear'.
3096// Of those seven cases, the following three cases
3097// 0 only the side is visible
3098// 2 side and upper surface are visible
3099// 5 side and lower surface are visible
3100// have indices which separate visible and invisible portions of the side
3101// surface.
3102
3103 if(appear==0||appear==2||appear==5){
3104 for(i=0;(i<=nseg)&&(n0!=2);i++){
3105
3106 /* each surface contains i-th and (i-1)th apexes */
3107 /* visibility of two adjacent surfaces are compared to */
3108 /* obtain both visible-to-invisible and invisible-to-visible */
3109 /* trasition indices */
3110
3111 j=i;
3112 if(j>=nseg) j-=nseg;
3113 k=j-1;
3114 if(k<0) k+=nseg;
3115 is=0;
3116 /* vector along lower surface */
3117 x1=ten0[j].x-ten0[k].x;
3118 y1=ten0[j].y-ten0[k].y;
3119 z1=ten0[j].z-ten0[k].z;
3120 /* vector directed from lower surface to upper sureace */
3121 x2=ten1[k].x-ten0[k].x;
3122 y2=ten1[k].y-ten0[k].y;
3123 z2=ten1[k].z-ten0[k].z;
3124 /* if surface is visible is = 1 */
3125 if((y1*z2-z1*y2)*(eye[0]-ten0[k].x)
3126 +(z1*x2-x1*z2)*(eye[1]-ten0[k].y)
3127 +(x1*y2-y1*x2)*(eye[2]-ten0[k].z)>0.) is=1; // visible
3128 if((i!=0)&&(is!=is1)) {
3129 if(is>is1) n1=k; /* k is invisible to visible */
3130 if(is<is1) n2=k; /* k is visible to invisible */
3131 n0++;
3132 }
3133 if(n0!=2) is1=is;
3134 }
3135 }
3136
3137/* now, categorization is over, process each cases */
3138
3139// The order of processing is as followes
3140// appear solid tube-near tube-far
3141// 0 x x x
3142// 2,5 x
3143// 2,5 x
3144// 5 x
3145// 2 x
3146// 1,3,4,6 x
3147// 1,3,4,6 x
3148// 1,3,4,6 x
3149
3150 line=0;
3151 if(appear==0||((appear==2||appear==5)&&isTube&&m==0)){
3152 // appear=0,both for a solid and a tube, near and far side
3153 // appear=2 or 5, near side of the tube
3154/* side only */
3155 blk->start=line;
3156 cont->start=line;
3157 n0=n2-n1; /* number of visible points - 1 */
3158 if(n0<0) n0+=nseg;
3159 for(i=0;i<n0;i++){
3160 j=n1+i+1;
3161 if(j>=nseg) j-=nseg;
3162 k=j-1;
3163 if(k<0) k+=nseg;
3164/* along lower surface normal direction (to the order of smaller to larger indices) */
3165 s=ten0[k];
3166 FrameToBodyTAM(&s,po,&d);
3167 NormPoint3d(&d,&s,&r);
3168 ToPolar3d(&s,&direc);
3169 seg[line].sx=direc.x;
3170 seg[line].sy=1.5707963267-direc.y;
3171 s=ten0[j];
3172 FrameToBodyTAM(&s,po,&d);
3173 NormPoint3d(&d,&s,&r);
3174 ToPolar3d(&s,&direc);
3175 seg[line].ex=direc.x;
3176 seg[line].ey=1.5707963267-direc.y;
3177 line++;
3178/* along upper surface reversed order */
3179 s=ten1[j];
3180 FrameToBodyTAM(&s,po,&d);
3181 NormPoint3d(&d,&s,&r);
3182 ToPolar3d(&s,&direc);
3183 seg[line].sx=direc.x;
3184 seg[line].sy=1.5707963267-direc.y;
3185 s=ten1[k];
3186 FrameToBodyTAM(&s,po,&d);
3187 NormPoint3d(&d,&s,&r);
3188 ToPolar3d(&s,&direc);
3189 seg[line].ex=direc.x;
3190 seg[line].ey=1.5707963267-direc.y;
3191 line++;
3192 }
3193/* invisible-to-visible side line from top to bottom */
3194 s=ten1[n1];
3195 FrameToBodyTAM(&s,po,&d);
3196 NormPoint3d(&d,&s,&r);
3197 ToPolar3d(&s,&direc);
3198 seg[line].sx=direc.x;
3199 seg[line].sy=1.5707963267-direc.y;
3200 s=ten0[n1];
3201 FrameToBodyTAM(&s,po,&d);
3202 NormPoint3d(&d,&s,&r);
3203 ToPolar3d(&s,&direc);
3204 seg[line].ex=direc.x;
3205 seg[line].ey=1.5707963267-direc.y;
3206 line++;
3207/* visible-to-invisible side line from bottom to top */
3208 s=ten0[n2];
3209 FrameToBodyTAM(&s,po,&d);
3210 NormPoint3d(&d,&s,&r);
3211 ToPolar3d(&s,&direc);
3212 seg[line].sx=direc.x;
3213 seg[line].sy=1.5707963267-direc.y;
3214 s=ten1[n2];
3215 FrameToBodyTAM(&s,po,&d);
3216 NormPoint3d(&d,&s,&r);
3217 ToPolar3d(&s,&direc);
3218 seg[line].ex=direc.x;
3219 seg[line].ey=1.5707963267-direc.y;
3220 line++;
3221 blk->pte=line;
3222 cont->pte=line;
3223 delete[] ten0; // DisposPtr((Ptr)ten0);
3224 delete[] ten1; // DisposPtr((Ptr)ten1);
3225 return;
3226 }
3227
3228 if((appear==2||appear==5)&&isTube&&m==1){
3229 // appear=2 or 5, far side of the tube
3230/* side only */
3231 blk->start=line;
3232 cont->start=line;
3233 n0=n1-n2; /* number of invisible points - 1 */
3234 if(n0<0) n0+=nseg;
3235 for(i=0;i<n0;i++){
3236 j=n2+i+1;
3237 if(j>=nseg) j-=nseg;
3238 k=j-1;
3239 if(k<0) k+=nseg;
3240/* along lower surface in reverse direction */
3241 s=ten0[j];
3242 FrameToBodyTAM(&s,po,&d);
3243 NormPoint3d(&d,&s,&r);
3244 ToPolar3d(&s,&direc);
3245 seg[line].sx=direc.x;
3246 seg[line].sy=1.5707963267-direc.y;
3247 s=ten0[k];
3248 FrameToBodyTAM(&s,po,&d);
3249 NormPoint3d(&d,&s,&r);
3250 ToPolar3d(&s,&direc);
3251 seg[line].ex=direc.x;
3252 seg[line].ey=1.5707963267-direc.y;
3253 line++;
3254/* along upper surface in normal direction */
3255 s=ten1[k];
3256 FrameToBodyTAM(&s,po,&d);
3257 NormPoint3d(&d,&s,&r);
3258 ToPolar3d(&s,&direc);
3259 seg[line].sx=direc.x;
3260 seg[line].sy=1.5707963267-direc.y;
3261 s=ten1[j];
3262 FrameToBodyTAM(&s,po,&d);
3263 NormPoint3d(&d,&s,&r);
3264 ToPolar3d(&s,&direc);
3265 seg[line].ex=direc.x;
3266 seg[line].ey=1.5707963267-direc.y;
3267 line++;
3268 }
3269/* invisible-to-visible side line from top tp bottom */
3270 s=ten1[n1];
3271 FrameToBodyTAM(&s,po,&d);
3272 NormPoint3d(&d,&s,&r);
3273 ToPolar3d(&s,&direc);
3274 seg[line].sx=direc.x;
3275 seg[line].sy=1.5707963267-direc.y;
3276 s=ten0[n1];
3277 FrameToBodyTAM(&s,po,&d);
3278 NormPoint3d(&d,&s,&r);
3279 ToPolar3d(&s,&direc);
3280 seg[line].ex=direc.x;
3281 seg[line].ey=1.5707963267-direc.y;
3282 line++;
3283/* visible-to-invisible side line from bottom to top */
3284 s=ten0[n2];
3285 FrameToBodyTAM(&s,po,&d);
3286 NormPoint3d(&d,&s,&r);
3287 ToPolar3d(&s,&direc);
3288 seg[line].sx=direc.x;
3289 seg[line].sy=1.5707963267-direc.y;
3290 s=ten1[n2];
3291 FrameToBodyTAM(&s,po,&d);
3292 NormPoint3d(&d,&s,&r);
3293 ToPolar3d(&s,&direc);
3294 seg[line].ex=direc.x;
3295 seg[line].ey=1.5707963267-direc.y;
3296 line++;
3297 blk->pte=line;
3298 cont->pte=line;
3299 delete[] ten0; // DisposPtr((Ptr)ten0);
3300 delete[] ten1; // DisposPtr((Ptr)ten1);
3301 return;
3302 }
3303
3304 if(appear==5&&!isTube){ // for a solid
3305/* side and lower surface */
3306 blk->start=line;
3307 cont->start=line;
3308 n0=n2-n1;
3309 if(n0<0) n0+=nseg;
3310 for(i=0;i<n0;i++){
3311 j=n2-i-1;
3312 if(j<0) j+=nseg;
3313 k=j+1;
3314 if(k>=nseg) k-=nseg;
3315/* contor along the upper surface */
3316 s=ten1[k];
3317 FrameToBodyTAM(&s,po,&d);
3318 NormPoint3d(&d,&s,&r);
3319 ToPolar3d(&s,&direc);
3320 seg[line].sx=direc.x;
3321 seg[line].sy=1.5707963267-direc.y;
3322 s=ten1[j];
3323 FrameToBodyTAM(&s,po,&d);
3324 NormPoint3d(&d,&s,&r);
3325 ToPolar3d(&s,&direc);
3326 seg[line].ex=direc.x;
3327 seg[line].ey=1.5707963267-direc.y;
3328 line++;
3329 }
3330 s=ten1[n1];
3331 FrameToBodyTAM(&s,po,&d);
3332 NormPoint3d(&d,&s,&r);
3333 ToPolar3d(&s,&direc);
3334 seg[line].sx=direc.x;
3335 seg[line].sy=1.5707963267-direc.y;
3336 s=ten0[n1];
3337 FrameToBodyTAM(&s,po,&d);
3338 NormPoint3d(&d,&s,&r);
3339 ToPolar3d(&s,&direc);
3340 seg[line].ex=direc.x;
3341 seg[line].ey=1.5707963267-direc.y;
3342 line++;
3343 s=ten0[n2];
3344 FrameToBodyTAM(&s,po,&d);
3345 NormPoint3d(&d,&s,&r);
3346 ToPolar3d(&s,&direc);
3347 seg[line].sx=direc.x;
3348 seg[line].sy=1.5707963267-direc.y;
3349 s=ten1[n2];
3350 FrameToBodyTAM(&s,po,&d);
3351 NormPoint3d(&d,&s,&r);
3352 ToPolar3d(&s,&direc);
3353 seg[line].ex=direc.x;
3354 seg[line].ey=1.5707963267-direc.y;
3355 line++;
3356 n0=n1-n2;
3357 if(n0<0) n0=n0+nseg;
3358 cont->pte=line+n0;
3359/* part of the followings constitute contour */
3360 for(i=0;i<nseg;i++){
3361 j=n2+i+1;
3362 if(j>=nseg) j=j-nseg;
3363 k=j-1;
3364 if(k<0) k+=nseg;
3365 s=ten0[j];
3366 FrameToBodyTAM(&s,po,&d);
3367 NormPoint3d(&d,&s,&r);
3368 ToPolar3d(&s,&direc);
3369 seg[line].sx=direc.x;
3370 seg[line].sy=1.5707963267-direc.y;
3371 s=ten0[k];
3372 FrameToBodyTAM(&s,po,&d);
3373 NormPoint3d(&d,&s,&r);
3374 ToPolar3d(&s,&direc);
3375 seg[line].ex=direc.x;
3376 seg[line].ey=1.5707963267-direc.y;
3377 line++;
3378 }
3379 blk->pte=line;
3380 delete[] ten0; // DisposPtr((Ptr)ten0);
3381 delete[] ten1; // DisposPtr((Ptr)ten1);
3382 return;
3383 }
3384 if(appear==2&&!isTube){ // for a solid
3385/* side and upper surface */
3386 blk->start=line;
3387 cont->start=line;
3388 n0=n2-n1;
3389 if(n0<0) n0+=nseg;
3390/* line along lower surface */
3391 for(i=0;i<n0;i++){
3392 j=n1+i+1;
3393 if(j>=nseg) j-=nseg;
3394 k=j-1;
3395 if(k<0) k+=nseg;
3396 s=ten0[k];
3397 FrameToBodyTAM(&s,po,&d);
3398 NormPoint3d(&d,&s,&r);
3399 ToPolar3d(&s,&direc);
3400 seg[line].sx=direc.x;
3401 seg[line].sy=1.5707963267-direc.y;
3402 s=ten0[j];
3403 FrameToBodyTAM(&s,po,&d);
3404 NormPoint3d(&d,&s,&r);
3405 ToPolar3d(&s,&direc);
3406 seg[line].ex=direc.x;
3407 seg[line].ey=1.5707963267-direc.y;
3408 line++;
3409 }
3410 s=ten1[n1];
3411 FrameToBodyTAM(&s,po,&d);
3412 NormPoint3d(&d,&s,&r);
3413 ToPolar3d(&s,&direc);
3414 seg[line].sx=direc.x;
3415 seg[line].sy=1.5707963267-direc.y;
3416 s=ten0[n1];
3417 FrameToBodyTAM(&s,po,&d);
3418 NormPoint3d(&d,&s,&r);
3419 ToPolar3d(&s,&direc);
3420 seg[line].ex=direc.x;
3421 seg[line].ey=1.5707963267-direc.y;
3422 line++;
3423 s=ten0[n2];
3424 FrameToBodyTAM(&s,po,&d);
3425 NormPoint3d(&d,&s,&r);
3426 ToPolar3d(&s,&direc);
3427 seg[line].sx=direc.x;
3428 seg[line].sy=1.5707963267-direc.y;
3429 s=ten1[n2];
3430 FrameToBodyTAM(&s,po,&d);
3431 NormPoint3d(&d,&s,&r);
3432 ToPolar3d(&s,&direc);
3433 seg[line].ex=direc.x;
3434 seg[line].ey=1.5707963267-direc.y;
3435 line++;
3436 n0=n1-n2;
3437 if(n0<0) n0=n0+nseg;
3438 cont->pte=line+n0;
3439 for(i=0;i<nseg;i++){
3440 j=n2+i+1;
3441 if(j>=nseg) j=j-nseg;
3442 k=j-1;
3443 if(k<0) k+=nseg;
3444 s=ten1[k];
3445 FrameToBodyTAM(&s,po,&d);
3446 NormPoint3d(&d,&s,&r);
3447 ToPolar3d(&s,&direc);
3448 seg[line].sx=direc.x;
3449 seg[line].sy=1.5707963267-direc.y;
3450 s=ten1[j];
3451 FrameToBodyTAM(&s,po,&d);
3452 NormPoint3d(&d,&s,&r);
3453 ToPolar3d(&s,&direc);
3454 seg[line].ex=direc.x;
3455 seg[line].ey=1.5707963267-direc.y;
3456 line++;
3457 }
3458 blk->pte=line;
3459 delete[] ten0; // DisposPtr((Ptr)ten0);
3460 delete[] ten1; // DisposPtr((Ptr)ten1);
3461 return;
3462 }
3463 if(!isTube){ // for a solid, appear=1,3,4 or 6
3464 /* lower or upper surface only */
3465 blk->start=line;
3466 cont->pte=line;
3467 for(i=0;i<nseg;i++){ // contour circle
3468 j=i;
3469 k=j-1;
3470 if(k<0) k+=nseg;
3471 if(appear==1||appear==6){ // contour is upper surface
3472 if(appear==1){
3473 n=k;
3474 k=j;
3475 j=n;
3476 }
3477 s=ten1[j];
3478 FrameToBodyTAM(&s,po,&d);
3479 NormPoint3d(&d,&s,&r);
3480 ToPolar3d(&s,&direc);
3481 seg[line].sx=direc.x;
3482 seg[line].sy=1.5707963267-direc.y;
3483 s=ten1[k];
3484 FrameToBodyTAM(&s,po,&d);
3485 NormPoint3d(&d,&s,&r);
3486 ToPolar3d(&s,&direc);
3487 seg[line].ex=direc.x;
3488 seg[line].ey=1.5707963267-direc.y;
3489 }
3490 else{ // appear=3 or 4
3491 if(appear==3){
3492 n=k;
3493 k=j;
3494 j=n;
3495 }
3496 s=ten0[j];
3497 FrameToBodyTAM(&s,po,&d);
3498 NormPoint3d(&d,&s,&r);
3499 ToPolar3d(&s,&direc);
3500 seg[line].sx=direc.x;
3501 seg[line].sy=1.5707963267-direc.y;
3502 s=ten0[k];
3503 FrameToBodyTAM(&s,po,&d);
3504 NormPoint3d(&d,&s,&r);
3505 ToPolar3d(&s,&direc);
3506 seg[line].ex=direc.x;
3507 seg[line].ey=1.5707963267-direc.y;
3508 }
3509 line++;
3510 }
3511 cont->pte=line;
3512 if(appear==3){
3513 for(i=0;i<nseg;i++){ // upper surface is inner circle
3514 j=i;
3515 k=j-1;
3516 if(k<0) k+=nseg;
3517 s=ten1[j];
3518 FrameToBodyTAM(&s,po,&d);
3519 NormPoint3d(&d,&s,&r);
3520 ToPolar3d(&s,&direc);
3521 seg[line].sx=direc.x;
3522 seg[line].sy=1.5707963267-direc.y;
3523 s=ten1[k];
3524 FrameToBodyTAM(&s,po,&d);
3525 NormPoint3d(&d,&s,&r);
3526 ToPolar3d(&s,&direc);
3527 seg[line].ex=direc.x;
3528 seg[line].ey=1.5707963267-direc.y;
3529 }
3530 line++;
3531 }
3532 if(appear==6){
3533 for(i=0;i<nseg;i++){ // lower surface is inner circle
3534 j=i;
3535 k=j-1;
3536 if(k<0) k+=nseg;
3537 s=ten0[j];
3538 FrameToBodyTAM(&s,po,&d);
3539 NormPoint3d(&d,&s,&r);
3540 ToPolar3d(&s,&direc);
3541 seg[line].sx=direc.x;
3542 seg[line].sy=1.5707963267-direc.y;
3543 s=ten0[k];
3544 FrameToBodyTAM(&s,po,&d);
3545 NormPoint3d(&d,&s,&r);
3546 ToPolar3d(&s,&direc);
3547 seg[line].ex=direc.x;
3548 seg[line].ey=1.5707963267-direc.y;
3549 }
3550 line++;
3551 }
3552 blk->pte=line;
3553 delete[] ten0; // DisposPtr((Ptr)ten0);
3554 delete[] ten1; // DisposPtr((Ptr)ten1);
3555 return;
3556 }
3557// remaining cases are appear 1,3,4 and 6 of tubes
3558
3559// The upper surface is given by an array ten1[].
3560// The lower surface is given by an array ten0[].
3561// looked from... outer circle inner circle
3562// appear=1 the upper side upper circle (normal) lower circle (reverse)
3563// appear=3 the upper side lower circle (normal) upper circle (reverse)
3564// appear=4 the lower side lower circle (reverse) upper circle (normal)
3565// appear=6 the lower side upper circle (reverse) lower circle (normal)
3566
3567 cont->start=line;
3568
3569 if(m==0){ // near side
3570 istart=0;
3571 iend=45;
3572 }
3573 else{ // far side
3574 istart=45;
3575 iend=90;
3576 }
3577// 2 segments, which are part of the contour,
3578// but, not part of the block.
3579
3580 if(appear==1||appear==4){
3581 itb=iend; // top to botton transition
3582 ibt=istart; // bottom to top transition
3583 }
3584 if(appear==3||appear==6){
3585 itb=istart; // top to bottom transition
3586 ibt=iend;
3587 }
3588 s=ten1[itb];
3589 FrameToBodyTAM(&s,po,&d);
3590 NormPoint3d(&d,&s,&r);
3591 ToPolar3d(&s,&direc);
3592 seg[line].sx=direc.x;
3593 seg[line].sy=1.5707963267-direc.y;
3594 s=ten0[itb];
3595 FrameToBodyTAM(&s,po,&d);
3596 NormPoint3d(&d,&s,&r);
3597 ToPolar3d(&s,&direc);
3598 seg[line].ex=direc.x;
3599 seg[line].ey=1.5707963267-direc.y;
3600 line++;
3601 s=ten0[ibt];
3602 FrameToBodyTAM(&s,po,&d);
3603 NormPoint3d(&d,&s,&r);
3604 ToPolar3d(&s,&direc);
3605 seg[line].sx=direc.x;
3606 seg[line].sy=1.5707963267-direc.y;
3607 s=ten1[ibt];
3608 FrameToBodyTAM(&s,po,&d);
3609 NormPoint3d(&d,&s,&r);
3610 ToPolar3d(&s,&direc);
3611 seg[line].ex=direc.x;
3612 seg[line].ey=1.5707963267-direc.y;
3613 line++;
3614
3615
3616// now, the circular portion
3617 blk->start=line;
3618
3619 for(i=istart;i<iend;i++){
3620 if(appear==1||appear==4){
3621 j=i;
3622 k=i+1;
3623 }
3624 else{ // appear==3||appear==6
3625 j=i+1;
3626 k=i;
3627 }
3628 s=ten1[j];
3629 FrameToBodyTAM(&s,po,&d);
3630 NormPoint3d(&d,&s,&r);
3631 ToPolar3d(&s,&direc);
3632 seg[line].sx=direc.x;
3633 seg[line].sy=1.5707963267-direc.y;
3634 s=ten1[k];
3635 FrameToBodyTAM(&s,po,&d);
3636 NormPoint3d(&d,&s,&r);
3637 ToPolar3d(&s,&direc);
3638 seg[line].ex=direc.x;
3639 seg[line].ey=1.5707963267-direc.y;
3640 line++;
3641 }
3642 for(i=istart;i<iend;i++){
3643 if(appear==1||appear==4){
3644 j=i+1;
3645 k=i;
3646 }
3647 else{ // appear==3||appear==6
3648 j=i;
3649 k=i+1;
3650 }
3651 s=ten0[j];
3652 FrameToBodyTAM(&s,po,&d);
3653 NormPoint3d(&d,&s,&r);
3654 ToPolar3d(&s,&direc);
3655 seg[line].sx=direc.x;
3656 seg[line].sy=1.5707963267-direc.y;
3657 s=ten0[k];
3658 FrameToBodyTAM(&s,po,&d);
3659 NormPoint3d(&d,&s,&r);
3660 ToPolar3d(&s,&direc);
3661 seg[line].ex=direc.x;
3662 seg[line].ey=1.5707963267-direc.y;
3663 line++;
3664 }
3665 cont->pte=line;
3666 blk->pte=line;
3667 delete[] ten0; // DisposPtr((Ptr)ten0);
3668 delete[] ten1; // DisposPtr((Ptr)ten1);
3669}
3670
3671/******************************************************************************
3672 GetDrawData1
3673
3674 Return data for drawing for the case of oblong
3675
3676 po thePolar
3677
3678 seg array of line segments (clockwise angle from top,
3679 angle from line of sight)
3680 blk indices of segs which are to be drawn with lines
3681 cont indices of closed line segments. Inside of the closure
3682 will be painted by a color.
3683
3684 ******************************************************************************/
3685
3686void CMPbodyOfRot::GetDrawData1(TransAngleMatrix *po,
3687 Line2d *seg,StartPTE *blk,StartPTE *cont)
3688{
3689 int i,n;
3690 double z1,z2,r,ang0,ang1,ang2,ang,theta,dthet,d,d1,r1,d2,r2,l1,l2;
3691 TransAngleEuler tae;
3692 TransAngleMatrix tam;
3693 Point3d q,s1,u1,s2,u2,v,w,u,b;
3694 Line3d eye,tz,ty;
3695 Point2d e,f,g,a;
3696
3697 r=p0.x; // radius of sphere
3698 z1=p0.y+r; // bottom center of sphere
3699 z2=p1.y-r; // top center of sphere
3700
3701 tae.x=xg;
3702 tae.y=yg;
3703 tae.z=zg;
3704 tae.theta=thetag;
3705 tae.phi=phig;
3706 tae.psi=psig;
3707 TAEtoTAM(&tae,&tam);
3708
3709 q.x=0.;
3710 q.y=0.;
3711 q.z=z1; /* bottom center of sphere */
3712 BodyToFrameTAM(&q,&tam,&s1);
3713 eye.sx=s1.x; // center of the sphere
3714 eye.sy=s1.y;
3715 eye.sz=s1.z;
3716 eye.ex=po->x; // thePolar of CThreeD - position of the viewer's eye
3717 eye.ey=po->y;
3718 eye.ez=po->z;
3719
3720 NormLine3d(&eye,&u1,&d);
3721 d1=r*r/d; // distance to the center of tangent circle from sphere center
3722 r1=sqrt(r*r-d1*d1); // radius of the tangent circle
3723 l1=d-d1; // dist from circle center to eye
3724
3725 q.x=0.;
3726 q.y=0.;
3727 q.z=z2; /* top center of sphere */
3728 BodyToFrameTAM(&q,&tam,&s2);
3729 eye.sx=s2.x; // center of the sphere
3730 eye.sy=s2.y;
3731 eye.sz=s2.z;
3732 eye.ex=po->x; // thePolar of CThreeD - position of the viewer's eye
3733 eye.ey=po->y;
3734 eye.ez=po->z;
3735
3736 NormLine3d(&eye,&u2,&d);
3737 d2=r*r/d; // distance to the center of tangent circle from sphere center
3738 r2=sqrt(r*r-d2*d2); // radius of the tangent circle
3739 l2=d-d2; // dist from circle center to eye
3740
3741 ang0=acos(u1.x*u2.x+u1.y*u2.y+u1.z*u2.z); // angular separation of 2 centers
3742 ang1=atan2(r1,l1); // half-cone angle of bottom sphere
3743 ang2=atan2(r2,l2); // half-cone angle of top sphere
3744
3745 if(ang1>=0.98*(ang0+ang2)||ang2>=0.98*(ang0+ang1)){ // only one sphere is visible
3746 if(ang2>ang1){ // only top sphere is visible
3747 s1=s2; // sphere center position
3748 d1=d2; // dstance to the center of circle from sphere center
3749 r1=r2; // radius of tangent circle
3750 u1=u2; // unit vector toward viewer
3751 }
3752 ToPolar3d(&u1,&a);
3753
3754 tae.x=s1.x+d1*u1.x;
3755 tae.y=s1.y+d1*u1.y;
3756 tae.z=s1.z+d1*u1.z;
3757 tae.theta=1.5707963268-a.y; // z axis toward viewer's eye
3758 tae.phi=a.x+1.5707963268;
3759 tae.psi=0.;
3760 TAEtoTAM(&tae,&tam);
3761
3762 u.x=r1*cos(0.);
3763 u.y=r1*sin(0.);
3764 u.z=0.;
3765 BodyToFrameTAM(&u,&tam,&b);
3766 FrameToBodyTAM(&b,po,&u);
3767 NormPoint3d(&u,&b,&d);
3768 ToPolar3d(&b,&e);
3769 e.y=1.5707963267-e.y;
3770 f=e;
3771 for(i=0;i<90;i++){
3772 g=f;
3773 if(i<89){
3774 ang=.06981317008*(i+1); // every 4 degree
3775 u.x=r1*cos(ang);
3776 u.y=r1*sin(ang);
3777 u.z=0.;
3778 BodyToFrameTAM(&u,&tam,&b);
3779 FrameToBodyTAM(&b,po,&u);
3780 NormPoint3d(&u,&b,&d);
3781 ToPolar3d(&b,&f);
3782 f.y=1.5707963267-f.y;
3783 }
3784 else f=e;
3785 seg[i].sx=g.x;
3786 seg[i].sy=g.y;
3787 seg[i].ex=f.x;
3788 seg[i].ey=f.y;
3789
3790 }
3791 blk->start=0;
3792 blk->pte=90;
3793 cont->start=0;
3794 cont->pte=90;
3795 return;
3796 }
3797 // cylinder part is visible
3798 theta=1.5707963268+asin((ang1-ang2)/ang0); // half-visible angle of bottom circle
3799 // construct coordinate system with z toward eye and x toward bottom from top
3800 v.x=u2.x-u1.x; // unnormalized vector from top to bottom
3801 v.y=u2.y-u1.y;
3802 v.z=u2.z-u1.z;
3803 tz.sx=s1.x+d1*u1.x; // center of bottom circle
3804 tz.sy=s1.y+d1*u1.y;
3805 tz.sz=s1.z+d1*u1.z;
3806 tz.ex=tz.sx+u1.x; // vector toward viewer
3807 tz.ey=tz.sy+u1.y;
3808 tz.ez=tz.sz+u1.z;
3809
3810 VectorProduct(&u1,&v,&w); // obtain y-axis
3811 ty=tz;
3812 ty.ex=ty.sx+w.x; // vector toward viewer
3813 ty.ey=ty.sy+w.y;
3814 ty.ez=ty.sz+w.z;
3815 LinesToTAM(&ty,&tz,yzAxis,&tam);
3816
3817 dthet=.06981317008; // angle of a segment
3818 n=(int)(theta/dthet); // half number of segments
3819 theta=n*dthet;
3820 u.x=r1*cos(-theta);
3821 u.y=r1*sin(-theta);
3822 u.z=0.;
3823 BodyToFrameTAM(&u,&tam,&b);
3824 FrameToBodyTAM(&b,po,&u);
3825 NormPoint3d(&u,&b,&d);
3826 ToPolar3d(&b,&e);
3827 e.y=1.5707963267-e.y;
3828 f=e;
3829 n*=2;
3830 for(i=0;i<n;i++){
3831 g=f;
3832 ang=dthet*(i+1)-theta; // -theta+dthet to theta
3833 u.x=r1*cos(ang);
3834 u.y=r1*sin(ang);
3835 u.z=0.;
3836 BodyToFrameTAM(&u,&tam,&b);
3837 FrameToBodyTAM(&b,po,&u);
3838 NormPoint3d(&u,&b,&d);
3839 ToPolar3d(&b,&f);
3840 f.y=1.5707963267-f.y;
3841 seg[i].sx=g.x;
3842 seg[i].sy=g.y;
3843 seg[i].ex=f.x;
3844 seg[i].ey=f.y;
3845
3846 }
3847
3848 // construct coordinate system with z toward eye and x toward bottom from top
3849 v.x=u2.x-u1.x; // unnormalized vector from top to bottom
3850 v.y=u2.y-u1.y;
3851 v.z=u2.z-u1.z;
3852 tz.sx=s2.x+d2*u2.x; // center of top circle
3853 tz.sy=s2.y+d2*u2.y;
3854 tz.sz=s2.z+d2*u2.z;
3855 tz.ex=tz.sx+u2.x; // vector toward viewer
3856 tz.ey=tz.sy+u2.y;
3857 tz.ez=tz.sz+u2.z;
3858
3859 VectorProduct(&u2,&v,&w); // obtain y-axis
3860 ty=tz;
3861 ty.ex=ty.sx+w.x; // vector toward viewer
3862 ty.ey=ty.sy+w.y;
3863 ty.ez=ty.sz+w.z;
3864 LinesToTAM(&ty,&tz,yzAxis,&tam);
3865
3866 for(;i<91;i++){
3867 g=f;
3868 ang=dthet*i-theta;
3869 u.x=r2*cos(ang);
3870 u.y=r2*sin(ang);
3871 u.z=0.;
3872 BodyToFrameTAM(&u,&tam,&b);
3873 FrameToBodyTAM(&b,po,&u);
3874 NormPoint3d(&u,&b,&d);
3875 ToPolar3d(&b,&f);
3876 f.y=1.5707963267-f.y;
3877 seg[i].sx=g.x;
3878 seg[i].sy=g.y;
3879 seg[i].ex=f.x;
3880 seg[i].ey=f.y;
3881 }
3882 seg[i].sx=f.x;
3883 seg[i].sy=f.y;
3884 seg[i].ex=e.x;
3885 seg[i].ey=e.y;
3886 blk->start=0;
3887 blk->pte=92;
3888 cont->start=0;
3889 cont->pte=92;
3890}
3891
3892
3893/******************************************************************************
3894 FieldOfView
3895
3896 Get horizontal and vertical field of view to cover the object
3897 ******************************************************************************/
3898
3899void CMPbodyOfRot::FieldOfView(TransAngleMatrix *vu,Point2d *az,Point2d *el)
3900{
3901 TransAngleEuler tae;
3902 TransAngleMatrix tam;
3903 int i;
3904 Point3d s;
3905 Point3d f;
3906 Point2d a;
3907 double r0,z0,r1,z1,ca,sa,ang,d;
3908
3909 r0=p0.x;
3910 z0=p0.y;
3911 r1=p1.x;
3912 z1=p1.y;
3913 tae.x=xg; // Translate Angle Euler for the part
3914 tae.y=yg;
3915 tae.z=zg;
3916 tae.theta=thetag;
3917 tae.phi=phig;
3918 tae.psi=psig;
3919 TAEtoTAM(&tae,&tam);
3920 az->x=10.;
3921 az->y=-10.;
3922 el->x=10.;
3923 el->y=-10.;
3924 for(i=0;i<12;i++){
3925 ang=0.01745329*30.*i;
3926 ca=cos(ang);
3927 sa=sin(ang);
3928 s.x=r0*ca;
3929 s.y=r0*sa;
3930 s.z=z0;
3931 BodyToFrameTAM(&s,&tam,&f);
3932 FrameToBodyTAM(&f,vu,&s);
3933 NormPoint3d(&s,&s,&d);
3934 ToPolar3d(&s,&a);
3935 if(a.x<az->x) az->x=a.x;
3936 if(a.x>az->y) az->y=a.x;
3937 if(a.y<el->x) el->x=a.y;
3938 if(a.y>el->y) el->y=a.y;
3939 s.x=r1*ca;
3940 s.y=r1*sa;
3941 s.z=z1;
3942 BodyToFrameTAM(&s,&tam,&f);
3943 FrameToBodyTAM(&f,vu,&s);
3944 NormPoint3d(&s,&s,&d);
3945 ToPolar3d(&s,&a);
3946 if(a.x<az->x) az->x=a.x;
3947 if(a.x>az->y) az->y=a.x;
3948 if(a.y<el->x) el->x=a.y;
3949 if(a.y>el->y) el->y=a.y;
3950 }
3951}
3952
3953/********************************************************************
3954 CThreeD.cp
3955********************************************************************/
3956
3957void CThreeD::Draw()
3958{
3959 Line3d s;
3960 double aEye,dEye,rEye,xTgt,yTgt,zTgt;
3961 double w,h,d,f,ang1,ang2;
3962 Point2d p;
3963 Point3d x,y,z;
3964 double theta;
3965 double phi;
3966 double psi;
3967 double azi,inc;
3968
3969// outstr("at the start of CThreeD::Draw()\n");
3970// flush();
3971 AlignCoord();
3972
3973// VuControl();
3974// outstr("at the exit of CThreeD::VuControl()\n");
3975// flush();
3976
3977 aEye=Azi;
3978 dEye=Ele;
3979 rEye=Dst;
3980
3981 xTgt=Tgx;
3982 yTgt=Tgy;
3983 zTgt=Tgz;
3984
3985 if(!contAll){
3986 f=rEye*cos(d2r*dEye);
3987 w=f*cos(d2r*aEye)+xTgt;
3988 h=f*sin(d2r*aEye)+yTgt;
3989 d=rEye*sin(d2r*dEye)+zTgt;
3990 rEye=sqrt(w*w+h*h+d*d);
3991 aEye=atan2(h,w)/d2r;
3992 dEye=atan2(d,sqrt(w*w+h*h))/d2r;
3993 }
3994
3995 s.sx=rEye*cos(d2r*dEye)*cos(d2r*aEye);
3996 s.sy=rEye*cos(d2r*dEye)*sin(d2r*aEye);
3997 s.sz=rEye*sin(d2r*dEye);
3998
3999 s.ex=xTgt;
4000 s.ey=yTgt;
4001 s.ez=zTgt;
4002 MakeThePolar(&s,&thePolar); // Perspective
4003
4004// convert to double and prepare for conversion to polar coordinate
4005 w=0.5*Wid;
4006 h=0.5*Hei;
4007 d=sqrt(w*w+h*h);
4008 f=Foc;
4009 focus=Foc;
4010
4011// outstr("at the start of processing the sides\n");
4012// flush();
4013
4014// start with right side of the frame
4015 azi=atan2(w,-h); // azimuth
4016 inc=atan2(d,f); // inclination
4017 RtBm.x=azi;
4018 RtBm.y=1.5707963267-inc;
4019 ClipAngleMatrix(azi,inc,&frameRt,&angFrameRt);
4020
4021// top side of the frame
4022 azi=atan2(w,h); // azimuth
4023 inc=atan2(d,f); // inclination
4024 TpRt.x=azi;
4025 TpRt.y=1.5707963267-inc;
4026 ClipAngleMatrix(azi,inc,&frameTp,&angFrameTp);
4027
4028// left side of the frame
4029 azi=atan2(-w,h); // azimuth
4030 inc=atan2(d,f); // inclination
4031 LtTp.x=azi;
4032 LtTp.y=1.5707963267-inc;
4033 ClipAngleMatrix(azi,inc,&frameLt,&angFrameLt);
4034
4035// bottom side of the frame
4036 azi=atan2(-w,-h); // azimuth
4037 inc=atan2(d,f); // inclination
4038 BmLt.x=azi;
4039 BmLt.y=1.5707963267-inc;
4040 ClipAngleMatrix(azi,inc,&frameBm,&angFrameBm);
4041
4042// outstr("at the start of MemorySetUp()\n");
4043// flush();
4044
4045 MemorySetUp();
4046
4047// outstr("at the start of MakePart2()\n");
4048// flush();
4049
4050 MakePart2(); // part2 is surfaces to be used in the far-near comp
4051
4052// outstr("at the start of NearFarRelations()\n");
4053// flush();
4054
4055 NearFarRelations();
4056
4057// outstr("at the start of DrawView()\n");
4058// flush();
4059
4060 DrawView();
4061 part.clear();
4062 npart=0;
4063 message=0;
4064}
4065
4066/******************************************************************************
4067 AlignCoord
4068
4069 calculate general coordinate
4070******************************************************************************/
4071void CThreeD::AlignCoord(void)
4072{
4073 CMPpart *p1;
4074 CMPpart *p2;
4075 TransAngleEuler a,b,c;
4076 int i;
4077
4078 for(i=0;i<npart;i++){
4079 p1=part[i];
4080 a.x=p1->x0;
4081 a.y=p1->y0;
4082 a.z=p1->z0;
4083 a.theta=0.01745329252*p1->theta0;
4084 a.phi=0.01745329252*p1->phi0;
4085 a.psi=0.01745329252*p1->psi0;
4086 if(i==0) c=a;
4087 else{
4088 p2=part[p1->refPart];
4089 b.x=p2->xg;
4090 b.y=p2->yg;
4091 b.z=p2->zg;
4092 b.theta=p2->thetag;
4093 b.phi=p2->phig;
4094 b.psi=p2->psig;
4095 MoveTwiceTAE(&b,&a,&c);
4096 }
4097 p1->xg=c.x;
4098 p1->yg=c.y;
4099 p1->zg=c.z;
4100 p1->thetag=c.theta;
4101 p1->phig=c.phi;
4102 p1->psig=c.psi;
4103 }
4104}
4105
4106
4107void CThreeD::VuControl (void)
4108
4109{
4110 int i;
4111
4112 if(contAll){
4113 TransAngleEuler tae;
4114 TransAngleMatrix tam;
4115 Point2d az,azm,el,elm;
4116 double a,b,e,d;
4117 int i,j;
4118 CMPpart *p;
4119 Point3d los,tgt;
4120
4121 a=0.017453293*Azi;
4122 e=0.017453293*Ele;
4123 d=Dst;
4124 tae.x=d*cos(e)*cos(a);
4125 tae.y=d*cos(e)*sin(a);
4126 tae.z=d*sin(e);
4127 if(e>=0.){
4128 tae.theta=e;
4129 tae.phi=a-1.5707963267;
4130 tae.psi=-1.5707963267;
4131 }
4132 else{
4133 tae.theta=-e;
4134 tae.phi=a+1.5707963267;
4135 tae.psi=1.5707963267;
4136 }
4137 TAEtoTAM(&tae,&tam);
4138 azm.x=10.;
4139 azm.y=-10.;
4140 elm.x=10.;
4141 elm.y=-10.;
4142// outstr("in CThreeD::VuControl()\n");
4143// sprintf(msg,"npart=%i\n",npart);
4144// outstr(msg);
4145// flush();
4146 for(i=0;i<npart;i++){
4147 p=part[i];
4148 if(p->showDrawing){
4149 switch(p->kind){
4150 case MPlines:
4151 reinterpret_cast<CMPlines *>(p)->FieldOfView(&tam,&az,&el);
4152 break;
4153 case MPplane:
4154 reinterpret_cast<CMPplane *>(p)->FieldOfView(&tam,&az,&el);
4155 break;
4156 case MPbox:
4157 reinterpret_cast<CMPbox *>(p)->FieldOfView(&tam,&az,&el);
4158 break;
4159 case MPsphere:
4160 reinterpret_cast<CMPsphere *>(p)->FieldOfView(&tam,&az,&el);
4161 break;
4162 case MPbodyOfRot:
4163 reinterpret_cast<CMPbodyOfRot *>(p)->FieldOfView(&tam,&az,&el);
4164 break;
4165 default:
4166 az.x=10.;
4167 az.y=-10.;
4168 el.x=10.;
4169 el.y=-10.;
4170 break;
4171 }
4172 if(az.x<azm.x) azm.x=az.x;
4173 if(az.y>azm.y) azm.y=az.y;
4174 if(el.x<elm.x) elm.x=el.x;
4175 if(el.y>elm.y) elm.y=el.y;
4176 }
4177 }
4178 los.x=d*cos(0.5*(elm.x+elm.y))*cos(0.5*(azm.x+azm.y));
4179 los.y=d*cos(0.5*(elm.x+elm.y))*sin(0.5*(azm.x+azm.y));
4180 los.z=d*sin(0.5*(elm.x+elm.y));
4181 BodyToFrameTAM(&los,&tam,&tgt);
4182 Tgx=tgt.x;
4183 Tgy=tgt.y;
4184 Tgz=tgt.z;
4185 a=Wid/2-5;
4186 a=a/tan(0.5*(azm.y-azm.x));
4187 b=Hei/2-5;
4188 b=b/tan(0.5*(elm.y-elm.x));
4189 Foc=(int)((a<b)? a:b);
4190 }
4191}
4192
4193
4194/********************************************************************
4195 MemorySetUp
4196
4197 Analize itsList and allocate memory for following handles
4198
4199 part1part obtain the 'part1' number for a given 'part' number
4200 partpart1 obtain the 'part' number for a given 'part1' number
4201 part2part1 obtain the 'part2' number for a given 'part1' number
4202 part2 array of surfaces indexed by 'part2' number
4203 local array of surface points referred by part2
4204 viewed local converted to polar coordinate
4205 sorted part 1 sorted to the farthest order
4206
4207 part1part and partpart1 are defined in this routine
4208********************************************************************/
4209void CThreeD::MemorySetUp( void)
4210{
4211 int i,j,k,l,m;
4212 int ptesti;
4213 CMPpart *p;
4214 CMPpart *p0; // the first CMPplane of the consecutive group planes
4215 CMPpart *p1;
4216
4217
4218 part1part=new StartPTE[npart]; // (StartPTE *)NewPtr(sizeof(StartPTE)*npart);
4219
4220 npart1=0; // to be incremented by j
4221 npart2m=0; // to be incremented by k
4222 npoinm=0; // to be incremented by l
4223
4224 p0=NULL;
4225
4226 for(i=0;i<npart;i++){
4227 p=part[i]; // itsList->NthItem(i+1)
4228 if(p->showDrawing){
4229 switch(p->kind){
4230 case MPpoint:
4231 j=0; // points are not drawn
4232 k=0; // no near-far comparison
4233 l=0; // no surface points
4234 break;
4235 case MPlines:
4236 j=reinterpret_cast<CMPlines *>(p)->npoint;
4237 j--; // number of segments is number of points minus one
4238 k=j; // one line segment is one generalized surface
4239 l=j*2; // each generalized surface has two points
4240 break;
4241 case MPplane:
4242 j=1; // a plane is a drawing unit
4243 k=1; // one unit in the far-near comparison
4244 l=reinterpret_cast<CMPplane *>(p)->npoint; // points defining the plane
4245
4246 if(reinterpret_cast<CMPplane *>(p)->isGroup){
4247 if(p0==NULL) p0=p;
4248 p1=part[i+1];
4249 if(p1->kind==MPplane){
4250 if(reinterpret_cast<CMPplane *>(p1)->isGroup)
4251 reinterpret_cast<CMPplane *>(p)->otherHalf=reinterpret_cast<CMPplane *>(p1);
4252 else{
4253 reinterpret_cast<CMPplane *>(p)->otherHalf=reinterpret_cast<CMPplane *>(p0);
4254 p0=NULL;
4255 }
4256 }
4257 else{
4258 reinterpret_cast<CMPplane *>(p)->otherHalf=reinterpret_cast<CMPplane *>(p0);
4259 p0=NULL;
4260 }
4261 }
4262 if(reinterpret_cast<CMPplane *>(p)->isPolyhed)
4263 if(!reinterpret_cast<CMPplane *>(p)->FacingViewer((Point3d *)&thePolar)){
4264 j=0;
4265 k=0;
4266 l=0;
4267 }
4268 break;
4269 case MPbox:
4270 j=1; // a box is a drawing unit
4271 k=3; // 3 surfaces max
4272 l=12; // 4 points * 3 surfaces
4273 break;
4274 case MPsphere:
4275 j=1; // a sphere is a drawing unit
4276 k=1; // represented by a surface
4277 l=12; // represented by a 12 sided polygon
4278 break;
4279 case MPbodyOfRot:
4280 if(reinterpret_cast<CMPbodyOfRot *>(p)->isTube){
4281 j=2; // near and far sides
4282 k=12; // 12 surfaces combined, 6 surfaces each
4283 l=48; // 4 points * 12 surfaces
4284 }
4285 else{
4286 j=1; // represented by a drawing unit
4287 k=13; // 12 surfaces + top
4288 l=60; // 4 points * 12 surfaces + 12 points
4289 }
4290 break;
4291 }
4292 }
4293 else{ // if showDrawing is false skip data
4294 j=0;
4295 k=0;
4296 l=0;
4297 }
4298 part1part[i].start=npart1;
4299 npart1+=j;
4300 part1part[i].pte=npart1;
4301 npart2m+=k;
4302 npoinm+=l;
4303 }
4304 npoinm+=5; // alloance for increase by clipping
4305 partpart1=new StartPTE[npart1]; // (StartPTE *)NewPtr(sizeof(StartPTE)*npart1);
4306 for(i=0;i<npart;i++){
4307 j=part1part[i].start;
4308 k=part1part[i].pte;
4309 m=0;
4310 for(l=j;l<k;l++){
4311 partpart1[l].start=i;
4312 partpart1[l].pte=m++;
4313 }
4314 }
4315
4316 part2part1=new StartPTE[npart1]; // (StartPTE *)NewPtr(sizeof(StartPTE)*npart1);
4317 part2=new Surface[npart2m]; // (Surface *)NewPtr(sizeof(Surface)*npart2m);
4318 part1part2=new int[npart2m]; // (int *)NewPtr(sizeof(short)*npart2m);
4319 local=new Point2d[npoinm]; // (Point2d *)NewPtr(sizeof(Point2d)*npoinm);
4320 viewed=new Point2d[npoinm]; //(Point2d *)NewPtr(sizeof(Point2d)*npoinm);
4321// sorted=(int *)NewPtr(sizeof(short)*npart1);
4322
4323
4324}
4325
4326
4327/********************************************************************
4328 MakePart2
4329********************************************************************/
4330void CThreeD::MakePart2( void)
4331{
4332 int i,j,k,l,m,n;
4333 int ptesti;
4334 CMPpart *p;
4335 double x,y,z,x1,y1,z1;
4336 Surface surf1;
4337 Surface surf2;
4338 Point2d *surfPt1;
4339 Point2d *surfPt2;
4340 Point2d a;
4341 TransAngleMatrix tam;
4342
4343 npart2=0; // number of parts after no 2 decomposition
4344 npoin=0;
4345 surfPt1=new Point2d[50]; // (Point2d *)NewPtr(sizeof(Point2d)*50);
4346 surfPt2=new Point2d[50]; // (Point2d *)NewPtr(sizeof(Point2d)*50);
4347
4348// outstr("in MakePart2()\n");
4349// sprintf(msg,"npart=%i\n",npart);
4350// outstr(msg);
4351// flush();
4352
4353 for(i=0;i<npart;i++){
4354 j=part1part[i].start;
4355 k=part1part[i].pte;
4356// sprintf(msg,"i=%i j=%i k=%i\n",i,j,k);
4357// outstr(msg);
4358// flush();
4359 if(j<k){
4360 p=part[i]; // (CMPpart *)itsList->NthItem(i+1);
4361 switch(p->kind){
4362 case MPpoint:
4363 part2part1[j].start=-1; // part2 not generated
4364 part2part1[j].pte=-1;
4365 break;
4366 case MPlines:
4367 l=reinterpret_cast<CMPlines *>(p)->npoint;
4368 for(m=0;m<l-1;m++){
4369 reinterpret_cast<CMPlines *>(p)->GetSurface(m,&thePolar,&surf1,surfPt1);
4370 if(surf1.start<0){ // degenerate to a point
4371 surf2.start=-1;
4372 surf2.pte=-1;
4373 }
4374 else ClipSurface(&surf1,surfPt1,&surf2,surfPt2);
4375 if(surf2.start!=surf2.pte){
4376 part2[npart2]=surf2;
4377 part2[npart2].start=npoin;
4378 local[npoin++]=surfPt2[0];
4379 local[npoin++]=surfPt2[1];
4380 part2[npart2].pte=npoin;
4381 part1part2[npart2]=j;
4382 part2part1[j].start=npart2;
4383 npart2++;
4384 part2part1[j].pte=npart2;
4385 }
4386 else{
4387 part2part1[j].start=-1; // part2 not generated
4388 part2part1[j].pte=-1;
4389 }
4390 j++;
4391 }
4392 break;
4393 case MPplane:
4394 reinterpret_cast<CMPplane *>(p)->GetSurface(&thePolar,&surf1,surfPt1);
4395 ClipSurface(&surf1,surfPt1,&surf2,surfPt2);
4396 if(surf2.start!=surf2.pte){
4397 part2[npart2]=surf2;
4398 part2[npart2].start=npoin;
4399 for(n=surf2.start;n<surf2.pte;n++)
4400 local[npoin++]=surfPt2[n];
4401 part2[npart2].pte=npoin;
4402 part1part2[npart2]=j;
4403 part2part1[j].start=npart2;
4404 npart2++;
4405 part2part1[j].pte=npart2;
4406 }
4407 else{
4408 part2part1[j].start=-1; // part2 not generated
4409 part2part1[j].pte=-1;
4410 }
4411 break;
4412 case MPbox:
4413// outstr("in MakePart2() at MPbox\n");
4414// sprintf(msg,"npart2=%i\n",npart2);
4415// outstr(msg);
4416// flush();
4417
4418 part1part2[npart2]=j;
4419 part2part1[j].start=npart2;
4420 reinterpret_cast<CMPbox *>(p)->ClearVisSurface();
4421 for(m=0;m<3;m++){
4422
4423 if(reinterpret_cast<CMPbox *>(p)->GetSurface(m,&thePolar,&surf1,surfPt1)){
4424// outstr("at the exit of GetSurface()\n");
4425// sprintf(msg,"m=%i surf1.x0=%8.3f surf1.y0=%8.3f surf1.z0=%8.3f \n",m,surf1.x0,surf1.y0,surf1.z0);
4426// outstr(msg);
4427// sprintf(msg," surf1.theta0=%8.3f surf1.phi0=%8.3f surf1.psi0=%8.3f \n",surf1.theta0,surf1.phi0,surf1.psi0);
4428// outstr(msg);
4429// flush();
4430
4431 ClipSurface(&surf1,surfPt1,&surf2,surfPt2);
4432// outstr("at the exit of ClipSurface()\n");
4433// sprintf(msg," surf2.start=%i surf2.pte=%i npart2=%i npoin=%i j=%i\n",surf2.start,surf2.pte,npart2,npoin,j);
4434// outstr(msg);
4435// flush();
4436 if(surf2.start!=surf2.pte){
4437 part2[npart2]=surf2;
4438 part2[npart2].start=npoin;
4439 for(n=surf2.start;n<surf2.pte;n++)
4440 local[npoin++]=surfPt2[n];
4441 part2[npart2].pte=npoin;
4442 part1part2[npart2]=j;
4443 npart2++;
4444 }
4445 }
4446
4447 }
4448 if(part2part1[j].start!=npart2)
4449 part2part1[j].pte=npart2;
4450 else{
4451 part2part1[j].start=-1; // part2 not generated
4452 part2part1[j].pte=-1;
4453 }
4454 break;
4455 case MPsphere:
4456 reinterpret_cast<CMPsphere *>(p)->GetSurface(&thePolar,&surf1,surfPt1);
4457 ClipSurface(&surf1,surfPt1,&surf2,surfPt2);
4458 if(surf2.start!=surf2.pte){
4459 part2[npart2]=surf2;
4460 part2[npart2].start=npoin;
4461 for(n=surf2.start;n<surf2.pte;n++)
4462 local[npoin++]=surfPt2[n];
4463 part2[npart2].pte=npoin;
4464 part1part2[npart2]=j;
4465 part2part1[j].start=npart2;
4466 npart2++;
4467 part2part1[j].pte=npart2;
4468 }
4469 else{
4470 part2part1[j].start=-1; // part2 not generated
4471 part2part1[j].pte=-1;
4472 }
4473 break;
4474 case MPbodyOfRot:
4475 part1part2[npart2]=j;
4476 part2part1[j].start=npart2;
4477 reinterpret_cast<CMPbodyOfRot *>(p)->ClearVisSurface();
4478 for(m=0;m<13;m++){
4479 if(reinterpret_cast<CMPbodyOfRot *>(p)->GetSurface(m,true, // near side
4480 &thePolar,&surf1,surfPt1)){
4481 ClipSurface(&surf1,surfPt1,&surf2,surfPt2);
4482 if(surf2.start!=surf2.pte){
4483 part2[npart2]=surf2;
4484 part2[npart2].start=npoin;
4485 for(n=surf2.start;n<surf2.pte;n++)
4486 local[npoin++]=surfPt2[n];
4487 part2[npart2].pte=npoin;
4488 part1part2[npart2]=j;
4489 npart2++;
4490 }
4491 }
4492 }
4493 if(part2part1[j].start!=npart2)
4494 part2part1[j].pte=npart2;
4495 else{
4496 part2part1[j].start=-1; // part2 not generated
4497 part2part1[j].pte=-1;
4498 }
4499 if(reinterpret_cast<CMPbodyOfRot *>(p)->isTube){
4500 j++;
4501 part1part2[npart2]=j;
4502 part2part1[j].start=npart2;
4503 for(m=0;m<10;m++){
4504 if(reinterpret_cast<CMPbodyOfRot *>(p)->GetSurface(m,false,
4505 &thePolar,&surf1,surfPt1)){
4506 ClipSurface(&surf1,surfPt1,&surf2,surfPt2);
4507 if(surf2.start!=surf2.pte){
4508 part2[npart2]=surf2;
4509 part2[npart2].start=npoin;
4510 for(n=surf2.start;n<surf2.pte;n++)
4511 local[npoin++]=surfPt2[n];
4512 part1part2[npart2]=j;
4513 part2[npart2++].pte=npoin;
4514 }
4515 }
4516 }
4517 if(part2part1[j].start!=npart2)
4518 part2part1[j].pte=npart2;
4519 else{
4520 part2part1[j].start=-1; // part2 not generated
4521 part2part1[j].pte=-1;
4522 }
4523 }
4524 break;
4525 }
4526 }
4527 }
4528 delete[] surfPt1;
4529 delete[] surfPt2;
4530// convert local to viewed
4531 for(i=0;i<npart2;i++){
4532 surf1=part2[i];
4533 SurfthePolarTAM((TransAngleEuler *)&surf1,&thePolar,&tam);
4534 for(j=surf1.start;j<surf1.pte;j++){
4535 SurfToPolar(&local[j],&tam,&a);
4536 PolarToGnomo(&a,&viewed[j]);
4537 }
4538 }
4539 delete[] local; // allocated at MemorySetUp()
4540}
4541
4542
4543/********************************************************************
4544 NearFarRelations
4545
4546 establish near-far relationship
4547********************************************************************/
4548
4549void CThreeD::NearFarRelations(void)
4550{
4551 int i,j,k,l,m;
4552 CMPpart *p;
4553 Surface s1,s2;
4554 bool s1isLine;
4555 bool doit;
4556 Point2d p0,p1;
4557 TransAngleMatrix m1,m2;
4558 double d;
4559 char ss[100];
4560 int kind1,kind2;
4561 Point3d center;
4562 Point3d s1z,s2z; // normal to the surface vector in view coordinate
4563 double s1m,s2m; // multiplier in the expression calculating z
4564 long prog,progm;
4565 int progs;
4566 CFarNear *farN;
4567 FILE *fp;
4568
4569 farN=new CFarNear(npart1);
4570
4571 // fp=fopen("/tmp/gcs3DDATA","w");
4572 // fprintf(fp,"npart=%i npart1=%i npart2=%i\n",npart,npart1,npart2);
4573 // fclose(fp);
4574
4575// enter far-near relationship of tubes
4576 for(i=0;i<npart;i++){
4577 p=part[i];
4578 if(p->showDrawing&&p->kind==MPbodyOfRot){
4579 if(reinterpret_cast<CMPbodyOfRot *>(p)->isTube){
4580 j=part1part[i].start;
4581 farN->AddFarNear(j+1,j);
4582// outstr("in NearFarRelations()\n");
4583// sprintf(msg,"i=%i j=%i\n",i,j);
4584// outstr(msg);
4585// flush();
4586
4587 }
4588 }
4589 }
4590
4591// if a line coinsides a side of a plane, put the line in farther position
4592 for(i=0;i<npart;i++){
4593 p=part[i];
4594 if(p->showDrawing&&p->kind==MPplane){
4595 double s1z[3],s20[3];
4596 k=part1part[i].start;
4597 s1=part2[part2part1[k].start];
4598 s1z[0]=sin(s1.theta0)*sin(s1.phi0); // axis normal to the plane
4599 s1z[1]=-sin(s1.theta0)*cos(s1.phi0);
4600 s1z[2]=cos(s1.theta0);
4601 for(j=0;j<npart2;j++){
4602 s2=part2[j];
4603 if(s2.pte-s2.start==2){ // line
4604 s20[0]=s2.x0-s1.x0;
4605 s20[1]=s2.y0-s1.y0;
4606 s20[2]=s2.z0-s1.z0;
4607 d=s1z[0]*s20[0]+s1z[0]*s20[0]+s1z[0]*s20[0];
4608 if(fabs(d)<1.e-10){
4609 m=0;
4610 for(l=s1.start;l<s1.pte;l++){
4611 if(fabs(viewed[i].x-viewed[s2.start].x)<1.e-5
4612 &&fabs(viewed[i].y-viewed[s2.start].y)<1.e-5) m++;
4613 if(fabs(viewed[i].x-viewed[s2.start+1].x)<1.e-5
4614 &&fabs(viewed[i].y-viewed[s2.start+1].y)<1.e-5) m++;
4615 }
4616 if(m==2){
4617 farN->AddFarNear(part1part2[j],k); // line plane
4618 }
4619 }
4620 }
4621 }
4622 }
4623 }
4624
4625// outstr("in NearFarRelations()\n");
4626// sprintf(msg,"npart2=%i\n",npart2);
4627// outstr(msg);
4628// flush();
4629
4630// compare each surfaces in the combination triangle
4631 prog=0; // for progress status (long)
4632 progm=npart2;
4633 progm=progm*(progm-1)/2;
4634 for(i=0;i<npart2-1;i++){
4635 k=part1part2[i];
4636 s1=part2[i];
4637 if(s1.pte-s1.start==2) s1isLine=true;
4638 else s1isLine=false;
4639
4640// sprintf(msg,"i=%i k=%i s1.start=%i s1.pte=%i partpart1[k].start=%i\n",i,k,s1.start,s1.pte,partpart1[k].start);
4641// outstr(msg);
4642// flush();
4643
4644 p=part[partpart1[k].start];
4645 kind1=p->kind;
4646 if(kind1==MPbox) reinterpret_cast<CMPbox *>(p)->GetCenter(¢er);
4647 if(kind1==MPbodyOfRot) reinterpret_cast<CMPbodyOfRot *>(p)->GetCenter(¢er);
4648 SurfthePolarTAM((TransAngleEuler *)&s1,&thePolar,&m1);
4649 if(kind1==MPplane){ // used when both parts are planes
4650 Point3d p3;
4651 p3.x=0.;
4652 p3.y=0.;
4653 p3.z=1.; // a point along body z axis
4654 BodyToFrameTAM(&p3,&m1,&s1z);
4655 s1z.x-=m1.x;
4656 s1z.y-=m1.y;
4657 s1z.z-=m1.z;
4658 s1m=s1z.x*m1.x+s1z.y*m1.y+s1z.z*m1.z; // multiplier
4659 }
4660 for(j=i+1;j<npart2;j++){
4661 prog++;
4662 l=part1part2[j];
4663 s2=part2[j];
4664 doit=true;
4665 if(k==l) doit=false;
4666 else if(s1isLine&&(s2.pte-s2.start==2)) doit=false; // dont do it for lines
4667 else doit=!(farN->InferFarNear(k,l,&m));
4668// sprintf(msg,"j=%i k=%i l=%i doit=%i\n",j,k,l,doit);
4669// outstr(msg);
4670//flush();
4671// fp=fopen("/tmp/gcs3DDATA","w");
4672// fprintf(fp,"i=%i j=%i k=%i l=%i doit=%i\n",i,j,k,l,doit);
4673// fclose(fp);
4674
4675 if(doit){
4676// sprintf(msg,"part2[i].start=%i part2[i].pte=%i part2[j].start=%i part2[j].pte=%i\n",
4677// part2[i].start,part2[i].pte,part2[j].start,part2[j].pte);
4678// outstr(msg);
4679// flush();
4680
4681 if(Overlap(viewed,(StartPTE *)&part2[i].start,
4682 (StartPTE *)&part2[j].start,&p0)){
4683// outstr("Overlap\n");
4684// flush();
4685 SurfthePolarTAM((TransAngleEuler *)&s2,&thePolar,&m2);
4686 GnomoToPolar(&p0,&p1);
4687 FarNearDist(&m1,&m2,&p1,&d);
4688 if(d>0.001){
4689 farN->AddFarNear(k,l);
4690 doit=false;
4691 }
4692 if(d<-0.001){
4693 farN->AddFarNear(l,k);
4694 doit=false;
4695 }
4696 if(doit){
4697 if(kind1==MPplane||kind1==MPbox||kind1==MPbodyOfRot){
4698 p=part[partpart1[l].start];
4699 kind2=p->kind;
4700 if(kind1==MPplane){
4701 if(kind2==MPplane){ // process the planes with common side
4702 Point3d p3;
4703 double z1,z2;
4704 int m,n10,n1p,n1n,n20,n2p,n2n; // zero, plus or negative
4705 p3.x=0.;
4706 p3.y=0.;
4707 p3.z=1.;
4708 BodyToFrameTAM(&p3,&m2,&s2z);
4709 s2z.x-=m2.x;
4710 s2z.y-=m2.y;
4711 s2z.z-=m2.z;
4712 s2m=s2z.x*m2.x+s2z.y*m2.y+s2z.z*m2.z;
4713 n10=0;
4714 n1p=0;
4715 n1n=0;
4716 n20=0;
4717 n2p=0;
4718 n2n=0;
4719 if(fabs(s1z.x*s2z.x+s1z.y*s2z.y+s1z.z*s2z.z)<0.98){ // not parallel
4720 for(m=s1.start;m<s1.pte;m++){
4721 z1=s1m/(s1z.x*viewed[m].y+s1z.y*viewed[m].x+s1z.z);
4722 z2=s2m/(s2z.x*viewed[m].y+s2z.y*viewed[m].x+s2z.z);
4723 if(z2>0.){
4724 if(fabs(z1-z2)<0.001) n10++;
4725 else if(z1>z2) n1p++;
4726 else n1n++;
4727 }
4728 }
4729 for(m=s2.start;m<s2.pte;m++){
4730 z1=s1m/(s1z.x*viewed[m].y+s1z.y*viewed[m].x+s1z.z);
4731 z2=s2m/(s2z.x*viewed[m].y+s2z.y*viewed[m].x+s2z.z);
4732 if(z1>0.){
4733 if(fabs(z1-z2)<0.001) n20++;
4734 else if(z1>z2) n2p++;
4735 else n2n++;
4736 }
4737 }
4738 if(n10>1&&n20>1){
4739 if(n1p>0&&n2p>0){ // s1 is farther
4740 farN->AddFarNear(k,l);
4741 doit=false;
4742 }
4743 if(n1n>0&&n2n>0){ // s2 is farther
4744 farN->AddFarNear(l,k);
4745 doit=false;
4746 }
4747 } // if(n10>1&&n20>1){
4748 } // if(fabs(s1z.x*s2z.x+s1z.y*s2z.y+s1z.z*s2z.z)<0.98){
4749 } // if(kind2==MPplane){
4750 else if(kind2==MPbox||kind2==MPbodyOfRot){
4751 if(kind2==MPbox) reinterpret_cast<CMPbox *>(p)->GetCenter(¢er);
4752 if(kind2==MPbodyOfRot) reinterpret_cast<CMPbodyOfRot *>(p)->GetCenter(¢er);
4753 if(SameSideOfSurface(&s1,¢er,(Point3d *)&thePolar)){
4754 farN->AddFarNear(k,l);
4755 doit=false;
4756 }
4757 else{
4758 farN->AddFarNear(l,k);
4759 doit=false;
4760 }
4761 } // else if(kind2==MPbox||kind2==MPbodyOfRot){
4762 } // if(kind1==MPplane){
4763 else{ // kind1==MPbox or MPbodyOfRot
4764 if(kind2==MPplane){
4765 if(SameSideOfSurface(&s2,¢er,(Point3d *)&thePolar)){
4766 farN->AddFarNear(l,k);
4767 doit=false;
4768 }
4769 else{
4770 farN->AddFarNear(k,l);
4771 doit=false;
4772 }
4773 } // if(kind2==MPplane){
4774 } // else
4775 } // if(kind1==MPplane||kind1==MPbox||kind1==MPbodyOfRot){
4776 } // if(doit){
4777/* if(doit){
4778
4779 GnomoToPolar(&p0,&p1);
4780 FarNearDist(&m1,&m2,&p1,&d);
4781 if(d>0.){
4782 farN->AddFarNear(k,l);
4783 }
4784 if(d<0.){
4785 farN->AddFarNear(l,k);
4786 }
4787 }*/
4788 } // if(Overlap(viewed,(StartPTE *)&part2[i].start,(StartPTE *)&part2[j].start,&p0)){
4789 } // if(doit){
4790
4791 } // for(j=i+1;j<npart2;j++){
4792 } // for(i=0;i<npart2-1;i++){
4793
4794 delete[] part2part1; // allocated in MemorySetUp
4795 delete[] part1part2; // allocated in MemorySetUp
4796 delete[] part2; // allocated in MemorySetUp
4797 delete[] viewed; // allocated in MemorySetUp
4798 delete[] part1part; // allocated in MemorySetUp at the beginning
4799
4800 sorted=new int[npart1]; // (int *)NewPtr(sizeof(short)*npart1);
4801 fp=fopen("/tmp/gcs3DDATA","w");
4802 fprintf(fp,"before SetSorted npart1=%i\n",npart1);
4803 fclose(fp);
4804
4805 m=farN->SetSorted(sorted);
4806 if(m!=0){
4807// sprintf(ss,"\rloop found in farNear m=%i",m);
4808// Monitor(90,(Ptr)ss,true);
4809 }
4810 delete farN;
4811 fp=fopen("/tmp/gcs3DDATA","w");
4812 fprintf(fp,"at the end of NearFarRelations m=%i\n",m);
4813 fclose(fp);
4814
4815}
4816
4817
4818/********************************************************************
4819 DrawView
4820
4821 draw in the view
4822********************************************************************/
4823void CThreeD::DrawView(void)
4824{
4825 int i,j,k,l,m,n,o,q;
4826 StartPTE pt;
4827 CMPpart *p;
4828 Line2d *seg;
4829 StartPTE blk;
4830 StartPTE cont;
4831 double x,y;
4832 int ix,iy;
4833 double maxang;
4834 int red,green,blue;
4835 bool finished[200];
4836 bool isLines;
4837 FILE *fp;
4838 char *buff;
4839
4840
4841 maxang=atan(10000./focus);
4842
4843 seg=new Line2d[200]; // (Line2d *)NewPtr(sizeof(Line2d)*200);
4844
4845 // MakeView(2*Wid,2*Hei);
4846 // SetColor(100,100,100);
4847 // OpenPoly();
4848 // MoveTo(0,0);
4849 // LineTo(0,2*Hei);
4850 // LineTo(2*Wid,2*Hei);
4851 // LineTo(2*Wid,0);
4852 // ClosePoly();
4853 fp=fopen("/tmp/gcs3DDATA","w");
4854 fwrite(&npart,sizeof(int),1,fp);
4855 fwrite(&npart1,sizeof(int),1,fp);
4856 for(i=0;i<npart1;i++){
4857 j=sorted[i]; // part 1 index
4858 pt=partpart1[j]; // part index
4859 k=pt.start;
4860 p=part[k]; // (CMPpart *)itsList->NthItem(k+1);
4861 isLines=false;
4862 switch (p->kind){
4863 case MPlines:
4864 reinterpret_cast<CMPlines *>(p)->GetDrawData(pt.pte,&thePolar,seg,&blk,&cont);
4865 isLines=true;
4866 break;
4867 case MPplane:
4868 reinterpret_cast<CMPplane *>(p)->GetDrawData(&thePolar,seg,&blk,&cont);
4869 break;
4870 case MPbox:
4871 reinterpret_cast<CMPbox *>(p)->GetDrawData(&thePolar,seg,&blk,&cont);
4872 break;
4873 case MPsphere:
4874 reinterpret_cast<CMPsphere *>(p)->GetDrawData(&thePolar,seg,&blk,&cont);
4875 break;
4876 case MPbodyOfRot:
4877 reinterpret_cast<CMPbodyOfRot *>(p)->GetDrawData(pt.pte,&thePolar,seg,&blk,&cont);
4878 break;
4879 }
4880 l=blk.pte;
4881 k=4+4+2*4+2*4+l*4*8+10;
4882 buff=(char *)malloc(sizeof(char)*k);
4883 *(int *)buff=p->color;
4884 *(int *)(buff+4)=isLines;
4885 *(int *)(buff+8)=blk.start;
4886 *(int *)(buff+12)=blk.pte;
4887 *(int *)(buff+16)=cont.start;
4888 *(int *)(buff+20)=cont.pte;
4889 for(j=0;j<l;j++){
4890 *(double *)(buff+24+j*32)=seg[j].sx;
4891 *(double *)(buff+24+j*32+8)=seg[j].sy;
4892 *(double *)(buff+24+j*32+16)=seg[j].ex;
4893 *(double *)(buff+24+j*32+24)=seg[j].ey;
4894 }
4895 fwrite(&k,sizeof(int),1,fp);
4896 fwrite(buff,sizeof(char),k,fp);
4897 free(buff);
4898 }
4899 k=0; /* end of data */
4900 fwrite(&k,sizeof(int),1,fp);
4901 k=90;
4902 buff=(char *)malloc(sizeof(char)*k);
4903 *(double *)buff=thePolar.x;
4904 *(double *)(buff+8)=thePolar.y;
4905 *(double *)(buff+8*2)=thePolar.z;
4906 *(double *)(buff+8*3)=thePolar.c[0][0];
4907 *(double *)(buff+8*4)=thePolar.c[0][1];
4908 *(double *)(buff+8*5)=thePolar.c[0][2];
4909 *(int *)(buff+8*6)=p->kind;
4910 *(double *)(buff+52)=p->x0;
4911 *(double *)(buff+52+8)=p->y0;
4912 *(double *)(buff+52+8*2)=p->z0;
4913 *(double *)(buff+52+8*3)=p->xg;
4914 fwrite(buff,sizeof(char),k,fp);
4915 free(buff);
4916 fclose(fp);
4917#if 0
4918 k=cont.start; // supposed to cover all segments
4919 l=blk.pte;
4920 for(j=k;j<l;j++){
4921 x=seg[j].sx;
4922 y=seg[j].sy;
4923 if(y>maxang) y=maxang;
4924 y=2*focus*tan(y);
4925 seg[j].sx=y*sin(x);
4926 seg[j].sy=y*cos(x);
4927 x=seg[j].ex;
4928 y=seg[j].ey;
4929 if(y>maxang) y=maxang;
4930 y=2*focus*tan(y);
4931 seg[j].ex=y*sin(x);
4932 seg[j].ey=y*cos(x);
4933 }
4934
4935 if(l-k>1){
4936 switch(p->color){
4937 case 0:
4938 red=100;
4939 green=100;
4940 blue=100;
4941 break;
4942 case 1:
4943 red=75;
4944 green=75;
4945 blue=75;
4946 break;
4947 case 2:
4948 red=100;
4949 green=75;
4950 blue=75;
4951 break;
4952 case 3:
4953 red=100;
4954 green=100;
4955 blue=75;
4956 break;
4957 case 4:
4958 red=75;
4959 green=100;
4960 blue=75;
4961 break;
4962 case 5:
4963 red=75;
4964 green=100;
4965 blue=100;
4966 break;
4967 case 6:
4968 red=75;
4969 green=75;
4970 blue=100;
4971 break;
4972 case 7:
4973 red=100;
4974 green=75;
4975 blue=100;
4976 break;
4977 }
4978 // SetColor(red,green,blue);
4979 // OpenPoly();
4980// PenSize(1,1);
4981 k=cont.start; // start of contour
4982 l=cont.pte-cont.start; // number of contour segments
4983 q=k;
4984 ix=(int)(seg[q].sx+Wid);
4985 iy=(int)(Hei-seg[q].sy);
4986 // MoveTo(ix,iy);
4987 for(n=0;n<l;n++) finished[n]=false;
4988 for(m=0;m<l;m++){
4989 ix=(int)(seg[q].ex+Wid);
4990 iy=(int)(Hei-seg[q].ey);
4991 // LineTo(ix,iy);
4992 for(n=0;n<l;n++){
4993 if(!finished[n]){
4994 o=k+n;
4995 if(seg[q].ex==seg[o].sx && seg[q].ey==seg[o].sy){
4996 q=o;
4997 finished[n]=true;
4998 break;
4999 }
5000 }
5001 }
5002 }
5003 // ClosePoly();
5004 }
5005// PenSize(ContPen,ContPen);
5006// SetColor(0,0,0);
5007 if(isLines){
5008 switch(p->color){
5009 case 1:
5010// PenSize(2,2);
5011 break;
5012 case 2:
5013// PenSize(4,4);
5014 break;
5015 case 3:
5016// PenSize(8,8);
5017 break;
5018 case 4:
5019 red=100;
5020 green=0;
5021 blue=0;
5022 // SetColor(red,green,blue);
5023// PenSize(4,4);
5024 break;
5025 case 5:
5026 red=100;
5027 green=0;
5028 blue=0;
5029 // SetColor(red,green,blue);
5030// PenSize(8,8);
5031 break;
5032 case 6:
5033 red=0;
5034 green=0;
5035 blue=100;
5036 // SetColor(red,green,blue);
5037// PenSize(4,4);
5038 break;
5039 case 7:
5040 red=0;
5041 green=0;
5042 blue=100;
5043 // SetColor(red,green,blue);
5044// PenSize(8,8);
5045 break;
5046 }
5047 }
5048 k=blk.start;
5049 l=blk.pte;
5050 for(m=k;m<l;m++){
5051 ix=(int)(seg[m].sx+Wid);
5052 iy=(int)(Hei-seg[m].sy);
5053 // MoveTo(ix,iy);
5054 ix=(int)(seg[m].ex+Wid);
5055 iy=(int)(Hei-seg[m].ey);
5056 // LineTo(ix,iy);
5057 }
5058 if(isLines){
5059// PenSize(1,1);
5060 red=0;
5061 green=0;
5062 blue=0;
5063 // SetColor(red,green,blue);
5064 }
5065 }
5066#endif
5067 delete[] seg; // DisposPtr((Ptr)seg);
5068 delete[] partpart1; // DisposPtr((Ptr)partpart1);
5069 delete[] sorted; // DisposPtr((Ptr)sorted);
5070
5071
5072}
5073
5074
5075/********************************************************************
5076 ClipSurface
5077
5078 clip a surface by the view frame
5079********************************************************************/
5080void CThreeD::ClipSurface(Surface *surf1,Point2d *surfPt1,
5081 Surface *surf2,Point2d *surfPt2)
5082{
5083 TransAngleMatrix s; // body:surface, frame:thePolar
5084 Point2d p0,p1,p2,p3,p4;
5085 int nx; // number of crossing points
5086 Point2d p[8]; // crossing point
5087 int ix[8]; // start point index
5088 int jx[8]; // status 1 : coming-in, 2 : going-out
5089 int kx[8]; // clip-side 1:Rt, 2:Tp, 3:Lt, 4:Bm
5090 double ang[8]; // angle from the corner
5091 int status; // returned status
5092 double an; // returned angle
5093 bool inside;
5094 int i,j,k,l;
5095
5096 SurfthePolarTAM((TransAngleEuler *)surf1,&thePolar,&s); // Perspective
5097 SurfToPolar(&surfPt1[0],&s,&p0); // (x,y) to (alpha,delta)
5098 p2=p0;
5099 nx=0;
5100// outstr("in ClipSurface()\n");
5101// sprintf(msg," surf1->start=%i surf1->pte=%i \n",surf1->start,surf1->pte);
5102// outstr(msg);
5103// flush();
5104
5105 for(i=surf1->start;i<surf1->pte;i++){
5106 p1=p2;
5107 if(i+1==surf1->pte){
5108 if(surf1->pte-surf1->start==2) break;
5109 else p2=p0;
5110 }
5111 else SurfToPolar(&surfPt1[i+1],&s,&p2);
5112
5113 status=CrossClipSide(&frameRt,angFrameRt,&p1,&p2,&p3,&an); // Perspective
5114
5115 if(status<3){ // crossing or inside
5116 if(status>0){ // crossing
5117 p[nx]=p3; // crossing point in polar coordinate
5118 ix[nx]=i; // start point index
5119 jx[nx]=status; // status 1 or 2
5120 kx[nx]=1; // clip rect side - right
5121 ang[nx]=an; // distance from right-bottom corner
5122 nx++;
5123 }
5124 status=CrossClipSide(&frameTp,angFrameTp,&p1,&p2,&p3,&an);
5125
5126 }
5127 if(status<3){
5128 if(status>0){
5129 p[nx]=p3;
5130 ix[nx]=i;
5131 jx[nx]=status;
5132 kx[nx]=2;
5133 ang[nx]=an;
5134 nx++;
5135 }
5136 status=CrossClipSide(&frameLt,angFrameLt,&p1,&p2,&p3,&an);
5137
5138 }
5139 if(status<3){
5140 if(status>0){
5141 p[nx]=p3;
5142 ix[nx]=i;
5143 jx[nx]=status;
5144 kx[nx]=3;
5145 ang[nx]=an;
5146 nx++;
5147 }
5148 status=CrossClipSide(&frameBm,angFrameBm,&p1,&p2,&p3,&an);
5149
5150 }
5151 if(status<3){
5152 if(status>0){
5153 p[nx]=p3;
5154 ix[nx]=i;
5155 jx[nx]=status;
5156 kx[nx]=4;
5157 ang[nx]=an;
5158 nx++;
5159 }
5160 }
5161
5162
5163 }
5164// outstr("in ClipSurface()\n");
5165// sprintf(msg," nx=%i status=%i surf1->start=%i surf1->pte=%i \n",nx,status,surf1->start,surf1->pte);
5166// outstr(msg);
5167// flush();
5168
5169
5170 if(nx==0){
5171 if(status==0){ // the whole surface is inside the clip rect
5172 *surf2=*surf1;
5173 for(i=surf1->start;i<surf1->pte;i++) surfPt2[i]=surfPt1[i];
5174 return;
5175 }
5176 else{
5177 *surf2=*surf1;
5178 surf2->pte=surf2->start; // set past-the-end to start
5179 return;
5180 }
5181 }
5182// if two crossings occur for a segment, 'enter' comes first and then comes 'leave'
5183 for(i=1;i<nx;i++){
5184 if(ix[i]==ix[i-1]){
5185 if(jx[i]==1){ // enter
5186 p3=p[i];
5187 p[i]=p[i-1];
5188 p[i-1]=p3;
5189 j=jx[i];
5190 jx[i]=jx[i-1];
5191 jx[i-1]=j;
5192 j=kx[i];
5193 kx[i]=kx[i-1];
5194 kx[i-1]=j;
5195 an=ang[i];
5196 ang[i]=ang[i-1];
5197 ang[i-1]=an;
5198 }
5199 }
5200 }
5201
5202// add corner points after 'leave' and before 'enter'
5203 for(i=0;i<nx;i++){
5204 if(jx[i]==2){ // leave
5205 j=i+1;
5206 if(j==nx) j=0; // next enter point
5207 k=kx[i]; // side of leave point
5208 while(k!=kx[j]){
5209 k++;
5210 if(k==5) k=1;
5211 for(l=nx;l>i+1;l--){ // make room for a corner point
5212 p[l]=p[l-1];
5213 ix[l]=ix[l-1];
5214 jx[l]=jx[l-1];
5215 kx[l]=kx[l-1];
5216 ang[l]=ang[l-1];
5217 }
5218 switch(k){
5219 case 1:
5220 p[i+1]=RtBm;
5221 break;
5222 case 2:
5223 p[i+1]=TpRt;
5224 break;
5225 case 3:
5226 p[i+1]=LtTp;
5227 break;
5228 case 4:
5229 p[i+1]=BmLt;
5230 break;
5231 }
5232 ix[i+1]=ix[i];
5233 jx[i+1]=4; // status is corner point
5234 kx[i+1]=k;
5235 ang[i+1]=0.;
5236 nx++;
5237 i++;
5238 }
5239 }
5240 }
5241
5242// convert polar coordinate to surface coordinate
5243 for(i=0;i<nx;i++) PolarToSurf(&p[i],&s,&p[i]);
5244
5245// generate new surface
5246 j=0; // index to crossing points
5247 k=0; // index to output points
5248 for(i=surf1->start;i<surf1->pte;i++){
5249 inside=false;
5250 for(j=0;j<nx;j++){
5251 if(i<=ix[j]&&jx[j]==2) {
5252 inside=true;
5253 break;
5254 }
5255 if(i<=ix[j]&&jx[j]==1) {
5256 inside=false;
5257 break;
5258 }
5259 }
5260 if(j==nx&&jx[0]==2) inside=true;
5261 if(inside){
5262 surfPt2[k]=surfPt1[i];
5263 k++;
5264 }
5265 for(j=0;j<nx;j++){
5266 if(i==ix[j]){
5267 surfPt2[k]=p[j];
5268 k++;
5269 }
5270 }
5271 }
5272 *surf2=*surf1;
5273 surf2->start=0;
5274 surf2->pte=k;
5275
5276}
5277
5278/********************************************************************
5279 ToDB
5280
5281 generate stream data
5282********************************************************************/
5283char *CThreeD::ToDB(int *len)
5284{
5285 int i,j,k,l,m,n;
5286 CMPpart *p;
5287 Point2d *p2;
5288 Point3d *p3;
5289 char *e;
5290
5291 n=0;
5292 n+=4; /* npart */
5293 for(i=0;i<npart;i++){
5294 n+=4; /* i */
5295 p=part[i];
5296 n+=4; /* kind */
5297 j=strlen(p->desc.c_str());
5298 n+=4; /* j */
5299 n+=j+1; /* strcpy */
5300 n+=4; /* refPart */
5301 n+=8*6; /* x0,y0,z0,theta0,phi0,psi0 */
5302 n+=4; /* color */
5303 n+=4; /* showDrawing */
5304 switch(p->kind){
5305 case MPpoint:
5306 break;
5307 case MPlines:
5308 k=reinterpret_cast<CMPlines *>(p)->npoint;
5309 n+=4; /* k */
5310 n+=k*24; /* x,y,z */
5311 n+=4; /* line */
5312 break;
5313 case MPplane:
5314 k=reinterpret_cast<CMPplane *>(p)->npoint;
5315 n+=4; /* k */
5316 n+=k*16; /* x,y */
5317 n+=4; /* isPolyhed */
5318 n+=4; /* isGroup */
5319 break;
5320 case MPbox:
5321 n+=24; /* lx,ly,lz */
5322 n+=4; /* visibleSurface */
5323 break;
5324 case MPsphere:
5325 n+=8; /* r */
5326 break;
5327 case MPbodyOfRot:
5328 n+=4; /* npoint */
5329 n+=32; /* p0.x,p0.y,p1.x,p1.y */
5330 n+=16; /* isTube,oblong,appear,visibleSurfac */
5331 break;
5332 }
5333 }
5334 *len=n;
5335 e=(char *)malloc(sizeof(char)*n);
5336 n=0;
5337 *(int *)(e+n)=npart;
5338 n+=4;
5339 for(i=0;i<npart;i++){
5340 *(int *)(e+n)=i;
5341 n+=4;
5342 p=part[i];
5343 *(int *)(e+n)=p->kind;
5344 n+=4;
5345 j=strlen(p->desc.c_str());
5346 *(int *)(e+n)=j;
5347 n+=4;
5348 strcpy(e+n,p->desc.c_str());
5349 n+=j+1;
5350 *(int *)(e+n)=p->refPart;
5351 n+=4;
5352 *(double *)(e+n)=p->x0;
5353 n+=8;
5354 *(double *)(e+n)=p->y0;
5355 n+=8;
5356 *(double *)(e+n)=p->z0;
5357 n+=8;
5358 *(double *)(e+n)=p->theta0;
5359 n+=8;
5360 *(double *)(e+n)=p->phi0;
5361 n+=8;
5362 *(double *)(e+n)=p->psi0;
5363 n+=8;
5364 *(int *)(e+n)=p->color;
5365 n+=4;
5366 *(int *)(e+n)=p->showDrawing;
5367 n+=4;
5368 switch(p->kind){
5369 case MPpoint:
5370 break;
5371 case MPlines:
5372 k=reinterpret_cast<CMPlines *>(p)->npoint;
5373 *(int *)(e+n)=k;
5374 n+=4;
5375 for(j=0;j<k;j++){
5376 p3=&reinterpret_cast<CMPlines *>(p)->pt[j];
5377 *(double *)(e+n)=p3->x;
5378 n+=8;
5379 *(double *)(e+n)=p3->y;
5380 n+=8;
5381 *(double *)(e+n)=p3->z;
5382 n+=8;
5383 }
5384 *(int *)(e+n)=reinterpret_cast<CMPlines *>(p)->line;
5385 n+=4;
5386 break;
5387 case MPplane:
5388 k=reinterpret_cast<CMPplane *>(p)->npoint;
5389 *(int *)(e+n)=k;
5390 n+=4;
5391 for(j=0;j<k;j++){
5392 p2=&reinterpret_cast<CMPplane *>(p)->pt[j];
5393 *(double *)(e+n)=p2->x;
5394 n+=8;
5395 *(double *)(e+n)=p2->y;
5396 n+=8;
5397 }
5398 *(int *)(e+n)=reinterpret_cast<CMPplane *>(p)->isPolyhed;
5399 n+=4;
5400 *(int *)(e+n)=reinterpret_cast<CMPplane *>(p)->isGroup;
5401 n+=4;
5402 break;
5403 case MPbox:
5404 *(double *)(e+n)=reinterpret_cast<CMPbox *>(p)->lx;
5405 n+=8;
5406 *(double *)(e+n)=reinterpret_cast<CMPbox *>(p)->ly;
5407 n+=8;
5408 *(double *)(e+n)=reinterpret_cast<CMPbox *>(p)->lz;
5409 n+=8;
5410 *(int *)(e+n)=reinterpret_cast<CMPbox *>(p)->visibleSurface;
5411 n+=4;
5412 break;
5413 case MPsphere:
5414 *(double *)(e+n)=reinterpret_cast<CMPsphere *>(p)->r;
5415 n+=8;
5416 break;
5417 case MPbodyOfRot:
5418 *(int *)(e+n)=reinterpret_cast<CMPbodyOfRot *>(p)->npoint;
5419 n+=4;
5420 *(double *)(e+n)=reinterpret_cast<CMPbodyOfRot *>(p)->p0.x;
5421 n+=8;
5422 *(double *)(e+n)=reinterpret_cast<CMPbodyOfRot *>(p)->p0.y;
5423 n+=8;
5424 *(double *)(e+n)=reinterpret_cast<CMPbodyOfRot *>(p)->p1.x;
5425 n+=8;
5426 *(double *)(e+n)=reinterpret_cast<CMPbodyOfRot *>(p)->p1.y;
5427 n+=8;
5428 *(int *)(e+n)=reinterpret_cast<CMPbodyOfRot *>(p)->isTube;
5429 n+=4;
5430 *(int *)(e+n)=reinterpret_cast<CMPbodyOfRot *>(p)->oblong;
5431 n+=4;
5432 *(int *)(e+n)=reinterpret_cast<CMPbodyOfRot *>(p)->appear;
5433 n+=4;
5434 *(int *)(e+n)=reinterpret_cast<CMPbodyOfRot *>(p)->visibleSurface;
5435 n+=4;
5436 break;
5437 }
5438 }
5439 return e;
5440}
5441
5442/********************************************************************
5443 FromDB
5444
5445 load stream data
5446********************************************************************/
5447void CThreeD::FromDB(int len,char *e,double loc[6])
5448{
5449 int i,j,k,l,m,n,i1,i2;
5450 char s[200];
5451 CMPpart *p;
5452 Point2d *p2;
5453 Point3d *p3;
5454
5455 n=0;
5456 l=npart;
5457 m=*(int *)e;
5458 n+=4;
5459 for(i=0;i<m;i++){
5460 /* *(int *)(e+n)=i */
5461 n+=4;
5462 i1=*(int *)(e+n); // kind
5463 n+=4;
5464 i2=*(int *)(e+n); // strlen
5465 n+=4;
5466 strcpy(s,e+n);
5467 n+=i2+1;
5468 i2=*(int *)(e+n)+l; // refPart
5469 if(i==0) i2=0;
5470 n+=4;
5471 switch(i1){
5472 case MPpoint:
5473 part.push_back(new CMPpoint(s,i2,i1));
5474 break;
5475 case MPlines:
5476 part.push_back(new CMPlines(s,i2,i1));
5477 break;
5478 case MPplane:
5479 part.push_back(new CMPplane(s,i2,i1));
5480 break;
5481 case MPbox:
5482 part.push_back(new CMPbox(s,i2,i1));
5483 break;
5484 case MPsphere:
5485 part.push_back(new CMPsphere(s,i2,i1));
5486 break;
5487 case MPbodyOfRot:
5488 part.push_back(new CMPbodyOfRot(s,i2,i1));
5489 break;
5490 default:
5491 break;
5492 }
5493 npart++;
5494 p=part[npart-1];
5495 if(i==0){
5496 p->x0=loc[0];
5497 p->y0=loc[1];
5498 p->z0=loc[2];
5499 p->theta0=loc[3];
5500 p->phi0=loc[4];
5501 p->psi0=loc[5];
5502 n+=48;
5503 }
5504 else{
5505 p->x0=*(double *)(e+n);
5506 n+=8;
5507 p->y0=*(double *)(e+n);
5508 n+=8;
5509 p->z0=*(double *)(e+n);
5510 n+=8;
5511 p->theta0=*(double *)(e+n);
5512 n+=8;
5513 p->phi0=*(double *)(e+n);
5514 n+=8;
5515 p->psi0=*(double *)(e+n);
5516 n+=8;
5517 }
5518 p->color=*(int *)(e+n);
5519 n+=4;
5520 p->showDrawing=*(int *)(e+n);
5521 n+=4;
5522 switch(p->kind){
5523 case MPpoint:
5524 break;
5525 case MPlines:
5526 k=*(int *)(e+n);
5527 n+=4;
5528 for(j=0;j<k;j++){
5529 reinterpret_cast<CMPlines *>(p)->SetPoint(*(double *)(e+n),*(double *)(e+n+8),*(double *)(e+n+16));
5530 n+=24;
5531 }
5532 reinterpret_cast<CMPlines *>(p)->line=*(int *)(e+n);
5533 n+=4;
5534 break;
5535 case MPplane:
5536 k=*(int *)(e+n);
5537 n+=4;
5538 for(j=0;j<k;j++){
5539 reinterpret_cast<CMPplane *>(p)->SetPoint(*(double *)(e+n),*(double *)(e+n+8));
5540 n+=16;
5541 }
5542 reinterpret_cast<CMPplane *>(p)->isPolyhed=*(int *)(e+n);
5543 n+=4;
5544 reinterpret_cast<CMPplane *>(p)->isGroup=*(int *)(e+n);
5545 n+=4;
5546 break;
5547 case MPbox:
5548 reinterpret_cast<CMPbox *>(p)->SetSize(*(double *)(e+n),*(double *)(e+n+8),*(double *)(e+n+16));
5549 n+=24;
5550 reinterpret_cast<CMPbox *>(p)->visibleSurface=*(int *)(e+n);
5551 n+=4;
5552 break;
5553 case MPsphere:
5554 reinterpret_cast<CMPsphere *>(p)->r=*(double *)(e+n);
5555 n+=8;
5556 break;
5557 case MPbodyOfRot:
5558 reinterpret_cast<CMPbodyOfRot *>(p)->npoint=*(int *)(e+n);
5559 n+=4;
5560 reinterpret_cast<CMPbodyOfRot *>(p)->p0.x=*(double *)(e+n);
5561 n+=8;
5562 reinterpret_cast<CMPbodyOfRot *>(p)->p0.y=*(double *)(e+n);
5563 n+=8;
5564 reinterpret_cast<CMPbodyOfRot *>(p)->p1.x=*(double *)(e+n);
5565 n+=8;
5566 reinterpret_cast<CMPbodyOfRot *>(p)->p1.y=*(double *)(e+n);
5567 n+=8;
5568 reinterpret_cast<CMPbodyOfRot *>(p)->isTube=*(int *)(e+n);
5569 n+=4;
5570 reinterpret_cast<CMPbodyOfRot *>(p)->oblong=*(int *)(e+n);
5571 n+=4;
5572 reinterpret_cast<CMPbodyOfRot *>(p)->appear=*(int *)(e+n);
5573 n+=4;
5574 reinterpret_cast<CMPbodyOfRot *>(p)->visibleSurface=*(int *)(e+n);
5575 n+=4;
5576 break;
5577 }
5578 }
5579}
5580
5581/******************************************************************************
5582 CFarNear Far-Near relationship of 3D Drawing Objects
5583 ******************************************************************************/
5584
5585
5586CFarNear::CFarNear(int numpart1)
5587: npart1(numpart1)
5588{
5589 int i;
5590
5591 nmax=npart1*(npart1-1)/2;
5592 farNear=new StartPTE[nmax+1]; // (StartPTE *)NewPtr(sizeof(StartPTE)*nmax);
5593 farNearIndex=new long[npart1+1]; // (long *)NewPtr(sizeof(long)*(npart1+1));
5594 for(i=0;i<=npart1;i++) farNearIndex[i]=0L;
5595 n=0;
5596}
5597
5598/******************************************************************************
5599 Dispose
5600
5601 ******************************************************************************/
5602
5603CFarNear::~CFarNear()
5604{
5605 delete[] farNear; // DisposPtr((Ptr)farNear);
5606 delete[] farNearIndex; // DisposPtr((Ptr)farNearIndex);
5607}
5608
5609/******************************************************************************
5610 AddFarNear
5611
5612 ******************************************************************************/
5613
5614void CFarNear::AddFarNear(int far,int near)
5615{
5616 StartPTE a;
5617 long m;
5618 int i;
5619
5620 a.start=far;
5621 a.pte=near;
5622 m=farNearIndex[a.start+1];
5623 for(i=a.start+1;i<=npart1;i++) farNearIndex[i]++;
5624// BlockMove((Ptr)&farNear[m],(Ptr)&farNear[m+1],(n-m)*sizeof(StartPTE));
5625 for(i=n;i>=m;i--) farNear[i+1]=farNear[i];
5626 farNear[m]=a;
5627 n++;
5628// outstr("in AddFarNear()\n");
5629// for(i=0;i<n;i++){
5630// sprintf(msg," i=%i farNear[i].start=%i farNear[i].pte=%i \n",i,farNear[i].start,farNear[i].pte);
5631// outstr(msg);
5632// }
5633// flush();
5634
5635}
5636
5637/******************************************************************************
5638 inferFarNear
5639
5640 ******************************************************************************/
5641
5642bool CFarNear::InferFarNear(int p1,int p2,int *near)
5643{
5644// test if p1 is farther
5645 *near=p2;
5646 if(RecurseFarNear(p1,p2,0)) return true;
5647// test if p2 is farther
5648 *near=p1;
5649 if(RecurseFarNear(p2,p1,0)) return true;
5650 return false;
5651}
5652
5653/******************************************************************************
5654 recurseFarNear
5655
5656 ******************************************************************************/
5657
5658bool CFarNear::RecurseFarNear(int p1,int p2,long depth)
5659{
5660 long i,j,k;
5661
5662 depth++;
5663 if(depth>n) return false; // escape loop
5664
5665 j=farNearIndex[p1];
5666 k=farNearIndex[p1+1];
5667 for(i=j;i<k;i++){
5668 if(farNear[i].pte==p2) return true;
5669 if(RecurseFarNear(farNear[i].pte,p2,depth)) return true;
5670 }
5671 return false;
5672}
5673
5674/******************************************************************************
5675 SetSorted
5676
5677 ******************************************************************************/
5678
5679int CFarNear::SetSorted(int *sorted)
5680{
5681 int i,j,l;
5682 long k,m;
5683 int *status;
5684 FILE *fp;
5685
5686// outstr("in SetSorted()\n");
5687// sprintf(msg," npart1=%i n=%i \n",npart1,n);
5688// outstr(msg);
5689// flush();
5690// for(i=0;i<n;i++){
5691// sprintf(msg," farNear[i].start=%5i farNear[i].pte=%5i\n",farNear[i].start,farNear[i].pte);
5692// outstr(msg);
5693// }
5694// flush();
5695 fp=fopen("/tmp/gcs3DDATA","w");
5696 fprintf(fp,"in SetSorted()\n");
5697 fprintf(fp," npart1=%i n=%i \n",npart1,n);
5698 for(i=0;i<n;i++){
5699 fprintf(fp," farNear[i].start=%5i farNear[i].pte=%5i\n",farNear[i].start,farNear[i].pte);
5700 }
5701 fclose(fp);
5702
5703 status=new int[npart1]; // (int *)NewPtr(sizeof(short)*npart1);
5704 for(i=0;i<npart1;i++) status[i]=0;
5705 j=0;
5706 i=0;
5707 do{
5708 for(k=0;k<n;k++){
5709 l=farNear[k].start;
5710 if(l<10000){
5711 switch (status[l]){
5712 case 0:
5713 for(m=0;m<n;m++) if(l==farNear[m].pte) break;
5714 if(m==n){
5715 sorted[j++]=l;
5716 status[l]=2;
5717 farNear[k].start=10000;
5718 farNear[k].pte=10000;
5719 }
5720 else status[l]=1;
5721 break;
5722 case 1:
5723 break;
5724 case 2:
5725 case 3:
5726 farNear[k].start=10000;
5727 farNear[k].pte=10000;
5728 break;
5729 default:
5730 break;
5731 }
5732 }
5733 }
5734 m=0;
5735 for(k=0;k<npart1;k++){
5736 l=status[k];
5737 if(l==1){
5738 m++;
5739 status[k]=0;
5740 }
5741 if(l==2){
5742 if(m<10000) m+=10000;
5743 status[k]=3;
5744 }
5745 }
5746 if(m<10000) i++;
5747 } while(i<2);
5748 fp=fopen("/tmp/gcs3DDATA","w");
5749 fprintf(fp,"at the end of SetSorted()\n");
5750 fprintf(fp," npart1=%i n=%i j=%i \n",npart1,n,j);
5751 for(i=0;i<n;i++){
5752 fprintf(fp," farNear[i].start=%5i farNear[i].pte=%5i\n",farNear[i].start,farNear[i].pte);
5753 }
5754 for(i=0;i<npart1;i++) fprintf(fp," status[i]=%i sorted[i]=%i\n",status[i],sorted[i]);
5755 fclose(fp);
5756 for(k=0;k<npart1;k++) if(status[k]!=3) sorted[j++]=k;
5757 delete[] status; // DisposPtr((Ptr)status);
5758 fp=fopen("/tmp/gcs3DDATA","w");
5759 fprintf(fp,"before retun of SetSorted()\n");
5760 fprintf(fp," npart1=%i n=%i j=%i \n",npart1,n,j);
5761 for(i=0;i<n;i++){
5762 fprintf(fp," farNear[i].start=%5i farNear[i].pte=%5i\n",farNear[i].start,farNear[i].pte);
5763 }
5764 for(i=0;i<npart1;i++) fprintf(fp," status[i]=%i sorted[i]=%i\n",status[i],sorted[i]);
5765 fclose(fp);
5766 return m;
5767}
5768
5769/**************************************************
5770
5771 Initiate adding a rigid body element
5772
5773 should be followed by following calls as required
5774 setlocal
5775 setweight
5776 setdensity
5777 setnpoint
5778 set1Dpoint
5779 set2Dpoint
5780 set3Dpoint
5781
5782***************************************************/
5783
5784int addelm(char *s,int ref,int knd1)
5785{
5786 int knd,piggy;
5787 piggy=knd1/100;
5788 knd=knd1-piggy*100;
5789 switch(knd){
5790 case MPpoint:
5791 thd.part.push_back(new CMPpoint(s,ref,knd));
5792 break;
5793 case MPlines:
5794 thd.part.push_back(new CMPlines(s,ref,knd));
5795 break;
5796 case MPplane:
5797 thd.part.push_back(new CMPplane(s,ref,knd));
5798 if(piggy) (reinterpret_cast<CMPplane *>(thd.part[thd.npart]))->isGroup=1;
5799 break;
5800 case MPbox:
5801 thd.part.push_back(new CMPbox(s,ref,knd));
5802 break;
5803 case MPsphere:
5804 thd.part.push_back(new CMPsphere(s,ref,knd));
5805 break;
5806 case MPbodyOfRot:
5807 thd.part.push_back(new CMPbodyOfRot(s,ref,knd));
5808 break;
5809 default:
5810 return -1;
5811 break;
5812 }
5813 thd.npart++;
5814 return thd.npart-1;
5815}
5816
5817/****************************************************
5818
5819 set local coordinate
5820
5821****************************************************/
5822void setlocal(double x,double y,double z,double theta,double phi,double psi)
5823{
5824 thd.part[thd.npart-1]->x0=x;
5825 thd.part[thd.npart-1]->y0=y;
5826 thd.part[thd.npart-1]->z0=z;
5827 thd.part[thd.npart-1]->theta0=theta;
5828 thd.part[thd.npart-1]->phi0=phi;
5829 thd.part[thd.npart-1]->psi0=psi;
5830}
5831
5832/***************************************************
5833
5834 set element color
5835
5836***************************************************/
5837void setcolor(int c)
5838{
5839 thd.part[thd.npart-1]->color=c;
5840}
5841
5842/***************************************************
5843
5844 set line appearance
5845
5846***************************************************/
5847int setline(int c)
5848{
5849 if(thd.part[thd.npart-1]->kind!=MPlines) return -1;
5850 (reinterpret_cast<CMPlines *>(thd.part[thd.npart-1]))->line=c;
5851 return 0;
5852}
5853
5854/***************************************************
5855
5856 set if tube for a body of rotation
5857
5858***************************************************/
5859int isTube()
5860{
5861 if(thd.part[thd.npart-1]->kind!=MPbodyOfRot) return -1;
5862 (reinterpret_cast<CMPbodyOfRot *>(thd.part[thd.npart-1]))->isTube=true;
5863 return 0;
5864}
5865
5866/***************************************************
5867
5868 set if oblong for a body of rotation
5869
5870***************************************************/
5871int oblong()
5872{
5873 if(thd.part[thd.npart-1]->kind!=MPbodyOfRot) return -1;
5874 (reinterpret_cast<CMPbodyOfRot *>(thd.part[thd.npart-1]))->oblong=true;
5875 return 0;
5876}
5877
5878/***************************************************
5879
5880 set one dimensional point as in sphere radius
5881
5882***************************************************/
5883int set1Dpoint(double x)
5884{
5885 switch(thd.part[thd.npart-1]->kind){
5886 case MPsphere:
5887 (reinterpret_cast<CMPsphere *>(thd.part[thd.npart-1]))->SetRadius(x);
5888 break;
5889 default:
5890 return -1;
5891 break;
5892 }
5893 return 0;
5894}
5895
5896/***************************************************
5897
5898 set two dimensional point as in an apex of plane
5899
5900***************************************************/
5901int set2Dpoint(double x,double y)
5902{
5903 switch(thd.part[thd.npart-1]->kind){
5904 case MPplane:
5905 (reinterpret_cast<CMPplane *>(thd.part[thd.npart-1]))->SetPoint(x,y);
5906 break;
5907 case MPbodyOfRot:
5908 (reinterpret_cast<CMPbodyOfRot *>(thd.part[thd.npart-1]))->SetPoint(x,y);
5909 break;
5910 default:
5911 return -1;
5912 break;
5913 }
5914 return 0;
5915}
5916
5917/***************************************************
5918
5919 set three dimensional point as in a point in line
5920
5921***************************************************/
5922int set3Dpoint(double x,double y,double z)
5923{
5924 switch(thd.part[thd.npart-1]->kind){
5925 case MPlines:
5926 (reinterpret_cast<CMPlines *>(thd.part[thd.npart-1]))->SetPoint(x,y,z);
5927 break;
5928 case MPbox:
5929 (reinterpret_cast<CMPbox *>(thd.part[thd.npart-1]))->SetSize(x,y,z);
5930 break;
5931 default:
5932 return -1;
5933 break;
5934 }
5935 return 0;
5936}
5937
5938void viewFrom(double a,double b,double r)
5939{
5940 thd.Azi=a;
5941 thd.Ele=b;
5942 thd.Dst=r;
5943}
5944
5945void drawMessage(int i)
5946{
5947 thd.message=i;
5948}
5949
5950
5951void viewFixed(double tgx,double tgy,double tgz,int foc)
5952{
5953 thd.contAll=false;
5954 thd.Tgx=tgx;
5955 thd.Tgy=tgy;
5956 thd.Tgz=tgz;
5957 thd.Foc=foc/2;
5958}
5959
5960int draw3D(int wid,int hei)
5961{
5962 thd.Wid=wid/2;
5963 thd.Hei=hei/2;
5964 thd.Draw();
5965 return 0;
5966}
5967
5968void getCoord(int ref,double x,double y,double z,int *ix,int *iy)
5969{
5970/* to be implemented some time later */
5971}
5972
5973char *insert3D(int *len)
5974{
5975 char *s;
5976 s=thd.ToDB(len);
5977}
5978
5979void select3D(int len,char *d,double x,double y,double z,double theta,double phi,double psi)
5980{
5981 double loc[6];
5982 loc[0]=x;
5983 loc[1]=y;
5984 loc[2]=z;
5985 loc[3]=theta;
5986 loc[4]=phi;
5987 loc[5]=psi;
5988 thd.FromDB(len,d,loc);
5989}
5990
5991void parts3D(int style)
5992{
5993 char s[200];
5994 int i,j,k,l;
5995 CMPpart *p;
5996 Point2d *p2;
5997 Point3d *p3;
5998 sprintf(s,"thd.npart=%i\n",thd.npart);
5999 outstr(s);
6000 for(i=0;i<thd.npart;i++){
6001 sprintf(s,"part index=%i ",i);
6002 outstr(s);
6003 p=thd.part[i];
6004 sprintf(s,"kind=%i ",p->kind);
6005 outstr(s);
6006 strcpy(s,p->desc.c_str());
6007 outstr(s);
6008 sprintf(s,"\n refPart=%i ",p->refPart);
6009 outstr(s);
6010 sprintf(s,"x0=%g ",p->x0);
6011 outstr(s);
6012 sprintf(s,"y0=%g ",p->y0);
6013 outstr(s);
6014 sprintf(s,"z0=%g ",p->z0);
6015 outstr(s);
6016 sprintf(s,"theta0=%g ",p->theta0);
6017 outstr(s);
6018 sprintf(s,"phi0=%g ",p->phi0);
6019 outstr(s);
6020 sprintf(s,"psi0=%g ",p->psi0);
6021 outstr(s);
6022 sprintf(s,"color=%i ",p->color);
6023 outstr(s);
6024 sprintf(s,"showDrawing=%i\n",p->showDrawing);
6025 outstr(s);
6026 switch(p->kind){
6027 case MPpoint:
6028 break;
6029 case MPlines:
6030 k=reinterpret_cast<CMPlines *>(p)->npoint;
6031 sprintf(s,"npoint=%i\n",k);
6032 for(j=0;j<k;j++){
6033 p3=&reinterpret_cast<CMPlines *>(p)->pt[j];
6034 sprintf(s,"x=%g y=%g z=%g\n",p3->x,p3->y,p3->z);
6035 outstr(s);
6036 }
6037 sprintf(s,"line=%i\n",reinterpret_cast<CMPlines *>(p)->line);
6038 outstr(s);
6039 break;
6040 case MPplane:
6041 k=reinterpret_cast<CMPplane *>(p)->npoint;
6042 sprintf(s,"npoint=%i\n",k);
6043 for(j=0;j<k;j++){
6044 p2=&reinterpret_cast<CMPplane *>(p)->pt[j];
6045 sprintf(s,"x=%g y=%g\n",p2->x,p2->y);
6046 outstr(s);
6047 }
6048 sprintf(s,"isPolyhed=%i isGroup=%i\n",reinterpret_cast<CMPplane *>(p)->isPolyhed,
6049 reinterpret_cast<CMPplane *>(p)->isGroup);
6050 break;
6051 case MPbox:
6052 sprintf(s,"lx=%g ly=%g lz=%g visibleSurface=%i\n",
6053 reinterpret_cast<CMPbox *>(p)->lx,reinterpret_cast<CMPbox *>(p)->ly,
6054 reinterpret_cast<CMPbox *>(p)->lz,
6055 reinterpret_cast<CMPbox *>(p)->visibleSurface);
6056 outstr(s);
6057 break;
6058 case MPsphere:
6059 sprintf(s,"r=%g\n",reinterpret_cast<CMPsphere *>(p)->r);
6060 outstr(s);
6061 break;
6062 case MPbodyOfRot:
6063 sprintf(s,"npoint=%i ",reinterpret_cast<CMPbodyOfRot *>(p)->npoint);
6064 outstr(s);
6065 sprintf(s,"p0.x=%g p0.y=%g ",reinterpret_cast<CMPbodyOfRot *>(p)->p0.x,
6066 reinterpret_cast<CMPbodyOfRot *>(p)->p0.y);
6067 outstr(s);
6068 sprintf(s,"p1.x=%g p1.y=%g\n",reinterpret_cast<CMPbodyOfRot *>(p)->p1.x,
6069 reinterpret_cast<CMPbodyOfRot *>(p)->p1.y);
6070 outstr(s);
6071 sprintf(s,"isTube=%i ",reinterpret_cast<CMPbodyOfRot *>(p)->isTube);
6072 outstr(s);
6073 sprintf(s,"oblong=%i ",reinterpret_cast<CMPbodyOfRot *>(p)->oblong);
6074 outstr(s);
6075 sprintf(s,"appear=%i ",reinterpret_cast<CMPbodyOfRot *>(p)->appear);
6076 outstr(s);
6077 sprintf(s,"visibleSurface=%i\n",reinterpret_cast<CMPbodyOfRot *>(p)->visibleSurface);
6078 outstr(s);
6079 }
6080 }
6081 flush();
6082}
6083
6084int setting(double a[],int b[])
6085{
6086 thd.Azi=a[0];
6087 thd.Ele=a[1];
6088 thd.Dst=a[2];
6089 thd.Tgx=a[3];
6090 thd.Tgy=a[4];
6091 thd.Tgz=a[5];
6092 thd.Wid=b[0];
6093 thd.Hei=b[1];
6094 thd.Foc=b[2];
6095 thd.contAll=b[3];
6096 return 0;
6097}
6098
6099int drawobj(int id,double a[])
6100{
6101 double rh,lb,wb,la,wa,ll,wl,xc,yc,zc;
6102 double l,w,h;
6103 switch(id){
6104 case 10:
6105 rh=0.11; /* rh*2+0.01+lb+0.01+ll=1.64 */
6106 lb=0.7;
6107 la=0.6;
6108 ll=0.7;
6109 wb=0.3;
6110 wa=0.05;
6111 wl=0.09;
6112 addelm("ref point",0,MPpoint);
6113 setlocal(a[0],a[1],a[2],a[3],a[4],a[5]);
6114 addelm("head",0,MPsphere);
6115 zc=0.82-rh;
6116 setlocal(0.,0.,zc,0.,0.,0.);
6117 setcolor(MPyellow);
6118 set1Dpoint(rh);
6119 addelm("body",0,MPbox);
6120 zc=0.82-2.*rh-0.01-lb/2.;
6121 setlocal(0.,0.,zc,0.,0.,0.);
6122 setcolor(MPred);
6123 set3Dpoint(wb/2.,wb,lb);
6124 addelm("right leg",0,MPbox);
6125 zc=-0.82+ll/2.;
6126 setlocal(0.,-wl/2-0.02,zc,0.,0.,0.);
6127 setcolor(MPgray);
6128 set3Dpoint(wl+0.02,wl,ll);
6129 addelm("left leg",0,MPbox);
6130 zc=-0.82+ll/2.;
6131 setlocal(0.,wl/2+0.02,zc,0.,0.,0.);
6132 setcolor(MPgray);
6133 set3Dpoint(wl+0.02,wl,ll);
6134 addelm("right arm",0,MPbox);
6135 zc=0.82-2.*rh-0.01-la/2.-0.01;
6136 setlocal(0.,-(wb+wa)/2.-0.01,zc,0.,0.,0.);
6137 setcolor(MPgreen);
6138 set3Dpoint(wa,wa,la);
6139 addelm("left arm",0,MPbox);
6140 zc=0.82-2.*rh-0.01-la/2.-0.01;
6141 setlocal(0.,(wb+wa)/2.+0.01,zc,0.,0.,0.);
6142 setcolor(MPgreen);
6143 set3Dpoint(wa,wa,la);
6144 break;
6145 case 20:
6146 addelm("ref point",0,MPpoint);
6147 setlocal(a[0],a[1],a[2],a[3],a[4],a[5]);
6148 addelm("foliage",0,MPbodyOfRot);
6149 setlocal(0.,0.,-3.5+2.,0.,0.,0.);
6150 set2Dpoint(1.5,0.);
6151 set2Dpoint(1.5,5.);
6152 setcolor(MPgreen);
6153 oblong();
6154 addelm("trunk",0,MPbox);
6155 setlocal(0.,0.,-3.5+1.,0.,0.,0.);
6156 set3Dpoint(0.15,0.15,2.);
6157 setcolor(MPred);
6158 break;
6159 case 30:
6160 l=4.;
6161 w=1.8;
6162 h=1.5;
6163 addelm("point",0,0);
6164 setlocal(a[0],a[1],a[2],a[3],a[4],a[5]);
6165 addelm("lside",0,2);
6166 setlocal(0.5*l,0.5*w,-0.5*h,90.,180.,0.);
6167 set2Dpoint(0.,0.15*h);
6168 set2Dpoint(0.1*l,0.15*h);
6169 set2Dpoint(0.1*l+0.1,0.);
6170 set2Dpoint(0.1*l+0.40,0.);
6171 set2Dpoint(0.1*l+0.50,0.15*h);
6172 set2Dpoint(0.84*l-0.50,0.15*h);
6173 set2Dpoint(0.84*l-0.40,0.);
6174 set2Dpoint(0.84*l-0.1,0.);
6175 set2Dpoint(0.84*l,0.15*h);
6176 set2Dpoint(l,0.15*h);
6177 set2Dpoint(l,0.6*h);
6178 set2Dpoint(0.75*l,0.6*h);
6179 set2Dpoint(0.75*l,h);
6180 set2Dpoint(0.3*l,h);
6181 set2Dpoint(0.3*l,0.6*h);
6182 set2Dpoint(0.,0.6*h);
6183 set2Dpoint(0.,0.15*h);
6184 setcolor(2); // red
6185 addelm("rside",0,2);
6186 setlocal(0.5*l,-0.5*w,-0.5*h,90.,0.,0.);
6187 set2Dpoint(0.,0.15*h);
6188 set2Dpoint(0.,0.6*h);
6189 set2Dpoint(-0.3*l,0.6*h);
6190 set2Dpoint(-0.3*l,h);
6191 set2Dpoint(-0.75*l,h);
6192 set2Dpoint(-0.75*l,0.6*h);
6193 set2Dpoint(-l,0.6*h);
6194 set2Dpoint(-l,0.15*h);
6195 set2Dpoint(-0.84*l,0.15*h);
6196 set2Dpoint(-0.84*l+0.1,0.);
6197 set2Dpoint(-0.84*l+0.40,0.);
6198 set2Dpoint(-0.84*l+0.50,0.15*h);
6199 set2Dpoint(-0.1*l-0.50,0.15*h);
6200 set2Dpoint(-0.1*l-0.40,0.);
6201 set2Dpoint(-0.1*l-0.1,0.);
6202 set2Dpoint(-0.1*l,0.15*h);
6203 set2Dpoint(0.,0.15*h);
6204 setcolor(2);
6205 addelm("front",0,2);
6206 setlocal(0.5*l,-0.5*w,-0.5*h+0.15*h,90.,90.,0.);
6207 set2Dpoint(0.,0.);
6208 set2Dpoint(w,0.);
6209 set2Dpoint(w,(0.6-0.15)*h);
6210 set2Dpoint(0.,0.45*h);
6211 set2Dpoint(0.,0.);
6212 setcolor(2);
6213 addelm("fender",0,2);
6214 setlocal(0.5*l,-0.5*w,-0.5*h+0.6*h,0.,90.,0.);
6215 set2Dpoint(0.,0.);
6216 set2Dpoint(w,0.);
6217 set2Dpoint(w,0.3*l);
6218 set2Dpoint(0.,0.3*l);
6219 set2Dpoint(0.,0.);
6220 setcolor(2);
6221 addelm("front window",0,2);
6222 setlocal(0.2*l,-0.5*w,-0.5*h+0.6*h,90.,90.,0.);
6223 set2Dpoint(0.,0.);
6224 set2Dpoint(w,0.);
6225 set2Dpoint(w,0.4*h);
6226 set2Dpoint(0.,0.4*h);
6227 set2Dpoint(0.,0.);
6228 setcolor(6);
6229 addelm("roof",0,2);
6230 setlocal(0.2*l,-0.5*w,-0.5*h+h,0.,90.,0.);
6231 set2Dpoint(0.,0.);
6232 set2Dpoint(w,0.);
6233 set2Dpoint(w,(0.75-0.3)*l);
6234 set2Dpoint(0.,0.45*l);
6235 set2Dpoint(0.,0.);
6236 setcolor(2);
6237 addelm("rear window",0,2);
6238 setlocal(-0.25*l,-0.5*w,-0.5*h+h,90.,-90.,180.);
6239 set2Dpoint(0.,0.);
6240 set2Dpoint(w,0.);
6241 set2Dpoint(w,0.4*h);
6242 set2Dpoint(0.,0.4*h);
6243 set2Dpoint(0.,0.);
6244 setcolor(6);
6245 addelm("trunk",0,2);
6246 setlocal(-0.25*l,-0.5*w,-0.5*h+0.6*h,0.,90.,0.);
6247 set2Dpoint(0.,0.);
6248 set2Dpoint(w,0.);
6249 set2Dpoint(w,0.25*l);
6250 set2Dpoint(0.,0.25*l);
6251 set2Dpoint(0.,0.);
6252 setcolor(2);
6253 addelm("rear",0,2);
6254 setlocal(-0.5*l,-0.5*w,-0.5*h+0.6*h,90.,-90.,180.);
6255 set2Dpoint(0.,0.);
6256 set2Dpoint(w,0.);
6257 set2Dpoint(w,(0.6-0.15)*h);
6258 set2Dpoint(0.,0.45*h);
6259 set2Dpoint(0.,0.);
6260 setcolor(2);
6261 break;
6262 case 40:
6263 addelm("point",0,0);
6264 setlocal(a[0],a[1],a[2],a[3],a[4],a[5]);
6265 addelm("box",0,3);
6266 setlocal(0.,0.,0.,0.,0.,0.);
6267 set3Dpoint(1.,1.,1.);
6268 setcolor(2);
6269 break;
6270 default:
6271 break;
6272 }
6273 thd.Draw();
6274 return 0;
6275}
6276
6277