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