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