draw15.cc
0000#include <math.h>
0001#include <stdio.h>
0002#include "draw15.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/*
1138void 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 TAEtoTAM((TransAngleEuler *)surf,&tam);
1821 FrameToBodyTAM((Point3d *)po,&tam,&d); // if d>0 +z axis is facing the viewer
1822 np=0; // number of effective points
1823 if(d.z>0.){
1824 x1=pt[npoint-1].x;
1825 y1=pt[npoint-1].y;
1826 }
1827 else{
1828 x1=pt[0].x;
1829 y1=pt[0].y;
1830 }
1831 for(i=0;i<npoint;i++){
1832 if(d.z>0.) j=i;
1833 else j=npoint-i-1;
1834 surfPt[np].x=pt[j].x;
1835 surfPt[np].y=pt[j].y;
1836 if(x1!=surfPt[np].x||y1!=surfPt[np].y){
1837 x1=surfPt[np].x;
1838 y1=surfPt[np].y;
1839 if(np<47) np++; // taking into some allowance to max 50
1840 }
1841 }
1842 surf->pte=np;
1843}
1844
1845
1846/******************************************************************************
1847 GetDrawData
1848
1849 Return data for drawing
1850
1851 po thePolar
1852
1853 seg array of line segments (clockwise angle from top, angle from line of sight)
1854 blk indices of segs which are to be drawn with lines
1855 cont indices of closed line segments. Inside of the closure
1856 will be painted by a color.
1857
1858 ******************************************************************************/
1859
1860void CMPplane::GetDrawData(TransAngleMatrix *po,Line2d *seg,StartPTE *blk,StartPTE *cont)
1861{
1862 StartPTE blk1;
1863 StartPTE cont1;
1864 int i,j;
1865 double sx1,sy1,ex1,ey1;
1866 CMPplane *next;
1867
1868 cont->start=0;
1869 GetDrawData1(po,seg,blk,cont);
1870 if(otherHalf){
1871 bool face1,face2;
1872 face1=FacingViewer((Point3d *)po);
1873 next=otherHalf;
1874 while(next!=this){
1875 cont1.start=blk->pte;
1876 next->GetDrawData1(po,seg,&blk1,&cont1);
1877 face2=next->FacingViewer((Point3d *)po);
1878 for(i=blk->start;i<cont->pte;i++){
1879 if(face1==face2){
1880 sx1=seg[i].sx;
1881 sy1=seg[i].sy;
1882 ex1=seg[i].ex;
1883 ey1=seg[i].ey;
1884 }
1885 else{
1886 sx1=seg[i].ex;
1887 sy1=seg[i].ey;
1888 ex1=seg[i].sx;
1889 ey1=seg[i].sy;
1890 }
1891 for(j=cont1.start;j<cont1.pte;j++){
1892 double d0,d1,d2,d3;
1893 d0=seg[j].sx-ex1;
1894 d1=seg[j].sy-ey1;
1895 d2=seg[j].ex-sx1;
1896 d3=seg[j].ey-sy1;
1897 if(d0*d0+d1*d1+d2*d2+d3*d3<0.000001) break;
1898// if(seg[j].sx==ex1&&seg[j].sy==ey1&&seg[j].ex==sx1&&seg[j].ey==sy1)
1899// break;
1900 }
1901 if(j!=cont1.pte){
1902 for(j=i;j>0;j--) seg[j]=seg[j-1];
1903 seg[0].sx=sx1;
1904 seg[0].sy=sy1;
1905 seg[0].ex=ex1;
1906 seg[0].ey=ey1;
1907 blk->start++;
1908 }
1909 }
1910 next=next->otherHalf;
1911 }
1912 }
1913
1914
1915}
1916
1917void CMPplane::GetDrawData1(TransAngleMatrix *po,Line2d *seg,StartPTE *blk,StartPTE *cont)
1918{
1919 TransAngleEuler tae;
1920 TransAngleMatrix tam;
1921 Point3d a,b,c,d;
1922 Point2d e,f,g;
1923 double r;
1924 int i,j,k;
1925
1926 k=cont->start;
1927 tae.x=xg; // Translate Angle Euler for the part
1928 tae.y=yg;
1929 tae.z=zg;
1930 tae.theta=thetag;
1931 tae.phi=phig;
1932 tae.psi=psig;
1933 TAEtoTAM(&tae,&tam);
1934 a.x=pt[0].x;
1935 a.y=pt[0].y;
1936 a.z=0.;
1937 BodyToFrameTAM(&a,&tam,&b);
1938 FrameToBodyTAM(&b,po,&c);
1939 NormPoint3d(&c,&d,&r);
1940 ToPolar3d(&d,&e);
1941 e.y=1.5707963267-e.y;
1942 f=e;
1943 for(i=1;i<=npoint;i++){
1944 g=f;
1945 if(i<npoint){
1946 a.x=pt[i].x;
1947 a.y=pt[i].y;
1948 a.z=0.;
1949 BodyToFrameTAM(&a,&tam,&b);
1950 FrameToBodyTAM(&b,po,&c);
1951 NormPoint3d(&c,&d,&r);
1952 ToPolar3d(&d,&f);
1953 f.y=1.5707963267-f.y;
1954 }
1955 else f=e;
1956 seg[k].sx=g.x;
1957 seg[k].sy=g.y;
1958 seg[k].ex=f.x;
1959 seg[k].ey=f.y;
1960 k++;
1961
1962 }
1963 blk->start=cont->start;
1964 blk->pte=blk->start+npoint;
1965 cont->pte=cont->start+npoint;
1966}
1967
1968/******************************************************************************
1969 FieldOfView
1970
1971 Get horizontal and vertical field of view to cover the object
1972 ******************************************************************************/
1973
1974void CMPplane::FieldOfView(TransAngleMatrix *vu,Point2d *az,Point2d *el)
1975{
1976 TransAngleEuler tae;
1977 TransAngleMatrix tam;
1978 int i;
1979 Point2d p;
1980 Point3d s;
1981 Point3d f;
1982 Point2d a;
1983 double d;
1984
1985 tae.x=xg; // Translate Angle Euler for the part
1986 tae.y=yg;
1987 tae.z=zg;
1988 tae.theta=thetag;
1989 tae.phi=phig;
1990 tae.psi=psig;
1991 TAEtoTAM(&tae,&tam);
1992 az->x=10.;
1993 az->y=-10.;
1994 el->x=10.;
1995 el->y=-10.;
1996 for(i=0;i<npoint;i++){
1997 s.x=pt[i].x;
1998 s.y=pt[i].y;
1999 s.z=0.;
2000 BodyToFrameTAM(&s,&tam,&f);
2001 FrameToBodyTAM(&f,vu,&s);
2002 NormPoint3d(&s,&s,&d);
2003 ToPolar3d(&s,&a);
2004 if(a.x<az->x) az->x=a.x;
2005 if(a.x>az->y) az->y=a.x;
2006 if(a.y<el->x) el->x=a.y;
2007 if(a.y>el->y) el->y=a.y;
2008 }
2009}
2010
2011/*****************************************************************
2012 Return if the surface is facing the viewer
2013
2014 po thePolar casted to Point3d
2015*****************************************************************/
2016bool CMPplane::FacingViewer(Point3d *po)
2017{
2018 double nx,ny,nz;
2019
2020 ny=sin(thetag);
2021 nx=ny*cos(phig-1.5707963268);
2022 ny*=sin(phig-1.5707963268);
2023 nz=cos(thetag);
2024 return (po->x-xg)*nx+(po->y-yg)*ny+(po->z-zg)*nz>0.;
2025}
2026
2027/******************************************************************************
2028 CMPbox
2029
2030 ******************************************************************************/
2031
2032void CMPbox::SetSize(double x,double y,double z)
2033{
2034 lx=x;
2035 ly=y;
2036 lz=z;
2037}
2038
2039
2040/******************************************************************************
2041 ClearVisSurface
2042
2043 Clear 'visibleSurface' instance variable so that new visibility
2044 from a new viewpoint can be obtained in GetSurface
2045 ******************************************************************************/
2046void CMPbox::ClearVisSurface(void)
2047{
2048 visibleSurface=0;
2049}
2050
2051/******************************************************************************
2052 GetSurface
2053
2054 Return a surface
2055
2056 Six surfaces are related to an index number in the following way
2057
2058 index face toward...
2059 0 +x
2060 1 +y
2061 2 +z
2062 3 -x
2063 4 -y
2064 5 -z
2065
2066
2067 The instance variable visibleSurface is used to map between
2068 the above index and the reference index passed as the first parameter.
2069
2070 The returned bool value is false if no more visible surface exists
2071 for the asked reference number.
2072 ******************************************************************************/
2073
2074bool CMPbox::GetSurface(int n,TransAngleMatrix *po,Surface *surf,Point2d *surfPt)
2075{
2076 double x1,y1,z1,x2,y2;
2077 TransAngleEuler tae1;
2078 TransAngleMatrix tam1;
2079 TransAngleEuler tae2;
2080 TransAngleEuler tae;
2081 Point3d p;
2082 Point3d s;
2083 Point3d e;
2084 Point3d f;
2085 double length;
2086 Point2d a;
2087 int i,j,m;
2088
2089 x1=0.5*lx;
2090 y1=0.5*ly;
2091 z1=0.5*lz;
2092 tae1.x=xg; // Translate Angle Euler for the part
2093 tae1.y=yg;
2094 tae1.z=zg;
2095 tae1.theta=thetag;
2096 tae1.phi=phig;
2097 tae1.psi=psig;
2098
2099 if(visibleSurface==0){
2100 TAEtoTAM(&tae1,&tam1);
2101 j=1;
2102 for(i=0;i<6;i++){
2103 s.x=((i>2)? -1.:1.)*((i%3==0)? x1:0.);
2104 s.y=((i>2)? -1.:1.)*((i%3==1)? y1:0.);
2105 s.z=((i>2)? -1.:1.)*((i%3==2)? z1:0.);
2106 BodyToFrameTAM(&s,&tam1,&e);
2107 if((xg-e.x)*(po->x-e.x)+(yg-e.y)*(po->y-e.y)
2108 +(zg-e.z)*(po->z-e.z)<0.) {
2109// DebugStr("\psurface");
2110 visibleSurface|=j;
2111 }
2112 j=j<<1;
2113 }
2114 }
2115 j=1;
2116 i=0;
2117 for(m=0;m<6;m++){
2118 if(visibleSurface&j){
2119 if(i==n) break;
2120 i++;
2121 }
2122 j=j<<1;
2123 }
2124 if(m==6) return false;
2125 switch (m){
2126 case 0: // +x
2127 tae2.x=x1; // Translate Angle Euler for the side
2128 tae2.y=0.;
2129 tae2.z=0.;
2130 tae2.theta=1.5707963268; // rotate 90 deg around +y axis
2131 tae2.phi=1.5707963268;
2132 tae2.psi=-1.5707963268;
2133 x2=z1;
2134 y2=y1;
2135 break;
2136 case 1: // +y
2137 tae2.x=0.; // Translate Angle Euler for the side
2138 tae2.y=y1;
2139 tae2.z=0.;
2140 tae2.theta=1.5707963268; // rotate 90 deg around -x axis
2141 tae2.phi=3.141592653589;
2142 tae2.psi=3.141592653589;
2143 x2=x1;
2144 y2=z1;
2145 break;
2146 case 2: // +z
2147 tae2.x=0.; // Translate Angle Euler for the side
2148 tae2.y=0.;
2149 tae2.z=z1;
2150 tae2.theta=0.; // no rotation
2151 tae2.phi=0.;
2152 tae2.psi=0.;
2153 x2=x1;
2154 y2=y1;
2155 break;
2156 case 3: // -x
2157 tae2.x=-x1; // Translate Angle Euler for the side
2158 tae2.y=0.;
2159 tae2.z=0.;
2160 tae2.theta=1.5707963268; // rotate 90 deg around -y axis
2161 tae2.phi=-1.5707963268;
2162 tae2.psi=1.5707963268;
2163 x2=z1;
2164 y2=y1;
2165 break;
2166 case 4: // -y
2167 tae2.x=0.; // Translate Angle Euler for the side
2168 tae2.y=-y1;
2169 tae2.z=0.;
2170 tae2.theta=1.5707963268; // rotate 90 deg around -x axis
2171 tae2.phi=0.;
2172 tae2.psi=0.;
2173 x2=x1;
2174 y2=z1;
2175 break;
2176 case 5: // -z
2177 tae2.x=0.; // Translate Angle Euler for the side
2178 tae2.y=0.;
2179 tae2.z=-z1;
2180 tae2.theta=3.1415926536; // no rotation
2181 tae2.phi=0.;
2182 tae2.psi=0.;
2183 x2=x1;
2184 y2=y1;
2185 break;
2186 }
2187 MoveTwiceTAE(&tae1,&tae2,&tae);
2188 surf->x0=tae.x;
2189 surf->y0=tae.y;
2190 surf->z0=tae.z;
2191 surf->theta0=tae.theta;
2192 surf->phi0=tae.phi;
2193 surf->psi0=tae.psi;
2194 surf->start=0;
2195 surf->pte=4;
2196 surfPt[0].x=x2;
2197 surfPt[0].y=y2;
2198 surfPt[1].x=-x2;
2199 surfPt[1].y=y2;
2200 surfPt[2].x=-x2;
2201 surfPt[2].y=-y2;
2202 surfPt[3].x=x2;
2203 surfPt[3].y=-y2;
2204 return true;
2205}
2206
2207/******************************************************************************
2208 GetCenter
2209
2210 Return the center point in frame coordinate
2211
2212 ******************************************************************************/
2213
2214void CMPbox::GetCenter(Point3d *p)
2215{
2216 p->x=xg;
2217 p->y=yg;
2218 p->z=zg;
2219}
2220
2221/******************************************************************************
2222 GetDrawData
2223
2224 Return data for drawing
2225
2226 po thePolar
2227
2228 seg array of line segments (clockwise angle from top, angle from line of sight)
2229 blk indices of segs which are to be drawn with lines
2230 cont indices of closed line segments. Inside of the closure
2231 will be painted by a color.
2232
2233 ******************************************************************************/
2234
2235void CMPbox::GetDrawData(TransAngleMatrix *po,Line2d *seg,StartPTE *blk,StartPTE *cont)
2236{
2237 double x,y,z,x1,y1,z1,x2,y2,z2,dx,dy,dz;
2238 int isen[3][10];
2239 int i,j,k,l,m,n,nsen,n1,brnch,lin,line;
2240 TransAngleEuler tae1;
2241 TransAngleMatrix tam1;
2242 TransAngleEuler tae2;
2243 TransAngleEuler tae;
2244 Point3d apex;
2245 Point3d ten[8]; /* coordinate of 8 apexes */
2246 Point3d s,d;
2247 double r;
2248 Point2d direc;
2249
2250
2251/* set conversion parameters */
2252 dx=lx;
2253 dy=ly;
2254 dz=lz;
2255
2256 tae1.x=xg; // Translate Angle Euler for the part
2257 tae1.y=yg;
2258 tae1.z=zg;
2259 tae1.theta=thetag;
2260 tae1.phi=phig;
2261 tae1.psi=psig;
2262 TAEtoTAM(&tae1,&tam1);
2263
2264/* fill coordinate of the 8 apexes */
2265/* l=0 ( 0, 0, 0) l=1 (dx, 0, 0) l=2 ( 0,dy, 0) l=3 (dx,dy, 0) */
2266/* l=4 ( 0, 0,dz) l=5 (dx, 0,dz) l=6 ( 0,dy,dz) l=7 (dx,dy,dz) */
2267 for(i=0;i<2;i++){
2268 apex.z=dz*i-0.5*dz; /* float */
2269 for(j=0;j<2;j++){
2270 apex.y=dy*j-0.5*dy; /* float */
2271 for(k=0;k<2;k++){
2272 apex.x=dx*k-0.5*dx; /* float */
2273 l=4*i+2*j+k; /* l=0-7 */
2274 BodyToFrameTAM(&apex,&tam1,&ten[l]);
2275 }
2276 }
2277 }
2278
2279/* fill line information */
2280 for(i=0;i<10;i++){
2281 for(j=0;j<3;j++) isen[j][i]=0;
2282 }
2283 nsen=0;
2284 for(i=0;i<3;i++){
2285 for(j=0;j<2;j++){
2286
2287/***********************************************************
2288 (j=0) (j=1)
2289 (i=0) k=0,l=4,m=2+4=6 0->4->6 k=1,l=3,m=7 1->3->7
2290 (i=1) k=0,l=1,m=4+1=5 0->1->5 k=2,l=6,m=7 2->6->7
2291 (i=2) k=0,l=2,m=1+2=3 0->2->3 k=4,l=5,m=7 4->5->7
2292
2293 surface i-j direction
2294 0-0 -x
2295 0-1 +x
2296 1-0 -y
2297 1-1 +y
2298 2-0 -z
2299 2-1 +z
2300***********************************************************/
2301
2302 k=(int)(j*pow(2,i));
2303 l=(int)(k+pow(2,(i+2-j)%3));
2304 m=(int)(k+pow(2,(i+1)%3)+pow(2,(i+2)%3));
2305 x1=ten[l].x-ten[k].x;
2306 y1=ten[l].y-ten[k].y;
2307 z1=ten[l].z-ten[k].z;
2308 x2=ten[m].x-ten[l].x;
2309 y2=ten[m].y-ten[l].y;
2310 z2=ten[m].z-ten[l].z;
2311
2312 if((y1*z2-z1*y2)*(po->x-ten[l].x)+(z1*x2-x1*z2)*(po->y
2313 -ten[l].y)+(x1*y2-y1*x2)*(po->z-ten[l].z)>=0.){
2314
2315/* if the plane is facing to the observer's eye do : */
2316 m=k; /* initialize present point */
2317 for(n1=0;n1<4;n1++){ // do for four sorrounding points
2318 l=m; /* define previous point */
2319
2320 /* redefine present point */
2321 if(n1==0) m=(int)(k+pow(2,(i+2-j)%3));
2322 if(n1==1) m=(int)(k+pow(2,(i+1)%3)+pow(2,(i+2)%3));
2323 if(n1==2) m=(int)(k+pow(2,(i+j+1)%3));
2324 if(n1==3) m=k;
2325 // now, the side start at point l and ends at point m
2326 if(nsen==0) brnch=1;
2327 else {
2328 brnch=1;
2329 for(n=0;n<nsen;n++){
2330 if(isen[1][n]==l && isen[0][n]==m){ // already there
2331 isen[2][n]=2; /* double line */
2332 brnch=0; // don't add this line
2333 break;
2334 }
2335 }
2336 }
2337 if(brnch==1){ // newly added line
2338 isen[0][nsen]=l; // start point
2339 isen[1][nsen]=m; // end point
2340 isen[2][nsen]=1; // single line
2341 nsen+=1;
2342 }
2343 } /* n1 */
2344 } /* facing this direction */
2345 } /* j */
2346 } /* i */
2347 blk->start=0; /* start line of a block */
2348 cont->start=0; /* start line of a contour */
2349
2350/* first, collect single lines and place in the first part */
2351 line=0;
2352 for(i=0;i<nsen;i++){
2353 if(isen[2][i]==1){ // single line
2354 for(j=0;j<2;j++){
2355 k=isen[j][i];
2356 s.x=ten[k].x;
2357 s.y=ten[k].y;
2358 s.z=ten[k].z;
2359 FrameToBodyTAM(&s,po,&d);
2360 NormPoint3d(&d,&s,&r);
2361 ToPolar3d(&s,&direc);
2362 if(j==0){
2363 seg[line].sx=direc.x;
2364 seg[line].sy=1.5707963267-direc.y;
2365 }
2366 else{
2367 seg[line].ex=direc.x;
2368 seg[line].ey=1.5707963267-direc.y;
2369 }
2370 }
2371 line+=1;
2372 }
2373 }
2374
2375 cont->pte=line; /* next-to-end line of the contour */
2376
2377/* then, collect double lines and place in the last part */
2378 for(i=0;i<nsen;i++){
2379 if(isen[2][i]==2){
2380 for(j=0;j<2;j++){
2381 k=isen[j][i];
2382 s.x=ten[k].x;
2383 s.y=ten[k].y;
2384 s.z=ten[k].z;
2385 FrameToBodyTAM(&s,po,&d);
2386 NormPoint3d(&d,&s,&r);
2387 ToPolar3d(&s,&direc);
2388 if(j==0){
2389 seg[line].sx=direc.x;
2390 seg[line].sy=1.5707963267-direc.y;
2391 }
2392 else{
2393 seg[line].ex=direc.x;
2394 seg[line].ey=1.5707963267-direc.y;
2395 }
2396 }
2397 line+=1;
2398 }
2399 }
2400 blk->pte=line; /* next-to-end line of the block */
2401}
2402
2403
2404/******************************************************************************
2405 FieldOfView
2406
2407 Get horizontal and vertical field of view to cover the object
2408 ******************************************************************************/
2409
2410void CMPbox::FieldOfView(TransAngleMatrix *vu,Point2d *az,Point2d *el)
2411{
2412 double x1,y1,z1,d;
2413 TransAngleEuler tae;
2414 TransAngleMatrix tam;
2415 int i;
2416 Point3d s;
2417 Point3d f;
2418 Point2d a;
2419
2420 x1=0.5*lx;
2421 y1=0.5*ly;
2422 z1=0.5*lz;
2423 tae.x=xg; // Translate Angle Euler for the part
2424 tae.y=yg;
2425 tae.z=zg;
2426 tae.theta=thetag;
2427 tae.phi=phig;
2428 tae.psi=psig;
2429 TAEtoTAM(&tae,&tam);
2430 az->x=10.;
2431 az->y=-10.;
2432 el->x=10.;
2433 el->y=-10.;
2434 for(i=0;i<8;i++){
2435 s.x=i&1? x1:-x1;
2436 s.y=i&2? y1:-y1;
2437 s.z=i&4? z1:-z1;
2438 BodyToFrameTAM(&s,&tam,&f);
2439 FrameToBodyTAM(&f,vu,&s);
2440 NormPoint3d(&s,&s,&d);
2441 ToPolar3d(&s,&a);
2442 if(a.x<az->x) az->x=a.x;
2443 if(a.x>az->y) az->y=a.x;
2444 if(a.y<el->x) el->x=a.y;
2445 if(a.y>el->y) el->y=a.y;
2446 }
2447}
2448
2449
2450/******************************************************************************
2451 CMPsphere - sphere
2452 ******************************************************************************/
2453
2454void CMPsphere::SetRadius(double x)
2455{
2456 r=x;
2457}
2458
2459
2460/******************************************************************************
2461 GetSurface
2462
2463 Return a line segment
2464
2465 ******************************************************************************/
2466
2467void CMPsphere::GetSurface(TransAngleMatrix *po,Surface *surf,Point2d *surfPt)
2468{
2469 Line3d eye;
2470 Point3d u;
2471 double d,ra,d1,r1;
2472 Point2d a;
2473 int i;
2474 double ang;
2475
2476 eye.sx=xg; // center of the sphere
2477 eye.sy=yg;
2478 eye.sz=zg;
2479 eye.ex=po->x; // thePolar of CThreeD - position of the viewer's eye
2480 eye.ey=po->y;
2481 eye.ez=po->z;
2482
2483 NormLine3d(&eye,&u,&d);
2484 ToPolar3d(&u,&a);
2485
2486 ra=r; // radius of the sphere
2487 d1=ra*ra/d; // distance to the center of tangent circle from sphere center
2488 r1=sqrt(ra*ra-d1*d1); // radius of the tangent circle
2489 r1*=1.03528; // r1/cos(15deg) to enable establishing the far-near
2490 // relationship with proximate parts.
2491
2492 surf->x0=xg+d1*u.x;
2493 surf->y0=yg+d1*u.y;
2494 surf->z0=zg+d1*u.z;
2495 surf->theta0=1.5707963268-a.y; // z axis toward viewer's eye
2496 surf->phi0=a.x+1.5707963268;
2497 surf->psi0=0.;
2498 surf->start=0;
2499 surf->pte=12;
2500 for(i=0;i<12;i++){
2501 ang=0.52359877559*i; // every 30 degree
2502 surfPt[i].x=r1*cos(ang);
2503 surfPt[i].y=r1*sin(ang);
2504 }
2505}
2506
2507
2508/******************************************************************************
2509 GetDrawData
2510
2511 Return data for drawing
2512
2513 po thePolar
2514
2515 seg array of line segments (clockwise angle from top, angle from line of sight)
2516 blk indices of segs which are to be drawn with lines
2517 cont indices of closed line segments. Inside of the closure
2518 will be painted by a color.
2519
2520 ******************************************************************************/
2521
2522void CMPsphere::GetDrawData(TransAngleMatrix *po,Line2d *seg,StartPTE *blk,StartPTE *cont)
2523{
2524 TransAngleEuler tae;
2525 TransAngleMatrix tam;
2526 Line3d eye;
2527 Point3d u,b,c;
2528 Point2d e,f,g;
2529 double d,ra,d1,r1;
2530 Point2d a;
2531 int i;
2532 double ang;
2533
2534 eye.sx=xg; // center of the sphere
2535 eye.sy=yg;
2536 eye.sz=zg;
2537 eye.ex=po->x; // thePolar of CThreeD - position of the viewer's eye
2538 eye.ey=po->y;
2539 eye.ez=po->z;
2540
2541 NormLine3d(&eye,&u,&d);
2542 ToPolar3d(&u,&a);
2543
2544 ra=r; // radius of the sphere
2545 d1=ra*ra/d; // distance to the center of tangent circle from sphere center
2546 r1=sqrt(ra*ra-d1*d1); // radius of the tangent circle
2547
2548 tae.x=xg+d1*u.x;
2549 tae.y=yg+d1*u.y;
2550 tae.z=zg+d1*u.z;
2551 tae.theta=1.5707963268-a.y; // z axis toward viewer's eye
2552 tae.phi=a.x+1.5707963268;
2553 tae.psi=0.;
2554 TAEtoTAM(&tae,&tam);
2555
2556 u.x=r1*cos(0.);
2557 u.y=r1*sin(0.);
2558 u.z=0.;
2559 BodyToFrameTAM(&u,&tam,&b);
2560 FrameToBodyTAM(&b,po,&u);
2561 NormPoint3d(&u,&b,&d);
2562 ToPolar3d(&b,&e);
2563 e.y=1.5707963267-e.y;
2564 f=e;
2565 for(i=0;i<90;i++){
2566 g=f;
2567 if(i<89){
2568 ang=.06981317008*(i+1); // every 4 degree
2569 u.x=r1*cos(ang);
2570 u.y=r1*sin(ang);
2571 u.z=0.;
2572 BodyToFrameTAM(&u,&tam,&b);
2573 FrameToBodyTAM(&b,po,&u);
2574 NormPoint3d(&u,&b,&d);
2575 ToPolar3d(&b,&f);
2576 f.y=1.5707963267-f.y;
2577 }
2578 else f=e;
2579 seg[i].sx=g.x;
2580 seg[i].sy=g.y;
2581 seg[i].ex=f.x;
2582 seg[i].ey=f.y;
2583
2584 }
2585 blk->start=0;
2586 blk->pte=90;
2587 cont->start=0;
2588 cont->pte=90;
2589}
2590
2591
2592
2593/******************************************************************************
2594 FieldOfView
2595
2596 Get horizontal and vertical field of view to cover the object
2597 ******************************************************************************/
2598
2599void CMPsphere::FieldOfView(TransAngleMatrix *vu,Point2d *az,Point2d *el)
2600{
2601 Point3d s;
2602 Point3d f;
2603 Point2d a;
2604 double b,d;
2605
2606 f.x=xg;
2607 f.y=yg;
2608 f.z=zg;
2609 FrameToBodyTAM(&f,vu,&s);
2610 NormPoint3d(&s,&s,&d);
2611 ToPolar3d(&s,&a);
2612 b=asin(r/d);
2613 az->x=a.x-b;
2614 az->y=a.x+b;
2615 el->x=a.y-b;
2616 el->y=a.y+b;
2617}
2618
2619/******************************************************************************
2620 CMPbodyOfRot - bodyOfRot
2621 ******************************************************************************/
2622
2623void CMPbodyOfRot::SetPoint(double x,double z)
2624{
2625 switch (npoint){
2626 case 0:
2627 p0.x=x;
2628 p0.y=z;
2629 npoint=1;
2630 break;
2631 case 1:
2632 p1.x=x;
2633 p1.y=z;
2634 npoint=2;
2635 break;
2636 default:
2637 break;
2638 }
2639}
2640
2641/******************************************************************************
2642 ClearVisSurface
2643
2644 Clear 'visibleSurface' instance variable so that new visibility
2645 from a new viewpoint can be obtained in GetSurface
2646 ******************************************************************************/
2647void CMPbodyOfRot::ClearVisSurface(void)
2648{
2649 visibleSurface=0;
2650}
2651
2652/******************************************************************************
2653 GetSurface
2654
2655 Return a surface
2656
2657 14 surfaces are related to an index number in the following way
2658
2659 index face toward...
2660 0 - 11 azimuth 30*index+15 (deg)
2661 12 +z
2662 13 -z
2663
2664 The instance variable visibleSurface is used to map between
2665 the above index and the reference index passed as the first parameter.
2666
2667 The returned bool value is false if no more visible surface exists
2668 for the asked reference number.
2669
2670 ******************************************************************************/
2671
2672bool CMPbodyOfRot::GetSurface(int n,bool nearside,TransAngleMatrix *po,
2673 Surface *surf,Point2d *surfPt)
2674{
2675 Point2d p2,p3;
2676 TransAngleEuler tae1;
2677 TransAngleMatrix tam1;
2678 double alpha,delta,r;
2679
2680 TransAngleEuler tae2;
2681 TransAngleEuler tae;
2682 Point3d s;
2683 Point3d e;
2684 int i,j,k,m;
2685
2686 p2.x=p0.x; // bottom radial point
2687 p2.y=p0.y;
2688 p3.x=p1.x; // top radial point
2689 p3.y=p1.y;
2690
2691 if(p2.x==0.) p2.x=p3.x*0.01; // cone
2692 if(p3.x==0.) p3.x=p2.x*0.01;
2693 if(p2.y==p3.y) { // didc
2694 if(p2.x>p3.x) p3.y+=0.01*p2.x;
2695 else p2.y+=0.01*p3.x;
2696 }
2697
2698 p2.x*=1.0178; // make the average radius same as the real radius
2699 p3.x*=1.0178;
2700
2701 tae1.x=xg; // Translate Angle Euler for the part
2702 tae1.y=yg;
2703 tae1.z=zg;
2704 tae1.theta=thetag;
2705 tae1.phi=phig;
2706 tae1.psi=psig;
2707
2708 if(visibleSurface==0){
2709 TAEtoTAM(&tae1,&tam1);
2710 j=1;
2711 delta=atan2((p2.x-p3.x)*cos(0.017453293*15.),p3.y-p2.y);
2712 r=-p2.y*(p3.x-p2.x)/(p3.y-p2.y)+p2.x;
2713 r*=cos(0.017453293*15.);
2714 s.z=r*cos(delta)*sin(delta);
2715 r=r*cos(delta)*cos(delta);
2716 for(i=0;i<12;i++){
2717 alpha=0.017453293*(30.*i+15.);
2718 s.x=r*cos(alpha);
2719 s.y=r*sin(alpha);
2720 BodyToFrameTAM(&s,&tam1,&e);
2721 if((xg-e.x)*(po->x-e.x)+(yg-e.y)*(po->y-e.y)
2722 +(zg-e.z)*(po->z-e.z)<0.) visibleSurface|=j;
2723 j=j<<1;
2724 }
2725 FrameToBodyTAM((Point3d *)po,&tam1,&s); // position of viewer
2726 if((s.z-p3.y)*(p2.y-p3.y)<0.) visibleSurface|=j; // upper surface
2727 j=j<<1;
2728 if((s.z-p2.y)*(p3.y-p2.y)<0.) visibleSurface|=j; // lower surface
2729
2730 // determine instance variable 'appear'
2731 j=1<<12;
2732 if(visibleSurface&j){
2733 j=1;
2734 k=0;
2735 for(i=0;i<12;i++){
2736 if(visibleSurface&j) k++;
2737 j=j<<1;
2738 }
2739 if(k==0) appear=1;
2740 else if(k==12) appear=3;
2741 else appear=2;
2742 }
2743 else{
2744 j=j<<1;
2745 if(visibleSurface&j){
2746 j=1;
2747 k=0;
2748 for(i=0;i<12;i++){
2749 if(visibleSurface&j) k++;
2750 j=j<<1;
2751 }
2752 if(k==0) appear=4;
2753 else if(k==12) appear=6;
2754 else appear=5;
2755 }
2756 else appear=0;
2757 }
2758 }
2759 if(isTube){
2760 if(appear==0||((appear==2||appear==5)&&nearside)){
2761 j=1;
2762 i=0;
2763 for(m=0;m<12;m++){
2764 if(visibleSurface&j){
2765 if(i==n) break;
2766 i++;
2767 }
2768 j=j<<1;
2769 }
2770 if(m==12) return false;
2771
2772 delta=atan2((p2.x-p3.x)*cos(0.017453293*15.),p3.y-p2.y);
2773 alpha=0.017453293*(30.*m+15.);
2774 r=p2.x*cos(0.017453293*15.); // radius to the center of the bottom segment
2775
2776 tae2.x=r*cos(alpha);
2777 tae2.y=r*sin(alpha);
2778 tae2.z=p2.y;
2779 tae2.theta=1.5707963268-delta;
2780 tae2.phi=alpha+1.5707963268;
2781 tae2.psi=0.;
2782
2783 surfPt[0].x=-r*tan(0.017453293*15.); // left-bottom
2784 surfPt[0].y=0.;
2785 surfPt[1].x=-surfPt[0].x; // right-bottom
2786 surfPt[1].y=0.;
2787 r=p3.x*cos(0.017453293*15.); // radius to the center of the top segment
2788 surfPt[2].x=r*tan(0.017453293*15.); // right-top
2789 surfPt[2].y=(p3.y-p2.y)/cos(delta);
2790 surfPt[3].x=-surfPt[2].x; // left-top
2791 surfPt[3].y=surfPt[2].y;
2792
2793 MoveTwiceTAE(&tae1,&tae2,&tae);
2794 surf->x0=tae.x;
2795 surf->y0=tae.y;
2796 surf->z0=tae.z;
2797 surf->theta0=tae.theta;
2798 surf->phi0=tae.phi;
2799 surf->psi0=tae.psi;
2800 surf->start=0;
2801 if(m<12) surf->pte=4;
2802 else surf->pte=12;
2803 return true;
2804 }
2805 if((appear==2||appear==5)&&!nearside){
2806 j=1;
2807 i=0;
2808 for(m=0;m<12;m++){
2809 if(!(visibleSurface&j)){
2810 if(i==n) break;
2811 i++;
2812 }
2813 j=j<<1;
2814 }
2815 if(m==12) return false;
2816
2817 delta=atan2((p2.x-p3.x)*cos(0.017453293*15.),p3.y-p2.y);
2818 alpha=0.017453293*(30.*m+15.);
2819 r=p2.x*cos(0.017453293*15.); // radius to the center of the bottom segment
2820
2821 tae2.x=r*cos(alpha);
2822 tae2.y=r*sin(alpha);
2823 tae2.z=p2.y;
2824 tae2.theta=1.5707963268-delta;
2825 tae2.phi=alpha+1.5707963268;
2826 tae2.psi=0.;
2827
2828 surfPt[0].x=r*tan(0.017453293*15.); // left-bottom
2829 surfPt[0].y=0.;
2830 surfPt[1].x=-surfPt[0].x; // right-bottom
2831 surfPt[1].y=0.;
2832 r=p3.x*cos(0.017453293*15.); // radius to the center of the top segment
2833 surfPt[2].x=-r*tan(0.017453293*15.); // right-top
2834 surfPt[2].y=(p3.y-p2.y)/cos(delta);
2835 surfPt[3].x=-surfPt[2].x; // left-top
2836 surfPt[3].y=surfPt[2].y;
2837
2838 MoveTwiceTAE(&tae1,&tae2,&tae);
2839 surf->x0=tae.x;
2840 surf->y0=tae.y;
2841 surf->z0=tae.z;
2842 surf->theta0=tae.theta;
2843 surf->phi0=tae.phi;
2844 surf->psi0=tae.psi;
2845 surf->start=0;
2846 if(m<12) surf->pte=4;
2847 else surf->pte=12;
2848 return true;
2849 }
2850 if(appear==1||appear==4||appear==3||appear==6){
2851 if(n>5) return false;
2852 if(nearside) m=n;
2853 else m=n+6;
2854
2855 delta=atan2((p2.x-p3.x)*cos(0.017453293*15.),p3.y-p2.y);
2856 alpha=0.017453293*(30.*m+15.);
2857 r=p2.x*cos(0.017453293*15.); // radius to the center of the bottom segment
2858 if(appear==1||appear==4){
2859 alpha-=3.14159265;
2860 delta=-delta;
2861 }
2862 tae2.x=r*cos(alpha);
2863 tae2.y=r*sin(alpha);
2864 tae2.z=p2.y;
2865 tae2.theta=1.5707963268-delta;
2866 tae2.phi=alpha+1.5707963268;
2867 tae2.psi=0.;
2868
2869 surfPt[0].x=-r*tan(0.017453293*15.); // left-bottom
2870 surfPt[0].y=0.;
2871 surfPt[1].x=-surfPt[0].x; // right-bottom
2872 surfPt[1].y=0.;
2873 r=p3.x*cos(0.017453293*15.); // radius to the center of the top segment
2874 surfPt[2].x=r*tan(0.017453293*15.); // right-top
2875 surfPt[2].y=(p3.y-p2.y)/cos(delta);
2876 surfPt[3].x=-surfPt[2].x; // left-top
2877 surfPt[3].y=surfPt[2].y;
2878
2879 MoveTwiceTAE(&tae1,&tae2,&tae);
2880 surf->x0=tae.x;
2881 surf->y0=tae.y;
2882 surf->z0=tae.z;
2883 surf->theta0=tae.theta;
2884 surf->phi0=tae.phi;
2885 surf->psi0=tae.psi;
2886 surf->start=0;
2887 if(m<12) surf->pte=4;
2888 else surf->pte=12;
2889 return true;
2890 }
2891 }
2892 else{ // solid
2893 j=1;
2894 i=0;
2895 for(m=0;m<14;m++){
2896 if(visibleSurface&j){
2897 if(i==n) break;
2898 i++;
2899 }
2900 j=j<<1;
2901 }
2902 if(m==14) return false;
2903
2904 if(m<12){
2905 delta=atan2((p2.x-p3.x)*cos(0.017453293*15.),p3.y-p2.y);
2906 alpha=0.017453293*(30.*m+15.);
2907 r=p2.x*cos(0.017453293*15.); // radius to the center of the bottom segment
2908
2909 tae2.x=r*cos(alpha);
2910 tae2.y=r*sin(alpha);
2911 tae2.z=p2.y;
2912 tae2.theta=1.5707963268-delta;
2913 tae2.phi=alpha+1.5707963268;
2914 tae2.psi=0.;
2915
2916 surfPt[0].x=-r*tan(0.017453293*15.); // left-bottom
2917 surfPt[0].y=0.;
2918 surfPt[1].x=-surfPt[0].x; // right-bottom
2919 surfPt[1].y=0.;
2920 r=p3.x*cos(0.017453293*15.); // radius to the center of the top segment
2921 surfPt[2].x=r*tan(0.017453293*15.); // right-top
2922 surfPt[2].y=(p3.y-p2.y)/cos(delta);
2923 surfPt[3].x=-surfPt[2].x; // left-top
2924 surfPt[3].y=surfPt[2].y;
2925 }
2926 if(m==12){
2927 tae2.x=0.;
2928 tae2.y=0.;
2929 tae2.z=p3.y;
2930 tae2.theta=0.;
2931 tae2.phi=0.;
2932 tae2.psi=0.;
2933
2934 r=p3.x;
2935 for(i=0;i<12;i++){
2936 alpha=0.017453293*30.*i;
2937 surfPt[i].x=r*cos(alpha);
2938 surfPt[i].y=r*sin(alpha);
2939 }
2940 }
2941 if(m==13){
2942 tae2.x=0.;
2943 tae2.y=0.;
2944 tae2.z=p2.y;
2945 tae2.theta=3.1415926536;
2946 tae2.phi=0.;
2947 tae2.psi=0.;
2948
2949 r=p2.x;
2950 for(i=0;i<12;i++){
2951 alpha=0.017453293*30.*i;
2952 surfPt[i].x=r*cos(alpha);
2953 surfPt[i].y=r*sin(alpha);
2954 }
2955 }
2956
2957 MoveTwiceTAE(&tae1,&tae2,&tae);
2958 surf->x0=tae.x;
2959 surf->y0=tae.y;
2960 surf->z0=tae.z;
2961 surf->theta0=tae.theta;
2962 surf->phi0=tae.phi;
2963 surf->psi0=tae.psi;
2964 surf->start=0;
2965 if(m<12) surf->pte=4;
2966 else surf->pte=12;
2967 return true;
2968 }
2969}
2970
2971/******************************************************************************
2972 GetCenter
2973
2974 Return the center point in frame coordinate
2975
2976 ******************************************************************************/
2977
2978void CMPbodyOfRot::GetCenter(Point3d *p)
2979{
2980 Point3d pt;
2981 TransAngleEuler tae;
2982 TransAngleMatrix tam;
2983
2984 pt.x=0.;
2985 pt.y=0.;
2986 pt.z=0.5*(p0.y+p1.y);
2987
2988 tae.x=xg;
2989 tae.y=yg;
2990 tae.z=zg;
2991 tae.theta=thetag;
2992 tae.phi=phig;
2993 tae.psi=psig;
2994
2995 TAEtoTAM(&tae,&tam);
2996 BodyToFrameTAM(&pt,&tam,p);
2997}
2998
2999/******************************************************************************
3000 GetDrawData
3001
3002 Return data for drawing
3003
3004 m segment number (0 - near side, 1 - far side)
3005 po thePolar
3006
3007 seg array of line segments (clockwise angle from top,
3008 angle from line of sight)
3009 blk indices of segs which are to be drawn with lines
3010 cont indices of closed line segments. Inside of the closure
3011 will be painted by a color.
3012
3013 ******************************************************************************/
3014
3015void CMPbodyOfRot::GetDrawData(int m,TransAngleMatrix *po,
3016 Line2d *seg,StartPTE *blk,StartPTE *cont)
3017{
3018 double eye[3];
3019 Point3d *ten0;
3020 Point3d *ten1;
3021 double x1,z1,x2,z2,y1,y2;
3022 TransAngleEuler tae;
3023 TransAngleMatrix tam1;
3024 int nseg,i,n0,n1,n2,j,k,is,is1,line,istart,iend,itb,ibt,n;
3025 double deld,a,co,si,r;
3026 Point3d q,s,d;
3027 Point2d direc;
3028
3029 if(oblong){
3030 GetDrawData1(po,seg,blk,cont);
3031 return;
3032 }
3033
3034 eye[0]=po->x;
3035 eye[1]=po->y;
3036 eye[2]=po->z;
3037
3038// lower surface ten0, upper surface ten1
3039// indices are from 0 to 90, 90th data being the same as 0th data
3040 ten0=new Point3d[91];
3041 ten1=new Point3d[91];
3042 x1=p0.x;
3043 z1=p0.y;
3044 x2=p1.x;
3045 z2=p1.y;
3046
3047 if(x1==0.) x1=x2*0.01; // cone
3048 if(x2==0.) x2=x1*0.01;
3049 if(z1==z2) { // didc
3050 if(x1>x2) z2+=0.01*x1;
3051 else z1-=0.01*x2;
3052 }
3053
3054// set conversion parameters
3055 tae.x=xg;
3056 tae.y=yg;
3057 tae.z=zg;
3058 tae.theta=thetag;
3059 tae.phi=phig;
3060 tae.psi=psig;
3061
3062 TAEtoTAM(&tae,&tam1);
3063
3064// At this point, initial conditions are set.
3065// tam1 defines a local coordinate.
3066// In the local coordinate, two points (x1,z1) and (x2,z2) are given.
3067// The body of rotation is given by rotating
3068// the line connecting the two points around z-axis.
3069// The viewer is given by eye[]
3070
3071/* fill coordinates of 2*nseg apexes */
3072
3073 nseg=90;
3074 deld=nseg;
3075 deld=2.*3.1415926536/deld;
3076 for(i=0;i<=nseg;i++){ // 91 data are set to each of the arrays
3077 a=i*deld;
3078 co=cos(a);
3079 si=sin(a);
3080 q.x=x1*co;
3081 q.y=x1*si;
3082 q.z=z1; /* lower surface */
3083 BodyToFrameTAM(&q,&tam1,&ten0[i]);
3084 q.x=x2*co;
3085 q.y=x2*si;
3086 q.z=z2; /* uppser surface */
3087 BodyToFrameTAM(&q,&tam1,&ten1[i]);
3088 }
3089
3090// the outline was replaced by two circles ten0[] and ten1[]
3091/* obtain side surface segments which are visible */
3092 n0=0;
3093 n1=0; // invisible-to-visible index
3094 n2=0; // visible-to-invisible index
3095
3096// In CMPbodyOfRot::GetSurface, we have already categorized
3097// into seven cases, which are inducated by the variable 'appear'.
3098// Of those seven cases, the following three cases
3099// 0 only the side is visible
3100// 2 side and upper surface are visible
3101// 5 side and lower surface are visible
3102// have indices which separate visible and invisible portions of the side
3103// surface.
3104
3105 if(appear==0||appear==2||appear==5){
3106 for(i=0;(i<=nseg)&&(n0!=2);i++){
3107
3108 /* each surface contains i-th and (i-1)th apexes */
3109 /* visibility of two adjacent surfaces are compared to */
3110 /* obtain both visible-to-invisible and invisible-to-visible */
3111 /* trasition indices */
3112
3113 j=i;
3114 if(j>=nseg) j-=nseg;
3115 k=j-1;
3116 if(k<0) k+=nseg;
3117 is=0;
3118 /* vector along lower surface */
3119 x1=ten0[j].x-ten0[k].x;
3120 y1=ten0[j].y-ten0[k].y;
3121 z1=ten0[j].z-ten0[k].z;
3122 /* vector directed from lower surface to upper sureace */
3123 x2=ten1[k].x-ten0[k].x;
3124 y2=ten1[k].y-ten0[k].y;
3125 z2=ten1[k].z-ten0[k].z;
3126 /* if surface is visible is = 1 */
3127 if((y1*z2-z1*y2)*(eye[0]-ten0[k].x)
3128 +(z1*x2-x1*z2)*(eye[1]-ten0[k].y)
3129 +(x1*y2-y1*x2)*(eye[2]-ten0[k].z)>0.) is=1; // visible
3130 if((i!=0)&&(is!=is1)) {
3131 if(is>is1) n1=k; /* k is invisible to visible */
3132 if(is<is1) n2=k; /* k is visible to invisible */
3133 n0++;
3134 }
3135 if(n0!=2) is1=is;
3136 }
3137 }
3138
3139/* now, categorization is over, process each cases */
3140
3141// The order of processing is as followes
3142// appear solid tube-near tube-far
3143// 0 x x x
3144// 2,5 x
3145// 2,5 x
3146// 5 x
3147// 2 x
3148// 1,3,4,6 x
3149// 1,3,4,6 x
3150// 1,3,4,6 x
3151
3152 line=0;
3153 if(appear==0||((appear==2||appear==5)&&isTube&&m==0)){
3154 // appear=0,both for a solid and a tube, near and far side
3155 // appear=2 or 5, near side of the tube
3156/* side only */
3157 blk->start=line;
3158 cont->start=line;
3159 n0=n2-n1; /* number of visible points - 1 */
3160 if(n0<0) n0+=nseg;
3161 for(i=0;i<n0;i++){
3162 j=n1+i+1;
3163 if(j>=nseg) j-=nseg;
3164 k=j-1;
3165 if(k<0) k+=nseg;
3166/* along lower surface normal direction (to the order of smaller to larger indices) */
3167 s=ten0[k];
3168 FrameToBodyTAM(&s,po,&d);
3169 NormPoint3d(&d,&s,&r);
3170 ToPolar3d(&s,&direc);
3171 seg[line].sx=direc.x;
3172 seg[line].sy=1.5707963267-direc.y;
3173 s=ten0[j];
3174 FrameToBodyTAM(&s,po,&d);
3175 NormPoint3d(&d,&s,&r);
3176 ToPolar3d(&s,&direc);
3177 seg[line].ex=direc.x;
3178 seg[line].ey=1.5707963267-direc.y;
3179 line++;
3180/* along upper surface reversed order */
3181 s=ten1[j];
3182 FrameToBodyTAM(&s,po,&d);
3183 NormPoint3d(&d,&s,&r);
3184 ToPolar3d(&s,&direc);
3185 seg[line].sx=direc.x;
3186 seg[line].sy=1.5707963267-direc.y;
3187 s=ten1[k];
3188 FrameToBodyTAM(&s,po,&d);
3189 NormPoint3d(&d,&s,&r);
3190 ToPolar3d(&s,&direc);
3191 seg[line].ex=direc.x;
3192 seg[line].ey=1.5707963267-direc.y;
3193 line++;
3194 }
3195/* invisible-to-visible side line from top to bottom */
3196 s=ten1[n1];
3197 FrameToBodyTAM(&s,po,&d);
3198 NormPoint3d(&d,&s,&r);
3199 ToPolar3d(&s,&direc);
3200 seg[line].sx=direc.x;
3201 seg[line].sy=1.5707963267-direc.y;
3202 s=ten0[n1];
3203 FrameToBodyTAM(&s,po,&d);
3204 NormPoint3d(&d,&s,&r);
3205 ToPolar3d(&s,&direc);
3206 seg[line].ex=direc.x;
3207 seg[line].ey=1.5707963267-direc.y;
3208 line++;
3209/* visible-to-invisible side line from bottom to top */
3210 s=ten0[n2];
3211 FrameToBodyTAM(&s,po,&d);
3212 NormPoint3d(&d,&s,&r);
3213 ToPolar3d(&s,&direc);
3214 seg[line].sx=direc.x;
3215 seg[line].sy=1.5707963267-direc.y;
3216 s=ten1[n2];
3217 FrameToBodyTAM(&s,po,&d);
3218 NormPoint3d(&d,&s,&r);
3219 ToPolar3d(&s,&direc);
3220 seg[line].ex=direc.x;
3221 seg[line].ey=1.5707963267-direc.y;
3222 line++;
3223 blk->pte=line;
3224 cont->pte=line;
3225 delete[] ten0; // DisposPtr((Ptr)ten0);
3226 delete[] ten1; // DisposPtr((Ptr)ten1);
3227 return;
3228 }
3229
3230 if((appear==2||appear==5)&&isTube&&m==1){
3231 // appear=2 or 5, far side of the tube
3232/* side only */
3233 blk->start=line;
3234 cont->start=line;
3235 n0=n1-n2; /* number of invisible points - 1 */
3236 if(n0<0) n0+=nseg;
3237 for(i=0;i<n0;i++){
3238 j=n2+i+1;
3239 if(j>=nseg) j-=nseg;
3240 k=j-1;
3241 if(k<0) k+=nseg;
3242/* along lower surface in reverse direction */
3243 s=ten0[j];
3244 FrameToBodyTAM(&s,po,&d);
3245 NormPoint3d(&d,&s,&r);
3246 ToPolar3d(&s,&direc);
3247 seg[line].sx=direc.x;
3248 seg[line].sy=1.5707963267-direc.y;
3249 s=ten0[k];
3250 FrameToBodyTAM(&s,po,&d);
3251 NormPoint3d(&d,&s,&r);
3252 ToPolar3d(&s,&direc);
3253 seg[line].ex=direc.x;
3254 seg[line].ey=1.5707963267-direc.y;
3255 line++;
3256/* along upper surface in normal direction */
3257 s=ten1[k];
3258 FrameToBodyTAM(&s,po,&d);
3259 NormPoint3d(&d,&s,&r);
3260 ToPolar3d(&s,&direc);
3261 seg[line].sx=direc.x;
3262 seg[line].sy=1.5707963267-direc.y;
3263 s=ten1[j];
3264 FrameToBodyTAM(&s,po,&d);
3265 NormPoint3d(&d,&s,&r);
3266 ToPolar3d(&s,&direc);
3267 seg[line].ex=direc.x;
3268 seg[line].ey=1.5707963267-direc.y;
3269 line++;
3270 }
3271/* invisible-to-visible side line from top tp bottom */
3272 s=ten1[n1];
3273 FrameToBodyTAM(&s,po,&d);
3274 NormPoint3d(&d,&s,&r);
3275 ToPolar3d(&s,&direc);
3276 seg[line].sx=direc.x;
3277 seg[line].sy=1.5707963267-direc.y;
3278 s=ten0[n1];
3279 FrameToBodyTAM(&s,po,&d);
3280 NormPoint3d(&d,&s,&r);
3281 ToPolar3d(&s,&direc);
3282 seg[line].ex=direc.x;
3283 seg[line].ey=1.5707963267-direc.y;
3284 line++;
3285/* visible-to-invisible side line from bottom to top */
3286 s=ten0[n2];
3287 FrameToBodyTAM(&s,po,&d);
3288 NormPoint3d(&d,&s,&r);
3289 ToPolar3d(&s,&direc);
3290 seg[line].sx=direc.x;
3291 seg[line].sy=1.5707963267-direc.y;
3292 s=ten1[n2];
3293 FrameToBodyTAM(&s,po,&d);
3294 NormPoint3d(&d,&s,&r);
3295 ToPolar3d(&s,&direc);
3296 seg[line].ex=direc.x;
3297 seg[line].ey=1.5707963267-direc.y;
3298 line++;
3299 blk->pte=line;
3300 cont->pte=line;
3301 delete[] ten0; // DisposPtr((Ptr)ten0);
3302 delete[] ten1; // DisposPtr((Ptr)ten1);
3303 return;
3304 }
3305
3306 if(appear==5&&!isTube){ // for a solid
3307/* side and lower surface */
3308 blk->start=line;
3309 cont->start=line;
3310 n0=n2-n1;
3311 if(n0<0) n0+=nseg;
3312 for(i=0;i<n0;i++){
3313 j=n2-i-1;
3314 if(j<0) j+=nseg;
3315 k=j+1;
3316 if(k>=nseg) k-=nseg;
3317/* contor along the upper surface */
3318 s=ten1[k];
3319 FrameToBodyTAM(&s,po,&d);
3320 NormPoint3d(&d,&s,&r);
3321 ToPolar3d(&s,&direc);
3322 seg[line].sx=direc.x;
3323 seg[line].sy=1.5707963267-direc.y;
3324 s=ten1[j];
3325 FrameToBodyTAM(&s,po,&d);
3326 NormPoint3d(&d,&s,&r);
3327 ToPolar3d(&s,&direc);
3328 seg[line].ex=direc.x;
3329 seg[line].ey=1.5707963267-direc.y;
3330 line++;
3331 }
3332 s=ten1[n1];
3333 FrameToBodyTAM(&s,po,&d);
3334 NormPoint3d(&d,&s,&r);
3335 ToPolar3d(&s,&direc);
3336 seg[line].sx=direc.x;
3337 seg[line].sy=1.5707963267-direc.y;
3338 s=ten0[n1];
3339 FrameToBodyTAM(&s,po,&d);
3340 NormPoint3d(&d,&s,&r);
3341 ToPolar3d(&s,&direc);
3342 seg[line].ex=direc.x;
3343 seg[line].ey=1.5707963267-direc.y;
3344 line++;
3345 s=ten0[n2];
3346 FrameToBodyTAM(&s,po,&d);
3347 NormPoint3d(&d,&s,&r);
3348 ToPolar3d(&s,&direc);
3349 seg[line].sx=direc.x;
3350 seg[line].sy=1.5707963267-direc.y;
3351 s=ten1[n2];
3352 FrameToBodyTAM(&s,po,&d);
3353 NormPoint3d(&d,&s,&r);
3354 ToPolar3d(&s,&direc);
3355 seg[line].ex=direc.x;
3356 seg[line].ey=1.5707963267-direc.y;
3357 line++;
3358 n0=n1-n2;
3359 if(n0<0) n0=n0+nseg;
3360 cont->pte=line+n0;
3361/* part of the followings constitute contour */
3362 for(i=0;i<nseg;i++){
3363 j=n2+i+1;
3364 if(j>=nseg) j=j-nseg;
3365 k=j-1;
3366 if(k<0) k+=nseg;
3367 s=ten0[j];
3368 FrameToBodyTAM(&s,po,&d);
3369 NormPoint3d(&d,&s,&r);
3370 ToPolar3d(&s,&direc);
3371 seg[line].sx=direc.x;
3372 seg[line].sy=1.5707963267-direc.y;
3373 s=ten0[k];
3374 FrameToBodyTAM(&s,po,&d);
3375 NormPoint3d(&d,&s,&r);
3376 ToPolar3d(&s,&direc);
3377 seg[line].ex=direc.x;
3378 seg[line].ey=1.5707963267-direc.y;
3379 line++;
3380 }
3381 blk->pte=line;
3382 delete[] ten0; // DisposPtr((Ptr)ten0);
3383 delete[] ten1; // DisposPtr((Ptr)ten1);
3384 return;
3385 }
3386 if(appear==2&&!isTube){ // for a solid
3387/* side and upper surface */
3388 blk->start=line;
3389 cont->start=line;
3390 n0=n2-n1;
3391 if(n0<0) n0+=nseg;
3392/* line along lower surface */
3393 for(i=0;i<n0;i++){
3394 j=n1+i+1;
3395 if(j>=nseg) j-=nseg;
3396 k=j-1;
3397 if(k<0) k+=nseg;
3398 s=ten0[k];
3399 FrameToBodyTAM(&s,po,&d);
3400 NormPoint3d(&d,&s,&r);
3401 ToPolar3d(&s,&direc);
3402 seg[line].sx=direc.x;
3403 seg[line].sy=1.5707963267-direc.y;
3404 s=ten0[j];
3405 FrameToBodyTAM(&s,po,&d);
3406 NormPoint3d(&d,&s,&r);
3407 ToPolar3d(&s,&direc);
3408 seg[line].ex=direc.x;
3409 seg[line].ey=1.5707963267-direc.y;
3410 line++;
3411 }
3412 s=ten1[n1];
3413 FrameToBodyTAM(&s,po,&d);
3414 NormPoint3d(&d,&s,&r);
3415 ToPolar3d(&s,&direc);
3416 seg[line].sx=direc.x;
3417 seg[line].sy=1.5707963267-direc.y;
3418 s=ten0[n1];
3419 FrameToBodyTAM(&s,po,&d);
3420 NormPoint3d(&d,&s,&r);
3421 ToPolar3d(&s,&direc);
3422 seg[line].ex=direc.x;
3423 seg[line].ey=1.5707963267-direc.y;
3424 line++;
3425 s=ten0[n2];
3426 FrameToBodyTAM(&s,po,&d);
3427 NormPoint3d(&d,&s,&r);
3428 ToPolar3d(&s,&direc);
3429 seg[line].sx=direc.x;
3430 seg[line].sy=1.5707963267-direc.y;
3431 s=ten1[n2];
3432 FrameToBodyTAM(&s,po,&d);
3433 NormPoint3d(&d,&s,&r);
3434 ToPolar3d(&s,&direc);
3435 seg[line].ex=direc.x;
3436 seg[line].ey=1.5707963267-direc.y;
3437 line++;
3438 n0=n1-n2;
3439 if(n0<0) n0=n0+nseg;
3440 cont->pte=line+n0;
3441 for(i=0;i<nseg;i++){
3442 j=n2+i+1;
3443 if(j>=nseg) j=j-nseg;
3444 k=j-1;
3445 if(k<0) k+=nseg;
3446 s=ten1[k];
3447 FrameToBodyTAM(&s,po,&d);
3448 NormPoint3d(&d,&s,&r);
3449 ToPolar3d(&s,&direc);
3450 seg[line].sx=direc.x;
3451 seg[line].sy=1.5707963267-direc.y;
3452 s=ten1[j];
3453 FrameToBodyTAM(&s,po,&d);
3454 NormPoint3d(&d,&s,&r);
3455 ToPolar3d(&s,&direc);
3456 seg[line].ex=direc.x;
3457 seg[line].ey=1.5707963267-direc.y;
3458 line++;
3459 }
3460 blk->pte=line;
3461 delete[] ten0; // DisposPtr((Ptr)ten0);
3462 delete[] ten1; // DisposPtr((Ptr)ten1);
3463 return;
3464 }
3465 if(!isTube){ // for a solid, appear=1,3,4 or 6
3466 /* lower or upper surface only */
3467 blk->start=line;
3468 cont->pte=line;
3469 for(i=0;i<nseg;i++){ // contour circle
3470 j=i;
3471 k=j-1;
3472 if(k<0) k+=nseg;
3473 if(appear==1||appear==6){ // contour is upper surface
3474 if(appear==1){
3475 n=k;
3476 k=j;
3477 j=n;
3478 }
3479 s=ten1[j];
3480 FrameToBodyTAM(&s,po,&d);
3481 NormPoint3d(&d,&s,&r);
3482 ToPolar3d(&s,&direc);
3483 seg[line].sx=direc.x;
3484 seg[line].sy=1.5707963267-direc.y;
3485 s=ten1[k];
3486 FrameToBodyTAM(&s,po,&d);
3487 NormPoint3d(&d,&s,&r);
3488 ToPolar3d(&s,&direc);
3489 seg[line].ex=direc.x;
3490 seg[line].ey=1.5707963267-direc.y;
3491 }
3492 else{ // appear=3 or 4
3493 if(appear==3){
3494 n=k;
3495 k=j;
3496 j=n;
3497 }
3498 s=ten0[j];
3499 FrameToBodyTAM(&s,po,&d);
3500 NormPoint3d(&d,&s,&r);
3501 ToPolar3d(&s,&direc);
3502 seg[line].sx=direc.x;
3503 seg[line].sy=1.5707963267-direc.y;
3504 s=ten0[k];
3505 FrameToBodyTAM(&s,po,&d);
3506 NormPoint3d(&d,&s,&r);
3507 ToPolar3d(&s,&direc);
3508 seg[line].ex=direc.x;
3509 seg[line].ey=1.5707963267-direc.y;
3510 }
3511 line++;
3512 }
3513 cont->pte=line;
3514 if(appear==3){
3515 for(i=0;i<nseg;i++){ // upper surface is inner circle
3516 j=i;
3517 k=j-1;
3518 if(k<0) k+=nseg;
3519 s=ten1[j];
3520 FrameToBodyTAM(&s,po,&d);
3521 NormPoint3d(&d,&s,&r);
3522 ToPolar3d(&s,&direc);
3523 seg[line].sx=direc.x;
3524 seg[line].sy=1.5707963267-direc.y;
3525 s=ten1[k];
3526 FrameToBodyTAM(&s,po,&d);
3527 NormPoint3d(&d,&s,&r);
3528 ToPolar3d(&s,&direc);
3529 seg[line].ex=direc.x;
3530 seg[line].ey=1.5707963267-direc.y;
3531 }
3532 line++;
3533 }
3534 if(appear==6){
3535 for(i=0;i<nseg;i++){ // lower surface is inner circle
3536 j=i;
3537 k=j-1;
3538 if(k<0) k+=nseg;
3539 s=ten0[j];
3540 FrameToBodyTAM(&s,po,&d);
3541 NormPoint3d(&d,&s,&r);
3542 ToPolar3d(&s,&direc);
3543 seg[line].sx=direc.x;
3544 seg[line].sy=1.5707963267-direc.y;
3545 s=ten0[k];
3546 FrameToBodyTAM(&s,po,&d);
3547 NormPoint3d(&d,&s,&r);
3548 ToPolar3d(&s,&direc);
3549 seg[line].ex=direc.x;
3550 seg[line].ey=1.5707963267-direc.y;
3551 }
3552 line++;
3553 }
3554 blk->pte=line;
3555 delete[] ten0; // DisposPtr((Ptr)ten0);
3556 delete[] ten1; // DisposPtr((Ptr)ten1);
3557 return;
3558 }
3559// remaining cases are appear 1,3,4 and 6 of tubes
3560
3561// The upper surface is given by an array ten1[].
3562// The lower surface is given by an array ten0[].
3563// looked from... outer circle inner circle
3564// appear=1 the upper side upper circle (normal) lower circle (reverse)
3565// appear=3 the upper side lower circle (normal) upper circle (reverse)
3566// appear=4 the lower side lower circle (reverse) upper circle (normal)
3567// appear=6 the lower side upper circle (reverse) lower circle (normal)
3568
3569 cont->start=line;
3570
3571 if(m==0){ // near side
3572 istart=0;
3573 iend=45;
3574 }
3575 else{ // far side
3576 istart=45;
3577 iend=90;
3578 }
3579// 2 segments, which are part of the contour,
3580// but, not part of the block.
3581
3582 if(appear==1||appear==4){
3583 itb=iend; // top to botton transition
3584 ibt=istart; // bottom to top transition
3585 }
3586 if(appear==3||appear==6){
3587 itb=istart; // top to bottom transition
3588 ibt=iend;
3589 }
3590 s=ten1[itb];
3591 FrameToBodyTAM(&s,po,&d);
3592 NormPoint3d(&d,&s,&r);
3593 ToPolar3d(&s,&direc);
3594 seg[line].sx=direc.x;
3595 seg[line].sy=1.5707963267-direc.y;
3596 s=ten0[itb];
3597 FrameToBodyTAM(&s,po,&d);
3598 NormPoint3d(&d,&s,&r);
3599 ToPolar3d(&s,&direc);
3600 seg[line].ex=direc.x;
3601 seg[line].ey=1.5707963267-direc.y;
3602 line++;
3603 s=ten0[ibt];
3604 FrameToBodyTAM(&s,po,&d);
3605 NormPoint3d(&d,&s,&r);
3606 ToPolar3d(&s,&direc);
3607 seg[line].sx=direc.x;
3608 seg[line].sy=1.5707963267-direc.y;
3609 s=ten1[ibt];
3610 FrameToBodyTAM(&s,po,&d);
3611 NormPoint3d(&d,&s,&r);
3612 ToPolar3d(&s,&direc);
3613 seg[line].ex=direc.x;
3614 seg[line].ey=1.5707963267-direc.y;
3615 line++;
3616
3617
3618// now, the circular portion
3619 blk->start=line;
3620
3621 for(i=istart;i<iend;i++){
3622 if(appear==1||appear==4){
3623 j=i;
3624 k=i+1;
3625 }
3626 else{ // appear==3||appear==6
3627 j=i+1;
3628 k=i;
3629 }
3630 s=ten1[j];
3631 FrameToBodyTAM(&s,po,&d);
3632 NormPoint3d(&d,&s,&r);
3633 ToPolar3d(&s,&direc);
3634 seg[line].sx=direc.x;
3635 seg[line].sy=1.5707963267-direc.y;
3636 s=ten1[k];
3637 FrameToBodyTAM(&s,po,&d);
3638 NormPoint3d(&d,&s,&r);
3639 ToPolar3d(&s,&direc);
3640 seg[line].ex=direc.x;
3641 seg[line].ey=1.5707963267-direc.y;
3642 line++;
3643 }
3644 for(i=istart;i<iend;i++){
3645 if(appear==1||appear==4){
3646 j=i+1;
3647 k=i;
3648 }
3649 else{ // appear==3||appear==6
3650 j=i;
3651 k=i+1;
3652 }
3653 s=ten0[j];
3654 FrameToBodyTAM(&s,po,&d);
3655 NormPoint3d(&d,&s,&r);
3656 ToPolar3d(&s,&direc);
3657 seg[line].sx=direc.x;
3658 seg[line].sy=1.5707963267-direc.y;
3659 s=ten0[k];
3660 FrameToBodyTAM(&s,po,&d);
3661 NormPoint3d(&d,&s,&r);
3662 ToPolar3d(&s,&direc);
3663 seg[line].ex=direc.x;
3664 seg[line].ey=1.5707963267-direc.y;
3665 line++;
3666 }
3667 cont->pte=line;
3668 blk->pte=line;
3669 delete[] ten0; // DisposPtr((Ptr)ten0);
3670 delete[] ten1; // DisposPtr((Ptr)ten1);
3671}
3672
3673/******************************************************************************
3674 GetDrawData1
3675
3676 Return data for drawing for the case of oblong
3677
3678 po thePolar
3679
3680 seg array of line segments (clockwise angle from top,
3681 angle from line of sight)
3682 blk indices of segs which are to be drawn with lines
3683 cont indices of closed line segments. Inside of the closure
3684 will be painted by a color.
3685
3686 ******************************************************************************/
3687
3688void CMPbodyOfRot::GetDrawData1(TransAngleMatrix *po,
3689 Line2d *seg,StartPTE *blk,StartPTE *cont)
3690{
3691 int i,n;
3692 double z1,z2,r,ang0,ang1,ang2,ang,theta,dthet,d,d1,r1,d2,r2,l1,l2;
3693 TransAngleEuler tae;
3694 TransAngleMatrix tam;
3695 Point3d q,s1,u1,s2,u2,v,w,u,b;
3696 Line3d eye,tz,ty;
3697 Point2d e,f,g,a;
3698
3699 r=p0.x; // radius of sphere
3700 z1=p0.y+r; // bottom center of sphere
3701 z2=p1.y-r; // top center of sphere
3702
3703 tae.x=xg;
3704 tae.y=yg;
3705 tae.z=zg;
3706 tae.theta=thetag;
3707 tae.phi=phig;
3708 tae.psi=psig;
3709 TAEtoTAM(&tae,&tam);
3710
3711 q.x=0.;
3712 q.y=0.;
3713 q.z=z1; /* bottom center of sphere */
3714 BodyToFrameTAM(&q,&tam,&s1);
3715 eye.sx=s1.x; // center of the sphere
3716 eye.sy=s1.y;
3717 eye.sz=s1.z;
3718 eye.ex=po->x; // thePolar of CThreeD - position of the viewer's eye
3719 eye.ey=po->y;
3720 eye.ez=po->z;
3721
3722 NormLine3d(&eye,&u1,&d);
3723 d1=r*r/d; // distance to the center of tangent circle from sphere center
3724 r1=sqrt(r*r-d1*d1); // radius of the tangent circle
3725 l1=d-d1; // dist from circle center to eye
3726
3727 q.x=0.;
3728 q.y=0.;
3729 q.z=z2; /* top center of sphere */
3730 BodyToFrameTAM(&q,&tam,&s2);
3731 eye.sx=s2.x; // center of the sphere
3732 eye.sy=s2.y;
3733 eye.sz=s2.z;
3734 eye.ex=po->x; // thePolar of CThreeD - position of the viewer's eye
3735 eye.ey=po->y;
3736 eye.ez=po->z;
3737
3738 NormLine3d(&eye,&u2,&d);
3739 d2=r*r/d; // distance to the center of tangent circle from sphere center
3740 r2=sqrt(r*r-d2*d2); // radius of the tangent circle
3741 l2=d-d2; // dist from circle center to eye
3742
3743 ang0=acos(u1.x*u2.x+u1.y*u2.y+u1.z*u2.z); // angular separation of 2 centers
3744 ang1=atan2(r1,l1); // half-cone angle of bottom sphere
3745 ang2=atan2(r2,l2); // half-cone angle of top sphere
3746
3747 if(ang1>=0.98*(ang0+ang2)||ang2>=0.98*(ang0+ang1)){ // only one sphere is visible
3748 if(ang2>ang1){ // only top sphere is visible
3749 s1=s2; // sphere center position
3750 d1=d2; // dstance to the center of circle from sphere center
3751 r1=r2; // radius of tangent circle
3752 u1=u2; // unit vector toward viewer
3753 }
3754 ToPolar3d(&u1,&a);
3755
3756 tae.x=s1.x+d1*u1.x;
3757 tae.y=s1.y+d1*u1.y;
3758 tae.z=s1.z+d1*u1.z;
3759 tae.theta=1.5707963268-a.y; // z axis toward viewer's eye
3760 tae.phi=a.x+1.5707963268;
3761 tae.psi=0.;
3762 TAEtoTAM(&tae,&tam);
3763
3764 u.x=r1*cos(0.);
3765 u.y=r1*sin(0.);
3766 u.z=0.;
3767 BodyToFrameTAM(&u,&tam,&b);
3768 FrameToBodyTAM(&b,po,&u);
3769 NormPoint3d(&u,&b,&d);
3770 ToPolar3d(&b,&e);
3771 e.y=1.5707963267-e.y;
3772 f=e;
3773 for(i=0;i<90;i++){
3774 g=f;
3775 if(i<89){
3776 ang=.06981317008*(i+1); // every 4 degree
3777 u.x=r1*cos(ang);
3778 u.y=r1*sin(ang);
3779 u.z=0.;
3780 BodyToFrameTAM(&u,&tam,&b);
3781 FrameToBodyTAM(&b,po,&u);
3782 NormPoint3d(&u,&b,&d);
3783 ToPolar3d(&b,&f);
3784 f.y=1.5707963267-f.y;
3785 }
3786 else f=e;
3787 seg[i].sx=g.x;
3788 seg[i].sy=g.y;
3789 seg[i].ex=f.x;
3790 seg[i].ey=f.y;
3791
3792 }
3793 blk->start=0;
3794 blk->pte=90;
3795 cont->start=0;
3796 cont->pte=90;
3797 return;
3798 }
3799 // cylinder part is visible
3800 theta=1.5707963268+asin((ang1-ang2)/ang0); // half-visible angle of bottom circle
3801 // construct coordinate system with z toward eye and x toward bottom from top
3802 v.x=u2.x-u1.x; // unnormalized vector from top to bottom
3803 v.y=u2.y-u1.y;
3804 v.z=u2.z-u1.z;
3805 tz.sx=s1.x+d1*u1.x; // center of bottom circle
3806 tz.sy=s1.y+d1*u1.y;
3807 tz.sz=s1.z+d1*u1.z;
3808 tz.ex=tz.sx+u1.x; // vector toward viewer
3809 tz.ey=tz.sy+u1.y;
3810 tz.ez=tz.sz+u1.z;
3811
3812 VectorProduct(&u1,&v,&w); // obtain y-axis
3813 ty=tz;
3814 ty.ex=ty.sx+w.x; // vector toward viewer
3815 ty.ey=ty.sy+w.y;
3816 ty.ez=ty.sz+w.z;
3817 LinesToTAM(&ty,&tz,yzAxis,&tam);
3818
3819 dthet=.06981317008; // angle of a segment
3820 n=(int)(theta/dthet); // half number of segments
3821 theta=n*dthet;
3822 u.x=r1*cos(-theta);
3823 u.y=r1*sin(-theta);
3824 u.z=0.;
3825 BodyToFrameTAM(&u,&tam,&b);
3826 FrameToBodyTAM(&b,po,&u);
3827 NormPoint3d(&u,&b,&d);
3828 ToPolar3d(&b,&e);
3829 e.y=1.5707963267-e.y;
3830 f=e;
3831 n*=2;
3832 for(i=0;i<n;i++){
3833 g=f;
3834 ang=dthet*(i+1)-theta; // -theta+dthet to theta
3835 u.x=r1*cos(ang);
3836 u.y=r1*sin(ang);
3837 u.z=0.;
3838 BodyToFrameTAM(&u,&tam,&b);
3839 FrameToBodyTAM(&b,po,&u);
3840 NormPoint3d(&u,&b,&d);
3841 ToPolar3d(&b,&f);
3842 f.y=1.5707963267-f.y;
3843 seg[i].sx=g.x;
3844 seg[i].sy=g.y;
3845 seg[i].ex=f.x;
3846 seg[i].ey=f.y;
3847
3848 }
3849
3850 // construct coordinate system with z toward eye and x toward bottom from top
3851 v.x=u2.x-u1.x; // unnormalized vector from top to bottom
3852 v.y=u2.y-u1.y;
3853 v.z=u2.z-u1.z;
3854 tz.sx=s2.x+d2*u2.x; // center of top circle
3855 tz.sy=s2.y+d2*u2.y;
3856 tz.sz=s2.z+d2*u2.z;
3857 tz.ex=tz.sx+u2.x; // vector toward viewer
3858 tz.ey=tz.sy+u2.y;
3859 tz.ez=tz.sz+u2.z;
3860
3861 VectorProduct(&u2,&v,&w); // obtain y-axis
3862 ty=tz;
3863 ty.ex=ty.sx+w.x; // vector toward viewer
3864 ty.ey=ty.sy+w.y;
3865 ty.ez=ty.sz+w.z;
3866 LinesToTAM(&ty,&tz,yzAxis,&tam);
3867
3868 for(;i<91;i++){
3869 g=f;
3870 ang=dthet*i-theta;
3871 u.x=r2*cos(ang);
3872 u.y=r2*sin(ang);
3873 u.z=0.;
3874 BodyToFrameTAM(&u,&tam,&b);
3875 FrameToBodyTAM(&b,po,&u);
3876 NormPoint3d(&u,&b,&d);
3877 ToPolar3d(&b,&f);
3878 f.y=1.5707963267-f.y;
3879 seg[i].sx=g.x;
3880 seg[i].sy=g.y;
3881 seg[i].ex=f.x;
3882 seg[i].ey=f.y;
3883 }
3884 seg[i].sx=f.x;
3885 seg[i].sy=f.y;
3886 seg[i].ex=e.x;
3887 seg[i].ey=e.y;
3888 blk->start=0;
3889 blk->pte=92;
3890 cont->start=0;
3891 cont->pte=92;
3892}
3893
3894
3895/******************************************************************************
3896 FieldOfView
3897
3898 Get horizontal and vertical field of view to cover the object
3899 ******************************************************************************/
3900
3901void CMPbodyOfRot::FieldOfView(TransAngleMatrix *vu,Point2d *az,Point2d *el)
3902{
3903 TransAngleEuler tae;
3904 TransAngleMatrix tam;
3905 int i;
3906 Point3d s;
3907 Point3d f;
3908 Point2d a;
3909 double r0,z0,r1,z1,ca,sa,ang,d;
3910
3911 r0=p0.x;
3912 z0=p0.y;
3913 r1=p1.x;
3914 z1=p1.y;
3915 tae.x=xg; // Translate Angle Euler for the part
3916 tae.y=yg;
3917 tae.z=zg;
3918 tae.theta=thetag;
3919 tae.phi=phig;
3920 tae.psi=psig;
3921 TAEtoTAM(&tae,&tam);
3922 az->x=10.;
3923 az->y=-10.;
3924 el->x=10.;
3925 el->y=-10.;
3926 for(i=0;i<12;i++){
3927 ang=0.01745329*30.*i;
3928 ca=cos(ang);
3929 sa=sin(ang);
3930 s.x=r0*ca;
3931 s.y=r0*sa;
3932 s.z=z0;
3933 BodyToFrameTAM(&s,&tam,&f);
3934 FrameToBodyTAM(&f,vu,&s);
3935 NormPoint3d(&s,&s,&d);
3936 ToPolar3d(&s,&a);
3937 if(a.x<az->x) az->x=a.x;
3938 if(a.x>az->y) az->y=a.x;
3939 if(a.y<el->x) el->x=a.y;
3940 if(a.y>el->y) el->y=a.y;
3941 s.x=r1*ca;
3942 s.y=r1*sa;
3943 s.z=z1;
3944 BodyToFrameTAM(&s,&tam,&f);
3945 FrameToBodyTAM(&f,vu,&s);
3946 NormPoint3d(&s,&s,&d);
3947 ToPolar3d(&s,&a);
3948 if(a.x<az->x) az->x=a.x;
3949 if(a.x>az->y) az->y=a.x;
3950 if(a.y<el->x) el->x=a.y;
3951 if(a.y>el->y) el->y=a.y;
3952 }
3953}
3954
3955/********************************************************************
3956 CThreeD.cp
3957********************************************************************/
3958
3959void CThreeD::Draw()
3960{
3961 Line3d s;
3962 double aEye,dEye,rEye,xTgt,yTgt,zTgt;
3963 double w,h,d,f,ang1,ang2;
3964 Point2d p;
3965 Point3d x,y,z;
3966 double theta;
3967 double phi;
3968 double psi;
3969 double azi,inc;
3970
3971// outstr("at the start of CThreeD::Draw()\n");
3972// flush();
3973 AlignCoord();
3974
3975// VuControl();
3976// outstr("at the exit of CThreeD::VuControl()\n");
3977// flush();
3978
3979 aEye=Azi;
3980 dEye=Ele;
3981 rEye=Dst;
3982
3983 xTgt=Tgx;
3984 yTgt=Tgy;
3985 zTgt=Tgz;
3986
3987 if(!contAll){
3988 f=rEye*cos(d2r*dEye);
3989 w=f*cos(d2r*aEye)+xTgt;
3990 h=f*sin(d2r*aEye)+yTgt;
3991 d=rEye*sin(d2r*dEye)+zTgt;
3992 rEye=sqrt(w*w+h*h+d*d);
3993 aEye=atan2(h,w)/d2r;
3994 dEye=atan2(d,sqrt(w*w+h*h))/d2r;
3995 }
3996
3997 s.sx=rEye*cos(d2r*dEye)*cos(d2r*aEye);
3998 s.sy=rEye*cos(d2r*dEye)*sin(d2r*aEye);
3999 s.sz=rEye*sin(d2r*dEye);
4000
4001 s.ex=xTgt;
4002 s.ey=yTgt;
4003 s.ez=zTgt;
4004 MakeThePolar(&s,&thePolar); // Perspective
4005
4006// convert to double and prepare for conversion to polar coordinate
4007 w=0.5*Wid;
4008 h=0.5*Hei;
4009 d=sqrt(w*w+h*h);
4010 f=Foc;
4011 focus=Foc;
4012
4013// outstr("at the start of processing the sides\n");
4014// flush();
4015
4016// start with right side of the frame
4017 azi=atan2(w,-h); // azimuth
4018 inc=atan2(d,f); // inclination
4019 RtBm.x=azi;
4020 RtBm.y=1.5707963267-inc;
4021 ClipAngleMatrix(azi,inc,&frameRt,&angFrameRt);
4022
4023// top side of the frame
4024 azi=atan2(w,h); // azimuth
4025 inc=atan2(d,f); // inclination
4026 TpRt.x=azi;
4027 TpRt.y=1.5707963267-inc;
4028 ClipAngleMatrix(azi,inc,&frameTp,&angFrameTp);
4029
4030// left side of the frame
4031 azi=atan2(-w,h); // azimuth
4032 inc=atan2(d,f); // inclination
4033 LtTp.x=azi;
4034 LtTp.y=1.5707963267-inc;
4035 ClipAngleMatrix(azi,inc,&frameLt,&angFrameLt);
4036
4037// bottom side of the frame
4038 azi=atan2(-w,-h); // azimuth
4039 inc=atan2(d,f); // inclination
4040 BmLt.x=azi;
4041 BmLt.y=1.5707963267-inc;
4042 ClipAngleMatrix(azi,inc,&frameBm,&angFrameBm);
4043
4044// outstr("at the start of MemorySetUp()\n");
4045// flush();
4046
4047 MemorySetUp();
4048
4049// outstr("at the start of MakePart2()\n");
4050// flush();
4051
4052 MakePart2(); // part2 is surfaces to be used in the far-near comp
4053
4054// outstr("at the start of NearFarRelations()\n");
4055// flush();
4056
4057 NearFarRelations();
4058
4059// outstr("at the start of DrawView()\n");
4060// flush();
4061
4062 DrawView();
4063 part.clear();
4064 npart=0;
4065 message=0;
4066}
4067
4068/******************************************************************************
4069 AlignCoord
4070
4071 calculate general coordinate
4072******************************************************************************/
4073void CThreeD::AlignCoord(void)
4074{
4075 CMPpart *p1;
4076 CMPpart *p2;
4077 TransAngleEuler a,b,c;
4078 int i;
4079
4080 for(i=0;i<npart;i++){
4081 p1=part[i];
4082 a.x=p1->x0;
4083 a.y=p1->y0;
4084 a.z=p1->z0;
4085 a.theta=0.01745329252*p1->theta0;
4086 a.phi=0.01745329252*p1->phi0;
4087 a.psi=0.01745329252*p1->psi0;
4088 if(i==0) c=a;
4089 else{
4090 p2=part[p1->refPart];
4091 b.x=p2->xg;
4092 b.y=p2->yg;
4093 b.z=p2->zg;
4094 b.theta=p2->thetag;
4095 b.phi=p2->phig;
4096 b.psi=p2->psig;
4097 MoveTwiceTAE(&b,&a,&c);
4098 }
4099 p1->xg=c.x;
4100 p1->yg=c.y;
4101 p1->zg=c.z;
4102 p1->thetag=c.theta;
4103 p1->phig=c.phi;
4104 p1->psig=c.psi;
4105 }
4106}
4107
4108
4109void CThreeD::VuControl (void)
4110
4111{
4112 int i;
4113
4114 if(contAll){
4115 TransAngleEuler tae;
4116 TransAngleMatrix tam;
4117 Point2d az,azm,el,elm;
4118 double a,b,e,d;
4119 int i,j;
4120 CMPpart *p;
4121 Point3d los,tgt;
4122
4123 a=0.017453293*Azi;
4124 e=0.017453293*Ele;
4125 d=Dst;
4126 tae.x=d*cos(e)*cos(a);
4127 tae.y=d*cos(e)*sin(a);
4128 tae.z=d*sin(e);
4129 if(e>=0.){
4130 tae.theta=e;
4131 tae.phi=a-1.5707963267;
4132 tae.psi=-1.5707963267;
4133 }
4134 else{
4135 tae.theta=-e;
4136 tae.phi=a+1.5707963267;
4137 tae.psi=1.5707963267;
4138 }
4139 TAEtoTAM(&tae,&tam);
4140 azm.x=10.;
4141 azm.y=-10.;
4142 elm.x=10.;
4143 elm.y=-10.;
4144// outstr("in CThreeD::VuControl()\n");
4145// sprintf(msg,"npart=%i\n",npart);
4146// outstr(msg);
4147// flush();
4148 for(i=0;i<npart;i++){
4149 p=part[i];
4150 if(p->showDrawing){
4151 switch(p->kind){
4152 case MPlines:
4153 reinterpret_cast<CMPlines *>(p)->FieldOfView(&tam,&az,&el);
4154 break;
4155 case MPplane:
4156 reinterpret_cast<CMPplane *>(p)->FieldOfView(&tam,&az,&el);
4157 break;
4158 case MPbox:
4159 reinterpret_cast<CMPbox *>(p)->FieldOfView(&tam,&az,&el);
4160 break;
4161 case MPsphere:
4162 reinterpret_cast<CMPsphere *>(p)->FieldOfView(&tam,&az,&el);
4163 break;
4164 case MPbodyOfRot:
4165 reinterpret_cast<CMPbodyOfRot *>(p)->FieldOfView(&tam,&az,&el);
4166 break;
4167 default:
4168 az.x=10.;
4169 az.y=-10.;
4170 el.x=10.;
4171 el.y=-10.;
4172 break;
4173 }
4174 if(az.x<azm.x) azm.x=az.x;
4175 if(az.y>azm.y) azm.y=az.y;
4176 if(el.x<elm.x) elm.x=el.x;
4177 if(el.y>elm.y) elm.y=el.y;
4178 }
4179 }
4180 los.x=d*cos(0.5*(elm.x+elm.y))*cos(0.5*(azm.x+azm.y));
4181 los.y=d*cos(0.5*(elm.x+elm.y))*sin(0.5*(azm.x+azm.y));
4182 los.z=d*sin(0.5*(elm.x+elm.y));
4183 BodyToFrameTAM(&los,&tam,&tgt);
4184 Tgx=tgt.x;
4185 Tgy=tgt.y;
4186 Tgz=tgt.z;
4187 a=Wid/2-5;
4188 a=a/tan(0.5*(azm.y-azm.x));
4189 b=Hei/2-5;
4190 b=b/tan(0.5*(elm.y-elm.x));
4191 Foc=(int)((a<b)? a:b);
4192 }
4193}
4194
4195
4196/********************************************************************
4197 MemorySetUp
4198
4199 Analize itsList and allocate memory for following handles
4200
4201 part1part obtain the 'part1' number for a given 'part' number
4202 partpart1 obtain the 'part' number for a given 'part1' number
4203 part2part1 obtain the 'part2' number for a given 'part1' number
4204 part2 array of surfaces indexed by 'part2' number
4205 local array of surface points referred by part2
4206 viewed local converted to polar coordinate
4207 sorted part 1 sorted to the farthest order
4208
4209 part1part and partpart1 are defined in this routine
4210********************************************************************/
4211void CThreeD::MemorySetUp( void)
4212{
4213 int i,j,k,l,m;
4214 int ptesti;
4215 CMPpart *p;
4216 CMPpart *p0; // the first CMPplane of the consecutive group planes
4217 CMPpart *p1;
4218
4219
4220 part1part=new StartPTE[npart]; // (StartPTE *)NewPtr(sizeof(StartPTE)*npart);
4221
4222 npart1=0; // to be incremented by j
4223 npart2m=0; // to be incremented by k
4224 npoinm=0; // to be incremented by l
4225
4226 p0=NULL;
4227
4228 for(i=0;i<npart;i++){
4229 p=part[i]; // itsList->NthItem(i+1)
4230 if(p->showDrawing){
4231 switch(p->kind){
4232 case MPpoint:
4233 j=0; // points are not drawn
4234 k=0; // no near-far comparison
4235 l=0; // no surface points
4236 break;
4237 case MPlines:
4238 j=reinterpret_cast<CMPlines *>(p)->npoint;
4239 j--; // number of segments is number of points minus one
4240 k=j; // one line segment is one generalized surface
4241 l=j*2; // each generalized surface has two points
4242 break;
4243 case MPplane:
4244 j=1; // a plane is a drawing unit
4245 k=1; // one unit in the far-near comparison
4246 l=reinterpret_cast<CMPplane *>(p)->npoint; // points defining the plane
4247
4248 if(reinterpret_cast<CMPplane *>(p)->isGroup){
4249 if(p0==NULL) p0=p;
4250 p1=part[i+1];
4251 if(p1->kind==MPplane){
4252 if(reinterpret_cast<CMPplane *>(p1)->isGroup)
4253 reinterpret_cast<CMPplane *>(p)->otherHalf=reinterpret_cast<CMPplane *>(p1);
4254 else{
4255 reinterpret_cast<CMPplane *>(p)->otherHalf=reinterpret_cast<CMPplane *>(p0);
4256 p0=NULL;
4257 }
4258 }
4259 else{
4260 reinterpret_cast<CMPplane *>(p)->otherHalf=reinterpret_cast<CMPplane *>(p0);
4261 p0=NULL;
4262 }
4263 }
4264 if(reinterpret_cast<CMPplane *>(p)->isPolyhed)
4265 if(!reinterpret_cast<CMPplane *>(p)->FacingViewer((Point3d *)&thePolar)){
4266 j=0;
4267 k=0;
4268 l=0;
4269 }
4270 break;
4271 case MPbox:
4272 j=1; // a box is a drawing unit
4273 k=3; // 3 surfaces max
4274 l=12; // 4 points * 3 surfaces
4275 break;
4276 case MPsphere:
4277 j=1; // a sphere is a drawing unit
4278 k=1; // represented by a surface
4279 l=12; // represented by a 12 sided polygon
4280 break;
4281 case MPbodyOfRot:
4282 if(reinterpret_cast<CMPbodyOfRot *>(p)->isTube){
4283 j=2; // near and far sides
4284 k=12; // 12 surfaces combined, 6 surfaces each
4285 l=48; // 4 points * 12 surfaces
4286 }
4287 else{
4288 j=1; // represented by a drawing unit
4289 k=13; // 12 surfaces + top
4290 l=60; // 4 points * 12 surfaces + 12 points
4291 }
4292 break;
4293 }
4294 }
4295 else{ // if showDrawing is false skip data
4296 j=0;
4297 k=0;
4298 l=0;
4299 }
4300 part1part[i].start=npart1;
4301 npart1+=j;
4302 part1part[i].pte=npart1;
4303 npart2m+=k;
4304 npoinm+=l;
4305 }
4306 npoinm+=5; // alloance for increase by clipping
4307 partpart1=new StartPTE[npart1]; // (StartPTE *)NewPtr(sizeof(StartPTE)*npart1);
4308 for(i=0;i<npart;i++){
4309 j=part1part[i].start;
4310 k=part1part[i].pte;
4311 m=0;
4312 for(l=j;l<k;l++){
4313 partpart1[l].start=i;
4314 partpart1[l].pte=m++;
4315 }
4316 }
4317
4318 part2part1=new StartPTE[npart1]; // (StartPTE *)NewPtr(sizeof(StartPTE)*npart1);
4319 part2=new Surface[npart2m]; // (Surface *)NewPtr(sizeof(Surface)*npart2m);
4320 part1part2=new int[npart2m]; // (int *)NewPtr(sizeof(short)*npart2m);
4321 local=new Point2d[npoinm]; // (Point2d *)NewPtr(sizeof(Point2d)*npoinm);
4322 viewed=new Point2d[npoinm]; //(Point2d *)NewPtr(sizeof(Point2d)*npoinm);
4323// sorted=(int *)NewPtr(sizeof(short)*npart1);
4324
4325
4326}
4327
4328
4329/********************************************************************
4330 MakePart2
4331********************************************************************/
4332void CThreeD::MakePart2( void)
4333{
4334 int i,j,k,l,m,n;
4335 int ptesti;
4336 CMPpart *p;
4337 double x,y,z,x1,y1,z1;
4338 Surface surf1;
4339 Surface surf2;
4340 Point2d *surfPt1;
4341 Point2d *surfPt2;
4342 Point2d a;
4343 TransAngleMatrix tam;
4344
4345 npart2=0; // number of parts after no 2 decomposition
4346 npoin=0;
4347 surfPt1=new Point2d[50]; // (Point2d *)NewPtr(sizeof(Point2d)*50);
4348 surfPt2=new Point2d[50]; // (Point2d *)NewPtr(sizeof(Point2d)*50);
4349
4350// outstr("in MakePart2()\n");
4351// sprintf(msg,"npart=%i\n",npart);
4352// outstr(msg);
4353// flush();
4354
4355 for(i=0;i<npart;i++){
4356 j=part1part[i].start;
4357 k=part1part[i].pte;
4358// sprintf(msg,"i=%i j=%i k=%i\n",i,j,k);
4359// outstr(msg);
4360// flush();
4361 if(j<k){
4362 p=part[i]; // (CMPpart *)itsList->NthItem(i+1);
4363 switch(p->kind){
4364 case MPpoint:
4365 part2part1[j].start=-1; // part2 not generated
4366 part2part1[j].pte=-1;
4367 break;
4368 case MPlines:
4369 l=reinterpret_cast<CMPlines *>(p)->npoint;
4370 for(m=0;m<l-1;m++){
4371 reinterpret_cast<CMPlines *>(p)->GetSurface(m,&thePolar,&surf1,surfPt1);
4372 if(surf1.start<0){ // degenerate to a point
4373 surf2.start=-1;
4374 surf2.pte=-1;
4375 }
4376 else ClipSurface(&surf1,surfPt1,&surf2,surfPt2);
4377 if(surf2.start!=surf2.pte){
4378 part2[npart2]=surf2;
4379 part2[npart2].start=npoin;
4380 local[npoin++]=surfPt2[0];
4381 local[npoin++]=surfPt2[1];
4382 part2[npart2].pte=npoin;
4383 part1part2[npart2]=j;
4384 part2part1[j].start=npart2;
4385 npart2++;
4386 part2part1[j].pte=npart2;
4387 }
4388 else{
4389 part2part1[j].start=-1; // part2 not generated
4390 part2part1[j].pte=-1;
4391 }
4392 j++;
4393 }
4394 break;
4395 case MPplane:
4396 reinterpret_cast<CMPplane *>(p)->GetSurface(&thePolar,&surf1,surfPt1);
4397 ClipSurface(&surf1,surfPt1,&surf2,surfPt2);
4398 if(surf2.start!=surf2.pte){
4399 part2[npart2]=surf2;
4400 part2[npart2].start=npoin;
4401 for(n=surf2.start;n<surf2.pte;n++)
4402 local[npoin++]=surfPt2[n];
4403 part2[npart2].pte=npoin;
4404 part1part2[npart2]=j;
4405 part2part1[j].start=npart2;
4406 npart2++;
4407 part2part1[j].pte=npart2;
4408 }
4409 else{
4410 part2part1[j].start=-1; // part2 not generated
4411 part2part1[j].pte=-1;
4412 }
4413 break;
4414 case MPbox:
4415// outstr("in MakePart2() at MPbox\n");
4416// sprintf(msg,"npart2=%i\n",npart2);
4417// outstr(msg);
4418// flush();
4419
4420 part1part2[npart2]=j;
4421 part2part1[j].start=npart2;
4422 reinterpret_cast<CMPbox *>(p)->ClearVisSurface();
4423 for(m=0;m<3;m++){
4424
4425 if(reinterpret_cast<CMPbox *>(p)->GetSurface(m,&thePolar,&surf1,surfPt1)){
4426// outstr("at the exit of GetSurface()\n");
4427// sprintf(msg,"m=%i surf1.x0=%8.3f surf1.y0=%8.3f surf1.z0=%8.3f \n",m,surf1.x0,surf1.y0,surf1.z0);
4428// outstr(msg);
4429// sprintf(msg," surf1.theta0=%8.3f surf1.phi0=%8.3f surf1.psi0=%8.3f \n",surf1.theta0,surf1.phi0,surf1.psi0);
4430// outstr(msg);
4431// flush();
4432
4433 ClipSurface(&surf1,surfPt1,&surf2,surfPt2);
4434// outstr("at the exit of ClipSurface()\n");
4435// sprintf(msg," surf2.start=%i surf2.pte=%i npart2=%i npoin=%i j=%i\n",surf2.start,surf2.pte,npart2,npoin,j);
4436// outstr(msg);
4437// flush();
4438 if(surf2.start!=surf2.pte){
4439 part2[npart2]=surf2;
4440 part2[npart2].start=npoin;
4441 for(n=surf2.start;n<surf2.pte;n++)
4442 local[npoin++]=surfPt2[n];
4443 part2[npart2].pte=npoin;
4444 part1part2[npart2]=j;
4445 npart2++;
4446 }
4447 }
4448
4449 }
4450 if(part2part1[j].start!=npart2)
4451 part2part1[j].pte=npart2;
4452 else{
4453 part2part1[j].start=-1; // part2 not generated
4454 part2part1[j].pte=-1;
4455 }
4456 break;
4457 case MPsphere:
4458 reinterpret_cast<CMPsphere *>(p)->GetSurface(&thePolar,&surf1,surfPt1);
4459 ClipSurface(&surf1,surfPt1,&surf2,surfPt2);
4460 if(surf2.start!=surf2.pte){
4461 part2[npart2]=surf2;
4462 part2[npart2].start=npoin;
4463 for(n=surf2.start;n<surf2.pte;n++)
4464 local[npoin++]=surfPt2[n];
4465 part2[npart2].pte=npoin;
4466 part1part2[npart2]=j;
4467 part2part1[j].start=npart2;
4468 npart2++;
4469 part2part1[j].pte=npart2;
4470 }
4471 else{
4472 part2part1[j].start=-1; // part2 not generated
4473 part2part1[j].pte=-1;
4474 }
4475 break;
4476 case MPbodyOfRot:
4477 part1part2[npart2]=j;
4478 part2part1[j].start=npart2;
4479 reinterpret_cast<CMPbodyOfRot *>(p)->ClearVisSurface();
4480 for(m=0;m<13;m++){
4481 if(reinterpret_cast<CMPbodyOfRot *>(p)->GetSurface(m,true, // near side
4482 &thePolar,&surf1,surfPt1)){
4483 ClipSurface(&surf1,surfPt1,&surf2,surfPt2);
4484 if(surf2.start!=surf2.pte){
4485 part2[npart2]=surf2;
4486 part2[npart2].start=npoin;
4487 for(n=surf2.start;n<surf2.pte;n++)
4488 local[npoin++]=surfPt2[n];
4489 part2[npart2].pte=npoin;
4490 part1part2[npart2]=j;
4491 npart2++;
4492 }
4493 }
4494 }
4495 if(part2part1[j].start!=npart2)
4496 part2part1[j].pte=npart2;
4497 else{
4498 part2part1[j].start=-1; // part2 not generated
4499 part2part1[j].pte=-1;
4500 }
4501 if(reinterpret_cast<CMPbodyOfRot *>(p)->isTube){
4502 j++;
4503 part1part2[npart2]=j;
4504 part2part1[j].start=npart2;
4505 for(m=0;m<10;m++){
4506 if(reinterpret_cast<CMPbodyOfRot *>(p)->GetSurface(m,false,
4507 &thePolar,&surf1,surfPt1)){
4508 ClipSurface(&surf1,surfPt1,&surf2,surfPt2);
4509 if(surf2.start!=surf2.pte){
4510 part2[npart2]=surf2;
4511 part2[npart2].start=npoin;
4512 for(n=surf2.start;n<surf2.pte;n++)
4513 local[npoin++]=surfPt2[n];
4514 part1part2[npart2]=j;
4515 part2[npart2++].pte=npoin;
4516 }
4517 }
4518 }
4519 if(part2part1[j].start!=npart2)
4520 part2part1[j].pte=npart2;
4521 else{
4522 part2part1[j].start=-1; // part2 not generated
4523 part2part1[j].pte=-1;
4524 }
4525 }
4526 break;
4527 }
4528 }
4529 }
4530 delete[] surfPt1;
4531 delete[] surfPt2;
4532// convert local to viewed
4533 for(i=0;i<npart2;i++){
4534 surf1=part2[i];
4535 SurfthePolarTAM((TransAngleEuler *)&surf1,&thePolar,&tam);
4536 for(j=surf1.start;j<surf1.pte;j++){
4537 SurfToPolar(&local[j],&tam,&a);
4538 PolarToGnomo(&a,&viewed[j]);
4539 }
4540 }
4541 delete[] local; // allocated at MemorySetUp()
4542}
4543
4544
4545/********************************************************************
4546 NearFarRelations
4547
4548 establish near-far relationship
4549********************************************************************/
4550
4551void CThreeD::NearFarRelations(void)
4552{
4553 int i,j,k,l,m;
4554 CMPpart *p;
4555 Surface s1,s2;
4556 bool s1isLine;
4557 bool doit;
4558 Point2d p0,p1;
4559 TransAngleMatrix m1,m2;
4560 double d;
4561 char ss[100];
4562 int kind1,kind2;
4563 Point3d center;
4564 Point3d s1z,s2z; // normal to the surface vector in view coordinate
4565 double s1m,s2m; // multiplier in the expression calculating z
4566 long prog,progm;
4567 int progs;
4568 CFarNear *farN;
4569 FILE *fp;
4570
4571 farN=new CFarNear(npart1);
4572
4573 // fp=fopen("/tmp/gcs3DDATA","w");
4574 // fprintf(fp,"npart=%i npart1=%i npart2=%i\n",npart,npart1,npart2);
4575 // fclose(fp);
4576
4577// enter far-near relationship of tubes
4578 for(i=0;i<npart;i++){
4579 p=part[i];
4580 if(p->showDrawing&&p->kind==MPbodyOfRot){
4581 if(reinterpret_cast<CMPbodyOfRot *>(p)->isTube){
4582 j=part1part[i].start;
4583 farN->AddFarNear(j+1,j);
4584// outstr("in NearFarRelations()\n");
4585// sprintf(msg,"i=%i j=%i\n",i,j);
4586// outstr(msg);
4587// flush();
4588
4589 }
4590 }
4591 }
4592
4593// if a line coinsides a side of a plane, put the line in farther position
4594 for(i=0;i<npart;i++){
4595 p=part[i];
4596 if(p->showDrawing&&p->kind==MPplane){
4597 double s1z[3],s20[3];
4598 k=part1part[i].start;
4599 s1=part2[part2part1[k].start];
4600 s1z[0]=sin(s1.theta0)*sin(s1.phi0); // axis normal to the plane
4601 s1z[1]=-sin(s1.theta0)*cos(s1.phi0);
4602 s1z[2]=cos(s1.theta0);
4603 for(j=0;j<npart2;j++){
4604 s2=part2[j];
4605 if(s2.pte-s2.start==2){ // line
4606 s20[0]=s2.x0-s1.x0;
4607 s20[1]=s2.y0-s1.y0;
4608 s20[2]=s2.z0-s1.z0;
4609 d=s1z[0]*s20[0]+s1z[0]*s20[0]+s1z[0]*s20[0];
4610 if(fabs(d)<1.e-10){
4611 m=0;
4612 for(l=s1.start;l<s1.pte;l++){
4613 if(fabs(viewed[i].x-viewed[s2.start].x)<1.e-5
4614 &&fabs(viewed[i].y-viewed[s2.start].y)<1.e-5) m++;
4615 if(fabs(viewed[i].x-viewed[s2.start+1].x)<1.e-5
4616 &&fabs(viewed[i].y-viewed[s2.start+1].y)<1.e-5) m++;
4617 }
4618 if(m==2){
4619 farN->AddFarNear(part1part2[j],k); // line plane
4620 }
4621 }
4622 }
4623 }
4624 }
4625 }
4626
4627// outstr("in NearFarRelations()\n");
4628// sprintf(msg,"npart2=%i\n",npart2);
4629// outstr(msg);
4630// flush();
4631
4632// compare each surfaces in the combination triangle
4633 prog=0; // for progress status (long)
4634 progm=npart2;
4635 progm=progm*(progm-1)/2;
4636 for(i=0;i<npart2-1;i++){
4637 k=part1part2[i];
4638 s1=part2[i];
4639 if(s1.pte-s1.start==2) s1isLine=true;
4640 else s1isLine=false;
4641
4642// 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);
4643// outstr(msg);
4644// flush();
4645
4646 p=part[partpart1[k].start];
4647 kind1=p->kind;
4648 if(kind1==MPbox) reinterpret_cast<CMPbox *>(p)->GetCenter(¢er);
4649 if(kind1==MPbodyOfRot) reinterpret_cast<CMPbodyOfRot *>(p)->GetCenter(¢er);
4650 SurfthePolarTAM((TransAngleEuler *)&s1,&thePolar,&m1);
4651 if(kind1==MPplane){ // used when both parts are planes
4652 Point3d p3;
4653 p3.x=0.;
4654 p3.y=0.;
4655 p3.z=1.; // a point along body z axis
4656 BodyToFrameTAM(&p3,&m1,&s1z);
4657 s1z.x-=m1.x;
4658 s1z.y-=m1.y;
4659 s1z.z-=m1.z;
4660 s1m=s1z.x*m1.x+s1z.y*m1.y+s1z.z*m1.z; // multiplier
4661 }
4662 for(j=i+1;j<npart2;j++){
4663 prog++;
4664 l=part1part2[j];
4665 s2=part2[j];
4666 doit=true;
4667 if(k==l) doit=false;
4668 else if(s1isLine&&(s2.pte-s2.start==2)) doit=false; // dont do it for lines
4669 else doit=!(farN->InferFarNear(k,l,&m));
4670// sprintf(msg,"j=%i k=%i l=%i doit=%i\n",j,k,l,doit);
4671// outstr(msg);
4672//flush();
4673// fp=fopen("/tmp/gcs3DDATA","w");
4674// fprintf(fp,"i=%i j=%i k=%i l=%i doit=%i\n",i,j,k,l,doit);
4675// fclose(fp);
4676
4677 if(doit){
4678// sprintf(msg,"part2[i].start=%i part2[i].pte=%i part2[j].start=%i part2[j].pte=%i\n",
4679// part2[i].start,part2[i].pte,part2[j].start,part2[j].pte);
4680// outstr(msg);
4681// flush();
4682
4683 if(Overlap(viewed,(StartPTE *)&part2[i].start,
4684 (StartPTE *)&part2[j].start,&p0)){
4685// outstr("Overlap\n");
4686// flush();
4687 SurfthePolarTAM((TransAngleEuler *)&s2,&thePolar,&m2);
4688 GnomoToPolar(&p0,&p1);
4689 FarNearDist(&m1,&m2,&p1,&d);
4690 if(d>0.001){
4691 farN->AddFarNear(k,l);
4692 doit=false;
4693 }
4694 if(d<-0.001){
4695 farN->AddFarNear(l,k);
4696 doit=false;
4697 }
4698 if(doit){
4699 if(kind1==MPplane||kind1==MPbox||kind1==MPbodyOfRot){
4700 p=part[partpart1[l].start];
4701 kind2=p->kind;
4702 if(kind1==MPplane){
4703 if(kind2==MPplane){ // process the planes with common side
4704 Point3d p3;
4705 double z1,z2;
4706 int m,n10,n1p,n1n,n20,n2p,n2n; // zero, plus or negative
4707 p3.x=0.;
4708 p3.y=0.;
4709 p3.z=1.;
4710 BodyToFrameTAM(&p3,&m2,&s2z);
4711 s2z.x-=m2.x;
4712 s2z.y-=m2.y;
4713 s2z.z-=m2.z;
4714 s2m=s2z.x*m2.x+s2z.y*m2.y+s2z.z*m2.z;
4715 n10=0;
4716 n1p=0;
4717 n1n=0;
4718 n20=0;
4719 n2p=0;
4720 n2n=0;
4721 if(fabs(s1z.x*s2z.x+s1z.y*s2z.y+s1z.z*s2z.z)<0.98){ // not parallel
4722 for(m=s1.start;m<s1.pte;m++){
4723 z1=s1m/(s1z.x*viewed[m].y+s1z.y*viewed[m].x+s1z.z);
4724 z2=s2m/(s2z.x*viewed[m].y+s2z.y*viewed[m].x+s2z.z);
4725 if(z2>0.){
4726 if(fabs(z1-z2)<0.001) n10++;
4727 else if(z1>z2) n1p++;
4728 else n1n++;
4729 }
4730 }
4731 for(m=s2.start;m<s2.pte;m++){
4732 z1=s1m/(s1z.x*viewed[m].y+s1z.y*viewed[m].x+s1z.z);
4733 z2=s2m/(s2z.x*viewed[m].y+s2z.y*viewed[m].x+s2z.z);
4734 if(z1>0.){
4735 if(fabs(z1-z2)<0.001) n20++;
4736 else if(z1>z2) n2p++;
4737 else n2n++;
4738 }
4739 }
4740 if(n10>1&&n20>1){
4741 if(n1p>0&&n2p>0){ // s1 is farther
4742 farN->AddFarNear(k,l);
4743 doit=false;
4744 }
4745 if(n1n>0&&n2n>0){ // s2 is farther
4746 farN->AddFarNear(l,k);
4747 doit=false;
4748 }
4749 } // if(n10>1&&n20>1){
4750 } // if(fabs(s1z.x*s2z.x+s1z.y*s2z.y+s1z.z*s2z.z)<0.98){
4751 } // if(kind2==MPplane){
4752 else if(kind2==MPbox||kind2==MPbodyOfRot){
4753 if(kind2==MPbox) reinterpret_cast<CMPbox *>(p)->GetCenter(¢er);
4754 if(kind2==MPbodyOfRot) reinterpret_cast<CMPbodyOfRot *>(p)->GetCenter(¢er);
4755 if(SameSideOfSurface(&s1,¢er,(Point3d *)&thePolar)){
4756 farN->AddFarNear(k,l);
4757 doit=false;
4758 }
4759 else{
4760 farN->AddFarNear(l,k);
4761 doit=false;
4762 }
4763 } // else if(kind2==MPbox||kind2==MPbodyOfRot){
4764 } // if(kind1==MPplane){
4765 else{ // kind1==MPbox or MPbodyOfRot
4766 if(kind2==MPplane){
4767 if(SameSideOfSurface(&s2,¢er,(Point3d *)&thePolar)){
4768 farN->AddFarNear(l,k);
4769 doit=false;
4770 }
4771 else{
4772 farN->AddFarNear(k,l);
4773 doit=false;
4774 }
4775 } // if(kind2==MPplane){
4776 } // else
4777 } // if(kind1==MPplane||kind1==MPbox||kind1==MPbodyOfRot){
4778 } // if(doit){
4779/* if(doit){
4780
4781 GnomoToPolar(&p0,&p1);
4782 FarNearDist(&m1,&m2,&p1,&d);
4783 if(d>0.){
4784 farN->AddFarNear(k,l);
4785 }
4786 if(d<0.){
4787 farN->AddFarNear(l,k);
4788 }
4789 }*/
4790 } // if(Overlap(viewed,(StartPTE *)&part2[i].start,(StartPTE *)&part2[j].start,&p0)){
4791 } // if(doit){
4792
4793 } // for(j=i+1;j<npart2;j++){
4794 } // for(i=0;i<npart2-1;i++){
4795
4796 delete[] part2part1; // allocated in MemorySetUp
4797 delete[] part1part2; // allocated in MemorySetUp
4798 delete[] part2; // allocated in MemorySetUp
4799 delete[] viewed; // allocated in MemorySetUp
4800 delete[] part1part; // allocated in MemorySetUp at the beginning
4801
4802 sorted=new int[npart1]; // (int *)NewPtr(sizeof(short)*npart1);
4803 fp=fopen("/tmp/gcs3DDATA","w");
4804 fprintf(fp,"before SetSorted npart1=%i\n",npart1);
4805 fclose(fp);
4806
4807 m=farN->SetSorted(sorted);
4808 if(m!=0){
4809// sprintf(ss,"\rloop found in farNear m=%i",m);
4810// Monitor(90,(Ptr)ss,true);
4811 }
4812 delete farN;
4813 fp=fopen("/tmp/gcs3DDATA","w");
4814 fprintf(fp,"at the end of NearFarRelations m=%i\n",m);
4815 fclose(fp);
4816
4817}
4818
4819
4820/********************************************************************
4821 DrawView
4822
4823 draw in the view
4824********************************************************************/
4825void CThreeD::DrawView(void)
4826{
4827 int i,j,k,l,m,n,o,q;
4828 StartPTE pt;
4829 CMPpart *p;
4830 Line2d *seg;
4831 StartPTE blk;
4832 StartPTE cont;
4833 double x,y;
4834 int ix,iy;
4835 double maxang;
4836 int red,green,blue;
4837 bool finished[200];
4838 bool isLines;
4839 FILE *fp;
4840 char *buff;
4841 int repeat;
4842 CMPdeco *pld;
4843
4844
4845 maxang=atan(10000./focus);
4846
4847 seg=new Line2d[200]; // (Line2d *)NewPtr(sizeof(Line2d)*200);
4848
4849 // MakeView(2*Wid,2*Hei);
4850 // SetColor(100,100,100);
4851 // OpenPoly();
4852 // MoveTo(0,0);
4853 // LineTo(0,2*Hei);
4854 // LineTo(2*Wid,2*Hei);
4855 // LineTo(2*Wid,0);
4856 // ClosePoly();
4857 fp=fopen("/tmp/gcs3DDATA","w");
4858 fwrite(&npart,sizeof(int),1,fp);
4859 fwrite(&npart1,sizeof(int),1,fp);
4860 for(i=0;i<npart1;i++){
4861 j=sorted[i]; // part 1 index
4862 pt=partpart1[j]; // part index
4863 k=pt.start;
4864 p=part[k]; // (CMPpart *)itsList->NthItem(k+1);
4865 isLines=false;
4866 repeat=1;
4867 switch (p->kind){
4868 case MPlines:
4869 reinterpret_cast<CMPlines *>(p)->GetDrawData(pt.pte,&thePolar,seg,&blk,&cont);
4870 isLines=true;
4871 break;
4872 case MPplane:
4873 reinterpret_cast<CMPplane *>(p)->GetDrawData(&thePolar,seg,&blk,&cont);
4874 l=reinterpret_cast<CMPplane *>(p)->ndeco;
4875 if(l!=0) repeat+=l;
4876 break;
4877 case MPbox:
4878 reinterpret_cast<CMPbox *>(p)->GetDrawData(&thePolar,seg,&blk,&cont);
4879 break;
4880 case MPsphere:
4881 reinterpret_cast<CMPsphere *>(p)->GetDrawData(&thePolar,seg,&blk,&cont);
4882 l=reinterpret_cast<CMPsphere *>(p)->ndeco;
4883 if(l!=0) repeat+=l;
4884 break;
4885 case MPbodyOfRot:
4886 reinterpret_cast<CMPbodyOfRot *>(p)->GetDrawData(pt.pte,&thePolar,seg,&blk,&cont);
4887 break;
4888 }
4889 for(m=0;m<repeat;m++){
4890 if(repeat!=1&&m>0){
4891 if(p->kind==MPplane)
4892 pld= reinterpret_cast<CMPplane *>(p)->deco[m-1];
4893 else
4894 pld= reinterpret_cast<CMPsphere *>(p)->deco[m-1];
4895 pld->GetDrawData(&thePolar,seg,&blk,&cont);
4896 j=pld->color;
4897 }
4898 else j=p->color;
4899 l=blk.pte;
4900 k=4+4+2*4+2*4+l*4*8+10;
4901 buff=(char *)malloc(sizeof(char)*k);
4902 *(int *)buff=j;
4903 *(int *)(buff+4)=isLines;
4904 *(int *)(buff+8)=blk.start;
4905 *(int *)(buff+12)=blk.pte;
4906 *(int *)(buff+16)=cont.start;
4907 *(int *)(buff+20)=cont.pte;
4908 for(j=0;j<l;j++){
4909 *(double *)(buff+24+j*32)=seg[j].sx;
4910 *(double *)(buff+24+j*32+8)=seg[j].sy;
4911 *(double *)(buff+24+j*32+16)=seg[j].ex;
4912 *(double *)(buff+24+j*32+24)=seg[j].ey;
4913 }
4914 fwrite(&k,sizeof(int),1,fp);
4915 fwrite(buff,sizeof(char),k,fp);
4916 free(buff);
4917 }
4918 if(repeat!=1){
4919 if(p->kind==MPplane) reinterpret_cast<CMPplane *>(p)->GetDrawData(&thePolar,seg,&blk,&cont);
4920 else reinterpret_cast<CMPsphere *>(p)->GetDrawData(&thePolar,seg,&blk,&cont);
4921 j=p->color;
4922 l=blk.pte;
4923 k=4+4+2*4+2*4+l*4*8+10;
4924 buff=(char *)malloc(sizeof(char)*k);
4925 *(int *)buff=j;
4926 *(int *)(buff+4)=isLines;
4927 *(int *)(buff+8)=blk.start;
4928 *(int *)(buff+12)=blk.pte;
4929 *(int *)(buff+16)=0;
4930 *(int *)(buff+20)=0;
4931 for(j=0;j<l;j++){
4932 *(double *)(buff+24+j*32)=seg[j].sx;
4933 *(double *)(buff+24+j*32+8)=seg[j].sy;
4934 *(double *)(buff+24+j*32+16)=seg[j].ex;
4935 *(double *)(buff+24+j*32+24)=seg[j].ey;
4936 }
4937 fwrite(&k,sizeof(int),1,fp);
4938 fwrite(buff,sizeof(char),k,fp);
4939 free(buff);
4940 }
4941 }
4942 k=0; /* end of data */
4943 fwrite(&k,sizeof(int),1,fp);
4944 k=90;
4945 buff=(char *)malloc(sizeof(char)*k);
4946 *(double *)buff=thePolar.x;
4947 *(double *)(buff+8)=thePolar.y;
4948 *(double *)(buff+8*2)=thePolar.z;
4949 *(double *)(buff+8*3)=thePolar.c[0][0];
4950 *(double *)(buff+8*4)=thePolar.c[0][1];
4951 *(double *)(buff+8*5)=thePolar.c[0][2];
4952 *(int *)(buff+8*6)=p->kind;
4953 *(double *)(buff+52)=p->x0;
4954 *(double *)(buff+52+8)=p->y0;
4955 *(double *)(buff+52+8*2)=p->z0;
4956 *(double *)(buff+52+8*3)=p->xg;
4957 fwrite(buff,sizeof(char),k,fp);
4958 free(buff);
4959 fclose(fp);
4960#if 0
4961 k=cont.start; // supposed to cover all segments
4962 l=blk.pte;
4963 for(j=k;j<l;j++){
4964 x=seg[j].sx;
4965 y=seg[j].sy;
4966 if(y>maxang) y=maxang;
4967 y=2*focus*tan(y);
4968 seg[j].sx=y*sin(x);
4969 seg[j].sy=y*cos(x);
4970 x=seg[j].ex;
4971 y=seg[j].ey;
4972 if(y>maxang) y=maxang;
4973 y=2*focus*tan(y);
4974 seg[j].ex=y*sin(x);
4975 seg[j].ey=y*cos(x);
4976 }
4977
4978 if(l-k>1){
4979 switch(p->color){
4980 case 0:
4981 red=100;
4982 green=100;
4983 blue=100;
4984 break;
4985 case 1:
4986 red=75;
4987 green=75;
4988 blue=75;
4989 break;
4990 case 2:
4991 red=100;
4992 green=75;
4993 blue=75;
4994 break;
4995 case 3:
4996 red=100;
4997 green=100;
4998 blue=75;
4999 break;
5000 case 4:
5001 red=75;
5002 green=100;
5003 blue=75;
5004 break;
5005 case 5:
5006 red=75;
5007 green=100;
5008 blue=100;
5009 break;
5010 case 6:
5011 red=75;
5012 green=75;
5013 blue=100;
5014 break;
5015 case 7:
5016 red=100;
5017 green=75;
5018 blue=100;
5019 break;
5020 }
5021 // SetColor(red,green,blue);
5022 // OpenPoly();
5023// PenSize(1,1);
5024 k=cont.start; // start of contour
5025 l=cont.pte-cont.start; // number of contour segments
5026 q=k;
5027 ix=(int)(seg[q].sx+Wid);
5028 iy=(int)(Hei-seg[q].sy);
5029 // MoveTo(ix,iy);
5030 for(n=0;n<l;n++) finished[n]=false;
5031 for(m=0;m<l;m++){
5032 ix=(int)(seg[q].ex+Wid);
5033 iy=(int)(Hei-seg[q].ey);
5034 // LineTo(ix,iy);
5035 for(n=0;n<l;n++){
5036 if(!finished[n]){
5037 o=k+n;
5038 if(seg[q].ex==seg[o].sx && seg[q].ey==seg[o].sy){
5039 q=o;
5040 finished[n]=true;
5041 break;
5042 }
5043 }
5044 }
5045 }
5046 // ClosePoly();
5047 }
5048// PenSize(ContPen,ContPen);
5049// SetColor(0,0,0);
5050 if(isLines){
5051 switch(p->color){
5052 case 1:
5053// PenSize(2,2);
5054 break;
5055 case 2:
5056// PenSize(4,4);
5057 break;
5058 case 3:
5059// PenSize(8,8);
5060 break;
5061 case 4:
5062 red=100;
5063 green=0;
5064 blue=0;
5065 // SetColor(red,green,blue);
5066// PenSize(4,4);
5067 break;
5068 case 5:
5069 red=100;
5070 green=0;
5071 blue=0;
5072 // SetColor(red,green,blue);
5073// PenSize(8,8);
5074 break;
5075 case 6:
5076 red=0;
5077 green=0;
5078 blue=100;
5079 // SetColor(red,green,blue);
5080// PenSize(4,4);
5081 break;
5082 case 7:
5083 red=0;
5084 green=0;
5085 blue=100;
5086 // SetColor(red,green,blue);
5087// PenSize(8,8);
5088 break;
5089 }
5090 }
5091 k=blk.start;
5092 l=blk.pte;
5093 for(m=k;m<l;m++){
5094 ix=(int)(seg[m].sx+Wid);
5095 iy=(int)(Hei-seg[m].sy);
5096 // MoveTo(ix,iy);
5097 ix=(int)(seg[m].ex+Wid);
5098 iy=(int)(Hei-seg[m].ey);
5099 // LineTo(ix,iy);
5100 }
5101 if(isLines){
5102// PenSize(1,1);
5103 red=0;
5104 green=0;
5105 blue=0;
5106 // SetColor(red,green,blue);
5107 }
5108 }
5109#endif
5110 delete[] seg; // DisposPtr((Ptr)seg);
5111 delete[] partpart1; // DisposPtr((Ptr)partpart1);
5112 delete[] sorted; // DisposPtr((Ptr)sorted);
5113
5114
5115}
5116
5117
5118/********************************************************************
5119 ClipSurface
5120
5121 clip a surface by the view frame
5122********************************************************************/
5123void CThreeD::ClipSurface(Surface *surf1,Point2d *surfPt1,
5124 Surface *surf2,Point2d *surfPt2)
5125{
5126 TransAngleMatrix s; // body:surface, frame:thePolar
5127 Point2d p0,p1,p2,p3,p4;
5128 int nx; // number of crossing points
5129 Point2d p[8]; // crossing point
5130 int ix[8]; // start point index
5131 int jx[8]; // status 1 : coming-in, 2 : going-out
5132 int kx[8]; // clip-side 1:Rt, 2:Tp, 3:Lt, 4:Bm
5133 double ang[8]; // angle from the corner
5134 int status; // returned status
5135 double an; // returned angle
5136 bool inside;
5137 int i,j,k,l;
5138
5139 SurfthePolarTAM((TransAngleEuler *)surf1,&thePolar,&s); // Perspective
5140 SurfToPolar(&surfPt1[0],&s,&p0); // (x,y) to (alpha,delta)
5141 p2=p0;
5142 nx=0;
5143// outstr("in ClipSurface()\n");
5144// sprintf(msg," surf1->start=%i surf1->pte=%i \n",surf1->start,surf1->pte);
5145// outstr(msg);
5146// flush();
5147
5148 for(i=surf1->start;i<surf1->pte;i++){
5149 p1=p2;
5150 if(i+1==surf1->pte){
5151 if(surf1->pte-surf1->start==2) break;
5152 else p2=p0;
5153 }
5154 else SurfToPolar(&surfPt1[i+1],&s,&p2);
5155
5156 status=CrossClipSide(&frameRt,angFrameRt,&p1,&p2,&p3,&an); // Perspective
5157
5158 if(status<3){ // crossing or inside
5159 if(status>0){ // crossing
5160 p[nx]=p3; // crossing point in polar coordinate
5161 ix[nx]=i; // start point index
5162 jx[nx]=status; // status 1 or 2
5163 kx[nx]=1; // clip rect side - right
5164 ang[nx]=an; // distance from right-bottom corner
5165 nx++;
5166 }
5167 status=CrossClipSide(&frameTp,angFrameTp,&p1,&p2,&p3,&an);
5168
5169 }
5170 if(status<3){
5171 if(status>0){
5172 p[nx]=p3;
5173 ix[nx]=i;
5174 jx[nx]=status;
5175 kx[nx]=2;
5176 ang[nx]=an;
5177 nx++;
5178 }
5179 status=CrossClipSide(&frameLt,angFrameLt,&p1,&p2,&p3,&an);
5180
5181 }
5182 if(status<3){
5183 if(status>0){
5184 p[nx]=p3;
5185 ix[nx]=i;
5186 jx[nx]=status;
5187 kx[nx]=3;
5188 ang[nx]=an;
5189 nx++;
5190 }
5191 status=CrossClipSide(&frameBm,angFrameBm,&p1,&p2,&p3,&an);
5192
5193 }
5194 if(status<3){
5195 if(status>0){
5196 p[nx]=p3;
5197 ix[nx]=i;
5198 jx[nx]=status;
5199 kx[nx]=4;
5200 ang[nx]=an;
5201 nx++;
5202 }
5203 }
5204
5205
5206 }
5207// outstr("in ClipSurface()\n");
5208// sprintf(msg," nx=%i status=%i surf1->start=%i surf1->pte=%i \n",nx,status,surf1->start,surf1->pte);
5209// outstr(msg);
5210// flush();
5211
5212
5213 if(nx==0){
5214 if(status==0){ // the whole surface is inside the clip rect
5215 *surf2=*surf1;
5216 for(i=surf1->start;i<surf1->pte;i++) surfPt2[i]=surfPt1[i];
5217 return;
5218 }
5219 else{
5220 *surf2=*surf1;
5221 surf2->pte=surf2->start; // set past-the-end to start
5222 return;
5223 }
5224 }
5225// if two crossings occur for a segment, 'enter' comes first and then comes 'leave'
5226 for(i=1;i<nx;i++){
5227 if(ix[i]==ix[i-1]){
5228 if(jx[i]==1){ // enter
5229 p3=p[i];
5230 p[i]=p[i-1];
5231 p[i-1]=p3;
5232 j=jx[i];
5233 jx[i]=jx[i-1];
5234 jx[i-1]=j;
5235 j=kx[i];
5236 kx[i]=kx[i-1];
5237 kx[i-1]=j;
5238 an=ang[i];
5239 ang[i]=ang[i-1];
5240 ang[i-1]=an;
5241 }
5242 }
5243 }
5244
5245// add corner points after 'leave' and before 'enter'
5246 for(i=0;i<nx;i++){
5247 if(jx[i]==2){ // leave
5248 j=i+1;
5249 if(j==nx) j=0; // next enter point
5250 k=kx[i]; // side of leave point
5251 while(k!=kx[j]){
5252 k++;
5253 if(k==5) k=1;
5254 for(l=nx;l>i+1;l--){ // make room for a corner point
5255 p[l]=p[l-1];
5256 ix[l]=ix[l-1];
5257 jx[l]=jx[l-1];
5258 kx[l]=kx[l-1];
5259 ang[l]=ang[l-1];
5260 }
5261 switch(k){
5262 case 1:
5263 p[i+1]=RtBm;
5264 break;
5265 case 2:
5266 p[i+1]=TpRt;
5267 break;
5268 case 3:
5269 p[i+1]=LtTp;
5270 break;
5271 case 4:
5272 p[i+1]=BmLt;
5273 break;
5274 }
5275 ix[i+1]=ix[i];
5276 jx[i+1]=4; // status is corner point
5277 kx[i+1]=k;
5278 ang[i+1]=0.;
5279 nx++;
5280 i++;
5281 }
5282 }
5283 }
5284
5285// convert polar coordinate to surface coordinate
5286 for(i=0;i<nx;i++) PolarToSurf(&p[i],&s,&p[i]);
5287
5288// generate new surface
5289 j=0; // index to crossing points
5290 k=0; // index to output points
5291 for(i=surf1->start;i<surf1->pte;i++){
5292 inside=false;
5293 for(j=0;j<nx;j++){
5294 if(i<=ix[j]&&jx[j]==2) {
5295 inside=true;
5296 break;
5297 }
5298 if(i<=ix[j]&&jx[j]==1) {
5299 inside=false;
5300 break;
5301 }
5302 }
5303 if(j==nx&&jx[0]==2) inside=true;
5304 if(inside){
5305 surfPt2[k]=surfPt1[i];
5306 k++;
5307 }
5308 for(j=0;j<nx;j++){
5309 if(i==ix[j]){
5310 surfPt2[k]=p[j];
5311 k++;
5312 }
5313 }
5314 }
5315 *surf2=*surf1;
5316 surf2->start=0;
5317 surf2->pte=k;
5318
5319}
5320
5321/********************************************************************
5322 ToDB
5323
5324 generate stream data
5325********************************************************************/
5326char *CThreeD::ToDB(int *len)
5327{
5328 int i,j,k,l,m,n;
5329 CMPpart *p;
5330 Point2d *p2;
5331 Point3d *p3;
5332 char *e;
5333
5334 n=0;
5335 n+=4; /* npart */
5336 for(i=0;i<npart;i++){
5337 n+=4; /* i */
5338 p=part[i];
5339 n+=4; /* kind */
5340 j=strlen(p->desc.c_str());
5341 n+=4; /* j */
5342 n+=j+1; /* strcpy */
5343 n+=4; /* refPart */
5344 n+=8*6; /* x0,y0,z0,theta0,phi0,psi0 */
5345 n+=4; /* color */
5346 n+=4; /* showDrawing */
5347 switch(p->kind){
5348 case MPpoint:
5349 break;
5350 case MPlines:
5351 k=reinterpret_cast<CMPlines *>(p)->npoint;
5352 n+=4; /* k */
5353 n+=k*24; /* x,y,z */
5354 n+=4; /* line */
5355 break;
5356 case MPplane:
5357 k=reinterpret_cast<CMPplane *>(p)->npoint;
5358 n+=4; /* k */
5359 n+=k*16; /* x,y */
5360 n+=4; /* isPolyhed */
5361 n+=4; /* isGroup */
5362 break;
5363 case MPbox:
5364 n+=24; /* lx,ly,lz */
5365 n+=4; /* visibleSurface */
5366 break;
5367 case MPsphere:
5368 n+=8; /* r */
5369 break;
5370 case MPbodyOfRot:
5371 n+=4; /* npoint */
5372 n+=32; /* p0.x,p0.y,p1.x,p1.y */
5373 n+=16; /* isTube,oblong,appear,visibleSurfac */
5374 break;
5375 }
5376 }
5377 *len=n;
5378 e=(char *)malloc(sizeof(char)*n);
5379 n=0;
5380 *(int *)(e+n)=npart;
5381 n+=4;
5382 for(i=0;i<npart;i++){
5383 *(int *)(e+n)=i;
5384 n+=4;
5385 p=part[i];
5386 *(int *)(e+n)=p->kind;
5387 n+=4;
5388 j=strlen(p->desc.c_str());
5389 *(int *)(e+n)=j;
5390 n+=4;
5391 strcpy(e+n,p->desc.c_str());
5392 n+=j+1;
5393 *(int *)(e+n)=p->refPart;
5394 n+=4;
5395 *(double *)(e+n)=p->x0;
5396 n+=8;
5397 *(double *)(e+n)=p->y0;
5398 n+=8;
5399 *(double *)(e+n)=p->z0;
5400 n+=8;
5401 *(double *)(e+n)=p->theta0;
5402 n+=8;
5403 *(double *)(e+n)=p->phi0;
5404 n+=8;
5405 *(double *)(e+n)=p->psi0;
5406 n+=8;
5407 *(int *)(e+n)=p->color;
5408 n+=4;
5409 *(int *)(e+n)=p->showDrawing;
5410 n+=4;
5411 switch(p->kind){
5412 case MPpoint:
5413 break;
5414 case MPlines:
5415 k=reinterpret_cast<CMPlines *>(p)->npoint;
5416 *(int *)(e+n)=k;
5417 n+=4;
5418 for(j=0;j<k;j++){
5419 p3=&reinterpret_cast<CMPlines *>(p)->pt[j];
5420 *(double *)(e+n)=p3->x;
5421 n+=8;
5422 *(double *)(e+n)=p3->y;
5423 n+=8;
5424 *(double *)(e+n)=p3->z;
5425 n+=8;
5426 }
5427 *(int *)(e+n)=reinterpret_cast<CMPlines *>(p)->line;
5428 n+=4;
5429 break;
5430 case MPplane:
5431 k=reinterpret_cast<CMPplane *>(p)->npoint;
5432 *(int *)(e+n)=k;
5433 n+=4;
5434 for(j=0;j<k;j++){
5435 p2=&reinterpret_cast<CMPplane *>(p)->pt[j];
5436 *(double *)(e+n)=p2->x;
5437 n+=8;
5438 *(double *)(e+n)=p2->y;
5439 n+=8;
5440 }
5441 *(int *)(e+n)=reinterpret_cast<CMPplane *>(p)->isPolyhed;
5442 n+=4;
5443 *(int *)(e+n)=reinterpret_cast<CMPplane *>(p)->isGroup;
5444 n+=4;
5445 break;
5446 case MPbox:
5447 *(double *)(e+n)=reinterpret_cast<CMPbox *>(p)->lx;
5448 n+=8;
5449 *(double *)(e+n)=reinterpret_cast<CMPbox *>(p)->ly;
5450 n+=8;
5451 *(double *)(e+n)=reinterpret_cast<CMPbox *>(p)->lz;
5452 n+=8;
5453 *(int *)(e+n)=reinterpret_cast<CMPbox *>(p)->visibleSurface;
5454 n+=4;
5455 break;
5456 case MPsphere:
5457 *(double *)(e+n)=reinterpret_cast<CMPsphere *>(p)->r;
5458 n+=8;
5459 break;
5460 case MPbodyOfRot:
5461 *(int *)(e+n)=reinterpret_cast<CMPbodyOfRot *>(p)->npoint;
5462 n+=4;
5463 *(double *)(e+n)=reinterpret_cast<CMPbodyOfRot *>(p)->p0.x;
5464 n+=8;
5465 *(double *)(e+n)=reinterpret_cast<CMPbodyOfRot *>(p)->p0.y;
5466 n+=8;
5467 *(double *)(e+n)=reinterpret_cast<CMPbodyOfRot *>(p)->p1.x;
5468 n+=8;
5469 *(double *)(e+n)=reinterpret_cast<CMPbodyOfRot *>(p)->p1.y;
5470 n+=8;
5471 *(int *)(e+n)=reinterpret_cast<CMPbodyOfRot *>(p)->isTube;
5472 n+=4;
5473 *(int *)(e+n)=reinterpret_cast<CMPbodyOfRot *>(p)->oblong;
5474 n+=4;
5475 *(int *)(e+n)=reinterpret_cast<CMPbodyOfRot *>(p)->appear;
5476 n+=4;
5477 *(int *)(e+n)=reinterpret_cast<CMPbodyOfRot *>(p)->visibleSurface;
5478 n+=4;
5479 break;
5480 }
5481 }
5482 return e;
5483}
5484
5485/********************************************************************
5486 FromDB
5487
5488 load stream data
5489********************************************************************/
5490void CThreeD::FromDB(int len,char *e,double loc[6])
5491{
5492 int i,j,k,l,m,n,i1,i2;
5493 char s[200];
5494 CMPpart *p;
5495 Point2d *p2;
5496 Point3d *p3;
5497
5498 n=0;
5499 l=npart;
5500 m=*(int *)e;
5501 n+=4;
5502 for(i=0;i<m;i++){
5503 /* *(int *)(e+n)=i */
5504 n+=4;
5505 i1=*(int *)(e+n); // kind
5506 n+=4;
5507 i2=*(int *)(e+n); // strlen
5508 n+=4;
5509 strcpy(s,e+n);
5510 n+=i2+1;
5511 i2=*(int *)(e+n)+l; // refPart
5512 if(i==0) i2=0;
5513 n+=4;
5514 switch(i1){
5515 case MPpoint:
5516 part.push_back(new CMPpoint(s,i2,i1));
5517 break;
5518 case MPlines:
5519 part.push_back(new CMPlines(s,i2,i1));
5520 break;
5521 case MPplane:
5522 part.push_back(new CMPplane(s,i2,i1));
5523 break;
5524 case MPbox:
5525 part.push_back(new CMPbox(s,i2,i1));
5526 break;
5527 case MPsphere:
5528 part.push_back(new CMPsphere(s,i2,i1));
5529 break;
5530 case MPbodyOfRot:
5531 part.push_back(new CMPbodyOfRot(s,i2,i1));
5532 break;
5533 default:
5534 break;
5535 }
5536 npart++;
5537 p=part[npart-1];
5538 if(i==0){
5539 p->x0=loc[0];
5540 p->y0=loc[1];
5541 p->z0=loc[2];
5542 p->theta0=loc[3];
5543 p->phi0=loc[4];
5544 p->psi0=loc[5];
5545 n+=48;
5546 }
5547 else{
5548 p->x0=*(double *)(e+n);
5549 n+=8;
5550 p->y0=*(double *)(e+n);
5551 n+=8;
5552 p->z0=*(double *)(e+n);
5553 n+=8;
5554 p->theta0=*(double *)(e+n);
5555 n+=8;
5556 p->phi0=*(double *)(e+n);
5557 n+=8;
5558 p->psi0=*(double *)(e+n);
5559 n+=8;
5560 }
5561 p->color=*(int *)(e+n);
5562 n+=4;
5563 p->showDrawing=*(int *)(e+n);
5564 n+=4;
5565 switch(p->kind){
5566 case MPpoint:
5567 break;
5568 case MPlines:
5569 k=*(int *)(e+n);
5570 n+=4;
5571 for(j=0;j<k;j++){
5572 reinterpret_cast<CMPlines *>(p)->SetPoint(*(double *)(e+n),*(double *)(e+n+8),*(double *)(e+n+16));
5573 n+=24;
5574 }
5575 reinterpret_cast<CMPlines *>(p)->line=*(int *)(e+n);
5576 n+=4;
5577 break;
5578 case MPplane:
5579 k=*(int *)(e+n);
5580 n+=4;
5581 for(j=0;j<k;j++){
5582 reinterpret_cast<CMPplane *>(p)->SetPoint(*(double *)(e+n),*(double *)(e+n+8));
5583 n+=16;
5584 }
5585 reinterpret_cast<CMPplane *>(p)->isPolyhed=*(int *)(e+n);
5586 n+=4;
5587 reinterpret_cast<CMPplane *>(p)->isGroup=*(int *)(e+n);
5588 n+=4;
5589 break;
5590 case MPbox:
5591 reinterpret_cast<CMPbox *>(p)->SetSize(*(double *)(e+n),*(double *)(e+n+8),*(double *)(e+n+16));
5592 n+=24;
5593 reinterpret_cast<CMPbox *>(p)->visibleSurface=*(int *)(e+n);
5594 n+=4;
5595 break;
5596 case MPsphere:
5597 reinterpret_cast<CMPsphere *>(p)->r=*(double *)(e+n);
5598 n+=8;
5599 break;
5600 case MPbodyOfRot:
5601 reinterpret_cast<CMPbodyOfRot *>(p)->npoint=*(int *)(e+n);
5602 n+=4;
5603 reinterpret_cast<CMPbodyOfRot *>(p)->p0.x=*(double *)(e+n);
5604 n+=8;
5605 reinterpret_cast<CMPbodyOfRot *>(p)->p0.y=*(double *)(e+n);
5606 n+=8;
5607 reinterpret_cast<CMPbodyOfRot *>(p)->p1.x=*(double *)(e+n);
5608 n+=8;
5609 reinterpret_cast<CMPbodyOfRot *>(p)->p1.y=*(double *)(e+n);
5610 n+=8;
5611 reinterpret_cast<CMPbodyOfRot *>(p)->isTube=*(int *)(e+n);
5612 n+=4;
5613 reinterpret_cast<CMPbodyOfRot *>(p)->oblong=*(int *)(e+n);
5614 n+=4;
5615 reinterpret_cast<CMPbodyOfRot *>(p)->appear=*(int *)(e+n);
5616 n+=4;
5617 reinterpret_cast<CMPbodyOfRot *>(p)->visibleSurface=*(int *)(e+n);
5618 n+=4;
5619 break;
5620 }
5621 }
5622}
5623
5624/******************************************************************************
5625 CFarNear Far-Near relationship of 3D Drawing Objects
5626 ******************************************************************************/
5627
5628
5629CFarNear::CFarNear(int numpart1)
5630: npart1(numpart1)
5631{
5632 int i;
5633
5634 nmax=npart1*(npart1-1)/2;
5635 farNear=new StartPTE[nmax+1]; // (StartPTE *)NewPtr(sizeof(StartPTE)*nmax);
5636 farNearIndex=new long[npart1+1]; // (long *)NewPtr(sizeof(long)*(npart1+1));
5637 for(i=0;i<=npart1;i++) farNearIndex[i]=0L;
5638 n=0;
5639}
5640
5641/******************************************************************************
5642 Dispose
5643
5644 ******************************************************************************/
5645
5646CFarNear::~CFarNear()
5647{
5648 delete[] farNear; // DisposPtr((Ptr)farNear);
5649 delete[] farNearIndex; // DisposPtr((Ptr)farNearIndex);
5650}
5651
5652/******************************************************************************
5653 AddFarNear
5654
5655 ******************************************************************************/
5656
5657void CFarNear::AddFarNear(int far,int near)
5658{
5659 StartPTE a;
5660 long m;
5661 int i;
5662
5663 a.start=far;
5664 a.pte=near;
5665 m=farNearIndex[a.start+1];
5666 for(i=a.start+1;i<=npart1;i++) farNearIndex[i]++;
5667// BlockMove((Ptr)&farNear[m],(Ptr)&farNear[m+1],(n-m)*sizeof(StartPTE));
5668 for(i=n;i>=m;i--) farNear[i+1]=farNear[i];
5669 farNear[m]=a;
5670 n++;
5671// outstr("in AddFarNear()\n");
5672// for(i=0;i<n;i++){
5673// sprintf(msg," i=%i farNear[i].start=%i farNear[i].pte=%i \n",i,farNear[i].start,farNear[i].pte);
5674// outstr(msg);
5675// }
5676// flush();
5677
5678}
5679
5680/******************************************************************************
5681 inferFarNear
5682
5683 ******************************************************************************/
5684
5685bool CFarNear::InferFarNear(int p1,int p2,int *near)
5686{
5687// test if p1 is farther
5688 *near=p2;
5689 if(RecurseFarNear(p1,p2,0)) return true;
5690// test if p2 is farther
5691 *near=p1;
5692 if(RecurseFarNear(p2,p1,0)) return true;
5693 return false;
5694}
5695
5696/******************************************************************************
5697 recurseFarNear
5698
5699 ******************************************************************************/
5700
5701bool CFarNear::RecurseFarNear(int p1,int p2,long depth)
5702{
5703 long i,j,k;
5704
5705 depth++;
5706 if(depth>n) return false; // escape loop
5707
5708 j=farNearIndex[p1];
5709 k=farNearIndex[p1+1];
5710 for(i=j;i<k;i++){
5711 if(farNear[i].pte==p2) return true;
5712 if(RecurseFarNear(farNear[i].pte,p2,depth)) return true;
5713 }
5714 return false;
5715}
5716
5717/******************************************************************************
5718 SetSorted
5719
5720 ******************************************************************************/
5721
5722int CFarNear::SetSorted(int *sorted)
5723{
5724 int i,j,l;
5725 long k,m;
5726 int *status;
5727 FILE *fp;
5728
5729// outstr("in SetSorted()\n");
5730// sprintf(msg," npart1=%i n=%i \n",npart1,n);
5731// outstr(msg);
5732// flush();
5733// for(i=0;i<n;i++){
5734// sprintf(msg," farNear[i].start=%5i farNear[i].pte=%5i\n",farNear[i].start,farNear[i].pte);
5735// outstr(msg);
5736// }
5737// flush();
5738 fp=fopen("/tmp/gcs3DDATA","w");
5739 fprintf(fp,"in SetSorted()\n");
5740 fprintf(fp," npart1=%i n=%i \n",npart1,n);
5741 for(i=0;i<n;i++){
5742 fprintf(fp," farNear[i].start=%5i farNear[i].pte=%5i\n",farNear[i].start,farNear[i].pte);
5743 }
5744 fclose(fp);
5745
5746 status=new int[npart1]; // (int *)NewPtr(sizeof(short)*npart1);
5747 for(i=0;i<npart1;i++) status[i]=0;
5748 j=0;
5749 i=0;
5750 do{
5751 for(k=0;k<n;k++){
5752 l=farNear[k].start;
5753 if(l<10000){
5754 switch (status[l]){
5755 case 0:
5756 for(m=0;m<n;m++) if(l==farNear[m].pte) break;
5757 if(m==n){
5758 sorted[j++]=l;
5759 status[l]=2;
5760 farNear[k].start=10000;
5761 farNear[k].pte=10000;
5762 }
5763 else status[l]=1;
5764 break;
5765 case 1:
5766 break;
5767 case 2:
5768 case 3:
5769 farNear[k].start=10000;
5770 farNear[k].pte=10000;
5771 break;
5772 default:
5773 break;
5774 }
5775 }
5776 }
5777 m=0;
5778 for(k=0;k<npart1;k++){
5779 l=status[k];
5780 if(l==1){
5781 m++;
5782 status[k]=0;
5783 }
5784 if(l==2){
5785 if(m<10000) m+=10000;
5786 status[k]=3;
5787 }
5788 }
5789 if(m<10000) i++;
5790 } while(i<2);
5791 fp=fopen("/tmp/gcs3DDATA","w");
5792 fprintf(fp,"at the end of SetSorted()\n");
5793 fprintf(fp," npart1=%i n=%i j=%i \n",npart1,n,j);
5794 for(i=0;i<n;i++){
5795 fprintf(fp," farNear[i].start=%5i farNear[i].pte=%5i\n",farNear[i].start,farNear[i].pte);
5796 }
5797 for(i=0;i<npart1;i++) fprintf(fp," status[i]=%i sorted[i]=%i\n",status[i],sorted[i]);
5798 fclose(fp);
5799 for(k=0;k<npart1;k++) if(status[k]!=3) sorted[j++]=k;
5800 delete[] status; // DisposPtr((Ptr)status);
5801 fp=fopen("/tmp/gcs3DDATA","w");
5802 fprintf(fp,"before retun of SetSorted()\n");
5803 fprintf(fp," npart1=%i n=%i j=%i \n",npart1,n,j);
5804 for(i=0;i<n;i++){
5805 fprintf(fp," farNear[i].start=%5i farNear[i].pte=%5i\n",farNear[i].start,farNear[i].pte);
5806 }
5807 for(i=0;i<npart1;i++) fprintf(fp," status[i]=%i sorted[i]=%i\n",status[i],sorted[i]);
5808 fclose(fp);
5809 return m;
5810}
5811
5812int CMPdeco::GetDrawData(TransAngleMatrix *po,Line2d *seg,StartPTE *blk,StartPTE *cont)
5813{
5814 TransAngleEuler tae;
5815 TransAngleMatrix tam;
5816 Point3d a,b,c,d;
5817 Point2d e,f,g;
5818 double r;
5819 int i,j,k,l;
5820 AngleEuler ae;
5821 AngleMatrix amb; /* body */
5822 Line3d eye;
5823 double d0,ra,d1,r1;
5824 AngleMatrix ame; /* eye */
5825 AngleMatrix amc; /* combined */
5826 Point2d *p1;
5827 int np1,nl1;
5828
5829 if(p->kind==MPplane){
5830 k=0;
5831 tae.x=p->xg; // Translate Angle Euler for the part
5832 tae.y=p->yg;
5833 tae.z=p->zg;
5834 tae.theta=p->thetag;
5835 tae.phi=p->phig;
5836 tae.psi=p->psig;
5837 TAEtoTAM(&tae,&tam);
5838 if(ncontour!=0){
5839 a.x=contour[0].x;
5840 a.y=contour[0].y;
5841 a.z=0.;
5842 BodyToFrameTAM(&a,&tam,&b);
5843 FrameToBodyTAM(&b,po,&c);
5844 NormPoint3d(&c,&d,&r);
5845 ToPolar3d(&d,&e);
5846 e.y=1.5707963267-e.y;
5847 f=e;
5848 for(i=1;i<=ncontour;i++){
5849 g=f;
5850 if(i<ncontour){
5851 a.x=contour[i].x;
5852 a.y=contour[i].y;
5853 a.z=0.;
5854 BodyToFrameTAM(&a,&tam,&b);
5855 FrameToBodyTAM(&b,po,&c);
5856 NormPoint3d(&c,&d,&r);
5857 ToPolar3d(&d,&f);
5858 f.y=1.5707963267-f.y;
5859 }
5860 else f=e;
5861 seg[k].sx=g.x;
5862 seg[k].sy=g.y;
5863 seg[k].ex=f.x;
5864 seg[k].ey=f.y;
5865 k++;
5866 }
5867 }
5868 cont->start=0;
5869 cont->pte=k;
5870 blk->start=k;
5871 if(nlineseg!=0){
5872 for(i=0;i<nlineseg;i++){
5873 a.x=lineseg[i].sx;
5874 a.y=lineseg[i].sy;
5875 a.z=0.;
5876 BodyToFrameTAM(&a,&tam,&b);
5877 FrameToBodyTAM(&b,po,&c);
5878 NormPoint3d(&c,&d,&r);
5879 ToPolar3d(&d,&e);
5880 e.y=1.5707963267-e.y;
5881 a.x=lineseg[i].ex;
5882 a.y=lineseg[i].ey;
5883 a.z=0.;
5884 BodyToFrameTAM(&a,&tam,&b);
5885 FrameToBodyTAM(&b,po,&c);
5886 NormPoint3d(&c,&d,&r);
5887 ToPolar3d(&d,&f);
5888 f.y=1.5707963267-f.y;
5889 seg[k].sx=e.x;
5890 seg[k].sy=e.y;
5891 seg[k].ex=f.x;
5892 seg[k].ey=f.y;
5893 k++;
5894 }
5895 }
5896 blk->pte=k;
5897 }
5898 else{ /* MPsphere */
5899 ae.theta=p->thetag;
5900 ae.phi=p->phig;
5901 ae.psi=p->psig;
5902 AEtoAM(&ae,&amb);
5903 eye.sx=p->xg; // center of the sphere
5904 eye.sy=p->yg;
5905 eye.sz=p->zg;
5906 eye.ex=po->x; // thePolar of CThreeD - position of the viewer's eye
5907 eye.ey=po->y;
5908 eye.ez=po->z;
5909 NormLine3d(&eye,&a,&d0);
5910 ToPolar3d(&a,&e);
5911 ae.theta=1.5707963268-e.y; // z axis toward viewer's eye
5912 ae.phi=e.x+1.5707963268;
5913 ae.psi=0.;
5914 tae.theta=ae.theta;
5915 tae.phi=ae.phi;
5916 tae.psi=ae.psi;
5917 AEtoAM(&ae,&ame);
5918 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];
5919 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];
5920 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];
5921 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];
5922 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];
5923 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];
5924 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];
5925 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];
5926 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];
5927 p1=(Point2d *)malloc(sizeof(Point2d)*(ncontour+90));
5928 for(i=0;i<ncontour;i++){
5929 f.x=contour[i].x*0.017453293;
5930 f.y=contour[i].y*0.017453293;
5931 BodyToFrameAMP(&f,&amc,&g);
5932 contour[i].x=g.x;
5933 contour[i].y=g.y;
5934 }
5935 r=reinterpret_cast<CMPsphere *>(p)->r;
5936 ra=asin(r/d0);
5937 j=0;
5938 l=0; // if the first point is inbound
5939 for(i=0;i<ncontour;i++){
5940 if(i==0) k=ncontour-1;
5941 else k=i-1;
5942 if((contour[i].y-ra)*(contour[k].y-ra)<0.){
5943 g.x=contour[k].x;
5944 e.x=contour[i].x;
5945 if(g.x>e.x+3.2) e.x=e.x+6.283185307;
5946 else if(e.x>g.x+3.2) g.x=g.x+6.283185307;
5947 f.x=g.x+(e.x-g.x)*(ra-contour[k].y)/(contour[i].y-contour[k].y);
5948 f.y=ra;
5949 if(contour[i].y>ra){ // inbound
5950 if(j!=0){
5951 if(p1[j-1].x>f.x) r1=p1[j-1].x-6.283185307;
5952 else r1=p1[j-1].x;
5953 for(d1=r1+0.06;d1<f.x;d1=d1+0.06){
5954 p1[j].x=d1;
5955 p1[j].y=ra;
5956 j++;
5957 }
5958 }
5959 else l=1;
5960 p1[j].x=f.x;
5961 p1[j].y=f.y;
5962 j++;
5963 p1[j].x=contour[i].x;
5964 p1[j].y=contour[i].y;
5965 j++;
5966 }
5967 else{ // outbound
5968 p1[j].x=f.x;
5969 p1[j].y=f.y;
5970 j++;
5971 }
5972 }
5973 else if(contour[i].y>ra){
5974 p1[j].x=contour[i].x;
5975 p1[j].y=contour[i].y;
5976 j++;
5977 }
5978 }
5979 if(l==1){
5980 if(p1[j-1].x>p1[0].x) r1=p1[j-1].x-6.283185307;
5981 else r1=p1[j-1].x;
5982 for(d1=r1+0.06;d1<p1[0].x;d1=d1+0.06){
5983 p1[j].x=d1;
5984 p1[j].y=ra;
5985 j++;
5986 }
5987 /*
5988 p1[j].x=p1[0].x;
5989 p1[j].y=p1[0].y;
5990 j++;*/
5991 }
5992 np1=j;
5993 k=0;
5994 tae.x=p->xg; // Translate Angle Euler for the part
5995 tae.y=p->yg;
5996 tae.z=p->zg;
5997 TAEtoTAM(&tae,&tam);
5998 if(np1!=0){
5999 a.z=r*cos(p1[0].y);
6000 a.x=a.z*cos(p1[0].x);
6001 a.y=a.z*sin(p1[0].x);
6002 a.z=r*sin(p1[0].y);
6003 BodyToFrameTAM(&a,&tam,&b);
6004 FrameToBodyTAM(&b,po,&c);
6005 NormPoint3d(&c,&d,&r1);
6006 ToPolar3d(&d,&e);
6007 e.y=1.5707963267-e.y;
6008 f=e;
6009 for(i=1;i<=np1;i++){
6010 g=f;
6011 if(i<np1){
6012 a.z=r*cos(p1[i].y);
6013 a.x=a.z*cos(p1[i].x);
6014 a.y=a.z*sin(p1[i].x);
6015 a.z=r*sin(p1[i].y);
6016 BodyToFrameTAM(&a,&tam,&b);
6017 FrameToBodyTAM(&b,po,&c);
6018 NormPoint3d(&c,&d,&r1);
6019 ToPolar3d(&d,&f);
6020 f.y=1.5707963267-f.y;
6021 }
6022 else f=e;
6023 seg[k].sx=g.x;
6024 seg[k].sy=g.y;
6025 seg[k].ex=f.x;
6026 seg[k].ey=f.y;
6027 k++;
6028 }
6029 }
6030 cont->start=0;
6031 cont->pte=k;
6032 free(p1);
6033 blk->start=k;
6034 j=0;
6035 for(i=0;i<nlineseg;i++){
6036 f.x=lineseg[i].sx*0.017453293;
6037 f.y=lineseg[i].sy*0.017453293;
6038 BodyToFrameAMP(&f,&amc,&e);
6039 f.x=lineseg[i].ex*0.017453293;
6040 f.y=lineseg[i].ey*0.017453293;
6041 BodyToFrameAMP(&f,&amc,&g);
6042 if(e.y<ra&&g.y<ra);
6043 else{
6044 if(e.y<ra){
6045 e.x=e.x+(g.x-e.x)*(ra-e.y)/(g.y-e.y);
6046 e.y=ra;
6047 }
6048 else if(g.y<ra){
6049 g.x=g.x+(e.x-g.x)*(ra-g.y)/(e.y-g.y);
6050 g.y=ra;
6051 }
6052 lineseg[j].sx=e.x;
6053 lineseg[j].sy=e.y;
6054 lineseg[j].ex=g.x;
6055 lineseg[j].ey=g.y;
6056 j++;
6057 }
6058 }
6059 nl1=j;
6060 for(i=0;i<nl1;i++){
6061 a.z=r*cos(lineseg[i].sy);
6062 a.x=a.z*cos(lineseg[i].sx);
6063 a.y=a.z*sin(lineseg[i].sx);
6064 a.z=r*sin(lineseg[i].sy);
6065 BodyToFrameTAM(&a,&tam,&b);
6066 FrameToBodyTAM(&b,po,&c);
6067 NormPoint3d(&c,&d,&r1);
6068 ToPolar3d(&d,&e);
6069 e.y=1.5707963267-e.y;
6070 a.z=r*cos(lineseg[i].ey);
6071 a.x=a.z*cos(lineseg[i].ex);
6072 a.y=a.z*sin(lineseg[i].ex);
6073 a.z=r*sin(lineseg[i].ey);
6074 BodyToFrameTAM(&a,&tam,&b);
6075 FrameToBodyTAM(&b,po,&c);
6076 NormPoint3d(&c,&d,&r1);
6077 ToPolar3d(&d,&f);
6078 f.y=1.5707963267-f.y;
6079 seg[k].sx=e.x;
6080 seg[k].sy=e.y;
6081 seg[k].ex=f.x;
6082 seg[k].ey=f.y;
6083 k++;
6084 }
6085 blk->pte=k;
6086 }
6087}
6088
6089
6090/**************************************************
6091
6092 Initiate adding a rigid body element
6093
6094 should be followed by following calls as required
6095 setlocal
6096 setweight
6097 setdensity
6098 setnpoint
6099 set1Dpoint
6100 set2Dpoint
6101 set3Dpoint
6102
6103***************************************************/
6104
6105int addelm(char *s,int ref,int knd1)
6106{
6107 int knd,piggy;
6108 piggy=knd1/100;
6109 knd=knd1-piggy*100;
6110 switch(knd){
6111 case MPpoint:
6112 thd.part.push_back(new CMPpoint(s,ref,knd));
6113 break;
6114 case MPlines:
6115 thd.part.push_back(new CMPlines(s,ref,knd));
6116 break;
6117 case MPplane:
6118 thd.part.push_back(new CMPplane(s,ref,knd));
6119 if(piggy) (reinterpret_cast<CMPplane *>(thd.part[thd.npart]))->isGroup=1;
6120 break;
6121 case MPbox:
6122 thd.part.push_back(new CMPbox(s,ref,knd));
6123 break;
6124 case MPsphere:
6125 thd.part.push_back(new CMPsphere(s,ref,knd));
6126 break;
6127 case MPbodyOfRot:
6128 thd.part.push_back(new CMPbodyOfRot(s,ref,knd));
6129 break;
6130 default:
6131 return -1;
6132 break;
6133 }
6134 thd.npart++;
6135 return thd.npart-1;
6136}
6137
6138/****************************************************
6139
6140 set local coordinate
6141
6142****************************************************/
6143void setlocal(double x,double y,double z,double theta,double phi,double psi)
6144{
6145 thd.part[thd.npart-1]->x0=x;
6146 thd.part[thd.npart-1]->y0=y;
6147 thd.part[thd.npart-1]->z0=z;
6148 thd.part[thd.npart-1]->theta0=theta;
6149 thd.part[thd.npart-1]->phi0=phi;
6150 thd.part[thd.npart-1]->psi0=psi;
6151}
6152
6153/***************************************************
6154
6155 set element color
6156
6157***************************************************/
6158void setcolor(int c)
6159{
6160 thd.part[thd.npart-1]->color=c;
6161}
6162
6163/***************************************************
6164
6165 set line appearance
6166
6167***************************************************/
6168int setline(int c)
6169{
6170 if(thd.part[thd.npart-1]->kind!=MPlines) return -1;
6171 (reinterpret_cast<CMPlines *>(thd.part[thd.npart-1]))->line=c;
6172 return 0;
6173}
6174
6175/***************************************************
6176
6177 set if tube for a body of rotation
6178
6179***************************************************/
6180int isTube()
6181{
6182 if(thd.part[thd.npart-1]->kind!=MPbodyOfRot) return -1;
6183 (reinterpret_cast<CMPbodyOfRot *>(thd.part[thd.npart-1]))->isTube=true;
6184 return 0;
6185}
6186
6187/***************************************************
6188
6189 set if oblong for a body of rotation
6190
6191***************************************************/
6192int oblong()
6193{
6194 if(thd.part[thd.npart-1]->kind!=MPbodyOfRot) return -1;
6195 (reinterpret_cast<CMPbodyOfRot *>(thd.part[thd.npart-1]))->oblong=true;
6196 return 0;
6197}
6198
6199/***************************************************
6200
6201 set one dimensional point as in sphere radius
6202
6203***************************************************/
6204int set1Dpoint(double x)
6205{
6206 switch(thd.part[thd.npart-1]->kind){
6207 case MPsphere:
6208 (reinterpret_cast<CMPsphere *>(thd.part[thd.npart-1]))->SetRadius(x);
6209 break;
6210 default:
6211 return -1;
6212 break;
6213 }
6214 return 0;
6215}
6216
6217/***************************************************
6218
6219 set two dimensional point as in an apex of plane
6220
6221***************************************************/
6222int set2Dpoint(double x,double y)
6223{
6224 switch(thd.part[thd.npart-1]->kind){
6225 case MPplane:
6226 (reinterpret_cast<CMPplane *>(thd.part[thd.npart-1]))->SetPoint(x,y);
6227 break;
6228 case MPbodyOfRot:
6229 (reinterpret_cast<CMPbodyOfRot *>(thd.part[thd.npart-1]))->SetPoint(x,y);
6230 break;
6231 default:
6232 return -1;
6233 break;
6234 }
6235 return 0;
6236}
6237
6238/***************************************************
6239
6240 set three dimensional point as in a point in line
6241
6242***************************************************/
6243int set3Dpoint(double x,double y,double z)
6244{
6245 switch(thd.part[thd.npart-1]->kind){
6246 case MPlines:
6247 (reinterpret_cast<CMPlines *>(thd.part[thd.npart-1]))->SetPoint(x,y,z);
6248 break;
6249 case MPbox:
6250 (reinterpret_cast<CMPbox *>(thd.part[thd.npart-1]))->SetSize(x,y,z);
6251 break;
6252 default:
6253 return -1;
6254 break;
6255 }
6256 return 0;
6257}
6258
6259void adddeco()
6260{
6261 CMPplane *pla;
6262 CMPsphere *psp;
6263 CMPdeco *pld;
6264
6265 if(thd.part[thd.npart-1]->kind==MPplane){
6266 pla=reinterpret_cast<CMPplane *>(thd.part[thd.npart-1]);
6267 pld=new CMPdeco(pla);
6268 pla->deco.push_back(pld);
6269 pla->ndeco++;
6270 }
6271 else{
6272 psp=reinterpret_cast<CMPsphere *>(thd.part[thd.npart-1]);
6273 pld=new CMPdeco(psp);
6274 psp->deco.push_back(pld);
6275 psp->ndeco++;
6276 }
6277}
6278
6279void decocontxy(double x,double y)
6280{
6281 CMPplane *pla;
6282 CMPsphere *psp;
6283 CMPdeco *pld;
6284 Point2d p;
6285
6286 if(thd.part[thd.npart-1]->kind==MPplane){
6287 pla=reinterpret_cast<CMPplane *>(thd.part[thd.npart-1]);
6288 pld=pla->deco[pla->ndeco-1];
6289 }
6290 else{
6291 psp=reinterpret_cast<CMPsphere *>(thd.part[thd.npart-1]);
6292 pld=psp->deco[psp->ndeco-1];
6293 }
6294 p.x=x;
6295 p.y=y;
6296 pld->contour.push_back(p);
6297 pld->ncontour++;
6298}
6299
6300void decocontco(int color)
6301{
6302 CMPplane *pla;
6303 CMPsphere *psp;
6304 CMPdeco *pld;
6305
6306 if(thd.part[thd.npart-1]->kind==MPplane){
6307 pla=reinterpret_cast<CMPplane *>(thd.part[thd.npart-1]);
6308 pld=pla->deco[pla->ndeco-1];
6309 }
6310 else{
6311 psp=reinterpret_cast<CMPsphere *>(thd.part[thd.npart-1]);
6312 pld=psp->deco[psp->ndeco-1];
6313 }
6314 pld->color=color;
6315}
6316
6317void decolinexy(double x0,double y0,double x1, double y1)
6318{
6319 CMPplane *pla;
6320 CMPsphere *psp;
6321 CMPdeco *pld;
6322 Line2d l;
6323
6324 if(thd.part[thd.npart-1]->kind==MPplane){
6325 pla=reinterpret_cast<CMPplane *>(thd.part[thd.npart-1]);
6326 pld=pla->deco[pla->ndeco-1];
6327 }
6328 else{
6329 psp=reinterpret_cast<CMPsphere *>(thd.part[thd.npart-1]);
6330 pld=psp->deco[psp->ndeco-1];
6331 }
6332 l.sx=x0;
6333 l.sy=y0;
6334 l.ex=x1;
6335 l.ey=y1;
6336 pld->lineseg.push_back(l);
6337 pld->nlineseg++;
6338}
6339
6340void viewFrom(double a,double b,double r)
6341{
6342 thd.Azi=a;
6343 thd.Ele=b;
6344 thd.Dst=r;
6345}
6346
6347void drawMessage(int i)
6348{
6349 thd.message=i;
6350}
6351
6352
6353void viewFixed(double tgx,double tgy,double tgz,int foc)
6354{
6355 thd.contAll=false;
6356 thd.Tgx=tgx;
6357 thd.Tgy=tgy;
6358 thd.Tgz=tgz;
6359 thd.Foc=foc/2;
6360}
6361
6362int draw3D(int wid,int hei)
6363{
6364 thd.Wid=wid/2;
6365 thd.Hei=hei/2;
6366 thd.Draw();
6367 return 0;
6368}
6369
6370void getCoord(int ref,double x,double y,double z,int *ix,int *iy)
6371{
6372/* to be implemented some time later */
6373}
6374
6375char *insert3D(int *len)
6376{
6377 char *s;
6378 s=thd.ToDB(len);
6379}
6380
6381void select3D(int len,char *d,double x,double y,double z,double theta,double phi,double psi)
6382{
6383 double loc[6];
6384 loc[0]=x;
6385 loc[1]=y;
6386 loc[2]=z;
6387 loc[3]=theta;
6388 loc[4]=phi;
6389 loc[5]=psi;
6390 thd.FromDB(len,d,loc);
6391}
6392
6393void parts3D(int style)
6394{
6395 char s[200];
6396 int i,j,k,l;
6397 CMPpart *p;
6398 Point2d *p2;
6399 Point3d *p3;
6400 sprintf(s,"thd.npart=%i\n",thd.npart);
6401 outstr(s);
6402 for(i=0;i<thd.npart;i++){
6403 sprintf(s,"part index=%i ",i);
6404 outstr(s);
6405 p=thd.part[i];
6406 sprintf(s,"kind=%i ",p->kind);
6407 outstr(s);
6408 strcpy(s,p->desc.c_str());
6409 outstr(s);
6410 sprintf(s,"\n refPart=%i ",p->refPart);
6411 outstr(s);
6412 sprintf(s,"x0=%g ",p->x0);
6413 outstr(s);
6414 sprintf(s,"y0=%g ",p->y0);
6415 outstr(s);
6416 sprintf(s,"z0=%g ",p->z0);
6417 outstr(s);
6418 sprintf(s,"theta0=%g ",p->theta0);
6419 outstr(s);
6420 sprintf(s,"phi0=%g ",p->phi0);
6421 outstr(s);
6422 sprintf(s,"psi0=%g ",p->psi0);
6423 outstr(s);
6424 sprintf(s,"color=%i ",p->color);
6425 outstr(s);
6426 sprintf(s,"showDrawing=%i\n",p->showDrawing);
6427 outstr(s);
6428 switch(p->kind){
6429 case MPpoint:
6430 break;
6431 case MPlines:
6432 k=reinterpret_cast<CMPlines *>(p)->npoint;
6433 sprintf(s,"npoint=%i\n",k);
6434 for(j=0;j<k;j++){
6435 p3=&reinterpret_cast<CMPlines *>(p)->pt[j];
6436 sprintf(s,"x=%g y=%g z=%g\n",p3->x,p3->y,p3->z);
6437 outstr(s);
6438 }
6439 sprintf(s,"line=%i\n",reinterpret_cast<CMPlines *>(p)->line);
6440 outstr(s);
6441 break;
6442 case MPplane:
6443 k=reinterpret_cast<CMPplane *>(p)->npoint;
6444 sprintf(s,"npoint=%i\n",k);
6445 for(j=0;j<k;j++){
6446 p2=&reinterpret_cast<CMPplane *>(p)->pt[j];
6447 sprintf(s,"x=%g y=%g\n",p2->x,p2->y);
6448 outstr(s);
6449 }
6450 sprintf(s,"isPolyhed=%i isGroup=%i\n",reinterpret_cast<CMPplane *>(p)->isPolyhed,
6451 reinterpret_cast<CMPplane *>(p)->isGroup);
6452 break;
6453 case MPbox:
6454 sprintf(s,"lx=%g ly=%g lz=%g visibleSurface=%i\n",
6455 reinterpret_cast<CMPbox *>(p)->lx,reinterpret_cast<CMPbox *>(p)->ly,
6456 reinterpret_cast<CMPbox *>(p)->lz,
6457 reinterpret_cast<CMPbox *>(p)->visibleSurface);
6458 outstr(s);
6459 break;
6460 case MPsphere:
6461 sprintf(s,"r=%g\n",reinterpret_cast<CMPsphere *>(p)->r);
6462 outstr(s);
6463 break;
6464 case MPbodyOfRot:
6465 sprintf(s,"npoint=%i ",reinterpret_cast<CMPbodyOfRot *>(p)->npoint);
6466 outstr(s);
6467 sprintf(s,"p0.x=%g p0.y=%g ",reinterpret_cast<CMPbodyOfRot *>(p)->p0.x,
6468 reinterpret_cast<CMPbodyOfRot *>(p)->p0.y);
6469 outstr(s);
6470 sprintf(s,"p1.x=%g p1.y=%g\n",reinterpret_cast<CMPbodyOfRot *>(p)->p1.x,
6471 reinterpret_cast<CMPbodyOfRot *>(p)->p1.y);
6472 outstr(s);
6473 sprintf(s,"isTube=%i ",reinterpret_cast<CMPbodyOfRot *>(p)->isTube);
6474 outstr(s);
6475 sprintf(s,"oblong=%i ",reinterpret_cast<CMPbodyOfRot *>(p)->oblong);
6476 outstr(s);
6477 sprintf(s,"appear=%i ",reinterpret_cast<CMPbodyOfRot *>(p)->appear);
6478 outstr(s);
6479 sprintf(s,"visibleSurface=%i\n",reinterpret_cast<CMPbodyOfRot *>(p)->visibleSurface);
6480 outstr(s);
6481 }
6482 }
6483 flush();
6484}
6485
6486int setting(double a[],int b[])
6487{
6488 thd.Azi=a[0];
6489 thd.Ele=a[1];
6490 thd.Dst=a[2];
6491 thd.Tgx=a[3];
6492 thd.Tgy=a[4];
6493 thd.Tgz=a[5];
6494 thd.Wid=b[0];
6495 thd.Hei=b[1];
6496 thd.Foc=b[2];
6497 thd.contAll=b[3];
6498 return 0;
6499}
6500
6501int drawobj(int id,double a[])
6502{
6503 int i;
6504 double rh,lb,wb,la,wa,ll,wl,xc,yc,zc;
6505 double l,w,h;
6506 double r;
6507 double ang;
6508 char str[10];
6509
6510 switch(id/10){
6511 case 30:
6512 rh=0.11; /* rh*2+0.01+lb+0.01+ll=1.64 */
6513 lb=0.7;
6514 la=0.6;
6515 ll=0.7;
6516 wb=0.3;
6517 wa=0.05;
6518 wl=0.09;
6519 addelm("ref point",0,MPpoint);
6520 setlocal(a[0],a[1],a[2],a[3],a[4],a[5]);
6521 addelm("head",0,MPsphere);
6522 zc=0.82-rh;
6523 setlocal(0.,0.,zc,0.,0.,0.);
6524 setcolor(MPyellow);
6525 set1Dpoint(rh);
6526 addelm("body",0,MPbox);
6527 zc=0.82-2.*rh-0.01-lb/2.;
6528 setlocal(0.,0.,zc,0.,0.,0.);
6529 setcolor(MPred);
6530 set3Dpoint(wb/2.,wb,lb);
6531 addelm("right leg",0,MPbox);
6532 zc=-0.82+ll/2.;
6533 setlocal(0.,-wl/2-0.02,zc,0.,0.,0.);
6534 setcolor(MPgray);
6535 set3Dpoint(wl+0.02,wl,ll);
6536 addelm("left leg",0,MPbox);
6537 zc=-0.82+ll/2.;
6538 setlocal(0.,wl/2+0.02,zc,0.,0.,0.);
6539 setcolor(MPgray);
6540 set3Dpoint(wl+0.02,wl,ll);
6541 addelm("right arm",0,MPbox);
6542 zc=0.82-2.*rh-0.01-la/2.-0.01;
6543 setlocal(0.,-(wb+wa)/2.-0.01,zc,0.,0.,0.);
6544 setcolor(MPgreen);
6545 set3Dpoint(wa,wa,la);
6546 addelm("left arm",0,MPbox);
6547 zc=0.82-2.*rh-0.01-la/2.-0.01;
6548 setlocal(0.,(wb+wa)/2.+0.01,zc,0.,0.,0.);
6549 setcolor(MPgreen);
6550 set3Dpoint(wa,wa,la);
6551 break;
6552 case 31:
6553 rh=0.11; /* rh*2+0.01+lb+0.01+ll=1.64 */
6554 lb=0.7;
6555 la=0.6;
6556 ll=0.7;
6557 wb=0.3;
6558 wa=0.05;
6559 wl=0.09;
6560 addelm("ref point",0,MPpoint);
6561 setlocal(a[0],a[1],a[2],a[3],a[4],a[5]);
6562 addelm("head",0,MPsphere);
6563 zc=0.82-rh;
6564 setlocal(0.,0.,zc,0.,0.,0.);
6565 setcolor(MPyellow);
6566 set1Dpoint(rh);
6567 adddeco();
6568 for(w=0.;w<360.;w=w+5.) decocontxy(w,30.);
6569 decocontco(MPgray);
6570 addelm("body",0,MPbox);
6571 zc=0.82-2.*rh-0.01-lb/2.;
6572 setlocal(0.,0.,zc,0.,0.,0.);
6573 setcolor(MPred);
6574 set3Dpoint(wb/2.,wb,lb);
6575 addelm("right leg",0,MPbox);
6576 zc=-0.82+ll/2.;
6577 setlocal(0.,-wl/2-0.02,zc,0.,0.,0.);
6578 setcolor(MPgray);
6579 set3Dpoint(wl+0.02,wl,ll);
6580 addelm("left leg",0,MPbox);
6581 zc=-0.82+ll/2.;
6582 setlocal(0.,wl/2+0.02,zc,0.,0.,0.);
6583 setcolor(MPgray);
6584 set3Dpoint(wl+0.02,wl,ll);
6585 addelm("right arm",0,MPbox);
6586 zc=0.82-2.*rh-0.01-la/2.-0.01;
6587 setlocal(0.,-(wb+wa)/2.-0.01,zc,0.,0.,0.);
6588 setcolor(MPgreen);
6589 set3Dpoint(wa,wa,la);
6590 addelm("left arm",0,MPbox);
6591 zc=0.82-2.*rh-0.01-la/2.-0.01;
6592 setlocal(0.,(wb+wa)/2.+0.01,zc,0.,0.,0.);
6593 setcolor(MPgreen);
6594 set3Dpoint(wa,wa,la);
6595 break;
6596 case 32:
6597 addelm("ref point",0,MPpoint);
6598 setlocal(a[0],a[1],a[2],a[3],a[4],a[5]);
6599 addelm("head",0,MPsphere);
6600 setlocal(0.,0.,0.,0.,0.,0.);
6601 setcolor(MPyellow);
6602 set1Dpoint(0.11);
6603 adddeco();
6604 decocontxy(-150.,-40.);
6605 decocontxy(-120.,-30.);
6606 decocontxy(-95.,-5.);
6607 decocontxy(-90.,5.);
6608 decocontxy(-80.,5.);
6609 decocontxy(-70.,10.);
6610 decocontxy(-50.,20.);
6611 decocontxy(-30.,35.);
6612 decocontxy(-20.,40.);
6613 decocontxy(0.,40.);
6614 decocontxy(20.,40.);
6615 decocontxy(30.,35.);
6616 decocontxy(50.,20.);
6617 decocontxy(70.,10.);
6618 decocontxy(80.,5.);
6619 decocontxy(90.,5.);
6620 decocontxy(95.,-5.);
6621 decocontxy(120.,-30.);
6622 decocontxy(150.,-40.);
6623 decocontxy(180.,-40.);
6624 decocontco(MPgray);
6625 adddeco();
6626 decolinexy(-20.,30,20,30);
6627 break;
6628 case 33:
6629 addelm("ref point",0,MPpoint);
6630 setlocal(a[0],a[1],a[2],a[3],a[4],a[5]);
6631 addelm("head",0,MPsphere);
6632 setlocal(0.,0.,0.,0.,0.,0.);
6633 setcolor(MPyellow);
6634 set1Dpoint(0.11);
6635 adddeco();
6636 decolinexy(-150.,-40.,-120.,-30.);
6637 decolinexy(-120.,-30.,-95.,-5.);
6638 decolinexy(-95.,-5.,-90.,5.);
6639 decolinexy(-90.,5.,-80.,5.);
6640 decolinexy(-80.,5.,-70.,10.);
6641 decolinexy(-70.,10.,-50.,20.);
6642 decolinexy(-50.,20.,-30.,35.);
6643 decolinexy(-30.,35.,-20.,40.);
6644 decolinexy(-20.,40.,0.,40.);
6645 decolinexy(0.,40.,20.,40.);
6646 decolinexy(20.,40.,30.,35.);
6647 decolinexy(30.,35.,50.,20.);
6648 decolinexy(50.,20.,70.,10.);
6649 decolinexy(70.,10.,80.,5.);
6650 decolinexy(80.,5.,90.,5.);
6651 decolinexy(90.,5.,95.,-5.);
6652 decolinexy(95.,-5.,120.,-30.);
6653 decolinexy(120.,-30.,150.,-40.);
6654 decolinexy(150.,-40.,180.,-40.);
6655 decolinexy(180.,-40.,-150.,-40.);
6656 // decocontco(MPgray);
6657 adddeco();
6658 decolinexy(-20.,30,20,30);
6659 break;
6660 case 40:
6661 addelm("point",0,0);
6662 setlocal(a[0],a[1],a[2],a[3],a[4],a[5]);
6663 addelm("flag",0,2);
6664 setlocal(0.,0.,0.,0.,0.,0.);
6665 r=0.3;
6666 h=10.*r/3.;
6667 w=3.*h/2.;
6668 set2Dpoint(-0.5*w,-0.5*h);
6669 set2Dpoint(0.5*w,-0.5*h);
6670 set2Dpoint(0.5*w,0.5*h);
6671 set2Dpoint(-0.5*w,0.5*h);
6672 setcolor(MPwhite);
6673 adddeco();
6674 for(ang=0.;ang<6.28;ang+=0.157) decocontxy(r*cos(ang),r*sin(ang));
6675 decocontco(MPred);
6676 break;
6677 case 41:
6678 addelm("point",0,0);
6679 setlocal(a[0],a[1],a[2],a[3],a[4],a[5]);
6680 addelm("flag",0,2);
6681 setlocal(0.,0.,0.,0.,0.,0.);
6682 r=0.3;
6683 h=10.*r/3.;
6684 w=1.9*h;
6685 set2Dpoint(-0.5*w,-0.5*h);
6686 set2Dpoint(0.5*w,-0.5*h);
6687 set2Dpoint(0.5*w,0.5*h);
6688 set2Dpoint(-0.5*w,0.5*h);
6689 setcolor(MPwhite);
6690 for(r=0.;r<13.;r=r+2.){
6691 adddeco();
6692 decocontxy(-0.5*w,0.5*h-h*r/13.);
6693 decocontxy(-0.5*w,0.5*h-h*(r+1.)/13.);
6694 decocontxy(0.5*w,0.5*h-h*(r+1.)/13.);
6695 decocontxy(0.5*w,0.5*h-h*r/13.);
6696 decocontco(MPred);
6697 }
6698 adddeco();
6699 decocontxy(-0.5*w,0.5*h);
6700 decocontxy(-0.5*w,0.5*h-(7./13.)*h);
6701 decocontxy(-0.5*w+0.76*h,0.5*h-(7./13.)*h);
6702 decocontxy(-0.5*w+0.76*h,0.5*h);
6703 decocontco(MPblue);
6704 i=0;
6705 for(l=-0.5*w+0.063*h;l<-0.5*w+0.76*h;l=l+0.063*h){
6706 if((i%2)==0){
6707 for(r=0.5*h-0.054*h;r>0.5*h-0.5385*h;r=r-0.108*h){
6708 adddeco();
6709 for(ang=0.314159;ang<6.28319;ang=ang+1.25664){
6710 decocontxy(l+0.0308*h*cos(ang),r+0.0308*h*sin(ang));
6711 decocontxy(l+0.0308*0.3820*h*cos(ang+0.628319),r+0.0308*0.3820*h*sin(ang+628319));
6712 }
6713 decocontco(MPwhite);
6714 }
6715 }
6716 else if(i<10){
6717 for(r=0.5*h-0.108*h;r>0.5*h-0.5385*h;r=r-0.108*h){
6718 adddeco();
6719 for(ang=0.314159;ang<6.28319;ang=ang+1.25664){
6720 decocontxy(l+0.0308*h*cos(ang),r+0.0308*h*sin(ang));
6721 decocontxy(l+0.0308*0.3820*h*cos(ang+0.628319),r+0.0308*0.3820*h*sin(ang+628319));
6722 }
6723 decocontco(MPwhite);
6724 }
6725 }
6726 i++;
6727 }
6728 break;
6729 case 50:
6730 case 51:
6731 case 52:
6732 case 53:
6733 switch(id){
6734 case 50:
6735 strcpy(str,"SOUTH");
6736 break;
6737 case 51:
6738 strcpy(str,"WEST");
6739 break;
6740 case 52:
6741 strcpy(str,"NORTH");
6742 break;
6743 case 53:
6744 strcpy(str,"EAST");
6745 break;
6746 }
6747 addelm("point",0,0);
6748 setlocal(a[0],a[1],a[2],a[3],a[4],a[5]);
6749 addelm("placard",0,2);
6750 setlocal(0.,0.,-0.01,0.,0.,0.);
6751 h=0.2;
6752 w=0.84;
6753 set2Dpoint(-0.5*w,-0.5*h);
6754 set2Dpoint(0.5*w,-0.5*h);
6755 set2Dpoint(0.5*w,0.5*h);
6756 set2Dpoint(-0.5*w,0.5*h);
6757 setcolor(MPwhite);
6758 adddeco();
6759 // decolinexy(-0.5*w,-0.5*h,0.5*w,0.5*h);
6760 // decolinexy(-0.5*w,0.5*h,0.5*w,-0.5*h);
6761 styleVF16(0.6*h/16.,0.6*h/16.,0.);
6762 drawVF16(-0.45*w,-0.3*h,str);
6763 decocontco(MPred);
6764 break;
6765 default:
6766 break;
6767 }
6768 thd.Draw();
6769 return 0;
6770}
6771
6772int styleVF16(double scx,double scy,double ang)
6773{
6774 scalex=scx;
6775 scaley=scy;
6776 angle=ang; /* degree */
6777 if(angle!=0.){
6778 cosa=cos(0.01745329252*angle);
6779 sina=sin(0.01745329252*angle);
6780 }
6781}
6782
6783int drawVF16(double xs,double ys,char *st)
6784{
6785 int i,j;
6786 char s[200];
6787 char s1[200];
6788 char *sp;
6789 char *sp1;
6790 iconv_t cd;
6791 size_t inleft,outleft;
6792 int ch;
6793 unsigned char a[200];
6794 double x,y,x0,y0,x1,y1,x2,y2,x3,y3;
6795 int penUp;
6796
6797 strcpy(s,st);
6798 sp=&s[0];
6799 sp1=&s1[0];
6800 inleft=strlen(s);
6801 outleft=199;
6802 cd=iconv_open("SHIFT-JIS","UTF-8");
6803 iconv(cd,&sp,&inleft,&sp1,&outleft);
6804 iconv_close(cd);
6805 s1[199-outleft]='\0';
6806 outleft=strlen(s1);
6807 x0=xs;
6808 y0=ys;
6809 x1=x0;
6810 y1=y0;
6811 for(i=0;i<outleft;i++){
6812 if((s1[i]&0x80)==0){
6813 s[0]=s1[i];
6814 s[1]='\0';
6815 }
6816 else{
6817 s[1]=s1[i];
6818 i++;
6819 s[0]=s1[i];
6820 s[2]='\0';
6821 }
6822 j=getVF16s(s,a);
6823 penUp=1;
6824 for(j=0;j<100;j++){
6825 ch=a[j];
6826 ch&=0xff;
6827 if(ch==0xff) break;
6828 if(ch!=0xfe){
6829 x=ch/16;
6830 y=ch-16*x;
6831 x=x*scalex;
6832 y=y*scaley;
6833 x2=x1+x;
6834 y2=y1+y;
6835 if(angle!=0.){
6836 x2=x0+(x1+x-x0)*cosa-(y1+y-y0)*sina;
6837 y2=y0+(x1+x-x0)*sina+(y1+y-y0)*cosa;
6838 }
6839 if(penUp);
6840 else decolinexy(x3,y3,x2,y2);
6841 x3=x2;
6842 y3=y2;
6843 penUp=0;
6844 }
6845 else penUp=1;
6846 }
6847 x1=x1+16*scalex;
6848 }
6849 return 0;
6850}
6851