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