draw17.cc
0000#include <math.h>
0001#include <stdio.h>
0002#include "draw17.h"
0003#include <iconv.h>
0004#include "gcs7.h"
0005
0006#define d2r 0.01745329252
0007#define r2d 57.295779513
0008
0009int outstr(char *buf); /* for zero-terminated string */
0010int flush();
0011char msg[200];
0012
0013CThreeD thd;
0014
0015double scalex,scaley,angle,cosa,sina;
0016
0017/*********************************************************************
0018
0019 DegreeToTAE
0020
0021 Put angles in degree into TransAngleEuler structure
0022
0023*********************************************************************/
0024void DegreeToTAE(double theta,double phi,double psi,TransAngleEuler *dest)
0025{
0026 dest->theta=d2r*theta;
0027 dest->phi=d2r*phi;
0028 dest->psi=d2r*psi;
0029}
0030
0031/*********************************************************************
0032
0033 DegreeToAE
0034
0035 Put angles in degree into AngleEuler structure
0036
0037*********************************************************************/
0038void DegreeToAE(double theta,double phi,double psi,AngleEuler *dest)
0039{
0040 dest->theta=d2r*theta;
0041 dest->phi=d2r*phi;
0042 dest->psi=d2r*psi;
0043}
0044
0045
0046/*********************************************************************
0047
0048 LinesToTAM
0049
0050 Construct TransAngleMatrix parameter from 2 Line3D parameters
0051
0052 a line segment on one of the coordinate axis
0053 b line segment on one of the coordinate axis
0054 w a and b are yzAxis, zxAxis or xyAxis
0055 dest TransAngleMatrix constructed
0056*********************************************************************/
0057void LinesToTAM(Line3d *a,Line3d *b,int w,TransAngleMatrix *dest)
0058{
0059 Point3d x,y,z;
0060 double d;
0061
0062 dest->x=a->sx;
0063 dest->y=a->sy;
0064 dest->z=a->sz;
0065
0066 switch (w){
0067 case yzAxis:
0068 NormLine3d(a,&y,&d);
0069 NormLine3d(b,&z,&d);
0070 x.x=y.y*z.z-y.z*z.y;
0071 x.y=y.z*z.x-y.x*z.z;
0072 x.z=y.x*z.y-y.y*z.x;
0073 break;
0074 case zxAxis:
0075 NormLine3d(a,&z,&d);
0076 NormLine3d(b,&x,&d);
0077 y.x=z.y*x.z-z.z*x.y;
0078 y.y=z.z*x.x-z.x*x.z;
0079 y.z=z.x*x.y-z.y*x.x;
0080 break;
0081 case xyAxis:
0082 NormLine3d(a,&x,&d);
0083 NormLine3d(b,&y,&d);
0084 z.x=x.y*y.z-x.z*y.y;
0085 z.y=x.z*y.x-x.x*y.z;
0086 z.z=x.x*y.y-x.y*y.x;
0087 break;
0088 }
0089 dest->c[0][0]=x.x;
0090 dest->c[1][0]=x.y;
0091 dest->c[2][0]=x.z;
0092 dest->c[0][1]=y.x;
0093 dest->c[1][1]=y.y;
0094 dest->c[2][1]=y.z;
0095 dest->c[0][2]=z.x;
0096 dest->c[1][2]=z.y;
0097 dest->c[2][2]=z.z;
0098}
0099
0100/*********************************************************************
0101
0102 LinesToTAE
0103
0104 Construct TransAngleEuler parameter from 2 Line3D parameters
0105
0106 a line segment on one of the coordinate axis
0107 b line segment on one of the coordinate axis
0108 w a and b are yzAxis, zxAxis or xyAxis
0109 dest TransAngleEuler constructed
0110*********************************************************************/
0111void LinesToTAE(Line3d *a,Line3d *b,int w,TransAngleEuler *dest)
0112{
0113 TransAngleMatrix tmp;
0114
0115 LinesToTAM(a,b,w,&tmp);
0116 dest->x=tmp.x;
0117 dest->y=tmp.y;
0118 dest->z=tmp.z;
0119
0120 if(tmp.c[0][2]==0.&&tmp.c[1][2]==0.){
0121 dest->theta=0.;
0122 dest->phi=0.;
0123 dest->psi=atan2(tmp.c[1][0],tmp.c[0][0]);
0124 }
0125 else{
0126 double x1,y1;
0127 dest->theta=acos(tmp.c[2][2]);
0128 dest->phi=atan2(tmp.c[1][2],tmp.c[0][2])+1.5707963268;
0129 x1=cos(dest->phi); // ascending node
0130 y1=sin(dest->phi);
0131
0132 // (y1*x.z-z1*x.y)*z.x+(z1*x.x-x1*x.z)*z.y+(x1*x.y-y1*x.x)*z.z
0133 // x1*x.x+y1*x.y+z1*x.z
0134 // dest->psi=atan2(y1*x.z*z.x-x1*x.z*z.y+(x1*x.y-y1*x.x)*z.z,x1*x.x+y1*x.y);
0135
0136 dest->psi=atan2(y1*tmp.c[2][0]*tmp.c[0][2]-x1*tmp.c[2][0]*tmp.c[2][1]
0137 +(x1*tmp.c[1][0]-y1*tmp.c[0][0])*tmp.c[2][2],
0138 x1*tmp.c[0][0]+y1*tmp.c[1][0]);
0139 }
0140}
0141
0142
0143/*********************************************************************
0144
0145 PointsToAM
0146
0147 Construct AngleMatrix parameter from 2 Point3D parameters
0148
0149 a point on one of the coordinate axis
0150 b point on one of the coordinate axis
0151 w a and b are yzAxis, zxAxis or xyAxis
0152 dest AngleMatrix constructed
0153*********************************************************************/
0154void PointsToAM(Point3d *a,Point3d *b,int w,AngleMatrix *dest)
0155{
0156 Point3d x,y,z;
0157 double d;
0158
0159
0160 switch (w){
0161 case yzAxis:
0162 NormPoint3d(a,&y,&d);
0163 NormPoint3d(b,&z,&d);
0164 x.x=y.y*z.z-y.z*z.y;
0165 x.y=y.z*z.x-y.x*z.z;
0166 x.z=y.x*z.y-y.y*z.x;
0167 break;
0168 case zxAxis:
0169 NormPoint3d(a,&z,&d);
0170 NormPoint3d(b,&x,&d);
0171 y.x=z.y*x.z-z.z*x.y;
0172 y.y=z.z*x.x-z.x*x.z;
0173 y.z=z.x*x.y-z.y*x.x;
0174 break;
0175 case xyAxis:
0176 NormPoint3d(a,&x,&d);
0177 NormPoint3d(b,&y,&d);
0178 z.x=x.y*y.z-x.z*y.y;
0179 z.y=x.z*y.x-x.x*y.z;
0180 z.z=x.x*y.y-x.y*y.x;
0181 break;
0182 }
0183 dest->c[0][0]=x.x;
0184 dest->c[1][0]=x.y;
0185 dest->c[2][0]=x.z;
0186 dest->c[0][1]=y.x;
0187 dest->c[1][1]=y.y;
0188 dest->c[2][1]=y.z;
0189 dest->c[0][2]=z.x;
0190 dest->c[1][2]=z.y;
0191 dest->c[2][2]=z.z;
0192}
0193
0194/*********************************************************************
0195
0196 PointsToAE
0197
0198 Construct AngleEuler parameter from 2 Point3D parameters
0199
0200 a point on one of the coordinate axis
0201 b point on one of the coordinate axis
0202 w a and b are yzAxis, zxAxis or xyAxis
0203 dest TransAngleEuler constructed
0204*********************************************************************/
0205void PointsToAE(Point3d *a,Point3d *b,int w,AngleEuler *dest)
0206{
0207 AngleMatrix tmp;
0208
0209 PointsToAM(a,b,w,&tmp);
0210
0211 if(tmp.c[0][2]==0.&&tmp.c[1][2]==0.){
0212 dest->theta=0.;
0213 dest->phi=0.;
0214 dest->psi=atan2(tmp.c[1][0],tmp.c[0][0]);
0215 }
0216 else{
0217 double x1,y1;
0218 dest->theta=acos(tmp.c[2][2]);
0219 dest->phi=atan2(tmp.c[1][2],tmp.c[0][2])+1.5707963268;
0220 x1=cos(dest->phi); // ascending node
0221 y1=sin(dest->phi);
0222
0223 // (y1*x.z-z1*x.y)*z.x+(z1*x.x-x1*x.z)*z.y+(x1*x.y-y1*x.x)*z.z
0224 // x1*x.x+y1*x.y+z1*x.z
0225 // dest->psi=atan2(y1*x.z*z.x-x1*x.z*z.y+(x1*x.y-y1*x.x)*z.z,x1*x.x+y1*x.y);
0226
0227 dest->psi=atan2(y1*tmp.c[2][0]*tmp.c[0][2]-x1*tmp.c[2][0]*tmp.c[2][1]
0228 +(x1*tmp.c[1][0]-y1*tmp.c[0][0])*tmp.c[2][2],
0229 x1*tmp.c[0][0]+y1*tmp.c[1][0]);
0230 }
0231}
0232
0233
0234/*********************************************************************
0235 NormLine3d
0236
0237 normalize a vector
0238
0239 source line segment to be normalized
0240
0241 dest normalized vector
0242 norm length of the vector
0243
0244*********************************************************************/
0245void NormLine3d(Line3d *source,Point3d *dest,double *norm)
0246{
0247 double x,y,z,d;
0248
0249 x=source->ex-source->sx;
0250 y=source->ey-source->sy;
0251 z=source->ez-source->sz;
0252
0253 d=sqrt(x*x+y*y+z*z);
0254 dest->x=x/d;
0255 dest->y=y/d;
0256 dest->z=z/d;
0257 *norm=d;
0258}
0259
0260/*********************************************************************
0261 NormPoint3d
0262
0263 normalize a vector
0264
0265 source vector to be normalized
0266
0267 dest normalized vector
0268 norm length of the vector
0269
0270*********************************************************************/
0271void NormPoint3d(Point3d *source,Point3d *dest,double *norm)
0272{
0273 double x,y,z,d;
0274
0275 x=source->x;
0276 y=source->y;
0277 z=source->z;
0278
0279 d=sqrt(x*x+y*y+z*z);
0280 dest->x=x/d;
0281 dest->y=y/d;
0282 dest->z=z/d;
0283 *norm=d;
0284}
0285
0286/*********************************************************************
0287 NormLine2d
0288
0289 normalize a vector
0290
0291 source line segment to be normalized
0292
0293 dest normalized vector
0294 norm length of the vector
0295
0296*********************************************************************/
0297void NormLine2d(Line2d *source,Point2d *dest,double *norm)
0298{
0299 double x,y,d;
0300
0301 x=source->ex-source->sx;
0302 y=source->ey-source->sy;
0303
0304 d=sqrt(x*x+y*y);
0305 dest->x=x/d;
0306 dest->y=y/d;
0307 *norm=d;
0308}
0309
0310/*********************************************************************
0311 NormPoint2d
0312
0313 normalize a vector
0314
0315 source vector to be normalized
0316
0317 dest normalized vector
0318 norm length of the vector
0319
0320*********************************************************************/
0321void NormPoint2d(Point2d *source,Point2d *dest,double *norm)
0322{
0323 double x,y,d;
0324
0325 x=source->x;
0326 y=source->y;
0327
0328 d=sqrt(x*x+y*y);
0329 dest->x=x/d;
0330 dest->y=y/d;
0331 *norm=d;
0332}
0333
0334
0335/*********************************************************************
0336 TAEtoTAM
0337
0338 convert TransAngleEuler to TransAngleMatrix
0339
0340 source TransAngleEuler
0341
0342 dest TransAngleMatrix
0343
0344*********************************************************************/
0345void TAEtoTAM(TransAngleEuler *source,TransAngleMatrix *dest)
0346{
0347 double sth,cth,sph,cph,sps,cps;
0348
0349 dest->x=source->x;
0350 dest->y=source->y;
0351 dest->z=source->z;
0352 sth=sin(source->theta);
0353 cth=cos(source->theta);
0354 sph=sin(source->phi);
0355 cph=cos(source->phi);
0356 sps=sin(source->psi);
0357 cps=cos(source->psi);
0358 dest->c[0][0]=cps*cph-sps*cth*sph; // x component of x'
0359 dest->c[0][1]=-sps*cph-cps*cth*sph;
0360 dest->c[0][2]=sth*sph;
0361 dest->c[1][0]=cps*sph+sps*cth*cph; // y component of x'
0362 dest->c[1][1]=-sps*sph+cps*cth*cph;
0363 dest->c[1][2]=-sth*cph;
0364 dest->c[2][0]=sps*sth; // z component of x'
0365 dest->c[2][1]=cps*sth;
0366 dest->c[2][2]=cth;
0367}
0368
0369
0370/*********************************************************************
0371 TAMtoTAE
0372
0373 convert TransAngleMatrix to TransAngleEuler
0374
0375 source TransAngleMatrix
0376
0377 dest TransAngleEuler
0378
0379*********************************************************************/
0380void TAMtoTAE(TransAngleMatrix *source,TransAngleEuler *dest)
0381{
0382 double a,ca,sa;
0383
0384 dest->x=source->x;
0385 dest->y=source->y;
0386 dest->z=source->z;
0387
0388 if(source->c[2][2]==1.){
0389 dest->theta=0.;
0390 dest->phi=0.;
0391 dest->psi=atan2(source->c[0][1],source->c[0][0]);
0392 return;
0393 }
0394 dest->theta=acos(source->c[2][2]);
0395 dest->phi=atan2(source->c[1][2],source->c[0][2])+1.5707963268;
0396 a=dest->phi;
0397 ca=cos(a);
0398 sa=sin(a);
0399 dest->psi=-atan2(source->c[0][1]*ca+source->c[1][1]*sa,
0400 source->c[0][0]*ca+source->c[1][0]*sa);
0401}
0402
0403
0404/*********************************************************************
0405 InverseTAM
0406
0407 Inverse body-frame order
0408
0409 source TransAngleMatrix
0410
0411 dest TransAngleMatrix
0412
0413*********************************************************************/
0414void InverseTAM(TransAngleMatrix *source,TransAngleMatrix *dest)
0415{
0416 Point3d a;
0417
0418 a.x=-source->x;
0419 a.y=-source->y;
0420 a.z=-source->z;
0421
0422 dest->x=source->c[0][0]*a.x+source->c[1][0]*a.y+source->c[2][0]*a.z;
0423 dest->y=source->c[0][1]*a.x+source->c[1][1]*a.y+source->c[2][1]*a.z;
0424 dest->z=source->c[0][2]*a.x+source->c[1][2]*a.y+source->c[2][2]*a.z;
0425
0426 dest->c[0][0]=source->c[0][0];
0427 dest->c[0][1]=source->c[1][0];
0428 dest->c[0][2]=source->c[2][0];
0429 dest->c[1][0]=source->c[0][1];
0430 dest->c[1][1]=source->c[1][1];
0431 dest->c[1][2]=source->c[2][1];
0432 dest->c[2][0]=source->c[0][2];
0433 dest->c[2][1]=source->c[1][2];
0434 dest->c[2][2]=source->c[2][2];
0435
0436}
0437
0438
0439
0440/*********************************************************************
0441 BodyToFrameTAM
0442
0443 convert Body coordinate to Frame coordinate
0444
0445 source Point3d
0446 m TransAngleMatrix
0447
0448 dest Point3d
0449
0450*********************************************************************/
0451void BodyToFrameTAM(Point3d *source,TransAngleMatrix *m,Point3d *dest)
0452{
0453 dest->x=m->x+m->c[0][0]*source->x+m->c[0][1]*source->y+m->c[0][2]*source->z;
0454 dest->y=m->y+m->c[1][0]*source->x+m->c[1][1]*source->y+m->c[1][2]*source->z;
0455 dest->z=m->z+m->c[2][0]*source->x+m->c[2][1]*source->y+m->c[2][2]*source->z;
0456}
0457
0458/*********************************************************************
0459 FrameToBodyTAM
0460
0461 convert Frame coordinate to Body coordinate
0462
0463 source Point3d
0464 m TransAngleMatrix
0465
0466 dest Point3d
0467
0468*********************************************************************/
0469void FrameToBodyTAM(Point3d *source,TransAngleMatrix *m,Point3d *dest)
0470{
0471 dest->x=m->c[0][0]*(source->x-m->x)
0472 +m->c[1][0]*(source->y-m->y)
0473 +m->c[2][0]*(source->z-m->z);
0474 dest->y=m->c[0][1]*(source->x-m->x)
0475 +m->c[1][1]*(source->y-m->y)
0476 +m->c[2][1]*(source->z-m->z);
0477 dest->z=m->c[0][2]*(source->x-m->x)
0478 +m->c[1][2]*(source->y-m->y)
0479 +m->c[2][2]*(source->z-m->z);
0480}
0481
0482
0483/*********************************************************************
0484 MoveTwiceTAE
0485
0486 obtain single conversion parameters from double consecutive
0487 coordinate conversion parameters
0488
0489 src1 TransAngleEuler cood1 - frame cood2 - body
0490 src2 TransAngleEuler cood2 - frame cood3 - body
0491
0492 dest TransAngleEuler cood1 - frame cood3 - body
0493
0494********************************************************************/
0495void MoveTwiceTAE(TransAngleEuler *src1,TransAngleEuler *src2,TransAngleEuler *dest)
0496{
0497 double x1,y1,z1,theta1,phi1,psi1;
0498 double x2,y2,z2,theta2,phi2,psi2;
0499 double x3,y3,z3,theta3,phi3,psi3;
0500
0501 double a[3][3],b[3][3],c[3],d[3],e[3];
0502 double cost,sint,cosf,sinf,cosp,sinp;
0503 bool locked;
0504
0505 x1=src1->x;
0506 y1=src1->y;
0507 z1=src1->z;
0508 theta1=src1->theta;
0509 phi1=src1->phi;
0510 psi1=src1->psi;
0511 x2=src2->x;
0512 y2=src2->y;
0513 z2=src2->z;
0514 theta2=src2->theta;
0515 phi2=src2->phi;
0516 psi2=src2->psi;
0517
0518 cost=cos(theta1);
0519 sint=sin(theta1);
0520 cosf=cos(phi1);
0521 sinf=sin(phi1);
0522 cosp=cos(psi1);
0523 sinp=sin(psi1);
0524 // if d[i] is a vector in cood2, c[j]=a[j][i]*d[i] is the vector in cood1
0525 a[0][0]=cosp*cosf-sinp*cost*sinf;
0526 a[0][1]=-sinp*cosf-cosp*cost*sinf;
0527 a[0][2]=sint*sinf;
0528
0529 a[1][0]=cosp*sinf+sinp*cost*cosf;
0530 a[1][1]=-sinp*sinf+cosp*cost*cosf;
0531 a[1][2]=-sint*cosf;
0532
0533 a[2][0]=sinp*sint;
0534 a[2][1]=cosp*sint;
0535 a[2][2]=cost;
0536
0537 cost=cos(theta2);
0538 sint=sin(theta2);
0539 cosf=cos(phi2);
0540 sinf=sin(phi2);
0541 cosp=cos(psi2);
0542 sinp=sin(psi2);
0543
0544 // if d[i] is a vector in cood3, c[j]=b[j][i]*d[i] is the vector in cood2
0545 b[0][0]=cosp*cosf-sinp*cost*sinf;
0546 b[0][1]=-sinp*cosf-cosp*cost*sinf;
0547 b[0][2]=sint*sinf;
0548
0549 b[1][0]=cosp*sinf+sinp*cost*cosf;
0550 b[1][1]=-sinp*sinf+cosp*cost*cosf;
0551 b[1][2]=-sint*cosf;
0552
0553 b[2][0]=sinp*sint;
0554 b[2][1]=cosp*sint;
0555 b[2][2]=cost;
0556
0557 x3=x1+a[0][0]*x2+a[0][1]*y2+a[0][2]*z2;
0558 y3=y1+a[1][0]*x2+a[1][1]*y2+a[1][2]*z2;
0559 z3=z1+a[2][0]*x2+a[2][1]*y2+a[2][2]*z2;
0560
0561 // if c is a unit z vector in coord 3, d=b*c is the vector in coord 2
0562 // and a*d is the vector in coord 1
0563 c[0]=0.;
0564 c[1]=0.;
0565 c[2]=1.;
0566 d[0]=b[0][0]*c[0]+b[0][1]*c[1]+b[0][2]*c[2];
0567 d[1]=b[1][0]*c[0]+b[1][1]*c[1]+b[1][2]*c[2];
0568 d[2]=b[2][0]*c[0]+b[2][1]*c[1]+b[2][2]*c[2];
0569 e[0]=a[0][0]*d[0]+a[0][1]*d[1]+a[0][2]*d[2];
0570 e[1]=a[1][0]*d[0]+a[1][1]*d[1]+a[1][2]*d[2];
0571 e[2]=a[2][0]*d[0]+a[2][1]*d[1]+a[2][2]*d[2];
0572
0573 // Euler angles of coord3 with reference to coord1 are to be derived
0574 // theta is the angle between z vector in coord3 and z vector in coord1
0575 // cosine of the theta is e[2] in the above expression
0576
0577 theta3=acos(e[2]);
0578 if(theta3<0.0005) {
0579 phi3=0.; // the case when theta = 0
0580 locked=true;
0581 }
0582 else {
0583 phi3=atan2(e[0],-e[1]);
0584 locked=false;
0585 }
0586
0587 // if c is a unit x vector in coord 3, d=b*c is the vector in coord 2
0588 // and a*d is the vector in coord 1
0589 c[0]=1.;
0590 c[1]=0.;
0591 c[2]=0.;
0592 d[0]=b[0][0]*c[0]+b[0][1]*c[1]+b[0][2]*c[2];
0593 d[1]=b[1][0]*c[0]+b[1][1]*c[1]+b[1][2]*c[2];
0594 d[2]=b[2][0]*c[0]+b[2][1]*c[1]+b[2][2]*c[2];
0595 e[0]=a[0][0]*d[0]+a[0][1]*d[1]+a[0][2]*d[2];
0596 e[1]=a[1][0]*d[0]+a[1][1]*d[1]+a[1][2]*d[2];
0597 e[2]=a[2][0]*d[0]+a[2][1]*d[1]+a[2][2]*d[2];
0598 // if c is a unit y vector in coord 3, d=b*c is the vector in coord 2
0599 // and a*d is the vector in coord 1
0600 c[0]=0.;
0601 c[1]=1.;
0602 c[2]=0.;
0603 d[0]=b[0][0]*c[0]+b[0][1]*c[1]+b[0][2]*c[2];
0604 d[1]=b[1][0]*c[0]+b[1][1]*c[1]+b[1][2]*c[2];
0605 d[2]=b[2][0]*c[0]+b[2][1]*c[1]+b[2][2]*c[2];
0606 c[0]=a[0][0]*d[0]+a[0][1]*d[1]+a[0][2]*d[2];
0607 c[1]=a[1][0]*d[0]+a[1][1]*d[1]+a[1][2]*d[2];
0608 c[2]=a[2][0]*d[0]+a[2][1]*d[1]+a[2][2]*d[2];
0609
0610 if(locked) psi3=atan2(e[1],e[0]);
0611 else psi3=atan2(e[2],c[2]);
0612
0613 dest->x=x3;
0614 dest->y=y3;
0615 dest->z=z3;
0616 dest->theta=theta3;
0617 dest->phi=phi3;
0618 dest->psi=psi3;
0619}
0620
0621
0622/*********************************************************************
0623 AEtoAM
0624
0625 convert CoordEuler to CoordMatrix
0626
0627 source AngleEuler
0628
0629 dest AngleMatrix
0630
0631*********************************************************************/
0632void AEtoAM(AngleEuler *source,AngleMatrix *dest)
0633{
0634 double sth,cth,sph,cph,sps,cps;
0635
0636 sth=sin(source->theta);
0637 cth=cos(source->theta);
0638 sph=sin(source->phi);
0639 cph=cos(source->phi);
0640 sps=sin(source->psi);
0641 cps=cos(source->psi);
0642 dest->c[0][0]=cps*cph-sps*cth*sph;
0643 dest->c[0][1]=-sps*cph-cps*cth*sph;
0644 dest->c[0][2]=sth*sph;
0645 dest->c[1][0]=cps*sph+sps*cth*cph;
0646 dest->c[1][1]=-sps*sph+cps*cth*cph;
0647 dest->c[1][2]=-sth*cph;
0648 dest->c[2][0]=sps*sth;
0649 dest->c[2][1]=cps*sth;
0650 dest->c[2][2]=cth;
0651}
0652
0653/*********************************************************************
0654 BodyToFrameAM
0655
0656 convert Body coordinate to Frame coordinate
0657
0658 source Point3d - Cartesian
0659 m AngleMatrix
0660
0661 dest Point3d - Cartesian
0662
0663*********************************************************************/
0664void BodyToFrameAM(Point3d *source,AngleMatrix *m,Point3d *dest)
0665{
0666
0667 dest->x=m->c[0][0]*source->x+m->c[0][1]*source->y+m->c[0][2]*source->z;
0668 dest->y=m->c[1][0]*source->x+m->c[1][1]*source->y+m->c[1][2]*source->z;
0669 dest->z=m->c[2][0]*source->x+m->c[2][1]*source->y+m->c[2][2]*source->z;
0670
0671}
0672
0673/*********************************************************************
0674 FrameToBodyAM
0675
0676 convert Frame coordinate to Body coordinate
0677
0678 source Point3d - Cartesian
0679 m AngleMatrix
0680
0681 dest Point3d - Cartesian
0682
0683*********************************************************************/
0684void FrameToBodyAM(Point3d *source,AngleMatrix *m,Point3d *dest)
0685{
0686 dest->x=m->c[0][0]*source->x+m->c[1][0]*source->y+m->c[2][0]*source->z;
0687 dest->y=m->c[0][1]*source->x+m->c[1][1]*source->y+m->c[2][1]*source->z;
0688 dest->z=m->c[0][2]*source->x+m->c[1][2]*source->y+m->c[2][2]*source->z;
0689}
0690
0691/*********************************************************************
0692 BodyToFrameAMP
0693
0694 convert Body coordinate to Frame coordinate - Polar
0695
0696 source Point2d - azimuth and elevation (rad)
0697 m AngleMatrix
0698
0699 dest Point2d - azimuth and elevation (rad)
0700
0701*********************************************************************/
0702void BodyToFrameAMP(Point2d *source,AngleMatrix *m,Point2d *dest)
0703{
0704 Point3d s,d; // unit vector
0705
0706 ToCartesian3d(source,&s);
0707
0708 d.x=m->c[0][0]*s.x+m->c[0][1]*s.y+m->c[0][2]*s.z;
0709 d.y=m->c[1][0]*s.x+m->c[1][1]*s.y+m->c[1][2]*s.z;
0710 d.z=m->c[2][0]*s.x+m->c[2][1]*s.y+m->c[2][2]*s.z;
0711
0712 ToPolar3d(&d,dest);
0713
0714}
0715
0716/*********************************************************************
0717 FrameToBodyAMP
0718
0719 convert Frame coordinate to Body coordinate - Polar
0720
0721 source Point2d - azimuth and elevation (rad)
0722 m AngleMatrix
0723
0724 dest Point2d - azimuth and elevation (rad)
0725
0726*********************************************************************/
0727void FrameToBodyAMP(Point2d *source,AngleMatrix *m,Point2d *dest)
0728{
0729 Point3d s,d;
0730
0731 ToCartesian3d(source,&s);
0732
0733 d.x=m->c[0][0]*s.x+m->c[1][0]*s.y+m->c[2][0]*s.z;
0734 d.y=m->c[0][1]*s.x+m->c[1][1]*s.y+m->c[2][1]*s.z;
0735 d.z=m->c[0][2]*s.x+m->c[1][2]*s.y+m->c[2][2]*s.z;
0736
0737 ToPolar3d(&d,dest);
0738
0739}
0740
0741
0742/*********************************************************************
0743 ToPolar3d
0744
0745 convert Catesian coordinate to Polar coordinate
0746
0747 source Point3dDouble - Cartesian with unit length
0748
0749 dest Point2dDouble - azimuth and elevation (rad)
0750
0751*********************************************************************/
0752void ToPolar3d(Point3d *source,Point2d *dest)
0753{
0754 dest->y=asin(source->z);
0755 if(source->z==1.||source->z==-1.) dest->x=0.;
0756 else dest->x=atan2(source->y,source->x);
0757}
0758
0759/*********************************************************************
0760 ToCartesian3d
0761
0762 convert Polar coordinate to Cartesian coordinate
0763
0764 source Point2dDouble - azimuth and elevation (rad)
0765
0766 dest Point3dDouble - Cartesian with unit length
0767
0768*********************************************************************/
0769void ToCartesian3d(Point2d *source,Point3d *dest)
0770{
0771 double cel;
0772 cel=cos(source->y); // cosine elevation
0773 dest->x=cel*cos(source->x);
0774 dest->y=cel*sin(source->x);
0775 dest->z=sin(source->y);
0776}
0777
0778/*********************************************************************
0779 VectorProduct
0780
0781 Vector product
0782
0783 u input
0784 v input
0785 w output uxv
0786
0787*********************************************************************/
0788void VectorProduct(Point3d *u,Point3d *v,Point3d *w)
0789{
0790 w->x=u->y*v->z-u->z*v->y;
0791 w->y=u->z*v->x-u->x*v->z;
0792 w->z=u->x*v->y-u->y*v->x;
0793}
0794
0795/*********************************************************************
0796 AcuteAngle
0797
0798 Test if the angle u-v-w is an acute angle
0799
0800 u point1
0801 v point2 - center point
0802 w point3
0803
0804 returned true if the angle is acute angle
0805 false if the angle is obtuse or right angle
0806
0807*********************************************************************/
0808bool AcuteAngle(Point3d *u,Point3d *v,Point3d *w)
0809{
0810 if((u->x-v->x)*(w->x-v->x)+(u->y-v->y)*(w->y-v->y)+
0811 (u->z-v->z)*(w->z-v->z)>0.) return true;
0812 else return false;
0813}
0814
0815/**************************************************************************
0816 MakeThePolar
0817
0818 'polar' is a coordinate system with its origin at the viewer's eye.
0819 Its z axis is directed toward line of sight and x-z plane is pararel
0820 to the world z axis. This coordinate is for generating polar coordinate
0821 where the north pole is directed to the line of sight and the azimuth
0822 is measured crockwise from the direction pointing upward.
0823
0824 sight starts at the eye and ends at the target
0825
0826 polar with origin at the eye and z-axis toward the line of sight
0827**************************************************************************/
0828void MakeThePolar(Line3d *sight,TransAngleMatrix *polar)
0829{
0830 Point3d x,y,z;
0831 double d;
0832
0833 polar->x=sight->sx;
0834 polar->y=sight->sy;
0835 polar->z=sight->sz;
0836
0837 NormLine3d(sight,&z,&d);
0838
0839 // z.x z.y z.z
0840 // 0 0 1
0841
0842 y.x=z.y;
0843 y.y=-z.x;
0844 y.z=0.;
0845 NormPoint3d(&y,&y,&d);
0846
0847 // z.y -z.x 0
0848 // z.x z.y z.z
0849
0850 x.x=-z.x*z.z;
0851 x.y=-z.y*z.z;
0852 x.z=z.y*z.y+z.x*z.x;
0853 NormPoint3d(&x,&x,&d);
0854
0855 polar->c[0][0]=x.x;
0856 polar->c[0][1]=y.x;
0857 polar->c[0][2]=z.x;
0858 polar->c[1][0]=x.y;
0859 polar->c[1][1]=y.y;
0860 polar->c[1][2]=z.y;
0861 polar->c[2][0]=x.z;
0862 polar->c[2][1]=y.z;
0863 polar->c[2][2]=z.z;
0864}
0865
0866/**************************************************************************
0867 WorldToPolarCnv
0868
0869 'polar' is a coordinate system with its origin at the viewer's eye.
0870 Its z axis is directed toward line of sight and x-z plane is pararel
0871 to the world z axis. This coordinate is for generating polar coordinate
0872 where the north pole is directed to the line of sight and the azimuth
0873 is measured crockwise from the direction pointing upward.
0874
0875 polar with origin at the eye and z-axis toward the line of sight
0876 s point in World coordinate
0877
0878 d polar coordinate az(rad), el(rad), and distance
0879 el measured fron boresight
0880
0881**************************************************************************/
0882void WorldToPolarCnv(Point3d *s,TransAngleMatrix *polar,Point3d *d)
0883{
0884 Point3d a;
0885 double b;
0886
0887 FrameToBodyTAM(s,polar,d);
0888 NormPoint3d(d,&a,&b);
0889 if(a.z==1.){
0890 d->x=0.;
0891 d->y=0.;
0892 d->z=b;
0893 }
0894 else {
0895 d->x=atan2(a.y,a.x);
0896 d->y=acos(a.z);
0897 d->z=b;
0898 }
0899}
0900
0901/**************************************************************************
0902 WorldToPerspCnv
0903
0904 'polar' is a coordinate system with its origin at the viewer's eye.
0905 Its z axis is directed toward line of sight and x-z plane is pararel
0906 to the world z axis. This coordinate is for generating polar coordinate
0907 where the north pole is directed to the line of sight and the azimuth
0908 is measured crockwise from the direction pointing upward.
0909
0910 polar with origin at the eye and z-axis toward the line of sight
0911 s point in World coordinate
0912
0913 d polar coordinate az(rad), el(rad), and distance
0914 el measured fron boresight
0915
0916**************************************************************************/
0917void WorldToPerspCnv(Point3d *s,TransAngleMatrix *polar,PointPerspectve *d)
0918{
0919 Point3d a;
0920 double b;
0921
0922 WorldToPolarCnv(s,polar,&a);
0923 d->az=a.x;
0924 d->el=a.y;
0925 d->r=a.z;
0926 b=tan(a.y);
0927 d->h=b*sin(a.x); // right plus
0928 d->v=b*cos(a.x); // up plus
0929}
0930
0931/**************************************************************************
0932 ClipAngleMatrix
0933
0934 Represent a side of a clip rect by a polar coordinate
0935 and an angular segment in its equatorial plane.
0936
0937 Input parameters are in the polar coordinate, where z axis
0938 is directed to the boresight.
0939
0940 azi azimuth (rad)
0941 inc inclination (rad - measured from z axis)
0942
0943 m conversion matrix (AngleMatrix)
0944 ang azimuth 0 to ang is a clip arc
0945**************************************************************************/
0946void ClipAngleMatrix(double azi,double inc,AngleMatrix *m,double *ang)
0947{
0948 double rot,az,de;
0949 double pi2=1.5707963268;
0950 Point2d s;
0951 Point3d d;
0952
0953 de=pi2-inc; // declination
0954
0955 m->c[0][0]=cos(de)*cos(azi); // body x' axis
0956 m->c[1][0]=cos(de)*sin(azi);
0957 m->c[2][0]=sin(de);
0958
0959 // rotate azimuth by an angle rot so that az falls 0 and 1.57079
0960 rot=0.;
0961 az=azi;
0962 while(az>pi2){
0963 az-=pi2;
0964 rot-=pi2;
0965 }
0966 while(az<0.){
0967 az+=pi2;
0968 rot+=pi2;
0969 }
0970
0971 // obtain *ang as angle between 2 vectors with the same declination
0972 d.x=cos(de)*cos(az);
0973 d.y=cos(de)*sin(az);
0974 d.z=sin(de);
0975 *ang=acos(d.x*d.x-d.y*d.y+d.z*d.z); // d and another vector symmet with x-xis
0976
0977 // now resume calculating z' axis
0978 s.x=-rot; // azimuth -rot
0979 s.y=-atan(tan(inc)*cos(az)); // elevation
0980 ToCartesian3d(&s,&d);
0981
0982 m->c[0][2]=d.x;
0983 m->c[1][2]=d.y;
0984 m->c[2][2]=d.z;
0985
0986 // y' axis as a vector product of z' and x'
0987 m->c[0][1]=m->c[1][2]*m->c[2][0] -m->c[2][2]*m->c[1][0];
0988 m->c[1][1]=m->c[2][2]*m->c[0][0] -m->c[0][2]*m->c[2][0];
0989 m->c[2][1]=m->c[0][2]*m->c[1][0] -m->c[1][2]*m->c[0][0];
0990
0991
0992}
0993
0994
0995/**************************************************************************
0996 SurfthePolarTAM
0997
0998 Make a coordinate system which converts two dimensional surface points
0999 to two dimensional polar coordinates on the viewer's sphere
1000
1001 s surface coordinate
1002 p polar coordinate ( usually 'thePolar' )
1003
1004 sp TransAngleMatrix where the surface is the body,
1005 and the polar is the frame
1006**************************************************************************/
1007void SurfthePolarTAM(TransAngleEuler *s,TransAngleMatrix *p,TransAngleMatrix *sp)
1008{
1009 TransAngleMatrix m;
1010 TransAngleEuler e,d;
1011
1012 InverseTAM(p,&m); // inverse of thePolar
1013 TAMtoTAE(&m,&e);
1014 MoveTwiceTAE(&e,s,&d);
1015 TAEtoTAM(&d,sp);
1016}
1017
1018/**************************************************************************
1019 SurfToPolar
1020
1021 Converts a two dimensional surface point
1022 to a two dimensional polar coordinate on the viewer's sphere
1023
1024 s surface coordinate (x,y)
1025 sp TransAngleMatrix where the surface is the body,
1026 and the polar is the frame
1027
1028 p polar coordinate (alpha,delta)
1029**************************************************************************/
1030void SurfToPolar(Point2d *s,TransAngleMatrix *sp,Point2d *p)
1031{
1032 Point3d a;
1033 double d;
1034
1035 a.x=sp->x+sp->c[0][0]*s->x+sp->c[0][1]*s->y;
1036 a.y=sp->y+sp->c[1][0]*s->x+sp->c[1][1]*s->y;
1037 a.z=sp->z+sp->c[2][0]*s->x+sp->c[2][1]*s->y;
1038
1039 d=sqrt(a.x*a.x+a.y*a.y);
1040 p->y=atan2(a.z,d);
1041 if(d==0.){
1042 p->x=0.;
1043 return;
1044 }
1045 p->x=atan2(a.y,a.x);
1046}
1047
1048/**************************************************************************
1049 PolarToSurf
1050
1051 Converts a two dimensional polar coordinate on the viewer's sphere
1052 to a two dimensional surface point
1053
1054 p polar coordinate (alpha,delta)
1055 sp TransAngleMatrix where the surface is the body,
1056 and the polar is the frame
1057
1058 s surface coordinate (x,y)
1059**************************************************************************/
1060void PolarToSurf(Point2d *p,TransAngleMatrix *sp,Point2d *s)
1061{
1062 Point3d a,b,c;
1063 double d;
1064
1065 a.x=cos(p->y)*cos(p->x);
1066 a.y=cos(p->y)*sin(p->x);
1067 a.z=sin(p->y);
1068 FrameToBodyTAM(&a,sp,&b);
1069 c.x=0.;
1070 c.y=0.;
1071 c.z=0.;
1072 FrameToBodyTAM(&c,sp,&a);
1073 s->x=(b.z*a.x-a.z*b.x)/(b.z-a.z);
1074 s->y=(b.z*a.y-a.z*b.y)/(b.z-a.z);
1075}
1076
1077/**************************************************************************
1078 CrossClipSide
1079
1080 Tests whether an arc given by 2 points in plolar coordinate
1081 intersects a side of the clipping rect, returnes if the arc
1082 lies outside of the clipping arc, if the arc could be inside
1083 of the clipping arc, or if the arc intersects the clipping arc.
1084 In the lattermost case, polar coordinate of the intersecting
1085 point will be returned as the last parameter.
1086
1087 frame coordinate system where a side of the clipping rect
1088 lies on the x-y plane, and x-axis corresponds to the
1089 start point of the side
1090 arc angle between the starting point and the end point
1091 of the arc corresponding to the clipping side
1092 p1 starting point the arc to be tested
1093 p2 end point the arc to be tested
1094 p crossing point
1095
1096 returned int 0 - could be inside of the clipping rect
1097 1 - intersect (p1 out, p2 in)
1098 2 - intersect (p1 in, p2 out)
1099 3 - outside
1100
1101**************************************************************************/
1102int CrossClipSide(AngleMatrix *frame,double arc,Point2d *p1,
1103 Point2d *p2,Point2d *p,double *ang)
1104{
1105 Point2d p3,p4,p8;
1106 Point3d p5,p6,p7;
1107
1108 FrameToBodyAMP(p1,frame,&p3);
1109 FrameToBodyAMP(p2,frame,&p4);
1110 if(p3.y>0.&&p4.y>0.) return 3; // outside
1111 if(p3.y<0.&&p4.y<0.) return 0;
1112 ToCartesian3d(&p3,&p5);
1113 ToCartesian3d(&p4,&p6);
1114
1115 VectorProduct(&p5,&p6,&p7);
1116
1117 ToPolar3d(&p7,&p8);
1118 if(p3.y>0.) p8.x-=1.5707963268; // p1 out, p2 in
1119 else p8.x+=1.5707963268;
1120
1121 if(p8.x>6.2831853072) p8.x-=6.2831853072;
1122 if(p8.x<0.) p8.x+=6.2831853072;
1123
1124 if(p8.x>arc) return 0; // could intersect with another side
1125
1126 p8.y=0.;
1127 BodyToFrameAMP(&p8,frame,p);
1128 *ang=p8.x; // angle from the corner
1129 if(p3.y>0.) return 1;
1130 else return 2;
1131}
1132
1133/****************************************************************************
1134 compactFarNear
1135
1136****************************************************************************/
1137/*
1138 void compactFarNear(StartPTE *farNear,int *n)
1139 {
1140 StartPTE a;
1141 int i,j;
1142 bool recursFarNear(StartPTE *farNear,int n,int p1,int p2,int depth);
1143
1144 for(i=0;i<*n;i++){
1145 a=farNear[i];
1146 farNear[i].start=-1;
1147 farNear[i].pte=-1;
1148 if(recursFarNear(farNear,*n,a.start,a.pte,0)){
1149 for(j=i+1;j<*n;j++) farNear[j-1]=farNear[j];
1150 *n--;
1151 }
1152 else farNear[i]=a;
1153 }
1154 }
1155*/
1156
1157
1158
1159/****************************************************************************
1160 inferFarNear
1161
1162 Given a set of far-near relationships, infers the far-near relationship
1163 of a given pair of items
1164
1165 farNear array of far-near relationhips - start:far, pte:near
1166 n number of data in array farNear
1167 p1 the first item
1168 p2 the second item
1169
1170 *near either p1 or p2, which is nearer
1171 returned if false, the relationship cannot be inferred
1172*****************************************************************************/
1173bool inferFarNear(StartPTE *farNear,long n,int p1,int p2,int *near)
1174{
1175
1176 // test if p1 is farther
1177 *near=p2;
1178 if(recursFarNear(farNear,n,p1,p2,0)) return true;
1179 // test if p2 is farther
1180 *near=p1;
1181 if(recursFarNear(farNear,n,p2,p1,0)) return true;
1182 return false;
1183}
1184
1185/****************************************************************************
1186 recursFarNear
1187
1188 finds recursively a path to the destination
1189
1190 farNear array of far-near relationhips - start:far, pte:near
1191 n number of data in array farNear
1192 p1 the first item
1193 p2 the second item
1194 depth depth of recursion - detect a loop condition and abort
1195
1196 returned if false, the relationship cannot be inferred
1197*****************************************************************************/
1198bool recursFarNear(StartPTE *farNear,long n,int p1,int p2,long depth)
1199{
1200 long i;
1201
1202 depth++;
1203 if(depth>n) return false; // escape loop
1204
1205 for(i=0;i<n;i++){
1206 if(farNear[i].start==p1){
1207 if(farNear[i].pte==p2) return true;
1208 if(recursFarNear(farNear,n,farNear[i].pte,p2,depth)) return true;
1209 }
1210 }
1211 return false;
1212}
1213
1214/****************************************************************************
1215 Overlap
1216
1217 tests if the given two surfaces are seen overlapped
1218
1219 p point array
1220 s1 indeces of points for a surface 1
1221 s2 indices of points for a surface 2
1222
1223 p0 typical overlapped point
1224
1225 returned if false, the surfaces are disjoint
1226*****************************************************************************/
1227bool Overlap(Point2d *p,StartPTE *s1,StartPTE *s2,Point2d *p0)
1228{
1229 Point2d s1max,s1min;
1230 Point2d s2max,s2min;
1231 int i,j;
1232 double x1,y1,x2,y2,x3,y3,x4,y4;
1233 double a,b,d;
1234 double l1,l2;
1235
1236 // for(i=s1->start;i<s1->pte;i++){
1237 // sprintf(msg,"Overlap s1 i= %3i p[i].x=%8.3f p[i].y=%8.3f\n",i,p[i].x,p[i].y);
1238 // outstr(msg);
1239 //}
1240 //flush();
1241 //for(i=s2->start;i<s2->pte;i++){
1242 // sprintf(msg,"Overlap s2 i= %3i p[i].x=%8.3f p[i].y=%8.3f\n",i,p[i].x,p[i].y);
1243 // outstr(msg);
1244 //}
1245 //flush();
1246
1247 s1max.x=-10000.;
1248 s1max.y=-10000.;
1249 s1min.x=10000.;
1250 s1min.y=10000.;
1251
1252 for(i=s1->start;i<s1->pte;i++){
1253 if(p[i].x>s1max.x) s1max.x=p[i].x;
1254 if(p[i].y>s1max.y) s1max.y=p[i].y;
1255 if(p[i].x<s1min.x) s1min.x=p[i].x;
1256 if(p[i].y<s1min.y) s1min.y=p[i].y;
1257 }
1258
1259 s2max.x=-10000.;
1260 s2max.y=-10000.;
1261 s2min.x=10000.;
1262 s2min.y=10000.;
1263
1264 for(i=s2->start;i<s2->pte;i++){
1265 if(p[i].x>s2max.x) s2max.x=p[i].x;
1266 if(p[i].y>s2max.y) s2max.y=p[i].y;
1267 if(p[i].x<s2min.x) s2min.x=p[i].x;
1268 if(p[i].y<s2min.y) s2min.y=p[i].y;
1269 }
1270
1271 if(s1max.x<s2min.x) return false;
1272 if(s2max.x<s1min.x) return false;
1273 if(s1max.y<s2min.y) return false;
1274 if(s2max.y<s1min.y) return false;
1275
1276 for(i=s1->start;i<s1->pte;i++){
1277 x1=p[i].x;
1278 y1=p[i].y;
1279 if(i==s1->pte-1){
1280 if(s1->pte-s1->start==2) break;
1281 x2=p[s1->start].x;
1282 y2=p[s1->start].y;
1283 }
1284 else{
1285 x2=p[i+1].x;
1286 y2=p[i+1].y;
1287 }
1288 l1=(x2-x1)*(x2-x1)+(y2-y1)*(y2-y1); // approximate l1*l2 by this
1289 for(j=s2->start;j<s2->pte;j++){
1290 x3=p[j].x;
1291 y3=p[j].y;
1292 if(j==s2->pte-1){
1293 if(s2->pte-s2->start==2) break;
1294 x4=p[s2->start].x;
1295 y4=p[s2->start].y;
1296 }
1297 else{
1298 x4=p[j+1].x;
1299 y4=p[j+1].y;
1300 }
1301 // detect simply disjoint cases
1302 while(1){
1303 if(x1<=x2){
1304 if(x3<=x4){
1305 if(x2<x3) break;
1306 if(x4<x1) break;
1307 }
1308 else{
1309 if(x2<x4) break;
1310 if(x3<x1) break;
1311 }
1312 }
1313 else{
1314 if(x3<=x4){
1315 if(x1<x3) break;
1316 if(x4<x2) break;
1317 }
1318 else{
1319 if(x1<x4) break;
1320 if(x3<x2) break;
1321 }
1322 }
1323 if(y1<=y2){
1324 if(y3<=y4){
1325 if(y2<y3) break;
1326 if(y4<y1) break;
1327 }
1328 else{
1329 if(y2<y4) break;
1330 if(y3<y1) break;
1331 }
1332 }
1333 else{
1334 if(y3<=y4){
1335 if(y1<y3) break;
1336 if(y4<y2) break;
1337 }
1338 else{
1339 if(y1<y4) break;
1340 if(y3<y2) break;
1341 }
1342 }
1343 // sprintf(msg,"Overlap x1=%8.3f y1=%8.3f x2=%8.3f y2=%8.3f x3=%8.3f y3=%8.3f x4=%8.3f y4=%8.3f \n",x1,y1,x2,y2,x3,y3,x4,y4);
1344 // outstr(msg);
1345 // flush();
1346 d=(x2-x1)*(y3-y4)-(x3-x4)*(y2-y1);
1347 if(fabs(d/l1)<0.001) break;
1348 a=((x3-x1)*(y3-y4)-(x3-x4)*(y3-y1))/d;
1349 if(a<-0.01||a>1.01) break;
1350 b=((x2-x1)*(y3-y1)-(x3-x1)*(y2-y1))/d;
1351 if(b<-0.01||b>1.01) break;
1352 if(a<0.01&&b<0.01) break;
1353 if(a<0.01&&b>0.99) break;
1354 if(a>0.99&&b<0.01) break;
1355 if(a>0.99&&b>0.99) break;
1356 p0->x=x1+(x2-x1)*a;
1357 p0->y=y1+(y2-y1)*a;
1358 return true;
1359 }
1360 }
1361 }
1362 // test which bounding rect encloses the other bounding rect
1363 if((s1max.x-s1min.x)<=(s2max.x-s2min.x)&&
1364 (s1max.y-s1min.y)<=(s2max.y-s2min.y)){
1365 i=s1->start;
1366 x1=0.5*(p[i].x+p[i+2].x);
1367 y1=0.5*(p[i].y+p[i+2].y);
1368 d=0.;
1369 for(j=s2->start;j<s2->pte;j++){
1370 x3=p[j].x;
1371 y3=p[j].y;
1372 if(j==s2->pte-1){
1373 if(s2->pte-s2->start==2) break;
1374 x4=p[s2->start].x;
1375 y4=p[s2->start].y;
1376 }
1377 else{
1378 x4=p[j+1].x;
1379 y4=p[j+1].y;
1380 }
1381 d+=ArgDif(x1,y1,x3,y3,x4,y4);
1382 }
1383 if(d>6.){
1384 p0->x=x1;
1385 p0->y=y1;
1386 return true;
1387 }
1388
1389 }
1390
1391 if((s1max.x-s1min.x)>=(s2max.x-s2min.x)&&
1392 (s1max.y-s1min.y)>=(s2max.y-s2min.y)){
1393 i=s2->start;
1394 x1=0.5*(p[i].x+p[i+2].x);
1395 y1=0.5*(p[i].y+p[i+2].y);
1396 d=0.;
1397 for(j=s1->start;j<s1->pte;j++){
1398 x3=p[j].x;
1399 y3=p[j].y;
1400 if(j==s1->pte-1){
1401 if(s1->pte-s1->start==2) break;
1402 x4=p[s1->start].x;
1403 y4=p[s1->start].y;
1404 }
1405 else{
1406 x4=p[j+1].x;
1407 y4=p[j+1].y;
1408 }
1409 d+=ArgDif(x1,y1,x3,y3,x4,y4);
1410 }
1411 if(d>6.){
1412 p0->x=x1;
1413 p0->y=y1;
1414 return true;
1415 }
1416 }
1417 return false;
1418}
1419
1420/****************************************************************************
1421 FarNearDist
1422
1423 distance separating surface1 and surface2 for a given direction of view
1424
1425 s1 coordinate conversion (surface:body view:frame)
1426 s2 coordinate conversion (surface:body view:frame)
1427 p polar coordinate of the line of sight
1428
1429 d distance to s1 minus distance to s2
1430*****************************************************************************/
1431void FarNearDist(TransAngleMatrix *s1,TransAngleMatrix *s2,Point2d *p,double *d)
1432{
1433 Point2d srf1,srf2;
1434 Point3d a;
1435
1436 PolarToSurf(p,s1,&srf1);
1437 PolarToSurf(p,s2,&srf2);
1438
1439 a.x=s1->x+s1->c[0][0]*srf1.x+s1->c[0][1]*srf1.y;
1440 a.y=s1->y+s1->c[1][0]*srf1.x+s1->c[1][1]*srf1.y;
1441 a.z=s1->z+s1->c[2][0]*srf1.x+s1->c[2][1]*srf1.y;
1442
1443 *d=sqrt(a.x*a.x+a.y*a.y+a.z*a.z);
1444
1445 a.x=s2->x+s2->c[0][0]*srf2.x+s2->c[0][1]*srf2.y;
1446 a.y=s2->y+s2->c[1][0]*srf2.x+s2->c[1][1]*srf2.y;
1447 a.z=s2->z+s2->c[2][0]*srf2.x+s2->c[2][1]*srf2.y;
1448
1449 *d-=sqrt(a.x*a.x+a.y*a.y+a.z*a.z);
1450
1451}
1452
1453/****************************************************************************
1454 PolarToGnomo
1455
1456 Polar to Gnomonic Conversion
1457
1458 p polar coordinate (x:aximuth, y:elevation)
1459 g gnomonic coordinate (x:toward polar y, y:toward polar x)
1460*****************************************************************************/
1461void PolarToGnomo(Point2d *p,Point2d *g)
1462{
1463 double r;
1464
1465 r=tan(1.5707963267-p->y);
1466 g->x=r*sin(p->x);
1467 g->y=r*cos(p->x);
1468}
1469
1470
1471/****************************************************************************
1472 GnomoToPolar
1473
1474 Gnomonic to Polar Conversion
1475
1476 g gnomonic coordinate (x:toward polar y, y:toward polar x)
1477 p polar coordinate (x:aximuth, y:elevation)
1478*****************************************************************************/
1479void GnomoToPolar(Point2d *g,Point2d *p)
1480{
1481 double r;
1482
1483 r=sqrt(g->x*g->x+g->y*g->y);
1484 p->y=1.5707963267-atan(r);
1485 p->x=atan2(g->x,g->y);
1486}
1487
1488
1489/****************************************************************************
1490 ArgDif
1491
1492 argument difference between two vectors
1493
1494 (x0,y0) origin
1495 (x1,y1) vector1 tip
1496 (x2,y2) vector2 tip
1497
1498 returned argument of vector2 - argument of vector1
1499*****************************************************************************/
1500double ArgDif(double x0,double y0,double x1,double y1,double x2,double y2)
1501{
1502 double ang;
1503
1504 ang=atan2(y2-y0,x2-x0)-atan2(y1-y0,x1-x0);
1505 if(fabs(ang)<3.141592) return ang;
1506 if(ang<0.) ang+=6.283185;
1507 else ang-=6.283185;
1508 return ang;
1509}
1510
1511
1512/****************************************************************************
1513 SameSideOfSurface
1514
1515 Return true if the Two Points are in the Same Side of a Surface
1516
1517 s Surface
1518 p1 point 1
1519 p2 point 2
1520
1521 returned true if point 1 and 2 are in the same side of s
1522*****************************************************************************/
1523bool SameSideOfSurface(Surface *s,Point3d *p1,Point3d *p2)
1524{
1525 Point3d z,d1,d2;
1526
1527 z.x=sin(s->theta0)*sin(s->phi0); // axis normal to the plane
1528 z.y=-sin(s->theta0)*cos(s->phi0);
1529 z.z=cos(s->theta0);
1530 d1.x=p1->x-s->x0;
1531 d1.y=p1->y-s->y0;
1532 d1.z=p1->z-s->z0;
1533 d2.x=p2->x-s->x0;
1534 d2.y=p2->y-s->y0;
1535 d2.z=p2->z-s->z0;
1536 if((d1.x*z.x+d1.y*z.y+d1.z*z.z)*(d2.x*z.x+d2.y*z.y+d2.z*z.z)>0.)
1537 return true;
1538 else return false;
1539}
1540
1541
1542
1543/******************************************************************************
1544 CMPpart
1545
1546 part of the concept
1547******************************************************************************/
1548void CMPpart::setCood(double x,double y,double z,double theta,double phi,double psi)
1549{
1550 x0=x; /* local coordinate referred to */
1551 y0=y; /* refPart */
1552 z0=z;
1553 theta0=theta;
1554 phi0=phi;
1555 psi0=psi;
1556}
1557
1558void CMPpart::setColor(int i)
1559{
1560 color=i;
1561}
1562
1563void CMPpart::hide()
1564{
1565 showDrawing=false;
1566}
1567
1568
1569/******************************************************************************
1570 CMPpoint
1571
1572 part of the concept - point
1573******************************************************************************/
1574
1575
1576/******************************************************************************
1577 CMPlines
1578
1579 part of the concept - lines
1580******************************************************************************/
1581
1582
1583void CMPlines::SetPoint(double x,double y,double z)
1584{
1585 Point3d p;
1586 p.x=x;
1587 p.y=y;
1588 p.z=z;
1589 pt.push_back(p);
1590 npoint++;
1591}
1592
1593
1594/******************************************************************************
1595 GetSurface
1596
1597 Return a line segment
1598
1599******************************************************************************/
1600
1601void CMPlines::GetSurface(int m,TransAngleMatrix *po,Surface *surf,Point2d *surfPt)
1602{
1603 double x1,y1,z1,x2,y2,z2;
1604 TransAngleEuler tae1;
1605 TransAngleMatrix tam1;
1606 TransAngleEuler tae2;
1607 TransAngleEuler tae;
1608 Point3d s;
1609 Point3d e;
1610 Point3d f;
1611 Point3d g;
1612 double length,d;
1613 Point2d a;
1614
1615 s=pt[m];
1616 e.x=pt[m+1].x-s.x;
1617 e.y=pt[m+1].y-s.y;
1618 e.z=pt[m+1].z-s.z;
1619
1620 if(e.x==0.&&e.y==0.&&e.z==0.){
1621 surf->start=-1;
1622 surf->pte=-1;
1623 return;
1624 }
1625
1626 tae1.x=xg; // Translate Angle Euler for the part
1627 tae1.y=yg;
1628 tae1.z=zg;
1629 tae1.theta=thetag;
1630 tae1.phi=phig;
1631 tae1.psi=psig;
1632 TAEtoTAM(&tae1,&tam1);
1633 FrameToBodyTAM((Point3d *)po,&tam1,&g); // viewer by part body coordinate
1634 g.x-=s.x; // direction to the viewer
1635 g.y-=s.y;
1636 g.z-=s.z;
1637 NormPoint3d(&g,&g,&d);
1638 tam1.x=s.x; // Translate Angle Matrix for the line segment
1639 tam1.y=s.y;
1640 tam1.z=s.z;
1641 NormPoint3d(&e,&f,&length); // normalized y-axis
1642 if(fabs(f.x*g.x+f.y*g.y+f.z*g.z)>0.99996){ // if the line is directed to viewer
1643 ToPolar3d(&g,&a); // viewer's direction
1644 a.y-=0.0174532; // Offset 1 deg from the direction to the viewer
1645 f.x=cos(a.y)*cos(a.x); // updated line direction to avoid anomaly
1646 f.y=cos(a.y)*sin(a.x);
1647 f.z=sin(a.y);
1648 }
1649 tam1.c[0][1]=f.x;
1650 tam1.c[1][1]=f.y;
1651 tam1.c[2][1]=f.z;
1652 VectorProduct(&f,&g,&s); // direction to x-axis
1653 NormPoint3d(&s,&s,&d);
1654 tam1.c[0][0]=s.x;
1655 tam1.c[1][0]=s.y;
1656 tam1.c[2][0]=s.z;
1657 VectorProduct(&s,&f,&g); // direction to z-axis
1658 tam1.c[0][2]=g.x;
1659 tam1.c[1][2]=g.y;
1660 tam1.c[2][2]=g.z;
1661 TAMtoTAE(&tam1,&tae2);
1662 MoveTwiceTAE(&tae1,&tae2,&tae);
1663 surf->x0=tae.x;
1664 surf->y0=tae.y;
1665 surf->z0=tae.z;
1666 surf->theta0=tae.theta;
1667 surf->phi0=tae.phi;
1668 surf->psi0=tae.psi;
1669 surf->start=0;
1670 surf->pte=2;
1671 surfPt[0].x=0.;
1672 surfPt[0].y=0.;
1673 surfPt[1].x=0.;
1674 surfPt[1].y=length;
1675}
1676
1677/******************************************************************************
1678 GetDrawData
1679
1680 Return data for drawing
1681
1682 m line segment number (0-)
1683 po thePolar
1684
1685 seg array of line segments (clockwise angle from top, angle from line of sight)
1686 blk indices of segs which are to be drawn with lines
1687 cont indices of closed line segments. Inside of the closure
1688 will be painted by a color.
1689
1690******************************************************************************/
1691
1692void CMPlines::GetDrawData(int m,TransAngleMatrix *po,
1693 Line2d *seg,StartPTE *blk,StartPTE *cont)
1694{
1695 TransAngleEuler tae;
1696 TransAngleMatrix tam;
1697 Point3d s;
1698 Point3d e;
1699 Point3d f;
1700 double r;
1701 Point2d a;
1702
1703 s=pt[m];
1704 e=pt[m+1];
1705
1706 tae.x=xg; // Translate Angle Euler for the part
1707 tae.y=yg;
1708 tae.z=zg;
1709 tae.theta=thetag;
1710 tae.phi=phig;
1711 tae.psi=psig;
1712 TAEtoTAM(&tae,&tam);
1713 BodyToFrameTAM(&s,&tam,&f);
1714 FrameToBodyTAM(&f,po,&s);
1715 NormPoint3d(&s,&f,&r);
1716 ToPolar3d(&f,&a);
1717 a.y=1.5707963267-a.y;
1718 seg[0].sx=a.x;
1719 seg[0].sy=a.y;
1720 BodyToFrameTAM(&e,&tam,&f);
1721 FrameToBodyTAM(&f,po,&e);
1722 NormPoint3d(&e,&f,&r);
1723 ToPolar3d(&f,&a);
1724 a.y=1.5707963267-a.y;
1725 seg[0].ex=a.x;
1726 seg[0].ey=a.y;
1727 blk->start=0;
1728 blk->pte=1;
1729 cont->start=0;
1730 cont->pte=1;
1731
1732}
1733
1734
1735/******************************************************************************
1736 FieldOfView
1737
1738 Get horizontal and vertical field of view to cover the object
1739******************************************************************************/
1740
1741void CMPlines::FieldOfView(TransAngleMatrix *vu,Point2d *az,Point2d *el)
1742{
1743 TransAngleEuler tae;
1744 TransAngleMatrix tam;
1745 int i;
1746 Point3d p;
1747 Point3d s;
1748 Point3d f;
1749 Point2d a;
1750 double d;
1751
1752 tae.x=xg; // Translate Angle Euler for the part
1753 tae.y=yg;
1754 tae.z=zg;
1755 tae.theta=thetag;
1756 tae.phi=phig;
1757 tae.psi=psig;
1758 TAEtoTAM(&tae,&tam);
1759 az->x=10.;
1760 az->y=-10.;
1761 el->x=10.;
1762 el->y=-10.;
1763 for(i=0;i<npoint;i++){
1764 s=pt[i];
1765 BodyToFrameTAM(&s,&tam,&f);
1766 FrameToBodyTAM(&f,vu,&s);
1767 NormPoint3d(&s,&s,&d);
1768 ToPolar3d(&s,&a);
1769 if(a.x<az->x) az->x=a.x;
1770 if(a.x>az->y) az->y=a.x;
1771 if(a.y<el->x) el->x=a.y;
1772 if(a.y>el->y) el->y=a.y;
1773 }
1774}
1775
1776/******************************************************************************
1777 CMPplane
1778
1779 part of the concept - plane
1780******************************************************************************/
1781
1782void CMPplane::SetPoint(double x,double y)
1783{
1784 Point2d p;
1785 p.x=x;
1786 p.y=y;
1787 pt.push_back(p);
1788 npoint++;
1789}
1790
1791/******************************************************************************
1792 GetSurface
1793
1794 Return the surface data
1795
1796 The surface is assumed to be given by points that are arranged to
1797 move clock-wise looking toward the z vector.
1798
1799 Both sides of the surface are assumed to be visible. Therefore,
1800 the order of the point array is reversed, when the viewer's eye
1801 is in the -z side.
1802
1803******************************************************************************/
1804
1805void CMPplane::GetSurface(TransAngleMatrix *po,Surface *surf,Point2d *surfPt)
1806{
1807 TransAngleMatrix tam;
1808 Point3d d;
1809 int i,j;
1810 int np;
1811 double x1,y1;
1812
1813 surf->x0=xg;
1814 surf->y0=yg;
1815 surf->z0=zg;
1816 surf->theta0=thetag;
1817 surf->phi0=phig;
1818 surf->psi0=psig;
1819 surf->start=0;
1820 if(npoint==0){
1821 surf->pte=0;
1822 return;
1823 }
1824 TAEtoTAM((TransAngleEuler *)surf,&tam);
1825 FrameToBodyTAM((Point3d *)po,&tam,&d); // if d>0 +z axis is facing the viewer
1826 np=0; // number of effective points
1827 if(d.z>0.){
1828 x1=pt[npoint-1].x;
1829 y1=pt[npoint-1].y;
1830 }
1831 else{
1832 x1=pt[0].x;
1833 y1=pt[0].y;
1834 }
1835 for(i=0;i<npoint;i++){
1836 if(d.z>0.) j=i;
1837 else j=npoint-i-1;
1838 surfPt[np].x=pt[j].x;
1839 surfPt[np].y=pt[j].y;
1840 if(x1!=surfPt[np].x||y1!=surfPt[np].y){
1841 x1=surfPt[np].x;
1842 y1=surfPt[np].y;
1843 if(np<47) np++; // taking into some allowance to max 50
1844 }
1845 }
1846 surf->pte=np;
1847}
1848
1849
1850/******************************************************************************
1851 GetDrawData
1852
1853 Return data for drawing
1854
1855 po thePolar
1856
1857 seg array of line segments (clockwise angle from top, angle from line of sight)
1858 blk indices of segs which are to be drawn with lines
1859 cont indices of closed line segments. Inside of the closure
1860 will be painted by a color.
1861
1862******************************************************************************/
1863
1864void CMPplane::GetDrawData(TransAngleMatrix *po,Line2d *seg,StartPTE *blk,StartPTE *cont)
1865{
1866 StartPTE blk1;
1867 StartPTE cont1;
1868 int i,j;
1869 double sx1,sy1,ex1,ey1;
1870 CMPplane *next;
1871
1872 cont->start=0;
1873 if(npoint==0){
1874 cont->pte=0;
1875 blk->start=0;
1876 blk->pte=0;
1877 return;
1878 }
1879 GetDrawData1(po,seg,blk,cont);
1880 if(otherHalf){
1881 bool face1,face2;
1882 face1=FacingViewer((Point3d *)po);
1883 next=otherHalf;
1884 while(next!=this){
1885 cont1.start=blk->pte;
1886 next->GetDrawData1(po,seg,&blk1,&cont1);
1887 face2=next->FacingViewer((Point3d *)po);
1888 for(i=blk->start;i<cont->pte;i++){
1889 if(face1==face2){
1890 sx1=seg[i].sx;
1891 sy1=seg[i].sy;
1892 ex1=seg[i].ex;
1893 ey1=seg[i].ey;
1894 }
1895 else{
1896 sx1=seg[i].ex;
1897 sy1=seg[i].ey;
1898 ex1=seg[i].sx;
1899 ey1=seg[i].sy;
1900 }
1901 for(j=cont1.start;j<cont1.pte;j++){
1902 double d0,d1,d2,d3;
1903 d0=seg[j].sx-ex1;
1904 d1=seg[j].sy-ey1;
1905 d2=seg[j].ex-sx1;
1906 d3=seg[j].ey-sy1;
1907 if(d0*d0+d1*d1+d2*d2+d3*d3<0.000001) break;
1908 // if(seg[j].sx==ex1&&seg[j].sy==ey1&&seg[j].ex==sx1&&seg[j].ey==sy1)
1909 // break;
1910 }
1911 if(j!=cont1.pte){
1912 for(j=i;j>0;j--) seg[j]=seg[j-1];
1913 seg[0].sx=sx1;
1914 seg[0].sy=sy1;
1915 seg[0].ex=ex1;
1916 seg[0].ey=ey1;
1917 blk->start++;
1918 }
1919 }
1920 next=next->otherHalf;
1921 }
1922 }
1923
1924
1925}
1926
1927void CMPplane::GetDrawData1(TransAngleMatrix *po,Line2d *seg,StartPTE *blk,StartPTE *cont)
1928{
1929 TransAngleEuler tae;
1930 TransAngleMatrix tam;
1931 Point3d a,b,c,d;
1932 Point2d e,f,g;
1933 double r;
1934 int i,j,k;
1935
1936 k=cont->start;
1937 tae.x=xg; // Translate Angle Euler for the part
1938 tae.y=yg;
1939 tae.z=zg;
1940 tae.theta=thetag;
1941 tae.phi=phig;
1942 tae.psi=psig;
1943 TAEtoTAM(&tae,&tam);
1944 a.x=pt[0].x;
1945 a.y=pt[0].y;
1946 a.z=0.;
1947 BodyToFrameTAM(&a,&tam,&b);
1948 FrameToBodyTAM(&b,po,&c);
1949 NormPoint3d(&c,&d,&r);
1950 ToPolar3d(&d,&e);
1951 e.y=1.5707963267-e.y;
1952 f=e;
1953 for(i=1;i<=npoint;i++){
1954 g=f;
1955 if(i<npoint){
1956 a.x=pt[i].x;
1957 a.y=pt[i].y;
1958 a.z=0.;
1959 BodyToFrameTAM(&a,&tam,&b);
1960 FrameToBodyTAM(&b,po,&c);
1961 NormPoint3d(&c,&d,&r);
1962 ToPolar3d(&d,&f);
1963 f.y=1.5707963267-f.y;
1964 }
1965 else f=e;
1966 seg[k].sx=g.x;
1967 seg[k].sy=g.y;
1968 seg[k].ex=f.x;
1969 seg[k].ey=f.y;
1970 k++;
1971
1972 }
1973 blk->start=cont->start;
1974 blk->pte=blk->start+npoint;
1975 cont->pte=cont->start+npoint;
1976}
1977
1978/******************************************************************************
1979 FieldOfView
1980
1981 Get horizontal and vertical field of view to cover the object
1982******************************************************************************/
1983
1984void CMPplane::FieldOfView(TransAngleMatrix *vu,Point2d *az,Point2d *el)
1985{
1986 TransAngleEuler tae;
1987 TransAngleMatrix tam;
1988 int i;
1989 Point2d p;
1990 Point3d s;
1991 Point3d f;
1992 Point2d a;
1993 double d;
1994
1995 tae.x=xg; // Translate Angle Euler for the part
1996 tae.y=yg;
1997 tae.z=zg;
1998 tae.theta=thetag;
1999 tae.phi=phig;
2000 tae.psi=psig;
2001 TAEtoTAM(&tae,&tam);
2002 az->x=10.;
2003 az->y=-10.;
2004 el->x=10.;
2005 el->y=-10.;
2006 for(i=0;i<npoint;i++){
2007 s.x=pt[i].x;
2008 s.y=pt[i].y;
2009 s.z=0.;
2010 BodyToFrameTAM(&s,&tam,&f);
2011 FrameToBodyTAM(&f,vu,&s);
2012 NormPoint3d(&s,&s,&d);
2013 ToPolar3d(&s,&a);
2014 if(a.x<az->x) az->x=a.x;
2015 if(a.x>az->y) az->y=a.x;
2016 if(a.y<el->x) el->x=a.y;
2017 if(a.y>el->y) el->y=a.y;
2018 }
2019}
2020
2021/*****************************************************************
2022 Return if the surface is facing the viewer
2023
2024 po thePolar casted to Point3d
2025*****************************************************************/
2026bool CMPplane::FacingViewer(Point3d *po)
2027{
2028 double nx,ny,nz;
2029
2030 ny=sin(thetag);
2031 nx=ny*cos(phig-1.5707963268);
2032 ny*=sin(phig-1.5707963268);
2033 nz=cos(thetag);
2034 return (po->x-xg)*nx+(po->y-yg)*ny+(po->z-zg)*nz>0.;
2035}
2036
2037/******************************************************************************
2038 CMPbox
2039
2040******************************************************************************/
2041
2042void CMPbox::SetSize(double x,double y,double z)
2043{
2044 lx=x;
2045 ly=y;
2046 lz=z;
2047}
2048
2049
2050/******************************************************************************
2051 ClearVisSurface
2052
2053 Clear 'visibleSurface' instance variable so that new visibility
2054 from a new viewpoint can be obtained in GetSurface
2055******************************************************************************/
2056void CMPbox::ClearVisSurface(void)
2057{
2058 visibleSurface=0;
2059}
2060
2061/******************************************************************************
2062 GetSurface
2063
2064 Return a surface
2065
2066 Six surfaces are related to an index number in the following way
2067
2068 index face toward...
2069 0 +x
2070 1 +y
2071 2 +z
2072 3 -x
2073 4 -y
2074 5 -z
2075
2076
2077 The instance variable visibleSurface is used to map between
2078 the above index and the reference index passed as the first parameter.
2079
2080 The returned bool value is false if no more visible surface exists
2081 for the asked reference number.
2082******************************************************************************/
2083
2084bool CMPbox::GetSurface(int n,TransAngleMatrix *po,Surface *surf,Point2d *surfPt)
2085{
2086 double x1,y1,z1,x2,y2;
2087 TransAngleEuler tae1;
2088 TransAngleMatrix tam1;
2089 TransAngleEuler tae2;
2090 TransAngleEuler tae;
2091 Point3d p;
2092 Point3d s;
2093 Point3d e;
2094 Point3d f;
2095 double length;
2096 Point2d a;
2097 int i,j,m;
2098
2099 x1=0.5*lx;
2100 y1=0.5*ly;
2101 z1=0.5*lz;
2102 tae1.x=xg; // Translate Angle Euler for the part
2103 tae1.y=yg;
2104 tae1.z=zg;
2105 tae1.theta=thetag;
2106 tae1.phi=phig;
2107 tae1.psi=psig;
2108
2109 if(visibleSurface==0){
2110 TAEtoTAM(&tae1,&tam1);
2111 j=1;
2112 for(i=0;i<6;i++){
2113 s.x=((i>2)? -1.:1.)*((i%3==0)? x1:0.);
2114 s.y=((i>2)? -1.:1.)*((i%3==1)? y1:0.);
2115 s.z=((i>2)? -1.:1.)*((i%3==2)? z1:0.);
2116 BodyToFrameTAM(&s,&tam1,&e);
2117 if((xg-e.x)*(po->x-e.x)+(yg-e.y)*(po->y-e.y)
2118 +(zg-e.z)*(po->z-e.z)<0.) {
2119 // DebugStr("\psurface");
2120 visibleSurface|=j;
2121 }
2122 j=j<<1;
2123 }
2124 }
2125 j=1;
2126 i=0;
2127 for(m=0;m<6;m++){
2128 if(visibleSurface&j){
2129 if(i==n) break;
2130 i++;
2131 }
2132 j=j<<1;
2133 }
2134 if(m==6) return false;
2135 switch (m){
2136 case 0: // +x
2137 tae2.x=x1; // Translate Angle Euler for the side
2138 tae2.y=0.;
2139 tae2.z=0.;
2140 tae2.theta=1.5707963268; // rotate 90 deg around +y axis
2141 tae2.phi=1.5707963268;
2142 tae2.psi=-1.5707963268;
2143 x2=z1;
2144 y2=y1;
2145 break;
2146 case 1: // +y
2147 tae2.x=0.; // Translate Angle Euler for the side
2148 tae2.y=y1;
2149 tae2.z=0.;
2150 tae2.theta=1.5707963268; // rotate 90 deg around -x axis
2151 tae2.phi=3.141592653589;
2152 tae2.psi=3.141592653589;
2153 x2=x1;
2154 y2=z1;
2155 break;
2156 case 2: // +z
2157 tae2.x=0.; // Translate Angle Euler for the side
2158 tae2.y=0.;
2159 tae2.z=z1;
2160 tae2.theta=0.; // no rotation
2161 tae2.phi=0.;
2162 tae2.psi=0.;
2163 x2=x1;
2164 y2=y1;
2165 break;
2166 case 3: // -x
2167 tae2.x=-x1; // Translate Angle Euler for the side
2168 tae2.y=0.;
2169 tae2.z=0.;
2170 tae2.theta=1.5707963268; // rotate 90 deg around -y axis
2171 tae2.phi=-1.5707963268;
2172 tae2.psi=1.5707963268;
2173 x2=z1;
2174 y2=y1;
2175 break;
2176 case 4: // -y
2177 tae2.x=0.; // Translate Angle Euler for the side
2178 tae2.y=-y1;
2179 tae2.z=0.;
2180 tae2.theta=1.5707963268; // rotate 90 deg around -x axis
2181 tae2.phi=0.;
2182 tae2.psi=0.;
2183 x2=x1;
2184 y2=z1;
2185 break;
2186 case 5: // -z
2187 tae2.x=0.; // Translate Angle Euler for the side
2188 tae2.y=0.;
2189 tae2.z=-z1;
2190 tae2.theta=3.1415926536; // no rotation
2191 tae2.phi=0.;
2192 tae2.psi=0.;
2193 x2=x1;
2194 y2=y1;
2195 break;
2196 }
2197 MoveTwiceTAE(&tae1,&tae2,&tae);
2198 surf->x0=tae.x;
2199 surf->y0=tae.y;
2200 surf->z0=tae.z;
2201 surf->theta0=tae.theta;
2202 surf->phi0=tae.phi;
2203 surf->psi0=tae.psi;
2204 surf->start=0;
2205 surf->pte=4;
2206 surfPt[0].x=x2;
2207 surfPt[0].y=y2;
2208 surfPt[1].x=-x2;
2209 surfPt[1].y=y2;
2210 surfPt[2].x=-x2;
2211 surfPt[2].y=-y2;
2212 surfPt[3].x=x2;
2213 surfPt[3].y=-y2;
2214 return true;
2215}
2216
2217/******************************************************************************
2218 GetCenter
2219
2220 Return the center point in frame coordinate
2221
2222******************************************************************************/
2223
2224void CMPbox::GetCenter(Point3d *p)
2225{
2226 p->x=xg;
2227 p->y=yg;
2228 p->z=zg;
2229}
2230
2231/******************************************************************************
2232 GetDrawData
2233
2234 Return data for drawing
2235
2236 po thePolar
2237
2238 seg array of line segments (clockwise angle from top, angle from line of sight)
2239 blk indices of segs which are to be drawn with lines
2240 cont indices of closed line segments. Inside of the closure
2241 will be painted by a color.
2242
2243******************************************************************************/
2244
2245void CMPbox::GetDrawData(TransAngleMatrix *po,Line2d *seg,StartPTE *blk,StartPTE *cont)
2246{
2247 double x,y,z,x1,y1,z1,x2,y2,z2,dx,dy,dz;
2248 int isen[3][10];
2249 int i,j,k,l,m,n,nsen,n1,brnch,lin,line;
2250 TransAngleEuler tae1;
2251 TransAngleMatrix tam1;
2252 TransAngleEuler tae2;
2253 TransAngleEuler tae;
2254 Point3d apex;
2255 Point3d ten[8]; /* coordinate of 8 apexes */
2256 Point3d s,d;
2257 double r;
2258 Point2d direc;
2259
2260
2261 /* set conversion parameters */
2262 dx=lx;
2263 dy=ly;
2264 dz=lz;
2265
2266 tae1.x=xg; // Translate Angle Euler for the part
2267 tae1.y=yg;
2268 tae1.z=zg;
2269 tae1.theta=thetag;
2270 tae1.phi=phig;
2271 tae1.psi=psig;
2272 TAEtoTAM(&tae1,&tam1);
2273
2274 /* fill coordinate of the 8 apexes */
2275 /* l=0 ( 0, 0, 0) l=1 (dx, 0, 0) l=2 ( 0,dy, 0) l=3 (dx,dy, 0) */
2276 /* l=4 ( 0, 0,dz) l=5 (dx, 0,dz) l=6 ( 0,dy,dz) l=7 (dx,dy,dz) */
2277 for(i=0;i<2;i++){
2278 apex.z=dz*i-0.5*dz; /* float */
2279 for(j=0;j<2;j++){
2280 apex.y=dy*j-0.5*dy; /* float */
2281 for(k=0;k<2;k++){
2282 apex.x=dx*k-0.5*dx; /* float */
2283 l=4*i+2*j+k; /* l=0-7 */
2284 BodyToFrameTAM(&apex,&tam1,&ten[l]);
2285 }
2286 }
2287 }
2288
2289 /* fill line information */
2290 for(i=0;i<10;i++){
2291 for(j=0;j<3;j++) isen[j][i]=0;
2292 }
2293 nsen=0;
2294 for(i=0;i<3;i++){
2295 for(j=0;j<2;j++){
2296
2297 /***********************************************************
2298 (j=0) (j=1)
2299 (i=0) k=0,l=4,m=2+4=6 0->4->6 k=1,l=3,m=7 1->3->7
2300 (i=1) k=0,l=1,m=4+1=5 0->1->5 k=2,l=6,m=7 2->6->7
2301 (i=2) k=0,l=2,m=1+2=3 0->2->3 k=4,l=5,m=7 4->5->7
2302
2303 surface i-j direction
2304 0-0 -x
2305 0-1 +x
2306 1-0 -y
2307 1-1 +y
2308 2-0 -z
2309 2-1 +z
2310 ***********************************************************/
2311
2312 k=(int)(j*pow(2,i));
2313 l=(int)(k+pow(2,(i+2-j)%3));
2314 m=(int)(k+pow(2,(i+1)%3)+pow(2,(i+2)%3));
2315 x1=ten[l].x-ten[k].x;
2316 y1=ten[l].y-ten[k].y;
2317 z1=ten[l].z-ten[k].z;
2318 x2=ten[m].x-ten[l].x;
2319 y2=ten[m].y-ten[l].y;
2320 z2=ten[m].z-ten[l].z;
2321
2322 if((y1*z2-z1*y2)*(po->x-ten[l].x)+(z1*x2-x1*z2)*(po->y
2323 -ten[l].y)+(x1*y2-y1*x2)*(po->z-ten[l].z)>=0.){
2324
2325 /* if the plane is facing to the observer's eye do : */
2326 m=k; /* initialize present point */
2327 for(n1=0;n1<4;n1++){ // do for four sorrounding points
2328 l=m; /* define previous point */
2329
2330 /* redefine present point */
2331 if(n1==0) m=(int)(k+pow(2,(i+2-j)%3));
2332 if(n1==1) m=(int)(k+pow(2,(i+1)%3)+pow(2,(i+2)%3));
2333 if(n1==2) m=(int)(k+pow(2,(i+j+1)%3));
2334 if(n1==3) m=k;
2335 // now, the side start at point l and ends at point m
2336 if(nsen==0) brnch=1;
2337 else {
2338 brnch=1;
2339 for(n=0;n<nsen;n++){
2340 if(isen[1][n]==l && isen[0][n]==m){ // already there
2341 isen[2][n]=2; /* double line */
2342 brnch=0; // don't add this line
2343 break;
2344 }
2345 }
2346 }
2347 if(brnch==1){ // newly added line
2348 isen[0][nsen]=l; // start point
2349 isen[1][nsen]=m; // end point
2350 isen[2][nsen]=1; // single line
2351 nsen+=1;
2352 }
2353 } /* n1 */
2354 } /* facing this direction */
2355 } /* j */
2356 } /* i */
2357 blk->start=0; /* start line of a block */
2358 cont->start=0; /* start line of a contour */
2359
2360 /* first, collect single lines and place in the first part */
2361 line=0;
2362 for(i=0;i<nsen;i++){
2363 if(isen[2][i]==1){ // single line
2364 for(j=0;j<2;j++){
2365 k=isen[j][i];
2366 s.x=ten[k].x;
2367 s.y=ten[k].y;
2368 s.z=ten[k].z;
2369 FrameToBodyTAM(&s,po,&d);
2370 NormPoint3d(&d,&s,&r);
2371 ToPolar3d(&s,&direc);
2372 if(j==0){
2373 seg[line].sx=direc.x;
2374 seg[line].sy=1.5707963267-direc.y;
2375 }
2376 else{
2377 seg[line].ex=direc.x;
2378 seg[line].ey=1.5707963267-direc.y;
2379 }
2380 }
2381 line+=1;
2382 }
2383 }
2384
2385 cont->pte=line; /* next-to-end line of the contour */
2386
2387 /* then, collect double lines and place in the last part */
2388 for(i=0;i<nsen;i++){
2389 if(isen[2][i]==2){
2390 for(j=0;j<2;j++){
2391 k=isen[j][i];
2392 s.x=ten[k].x;
2393 s.y=ten[k].y;
2394 s.z=ten[k].z;
2395 FrameToBodyTAM(&s,po,&d);
2396 NormPoint3d(&d,&s,&r);
2397 ToPolar3d(&s,&direc);
2398 if(j==0){
2399 seg[line].sx=direc.x;
2400 seg[line].sy=1.5707963267-direc.y;
2401 }
2402 else{
2403 seg[line].ex=direc.x;
2404 seg[line].ey=1.5707963267-direc.y;
2405 }
2406 }
2407 line+=1;
2408 }
2409 }
2410 blk->pte=line; /* next-to-end line of the block */
2411}
2412
2413
2414/******************************************************************************
2415 FieldOfView
2416
2417 Get horizontal and vertical field of view to cover the object
2418******************************************************************************/
2419
2420void CMPbox::FieldOfView(TransAngleMatrix *vu,Point2d *az,Point2d *el)
2421{
2422 double x1,y1,z1,d;
2423 TransAngleEuler tae;
2424 TransAngleMatrix tam;
2425 int i;
2426 Point3d s;
2427 Point3d f;
2428 Point2d a;
2429
2430 x1=0.5*lx;
2431 y1=0.5*ly;
2432 z1=0.5*lz;
2433 tae.x=xg; // Translate Angle Euler for the part
2434 tae.y=yg;
2435 tae.z=zg;
2436 tae.theta=thetag;
2437 tae.phi=phig;
2438 tae.psi=psig;
2439 TAEtoTAM(&tae,&tam);
2440 az->x=10.;
2441 az->y=-10.;
2442 el->x=10.;
2443 el->y=-10.;
2444 for(i=0;i<8;i++){
2445 s.x=i&1? x1:-x1;
2446 s.y=i&2? y1:-y1;
2447 s.z=i&4? z1:-z1;
2448 BodyToFrameTAM(&s,&tam,&f);
2449 FrameToBodyTAM(&f,vu,&s);
2450 NormPoint3d(&s,&s,&d);
2451 ToPolar3d(&s,&a);
2452 if(a.x<az->x) az->x=a.x;
2453 if(a.x>az->y) az->y=a.x;
2454 if(a.y<el->x) el->x=a.y;
2455 if(a.y>el->y) el->y=a.y;
2456 }
2457}
2458
2459
2460/******************************************************************************
2461 CMPsphere - sphere
2462******************************************************************************/
2463
2464void CMPsphere::SetRadius(double x)
2465{
2466 r=x;
2467}
2468
2469
2470/******************************************************************************
2471 GetSurface
2472
2473 Return a line segment
2474
2475******************************************************************************/
2476
2477void CMPsphere::GetSurface(TransAngleMatrix *po,Surface *surf,Point2d *surfPt)
2478{
2479 Line3d eye;
2480 Point3d u;
2481 double d,ra,d1,r1;
2482 Point2d a;
2483 int i;
2484 double ang;
2485
2486 eye.sx=xg; // center of the sphere
2487 eye.sy=yg;
2488 eye.sz=zg;
2489 eye.ex=po->x; // thePolar of CThreeD - position of the viewer's eye
2490 eye.ey=po->y;
2491 eye.ez=po->z;
2492
2493 NormLine3d(&eye,&u,&d);
2494 ToPolar3d(&u,&a);
2495
2496 ra=r; // radius of the sphere
2497 d1=ra*ra/d; // distance to the center of tangent circle from sphere center
2498 r1=sqrt(ra*ra-d1*d1); // radius of the tangent circle
2499 r1*=1.03528; // r1/cos(15deg) to enable establishing the far-near
2500 // relationship with proximate parts.
2501
2502 surf->x0=xg+d1*u.x;
2503 surf->y0=yg+d1*u.y;
2504 surf->z0=zg+d1*u.z;
2505 surf->theta0=1.5707963268-a.y; // z axis toward viewer's eye
2506 surf->phi0=a.x+1.5707963268;
2507 surf->psi0=0.;
2508 surf->start=0;
2509 surf->pte=12;
2510 for(i=0;i<12;i++){
2511 ang=0.52359877559*i; // every 30 degree
2512 surfPt[i].x=r1*cos(ang);
2513 surfPt[i].y=r1*sin(ang);
2514 }
2515}
2516
2517
2518/******************************************************************************
2519 GetDrawData
2520
2521 Return data for drawing
2522
2523 po thePolar
2524
2525 seg array of line segments (clockwise angle from top, angle from line of sight)
2526 blk indices of segs which are to be drawn with lines
2527 cont indices of closed line segments. Inside of the closure
2528 will be painted by a color.
2529
2530******************************************************************************/
2531
2532void CMPsphere::GetDrawData(TransAngleMatrix *po,Line2d *seg,StartPTE *blk,StartPTE *cont)
2533{
2534 TransAngleEuler tae;
2535 TransAngleMatrix tam;
2536 Line3d eye;
2537 Point3d u,b,c;
2538 Point2d e,f,g;
2539 double d,ra,d1,r1;
2540 Point2d a;
2541 int i;
2542 double ang;
2543
2544 eye.sx=xg; // center of the sphere
2545 eye.sy=yg;
2546 eye.sz=zg;
2547 eye.ex=po->x; // thePolar of CThreeD - position of the viewer's eye
2548 eye.ey=po->y;
2549 eye.ez=po->z;
2550
2551 NormLine3d(&eye,&u,&d);
2552 ToPolar3d(&u,&a);
2553
2554 ra=r; // radius of the sphere
2555 d1=ra*ra/d; // distance to the center of tangent circle from sphere center
2556 r1=sqrt(ra*ra-d1*d1); // radius of the tangent circle
2557
2558 tae.x=xg+d1*u.x;
2559 tae.y=yg+d1*u.y;
2560 tae.z=zg+d1*u.z;
2561 tae.theta=1.5707963268-a.y; // z axis toward viewer's eye
2562 tae.phi=a.x+1.5707963268;
2563 tae.psi=0.;
2564 TAEtoTAM(&tae,&tam);
2565
2566 u.x=r1*cos(0.);
2567 u.y=r1*sin(0.);
2568 u.z=0.;
2569 BodyToFrameTAM(&u,&tam,&b);
2570 FrameToBodyTAM(&b,po,&u);
2571 NormPoint3d(&u,&b,&d);
2572 ToPolar3d(&b,&e);
2573 e.y=1.5707963267-e.y;
2574 f=e;
2575 for(i=0;i<90;i++){
2576 g=f;
2577 if(i<89){
2578 ang=.06981317008*(i+1); // every 4 degree
2579 u.x=r1*cos(ang);
2580 u.y=r1*sin(ang);
2581 u.z=0.;
2582 BodyToFrameTAM(&u,&tam,&b);
2583 FrameToBodyTAM(&b,po,&u);
2584 NormPoint3d(&u,&b,&d);
2585 ToPolar3d(&b,&f);
2586 f.y=1.5707963267-f.y;
2587 }
2588 else f=e;
2589 seg[i].sx=g.x;
2590 seg[i].sy=g.y;
2591 seg[i].ex=f.x;
2592 seg[i].ey=f.y;
2593
2594 }
2595 blk->start=0;
2596 blk->pte=90;
2597 cont->start=0;
2598 cont->pte=90;
2599}
2600
2601
2602
2603/******************************************************************************
2604 FieldOfView
2605
2606 Get horizontal and vertical field of view to cover the object
2607******************************************************************************/
2608
2609void CMPsphere::FieldOfView(TransAngleMatrix *vu,Point2d *az,Point2d *el)
2610{
2611 Point3d s;
2612 Point3d f;
2613 Point2d a;
2614 double b,d;
2615
2616 f.x=xg;
2617 f.y=yg;
2618 f.z=zg;
2619 FrameToBodyTAM(&f,vu,&s);
2620 NormPoint3d(&s,&s,&d);
2621 ToPolar3d(&s,&a);
2622 b=asin(r/d);
2623 az->x=a.x-b;
2624 az->y=a.x+b;
2625 el->x=a.y-b;
2626 el->y=a.y+b;
2627}
2628
2629/******************************************************************************
2630 CMPbodyOfRot - bodyOfRot
2631******************************************************************************/
2632
2633void CMPbodyOfRot::SetPoint(double x,double z)
2634{
2635 switch (npoint){
2636 case 0:
2637 p0.x=x;
2638 p0.y=z;
2639 npoint=1;
2640 break;
2641 case 1:
2642 p1.x=x;
2643 p1.y=z;
2644 npoint=2;
2645 break;
2646 default:
2647 break;
2648 }
2649}
2650
2651/******************************************************************************
2652 ClearVisSurface
2653
2654 Clear 'visibleSurface' instance variable so that new visibility
2655 from a new viewpoint can be obtained in GetSurface
2656******************************************************************************/
2657void CMPbodyOfRot::ClearVisSurface(void)
2658{
2659 visibleSurface=0;
2660}
2661
2662/******************************************************************************
2663 GetSurface
2664
2665 Return a surface
2666
2667 14 surfaces are related to an index number in the following way
2668
2669 index face toward...
2670 0 - 11 azimuth 30*index+15 (deg)
2671 12 +z
2672 13 -z
2673
2674 The instance variable visibleSurface is used to map between
2675 the above index and the reference index passed as the first parameter.
2676
2677 The returned bool value is false if no more visible surface exists
2678 for the asked reference number.
2679
2680******************************************************************************/
2681
2682bool CMPbodyOfRot::GetSurface(int n,bool nearside,TransAngleMatrix *po,
2683 Surface *surf,Point2d *surfPt)
2684{
2685 Point2d p2,p3;
2686 TransAngleEuler tae1;
2687 TransAngleMatrix tam1;
2688 double alpha,delta,r;
2689
2690 TransAngleEuler tae2;
2691 TransAngleEuler tae;
2692 Point3d s;
2693 Point3d e;
2694 int i,j,k,m;
2695
2696 p2.x=p0.x; // bottom radial point
2697 p2.y=p0.y;
2698 p3.x=p1.x; // top radial point
2699 p3.y=p1.y;
2700
2701 if(p2.x==0.) p2.x=p3.x*0.01; // cone
2702 if(p3.x==0.) p3.x=p2.x*0.01;
2703 if(p2.y==p3.y) { // didc
2704 if(p2.x>p3.x) p3.y+=0.01*p2.x;
2705 else p2.y+=0.01*p3.x;
2706 }
2707
2708 p2.x*=1.0178; // make the average radius same as the real radius
2709 p3.x*=1.0178;
2710
2711 tae1.x=xg; // Translate Angle Euler for the part
2712 tae1.y=yg;
2713 tae1.z=zg;
2714 tae1.theta=thetag;
2715 tae1.phi=phig;
2716 tae1.psi=psig;
2717
2718 if(visibleSurface==0){
2719 TAEtoTAM(&tae1,&tam1);
2720 j=1;
2721 delta=atan2((p2.x-p3.x)*cos(0.017453293*15.),p3.y-p2.y);
2722 r=-p2.y*(p3.x-p2.x)/(p3.y-p2.y)+p2.x;
2723 r*=cos(0.017453293*15.);
2724 s.z=r*cos(delta)*sin(delta);
2725 r=r*cos(delta)*cos(delta);
2726 for(i=0;i<12;i++){
2727 alpha=0.017453293*(30.*i+15.);
2728 s.x=r*cos(alpha);
2729 s.y=r*sin(alpha);
2730 BodyToFrameTAM(&s,&tam1,&e);
2731 if((xg-e.x)*(po->x-e.x)+(yg-e.y)*(po->y-e.y)
2732 +(zg-e.z)*(po->z-e.z)<0.) visibleSurface|=j;
2733 j=j<<1;
2734 }
2735 FrameToBodyTAM((Point3d *)po,&tam1,&s); // position of viewer
2736 if((s.z-p3.y)*(p2.y-p3.y)<0.) visibleSurface|=j; // upper surface
2737 j=j<<1;
2738 if((s.z-p2.y)*(p3.y-p2.y)<0.) visibleSurface|=j; // lower surface
2739
2740 // determine instance variable 'appear'
2741 j=1<<12;
2742 if(visibleSurface&j){
2743 j=1;
2744 k=0;
2745 for(i=0;i<12;i++){
2746 if(visibleSurface&j) k++;
2747 j=j<<1;
2748 }
2749 if(k==0) appear=1;
2750 else if(k==12) appear=3;
2751 else appear=2;
2752 }
2753 else{
2754 j=j<<1;
2755 if(visibleSurface&j){
2756 j=1;
2757 k=0;
2758 for(i=0;i<12;i++){
2759 if(visibleSurface&j) k++;
2760 j=j<<1;
2761 }
2762 if(k==0) appear=4;
2763 else if(k==12) appear=6;
2764 else appear=5;
2765 }
2766 else appear=0;
2767 }
2768 }
2769 if(isTube){
2770 if(appear==0||((appear==2||appear==5)&&nearside)){
2771 j=1;
2772 i=0;
2773 for(m=0;m<12;m++){
2774 if(visibleSurface&j){
2775 if(i==n) break;
2776 i++;
2777 }
2778 j=j<<1;
2779 }
2780 if(m==12) return false;
2781
2782 delta=atan2((p2.x-p3.x)*cos(0.017453293*15.),p3.y-p2.y);
2783 alpha=0.017453293*(30.*m+15.);
2784 r=p2.x*cos(0.017453293*15.); // radius to the center of the bottom segment
2785
2786 tae2.x=r*cos(alpha);
2787 tae2.y=r*sin(alpha);
2788 tae2.z=p2.y;
2789 tae2.theta=1.5707963268-delta;
2790 tae2.phi=alpha+1.5707963268;
2791 tae2.psi=0.;
2792
2793 surfPt[0].x=-r*tan(0.017453293*15.); // left-bottom
2794 surfPt[0].y=0.;
2795 surfPt[1].x=-surfPt[0].x; // right-bottom
2796 surfPt[1].y=0.;
2797 r=p3.x*cos(0.017453293*15.); // radius to the center of the top segment
2798 surfPt[2].x=r*tan(0.017453293*15.); // right-top
2799 surfPt[2].y=(p3.y-p2.y)/cos(delta);
2800 surfPt[3].x=-surfPt[2].x; // left-top
2801 surfPt[3].y=surfPt[2].y;
2802
2803 MoveTwiceTAE(&tae1,&tae2,&tae);
2804 surf->x0=tae.x;
2805 surf->y0=tae.y;
2806 surf->z0=tae.z;
2807 surf->theta0=tae.theta;
2808 surf->phi0=tae.phi;
2809 surf->psi0=tae.psi;
2810 surf->start=0;
2811 if(m<12) surf->pte=4;
2812 else surf->pte=12;
2813 return true;
2814 }
2815 if((appear==2||appear==5)&&!nearside){
2816 j=1;
2817 i=0;
2818 for(m=0;m<12;m++){
2819 if(!(visibleSurface&j)){
2820 if(i==n) break;
2821 i++;
2822 }
2823 j=j<<1;
2824 }
2825 if(m==12) return false;
2826
2827 delta=atan2((p2.x-p3.x)*cos(0.017453293*15.),p3.y-p2.y);
2828 alpha=0.017453293*(30.*m+15.);
2829 r=p2.x*cos(0.017453293*15.); // radius to the center of the bottom segment
2830
2831 tae2.x=r*cos(alpha);
2832 tae2.y=r*sin(alpha);
2833 tae2.z=p2.y;
2834 tae2.theta=1.5707963268-delta;
2835 tae2.phi=alpha+1.5707963268;
2836 tae2.psi=0.;
2837
2838 surfPt[0].x=r*tan(0.017453293*15.); // left-bottom
2839 surfPt[0].y=0.;
2840 surfPt[1].x=-surfPt[0].x; // right-bottom
2841 surfPt[1].y=0.;
2842 r=p3.x*cos(0.017453293*15.); // radius to the center of the top segment
2843 surfPt[2].x=-r*tan(0.017453293*15.); // right-top
2844 surfPt[2].y=(p3.y-p2.y)/cos(delta);
2845 surfPt[3].x=-surfPt[2].x; // left-top
2846 surfPt[3].y=surfPt[2].y;
2847
2848 MoveTwiceTAE(&tae1,&tae2,&tae);
2849 surf->x0=tae.x;
2850 surf->y0=tae.y;
2851 surf->z0=tae.z;
2852 surf->theta0=tae.theta;
2853 surf->phi0=tae.phi;
2854 surf->psi0=tae.psi;
2855 surf->start=0;
2856 if(m<12) surf->pte=4;
2857 else surf->pte=12;
2858 return true;
2859 }
2860 if(appear==1||appear==4||appear==3||appear==6){
2861 if(n>5) return false;
2862 if(nearside) m=n;
2863 else m=n+6;
2864
2865 delta=atan2((p2.x-p3.x)*cos(0.017453293*15.),p3.y-p2.y);
2866 alpha=0.017453293*(30.*m+15.);
2867 r=p2.x*cos(0.017453293*15.); // radius to the center of the bottom segment
2868 if(appear==1||appear==4){
2869 alpha-=3.14159265;
2870 delta=-delta;
2871 }
2872 tae2.x=r*cos(alpha);
2873 tae2.y=r*sin(alpha);
2874 tae2.z=p2.y;
2875 tae2.theta=1.5707963268-delta;
2876 tae2.phi=alpha+1.5707963268;
2877 tae2.psi=0.;
2878
2879 surfPt[0].x=-r*tan(0.017453293*15.); // left-bottom
2880 surfPt[0].y=0.;
2881 surfPt[1].x=-surfPt[0].x; // right-bottom
2882 surfPt[1].y=0.;
2883 r=p3.x*cos(0.017453293*15.); // radius to the center of the top segment
2884 surfPt[2].x=r*tan(0.017453293*15.); // right-top
2885 surfPt[2].y=(p3.y-p2.y)/cos(delta);
2886 surfPt[3].x=-surfPt[2].x; // left-top
2887 surfPt[3].y=surfPt[2].y;
2888
2889 MoveTwiceTAE(&tae1,&tae2,&tae);
2890 surf->x0=tae.x;
2891 surf->y0=tae.y;
2892 surf->z0=tae.z;
2893 surf->theta0=tae.theta;
2894 surf->phi0=tae.phi;
2895 surf->psi0=tae.psi;
2896 surf->start=0;
2897 if(m<12) surf->pte=4;
2898 else surf->pte=12;
2899 return true;
2900 }
2901 }
2902 else{ // solid
2903 j=1;
2904 i=0;
2905 for(m=0;m<14;m++){
2906 if(visibleSurface&j){
2907 if(i==n) break;
2908 i++;
2909 }
2910 j=j<<1;
2911 }
2912 if(m==14) return false;
2913
2914 if(m<12){
2915 delta=atan2((p2.x-p3.x)*cos(0.017453293*15.),p3.y-p2.y);
2916 alpha=0.017453293*(30.*m+15.);
2917 r=p2.x*cos(0.017453293*15.); // radius to the center of the bottom segment
2918
2919 tae2.x=r*cos(alpha);
2920 tae2.y=r*sin(alpha);
2921 tae2.z=p2.y;
2922 tae2.theta=1.5707963268-delta;
2923 tae2.phi=alpha+1.5707963268;
2924 tae2.psi=0.;
2925
2926 surfPt[0].x=-r*tan(0.017453293*15.); // left-bottom
2927 surfPt[0].y=0.;
2928 surfPt[1].x=-surfPt[0].x; // right-bottom
2929 surfPt[1].y=0.;
2930 r=p3.x*cos(0.017453293*15.); // radius to the center of the top segment
2931 surfPt[2].x=r*tan(0.017453293*15.); // right-top
2932 surfPt[2].y=(p3.y-p2.y)/cos(delta);
2933 surfPt[3].x=-surfPt[2].x; // left-top
2934 surfPt[3].y=surfPt[2].y;
2935 }
2936 if(m==12){
2937 tae2.x=0.;
2938 tae2.y=0.;
2939 tae2.z=p3.y;
2940 tae2.theta=0.;
2941 tae2.phi=0.;
2942 tae2.psi=0.;
2943
2944 r=p3.x;
2945 for(i=0;i<12;i++){
2946 alpha=0.017453293*30.*i;
2947 surfPt[i].x=r*cos(alpha);
2948 surfPt[i].y=r*sin(alpha);
2949 }
2950 }
2951 if(m==13){
2952 tae2.x=0.;
2953 tae2.y=0.;
2954 tae2.z=p2.y;
2955 tae2.theta=3.1415926536;
2956 tae2.phi=0.;
2957 tae2.psi=0.;
2958
2959 r=p2.x;
2960 for(i=0;i<12;i++){
2961 alpha=0.017453293*30.*i;
2962 surfPt[i].x=r*cos(alpha);
2963 surfPt[i].y=r*sin(alpha);
2964 }
2965 }
2966
2967 MoveTwiceTAE(&tae1,&tae2,&tae);
2968 surf->x0=tae.x;
2969 surf->y0=tae.y;
2970 surf->z0=tae.z;
2971 surf->theta0=tae.theta;
2972 surf->phi0=tae.phi;
2973 surf->psi0=tae.psi;
2974 surf->start=0;
2975 if(m<12) surf->pte=4;
2976 else surf->pte=12;
2977 return true;
2978 }
2979}
2980
2981/******************************************************************************
2982 GetCenter
2983
2984 Return the center point in frame coordinate
2985
2986******************************************************************************/
2987
2988void CMPbodyOfRot::GetCenter(Point3d *p)
2989{
2990 Point3d pt;
2991 TransAngleEuler tae;
2992 TransAngleMatrix tam;
2993
2994 pt.x=0.;
2995 pt.y=0.;
2996 pt.z=0.5*(p0.y+p1.y);
2997
2998 tae.x=xg;
2999 tae.y=yg;
3000 tae.z=zg;
3001 tae.theta=thetag;
3002 tae.phi=phig;
3003 tae.psi=psig;
3004
3005 TAEtoTAM(&tae,&tam);
3006 BodyToFrameTAM(&pt,&tam,p);
3007}
3008
3009/******************************************************************************
3010 GetDrawData
3011
3012 Return data for drawing
3013
3014 m segment number (0 - near side, 1 - far side)
3015 po thePolar
3016
3017 seg array of line segments (clockwise angle from top,
3018 angle from line of sight)
3019 blk indices of segs which are to be drawn with lines
3020 cont indices of closed line segments. Inside of the closure
3021 will be painted by a color.
3022
3023******************************************************************************/
3024
3025void CMPbodyOfRot::GetDrawData(int m,TransAngleMatrix *po,
3026 Line2d *seg,StartPTE *blk,StartPTE *cont)
3027{
3028 double eye[3];
3029 Point3d *ten0;
3030 Point3d *ten1;
3031 double x1,z1,x2,z2,y1,y2;
3032 TransAngleEuler tae;
3033 TransAngleMatrix tam1;
3034 int nseg,i,n0,n1,n2,j,k,is,is1,line,istart,iend,itb,ibt,n;
3035 double deld,a,co,si,r;
3036 Point3d q,s,d;
3037 Point2d direc;
3038
3039 if(oblong){
3040 GetDrawData1(po,seg,blk,cont);
3041 return;
3042 }
3043
3044 eye[0]=po->x;
3045 eye[1]=po->y;
3046 eye[2]=po->z;
3047
3048 // lower surface ten0, upper surface ten1
3049 // indices are from 0 to 90, 90th data being the same as 0th data
3050 ten0=new Point3d[91];
3051 ten1=new Point3d[91];
3052 x1=p0.x;
3053 z1=p0.y;
3054 x2=p1.x;
3055 z2=p1.y;
3056
3057 if(x1==0.) x1=x2*0.01; // cone
3058 if(x2==0.) x2=x1*0.01;
3059 if(z1==z2) { // didc
3060 if(x1>x2) z2+=0.01*x1;
3061 else z1-=0.01*x2;
3062 }
3063
3064 // set conversion parameters
3065 tae.x=xg;
3066 tae.y=yg;
3067 tae.z=zg;
3068 tae.theta=thetag;
3069 tae.phi=phig;
3070 tae.psi=psig;
3071
3072 TAEtoTAM(&tae,&tam1);
3073
3074 // At this point, initial conditions are set.
3075 // tam1 defines a local coordinate.
3076 // In the local coordinate, two points (x1,z1) and (x2,z2) are given.
3077 // The body of rotation is given by rotating
3078 // the line connecting the two points around z-axis.
3079 // The viewer is given by eye[]
3080
3081 /* fill coordinates of 2*nseg apexes */
3082
3083 nseg=90;
3084 deld=nseg;
3085 deld=2.*3.1415926536/deld;
3086 for(i=0;i<=nseg;i++){ // 91 data are set to each of the arrays
3087 a=i*deld;
3088 co=cos(a);
3089 si=sin(a);
3090 q.x=x1*co;
3091 q.y=x1*si;
3092 q.z=z1; /* lower surface */
3093 BodyToFrameTAM(&q,&tam1,&ten0[i]);
3094 q.x=x2*co;
3095 q.y=x2*si;
3096 q.z=z2; /* uppser surface */
3097 BodyToFrameTAM(&q,&tam1,&ten1[i]);
3098 }
3099
3100 // the outline was replaced by two circles ten0[] and ten1[]
3101 /* obtain side surface segments which are visible */
3102 n0=0;
3103 n1=0; // invisible-to-visible index
3104 n2=0; // visible-to-invisible index
3105
3106 // In CMPbodyOfRot::GetSurface, we have already categorized
3107 // into seven cases, which are inducated by the variable 'appear'.
3108 // Of those seven cases, the following three cases
3109 // 0 only the side is visible
3110 // 2 side and upper surface are visible
3111 // 5 side and lower surface are visible
3112 // have indices which separate visible and invisible portions of the side
3113 // surface.
3114
3115 if(appear==0||appear==2||appear==5){
3116 for(i=0;(i<=nseg)&&(n0!=2);i++){
3117
3118 /* each surface contains i-th and (i-1)th apexes */
3119 /* visibility of two adjacent surfaces are compared to */
3120 /* obtain both visible-to-invisible and invisible-to-visible */
3121 /* trasition indices */
3122
3123 j=i;
3124 if(j>=nseg) j-=nseg;
3125 k=j-1;
3126 if(k<0) k+=nseg;
3127 is=0;
3128 /* vector along lower surface */
3129 x1=ten0[j].x-ten0[k].x;
3130 y1=ten0[j].y-ten0[k].y;
3131 z1=ten0[j].z-ten0[k].z;
3132 /* vector directed from lower surface to upper sureace */
3133 x2=ten1[k].x-ten0[k].x;
3134 y2=ten1[k].y-ten0[k].y;
3135 z2=ten1[k].z-ten0[k].z;
3136 /* if surface is visible is = 1 */
3137 if((y1*z2-z1*y2)*(eye[0]-ten0[k].x)
3138 +(z1*x2-x1*z2)*(eye[1]-ten0[k].y)
3139 +(x1*y2-y1*x2)*(eye[2]-ten0[k].z)>0.) is=1; // visible
3140 if((i!=0)&&(is!=is1)) {
3141 if(is>is1) n1=k; /* k is invisible to visible */
3142 if(is<is1) n2=k; /* k is visible to invisible */
3143 n0++;
3144 }
3145 if(n0!=2) is1=is;
3146 }
3147 }
3148
3149 /* now, categorization is over, process each cases */
3150
3151 // The order of processing is as followes
3152 // appear solid tube-near tube-far
3153 // 0 x x x
3154 // 2,5 x
3155 // 2,5 x
3156 // 5 x
3157 // 2 x
3158 // 1,3,4,6 x
3159 // 1,3,4,6 x
3160 // 1,3,4,6 x
3161
3162 line=0;
3163 if(appear==0||((appear==2||appear==5)&&isTube&&m==0)){
3164 // appear=0,both for a solid and a tube, near and far side
3165 // appear=2 or 5, near side of the tube
3166 /* side only */
3167 blk->start=line;
3168 cont->start=line;
3169 n0=n2-n1; /* number of visible points - 1 */
3170 if(n0<0) n0+=nseg;
3171 for(i=0;i<n0;i++){
3172 j=n1+i+1;
3173 if(j>=nseg) j-=nseg;
3174 k=j-1;
3175 if(k<0) k+=nseg;
3176 /* along lower surface normal direction (to the order of smaller to larger indices) */
3177 s=ten0[k];
3178 FrameToBodyTAM(&s,po,&d);
3179 NormPoint3d(&d,&s,&r);
3180 ToPolar3d(&s,&direc);
3181 seg[line].sx=direc.x;
3182 seg[line].sy=1.5707963267-direc.y;
3183 s=ten0[j];
3184 FrameToBodyTAM(&s,po,&d);
3185 NormPoint3d(&d,&s,&r);
3186 ToPolar3d(&s,&direc);
3187 seg[line].ex=direc.x;
3188 seg[line].ey=1.5707963267-direc.y;
3189 line++;
3190 /* along upper surface reversed order */
3191 s=ten1[j];
3192 FrameToBodyTAM(&s,po,&d);
3193 NormPoint3d(&d,&s,&r);
3194 ToPolar3d(&s,&direc);
3195 seg[line].sx=direc.x;
3196 seg[line].sy=1.5707963267-direc.y;
3197 s=ten1[k];
3198 FrameToBodyTAM(&s,po,&d);
3199 NormPoint3d(&d,&s,&r);
3200 ToPolar3d(&s,&direc);
3201 seg[line].ex=direc.x;
3202 seg[line].ey=1.5707963267-direc.y;
3203 line++;
3204 }
3205 /* invisible-to-visible side line from top to bottom */
3206 s=ten1[n1];
3207 FrameToBodyTAM(&s,po,&d);
3208 NormPoint3d(&d,&s,&r);
3209 ToPolar3d(&s,&direc);
3210 seg[line].sx=direc.x;
3211 seg[line].sy=1.5707963267-direc.y;
3212 s=ten0[n1];
3213 FrameToBodyTAM(&s,po,&d);
3214 NormPoint3d(&d,&s,&r);
3215 ToPolar3d(&s,&direc);
3216 seg[line].ex=direc.x;
3217 seg[line].ey=1.5707963267-direc.y;
3218 line++;
3219 /* visible-to-invisible side line from bottom to top */
3220 s=ten0[n2];
3221 FrameToBodyTAM(&s,po,&d);
3222 NormPoint3d(&d,&s,&r);
3223 ToPolar3d(&s,&direc);
3224 seg[line].sx=direc.x;
3225 seg[line].sy=1.5707963267-direc.y;
3226 s=ten1[n2];
3227 FrameToBodyTAM(&s,po,&d);
3228 NormPoint3d(&d,&s,&r);
3229 ToPolar3d(&s,&direc);
3230 seg[line].ex=direc.x;
3231 seg[line].ey=1.5707963267-direc.y;
3232 line++;
3233 blk->pte=line;
3234 cont->pte=line;
3235 delete[] ten0; // DisposPtr((Ptr)ten0);
3236 delete[] ten1; // DisposPtr((Ptr)ten1);
3237 return;
3238 }
3239
3240 if((appear==2||appear==5)&&isTube&&m==1){
3241 // appear=2 or 5, far side of the tube
3242 /* side only */
3243 blk->start=line;
3244 cont->start=line;
3245 n0=n1-n2; /* number of invisible points - 1 */
3246 if(n0<0) n0+=nseg;
3247 for(i=0;i<n0;i++){
3248 j=n2+i+1;
3249 if(j>=nseg) j-=nseg;
3250 k=j-1;
3251 if(k<0) k+=nseg;
3252 /* along lower surface in reverse direction */
3253 s=ten0[j];
3254 FrameToBodyTAM(&s,po,&d);
3255 NormPoint3d(&d,&s,&r);
3256 ToPolar3d(&s,&direc);
3257 seg[line].sx=direc.x;
3258 seg[line].sy=1.5707963267-direc.y;
3259 s=ten0[k];
3260 FrameToBodyTAM(&s,po,&d);
3261 NormPoint3d(&d,&s,&r);
3262 ToPolar3d(&s,&direc);
3263 seg[line].ex=direc.x;
3264 seg[line].ey=1.5707963267-direc.y;
3265 line++;
3266 /* along upper surface in normal direction */
3267 s=ten1[k];
3268 FrameToBodyTAM(&s,po,&d);
3269 NormPoint3d(&d,&s,&r);
3270 ToPolar3d(&s,&direc);
3271 seg[line].sx=direc.x;
3272 seg[line].sy=1.5707963267-direc.y;
3273 s=ten1[j];
3274 FrameToBodyTAM(&s,po,&d);
3275 NormPoint3d(&d,&s,&r);
3276 ToPolar3d(&s,&direc);
3277 seg[line].ex=direc.x;
3278 seg[line].ey=1.5707963267-direc.y;
3279 line++;
3280 }
3281 /* invisible-to-visible side line from top tp bottom */
3282 s=ten1[n1];
3283 FrameToBodyTAM(&s,po,&d);
3284 NormPoint3d(&d,&s,&r);
3285 ToPolar3d(&s,&direc);
3286 seg[line].sx=direc.x;
3287 seg[line].sy=1.5707963267-direc.y;
3288 s=ten0[n1];
3289 FrameToBodyTAM(&s,po,&d);
3290 NormPoint3d(&d,&s,&r);
3291 ToPolar3d(&s,&direc);
3292 seg[line].ex=direc.x;
3293 seg[line].ey=1.5707963267-direc.y;
3294 line++;
3295 /* visible-to-invisible side line from bottom to top */
3296 s=ten0[n2];
3297 FrameToBodyTAM(&s,po,&d);
3298 NormPoint3d(&d,&s,&r);
3299 ToPolar3d(&s,&direc);
3300 seg[line].sx=direc.x;
3301 seg[line].sy=1.5707963267-direc.y;
3302 s=ten1[n2];
3303 FrameToBodyTAM(&s,po,&d);
3304 NormPoint3d(&d,&s,&r);
3305 ToPolar3d(&s,&direc);
3306 seg[line].ex=direc.x;
3307 seg[line].ey=1.5707963267-direc.y;
3308 line++;
3309 blk->pte=line;
3310 cont->pte=line;
3311 delete[] ten0; // DisposPtr((Ptr)ten0);
3312 delete[] ten1; // DisposPtr((Ptr)ten1);
3313 return;
3314 }
3315
3316 if(appear==5&&!isTube){ // for a solid
3317 /* side and lower surface */
3318 blk->start=line;
3319 cont->start=line;
3320 n0=n2-n1;
3321 if(n0<0) n0+=nseg;
3322 for(i=0;i<n0;i++){
3323 j=n2-i-1;
3324 if(j<0) j+=nseg;
3325 k=j+1;
3326 if(k>=nseg) k-=nseg;
3327 /* contor along the upper surface */
3328 s=ten1[k];
3329 FrameToBodyTAM(&s,po,&d);
3330 NormPoint3d(&d,&s,&r);
3331 ToPolar3d(&s,&direc);
3332 seg[line].sx=direc.x;
3333 seg[line].sy=1.5707963267-direc.y;
3334 s=ten1[j];
3335 FrameToBodyTAM(&s,po,&d);
3336 NormPoint3d(&d,&s,&r);
3337 ToPolar3d(&s,&direc);
3338 seg[line].ex=direc.x;
3339 seg[line].ey=1.5707963267-direc.y;
3340 line++;
3341 }
3342 s=ten1[n1];
3343 FrameToBodyTAM(&s,po,&d);
3344 NormPoint3d(&d,&s,&r);
3345 ToPolar3d(&s,&direc);
3346 seg[line].sx=direc.x;
3347 seg[line].sy=1.5707963267-direc.y;
3348 s=ten0[n1];
3349 FrameToBodyTAM(&s,po,&d);
3350 NormPoint3d(&d,&s,&r);
3351 ToPolar3d(&s,&direc);
3352 seg[line].ex=direc.x;
3353 seg[line].ey=1.5707963267-direc.y;
3354 line++;
3355 s=ten0[n2];
3356 FrameToBodyTAM(&s,po,&d);
3357 NormPoint3d(&d,&s,&r);
3358 ToPolar3d(&s,&direc);
3359 seg[line].sx=direc.x;
3360 seg[line].sy=1.5707963267-direc.y;
3361 s=ten1[n2];
3362 FrameToBodyTAM(&s,po,&d);
3363 NormPoint3d(&d,&s,&r);
3364 ToPolar3d(&s,&direc);
3365 seg[line].ex=direc.x;
3366 seg[line].ey=1.5707963267-direc.y;
3367 line++;
3368 n0=n1-n2;
3369 if(n0<0) n0=n0+nseg;
3370 cont->pte=line+n0;
3371 /* part of the followings constitute contour */
3372 for(i=0;i<nseg;i++){
3373 j=n2+i+1;
3374 if(j>=nseg) j=j-nseg;
3375 k=j-1;
3376 if(k<0) k+=nseg;
3377 s=ten0[j];
3378 FrameToBodyTAM(&s,po,&d);
3379 NormPoint3d(&d,&s,&r);
3380 ToPolar3d(&s,&direc);
3381 seg[line].sx=direc.x;
3382 seg[line].sy=1.5707963267-direc.y;
3383 s=ten0[k];
3384 FrameToBodyTAM(&s,po,&d);
3385 NormPoint3d(&d,&s,&r);
3386 ToPolar3d(&s,&direc);
3387 seg[line].ex=direc.x;
3388 seg[line].ey=1.5707963267-direc.y;
3389 line++;
3390 }
3391 blk->pte=line;
3392 delete[] ten0; // DisposPtr((Ptr)ten0);
3393 delete[] ten1; // DisposPtr((Ptr)ten1);
3394 return;
3395 }
3396 if(appear==2&&!isTube){ // for a solid
3397 /* side and upper surface */
3398 blk->start=line;
3399 cont->start=line;
3400 n0=n2-n1;
3401 if(n0<0) n0+=nseg;
3402 /* line along lower surface */
3403 for(i=0;i<n0;i++){
3404 j=n1+i+1;
3405 if(j>=nseg) j-=nseg;
3406 k=j-1;
3407 if(k<0) k+=nseg;
3408 s=ten0[k];
3409 FrameToBodyTAM(&s,po,&d);
3410 NormPoint3d(&d,&s,&r);
3411 ToPolar3d(&s,&direc);
3412 seg[line].sx=direc.x;
3413 seg[line].sy=1.5707963267-direc.y;
3414 s=ten0[j];
3415 FrameToBodyTAM(&s,po,&d);
3416 NormPoint3d(&d,&s,&r);
3417 ToPolar3d(&s,&direc);
3418 seg[line].ex=direc.x;
3419 seg[line].ey=1.5707963267-direc.y;
3420 line++;
3421 }
3422 s=ten1[n1];
3423 FrameToBodyTAM(&s,po,&d);
3424 NormPoint3d(&d,&s,&r);
3425 ToPolar3d(&s,&direc);
3426 seg[line].sx=direc.x;
3427 seg[line].sy=1.5707963267-direc.y;
3428 s=ten0[n1];
3429 FrameToBodyTAM(&s,po,&d);
3430 NormPoint3d(&d,&s,&r);
3431 ToPolar3d(&s,&direc);
3432 seg[line].ex=direc.x;
3433 seg[line].ey=1.5707963267-direc.y;
3434 line++;
3435 s=ten0[n2];
3436 FrameToBodyTAM(&s,po,&d);
3437 NormPoint3d(&d,&s,&r);
3438 ToPolar3d(&s,&direc);
3439 seg[line].sx=direc.x;
3440 seg[line].sy=1.5707963267-direc.y;
3441 s=ten1[n2];
3442 FrameToBodyTAM(&s,po,&d);
3443 NormPoint3d(&d,&s,&r);
3444 ToPolar3d(&s,&direc);
3445 seg[line].ex=direc.x;
3446 seg[line].ey=1.5707963267-direc.y;
3447 line++;
3448 n0=n1-n2;
3449 if(n0<0) n0=n0+nseg;
3450 cont->pte=line+n0;
3451 for(i=0;i<nseg;i++){
3452 j=n2+i+1;
3453 if(j>=nseg) j=j-nseg;
3454 k=j-1;
3455 if(k<0) k+=nseg;
3456 s=ten1[k];
3457 FrameToBodyTAM(&s,po,&d);
3458 NormPoint3d(&d,&s,&r);
3459 ToPolar3d(&s,&direc);
3460 seg[line].sx=direc.x;
3461 seg[line].sy=1.5707963267-direc.y;
3462 s=ten1[j];
3463 FrameToBodyTAM(&s,po,&d);
3464 NormPoint3d(&d,&s,&r);
3465 ToPolar3d(&s,&direc);
3466 seg[line].ex=direc.x;
3467 seg[line].ey=1.5707963267-direc.y;
3468 line++;
3469 }
3470 blk->pte=line;
3471 delete[] ten0; // DisposPtr((Ptr)ten0);
3472 delete[] ten1; // DisposPtr((Ptr)ten1);
3473 return;
3474 }
3475 if(!isTube){ // for a solid, appear=1,3,4 or 6
3476 /* lower or upper surface only */
3477 blk->start=line;
3478 cont->pte=line;
3479 for(i=0;i<nseg;i++){ // contour circle
3480 j=i;
3481 k=j-1;
3482 if(k<0) k+=nseg;
3483 if(appear==1||appear==6){ // contour is upper surface
3484 if(appear==1){
3485 n=k;
3486 k=j;
3487 j=n;
3488 }
3489 s=ten1[j];
3490 FrameToBodyTAM(&s,po,&d);
3491 NormPoint3d(&d,&s,&r);
3492 ToPolar3d(&s,&direc);
3493 seg[line].sx=direc.x;
3494 seg[line].sy=1.5707963267-direc.y;
3495 s=ten1[k];
3496 FrameToBodyTAM(&s,po,&d);
3497 NormPoint3d(&d,&s,&r);
3498 ToPolar3d(&s,&direc);
3499 seg[line].ex=direc.x;
3500 seg[line].ey=1.5707963267-direc.y;
3501 }
3502 else{ // appear=3 or 4
3503 if(appear==3){
3504 n=k;
3505 k=j;
3506 j=n;
3507 }
3508 s=ten0[j];
3509 FrameToBodyTAM(&s,po,&d);
3510 NormPoint3d(&d,&s,&r);
3511 ToPolar3d(&s,&direc);
3512 seg[line].sx=direc.x;
3513 seg[line].sy=1.5707963267-direc.y;
3514 s=ten0[k];
3515 FrameToBodyTAM(&s,po,&d);
3516 NormPoint3d(&d,&s,&r);
3517 ToPolar3d(&s,&direc);
3518 seg[line].ex=direc.x;
3519 seg[line].ey=1.5707963267-direc.y;
3520 }
3521 line++;
3522 }
3523 cont->pte=line;
3524 if(appear==3){
3525 for(i=0;i<nseg;i++){ // upper surface is inner circle
3526 j=i;
3527 k=j-1;
3528 if(k<0) k+=nseg;
3529 s=ten1[j];
3530 FrameToBodyTAM(&s,po,&d);
3531 NormPoint3d(&d,&s,&r);
3532 ToPolar3d(&s,&direc);
3533 seg[line].sx=direc.x;
3534 seg[line].sy=1.5707963267-direc.y;
3535 s=ten1[k];
3536 FrameToBodyTAM(&s,po,&d);
3537 NormPoint3d(&d,&s,&r);
3538 ToPolar3d(&s,&direc);
3539 seg[line].ex=direc.x;
3540 seg[line].ey=1.5707963267-direc.y;
3541 }
3542 line++;
3543 }
3544 if(appear==6){
3545 for(i=0;i<nseg;i++){ // lower surface is inner circle
3546 j=i;
3547 k=j-1;
3548 if(k<0) k+=nseg;
3549 s=ten0[j];
3550 FrameToBodyTAM(&s,po,&d);
3551 NormPoint3d(&d,&s,&r);
3552 ToPolar3d(&s,&direc);
3553 seg[line].sx=direc.x;
3554 seg[line].sy=1.5707963267-direc.y;
3555 s=ten0[k];
3556 FrameToBodyTAM(&s,po,&d);
3557 NormPoint3d(&d,&s,&r);
3558 ToPolar3d(&s,&direc);
3559 seg[line].ex=direc.x;
3560 seg[line].ey=1.5707963267-direc.y;
3561 }
3562 line++;
3563 }
3564 blk->pte=line;
3565 delete[] ten0; // DisposPtr((Ptr)ten0);
3566 delete[] ten1; // DisposPtr((Ptr)ten1);
3567 return;
3568 }
3569 // remaining cases are appear 1,3,4 and 6 of tubes
3570
3571 // The upper surface is given by an array ten1[].
3572 // The lower surface is given by an array ten0[].
3573 // looked from... outer circle inner circle
3574 // appear=1 the upper side upper circle (normal) lower circle (reverse)
3575 // appear=3 the upper side lower circle (normal) upper circle (reverse)
3576 // appear=4 the lower side lower circle (reverse) upper circle (normal)
3577 // appear=6 the lower side upper circle (reverse) lower circle (normal)
3578
3579 cont->start=line;
3580
3581 if(m==0){ // near side
3582 istart=0;
3583 iend=45;
3584 }
3585 else{ // far side
3586 istart=45;
3587 iend=90;
3588 }
3589 // 2 segments, which are part of the contour,
3590 // but, not part of the block.
3591
3592 if(appear==1||appear==4){
3593 itb=iend; // top to botton transition
3594 ibt=istart; // bottom to top transition
3595 }
3596 if(appear==3||appear==6){
3597 itb=istart; // top to bottom transition
3598 ibt=iend;
3599 }
3600 s=ten1[itb];
3601 FrameToBodyTAM(&s,po,&d);
3602 NormPoint3d(&d,&s,&r);
3603 ToPolar3d(&s,&direc);
3604 seg[line].sx=direc.x;
3605 seg[line].sy=1.5707963267-direc.y;
3606 s=ten0[itb];
3607 FrameToBodyTAM(&s,po,&d);
3608 NormPoint3d(&d,&s,&r);
3609 ToPolar3d(&s,&direc);
3610 seg[line].ex=direc.x;
3611 seg[line].ey=1.5707963267-direc.y;
3612 line++;
3613 s=ten0[ibt];
3614 FrameToBodyTAM(&s,po,&d);
3615 NormPoint3d(&d,&s,&r);
3616 ToPolar3d(&s,&direc);
3617 seg[line].sx=direc.x;
3618 seg[line].sy=1.5707963267-direc.y;
3619 s=ten1[ibt];
3620 FrameToBodyTAM(&s,po,&d);
3621 NormPoint3d(&d,&s,&r);
3622 ToPolar3d(&s,&direc);
3623 seg[line].ex=direc.x;
3624 seg[line].ey=1.5707963267-direc.y;
3625 line++;
3626
3627
3628 // now, the circular portion
3629 blk->start=line;
3630
3631 for(i=istart;i<iend;i++){
3632 if(appear==1||appear==4){
3633 j=i;
3634 k=i+1;
3635 }
3636 else{ // appear==3||appear==6
3637 j=i+1;
3638 k=i;
3639 }
3640 s=ten1[j];
3641 FrameToBodyTAM(&s,po,&d);
3642 NormPoint3d(&d,&s,&r);
3643 ToPolar3d(&s,&direc);
3644 seg[line].sx=direc.x;
3645 seg[line].sy=1.5707963267-direc.y;
3646 s=ten1[k];
3647 FrameToBodyTAM(&s,po,&d);
3648 NormPoint3d(&d,&s,&r);
3649 ToPolar3d(&s,&direc);
3650 seg[line].ex=direc.x;
3651 seg[line].ey=1.5707963267-direc.y;
3652 line++;
3653 }
3654 for(i=istart;i<iend;i++){
3655 if(appear==1||appear==4){
3656 j=i+1;
3657 k=i;
3658 }
3659 else{ // appear==3||appear==6
3660 j=i;
3661 k=i+1;
3662 }
3663 s=ten0[j];
3664 FrameToBodyTAM(&s,po,&d);
3665 NormPoint3d(&d,&s,&r);
3666 ToPolar3d(&s,&direc);
3667 seg[line].sx=direc.x;
3668 seg[line].sy=1.5707963267-direc.y;
3669 s=ten0[k];
3670 FrameToBodyTAM(&s,po,&d);
3671 NormPoint3d(&d,&s,&r);
3672 ToPolar3d(&s,&direc);
3673 seg[line].ex=direc.x;
3674 seg[line].ey=1.5707963267-direc.y;
3675 line++;
3676 }
3677 cont->pte=line;
3678 blk->pte=line;
3679 delete[] ten0; // DisposPtr((Ptr)ten0);
3680 delete[] ten1; // DisposPtr((Ptr)ten1);
3681}
3682
3683/******************************************************************************
3684 GetDrawData1
3685
3686 Return data for drawing for the case of oblong
3687
3688 po thePolar
3689
3690 seg array of line segments (clockwise angle from top,
3691 angle from line of sight)
3692 blk indices of segs which are to be drawn with lines
3693 cont indices of closed line segments. Inside of the closure
3694 will be painted by a color.
3695
3696******************************************************************************/
3697
3698void CMPbodyOfRot::GetDrawData1(TransAngleMatrix *po,
3699 Line2d *seg,StartPTE *blk,StartPTE *cont)
3700{
3701 int i,n;
3702 double z1,z2,r,ang0,ang1,ang2,ang,theta,dthet,d,d1,r1,d2,r2,l1,l2;
3703 TransAngleEuler tae;
3704 TransAngleMatrix tam;
3705 Point3d q,s1,u1,s2,u2,v,w,u,b;
3706 Line3d eye,tz,ty;
3707 Point2d e,f,g,a;
3708
3709 r=p0.x; // radius of sphere
3710 z1=p0.y+r; // bottom center of sphere
3711 z2=p1.y-r; // top center of sphere
3712
3713 tae.x=xg;
3714 tae.y=yg;
3715 tae.z=zg;
3716 tae.theta=thetag;
3717 tae.phi=phig;
3718 tae.psi=psig;
3719 TAEtoTAM(&tae,&tam);
3720
3721 q.x=0.;
3722 q.y=0.;
3723 q.z=z1; /* bottom center of sphere */
3724 BodyToFrameTAM(&q,&tam,&s1);
3725 eye.sx=s1.x; // center of the sphere
3726 eye.sy=s1.y;
3727 eye.sz=s1.z;
3728 eye.ex=po->x; // thePolar of CThreeD - position of the viewer's eye
3729 eye.ey=po->y;
3730 eye.ez=po->z;
3731
3732 NormLine3d(&eye,&u1,&d);
3733 d1=r*r/d; // distance to the center of tangent circle from sphere center
3734 r1=sqrt(r*r-d1*d1); // radius of the tangent circle
3735 l1=d-d1; // dist from circle center to eye
3736
3737 q.x=0.;
3738 q.y=0.;
3739 q.z=z2; /* top center of sphere */
3740 BodyToFrameTAM(&q,&tam,&s2);
3741 eye.sx=s2.x; // center of the sphere
3742 eye.sy=s2.y;
3743 eye.sz=s2.z;
3744 eye.ex=po->x; // thePolar of CThreeD - position of the viewer's eye
3745 eye.ey=po->y;
3746 eye.ez=po->z;
3747
3748 NormLine3d(&eye,&u2,&d);
3749 d2=r*r/d; // distance to the center of tangent circle from sphere center
3750 r2=sqrt(r*r-d2*d2); // radius of the tangent circle
3751 l2=d-d2; // dist from circle center to eye
3752
3753 ang0=acos(u1.x*u2.x+u1.y*u2.y+u1.z*u2.z); // angular separation of 2 centers
3754 ang1=atan2(r1,l1); // half-cone angle of bottom sphere
3755 ang2=atan2(r2,l2); // half-cone angle of top sphere
3756
3757 if(ang1>=0.98*(ang0+ang2)||ang2>=0.98*(ang0+ang1)){ // only one sphere is visible
3758 if(ang2>ang1){ // only top sphere is visible
3759 s1=s2; // sphere center position
3760 d1=d2; // dstance to the center of circle from sphere center
3761 r1=r2; // radius of tangent circle
3762 u1=u2; // unit vector toward viewer
3763 }
3764 ToPolar3d(&u1,&a);
3765
3766 tae.x=s1.x+d1*u1.x;
3767 tae.y=s1.y+d1*u1.y;
3768 tae.z=s1.z+d1*u1.z;
3769 tae.theta=1.5707963268-a.y; // z axis toward viewer's eye
3770 tae.phi=a.x+1.5707963268;
3771 tae.psi=0.;
3772 TAEtoTAM(&tae,&tam);
3773
3774 u.x=r1*cos(0.);
3775 u.y=r1*sin(0.);
3776 u.z=0.;
3777 BodyToFrameTAM(&u,&tam,&b);
3778 FrameToBodyTAM(&b,po,&u);
3779 NormPoint3d(&u,&b,&d);
3780 ToPolar3d(&b,&e);
3781 e.y=1.5707963267-e.y;
3782 f=e;
3783 for(i=0;i<90;i++){
3784 g=f;
3785 if(i<89){
3786 ang=.06981317008*(i+1); // every 4 degree
3787 u.x=r1*cos(ang);
3788 u.y=r1*sin(ang);
3789 u.z=0.;
3790 BodyToFrameTAM(&u,&tam,&b);
3791 FrameToBodyTAM(&b,po,&u);
3792 NormPoint3d(&u,&b,&d);
3793 ToPolar3d(&b,&f);
3794 f.y=1.5707963267-f.y;
3795 }
3796 else f=e;
3797 seg[i].sx=g.x;
3798 seg[i].sy=g.y;
3799 seg[i].ex=f.x;
3800 seg[i].ey=f.y;
3801
3802 }
3803 blk->start=0;
3804 blk->pte=90;
3805 cont->start=0;
3806 cont->pte=90;
3807 return;
3808 }
3809 // cylinder part is visible
3810 theta=1.5707963268+asin((ang1-ang2)/ang0); // half-visible angle of bottom circle
3811 // construct coordinate system with z toward eye and x toward bottom from top
3812 v.x=u2.x-u1.x; // unnormalized vector from top to bottom
3813 v.y=u2.y-u1.y;
3814 v.z=u2.z-u1.z;
3815 tz.sx=s1.x+d1*u1.x; // center of bottom circle
3816 tz.sy=s1.y+d1*u1.y;
3817 tz.sz=s1.z+d1*u1.z;
3818 tz.ex=tz.sx+u1.x; // vector toward viewer
3819 tz.ey=tz.sy+u1.y;
3820 tz.ez=tz.sz+u1.z;
3821
3822 VectorProduct(&u1,&v,&w); // obtain y-axis
3823 ty=tz;
3824 ty.ex=ty.sx+w.x; // vector toward viewer
3825 ty.ey=ty.sy+w.y;
3826 ty.ez=ty.sz+w.z;
3827 LinesToTAM(&ty,&tz,yzAxis,&tam);
3828
3829 dthet=.06981317008; // angle of a segment
3830 n=(int)(theta/dthet); // half number of segments
3831 theta=n*dthet;
3832 u.x=r1*cos(-theta);
3833 u.y=r1*sin(-theta);
3834 u.z=0.;
3835 BodyToFrameTAM(&u,&tam,&b);
3836 FrameToBodyTAM(&b,po,&u);
3837 NormPoint3d(&u,&b,&d);
3838 ToPolar3d(&b,&e);
3839 e.y=1.5707963267-e.y;
3840 f=e;
3841 n*=2;
3842 for(i=0;i<n;i++){
3843 g=f;
3844 ang=dthet*(i+1)-theta; // -theta+dthet to theta
3845 u.x=r1*cos(ang);
3846 u.y=r1*sin(ang);
3847 u.z=0.;
3848 BodyToFrameTAM(&u,&tam,&b);
3849 FrameToBodyTAM(&b,po,&u);
3850 NormPoint3d(&u,&b,&d);
3851 ToPolar3d(&b,&f);
3852 f.y=1.5707963267-f.y;
3853 seg[i].sx=g.x;
3854 seg[i].sy=g.y;
3855 seg[i].ex=f.x;
3856 seg[i].ey=f.y;
3857
3858 }
3859
3860 // construct coordinate system with z toward eye and x toward bottom from top
3861 v.x=u2.x-u1.x; // unnormalized vector from top to bottom
3862 v.y=u2.y-u1.y;
3863 v.z=u2.z-u1.z;
3864 tz.sx=s2.x+d2*u2.x; // center of top circle
3865 tz.sy=s2.y+d2*u2.y;
3866 tz.sz=s2.z+d2*u2.z;
3867 tz.ex=tz.sx+u2.x; // vector toward viewer
3868 tz.ey=tz.sy+u2.y;
3869 tz.ez=tz.sz+u2.z;
3870
3871 VectorProduct(&u2,&v,&w); // obtain y-axis
3872 ty=tz;
3873 ty.ex=ty.sx+w.x; // vector toward viewer
3874 ty.ey=ty.sy+w.y;
3875 ty.ez=ty.sz+w.z;
3876 LinesToTAM(&ty,&tz,yzAxis,&tam);
3877
3878 for(;i<91;i++){
3879 g=f;
3880 ang=dthet*i-theta;
3881 u.x=r2*cos(ang);
3882 u.y=r2*sin(ang);
3883 u.z=0.;
3884 BodyToFrameTAM(&u,&tam,&b);
3885 FrameToBodyTAM(&b,po,&u);
3886 NormPoint3d(&u,&b,&d);
3887 ToPolar3d(&b,&f);
3888 f.y=1.5707963267-f.y;
3889 seg[i].sx=g.x;
3890 seg[i].sy=g.y;
3891 seg[i].ex=f.x;
3892 seg[i].ey=f.y;
3893 }
3894 seg[i].sx=f.x;
3895 seg[i].sy=f.y;
3896 seg[i].ex=e.x;
3897 seg[i].ey=e.y;
3898 blk->start=0;
3899 blk->pte=92;
3900 cont->start=0;
3901 cont->pte=92;
3902}
3903
3904
3905/******************************************************************************
3906 FieldOfView
3907
3908 Get horizontal and vertical field of view to cover the object
3909******************************************************************************/
3910
3911void CMPbodyOfRot::FieldOfView(TransAngleMatrix *vu,Point2d *az,Point2d *el)
3912{
3913 TransAngleEuler tae;
3914 TransAngleMatrix tam;
3915 int i;
3916 Point3d s;
3917 Point3d f;
3918 Point2d a;
3919 double r0,z0,r1,z1,ca,sa,ang,d;
3920
3921 r0=p0.x;
3922 z0=p0.y;
3923 r1=p1.x;
3924 z1=p1.y;
3925 tae.x=xg; // Translate Angle Euler for the part
3926 tae.y=yg;
3927 tae.z=zg;
3928 tae.theta=thetag;
3929 tae.phi=phig;
3930 tae.psi=psig;
3931 TAEtoTAM(&tae,&tam);
3932 az->x=10.;
3933 az->y=-10.;
3934 el->x=10.;
3935 el->y=-10.;
3936 for(i=0;i<12;i++){
3937 ang=0.01745329*30.*i;
3938 ca=cos(ang);
3939 sa=sin(ang);
3940 s.x=r0*ca;
3941 s.y=r0*sa;
3942 s.z=z0;
3943 BodyToFrameTAM(&s,&tam,&f);
3944 FrameToBodyTAM(&f,vu,&s);
3945 NormPoint3d(&s,&s,&d);
3946 ToPolar3d(&s,&a);
3947 if(a.x<az->x) az->x=a.x;
3948 if(a.x>az->y) az->y=a.x;
3949 if(a.y<el->x) el->x=a.y;
3950 if(a.y>el->y) el->y=a.y;
3951 s.x=r1*ca;
3952 s.y=r1*sa;
3953 s.z=z1;
3954 BodyToFrameTAM(&s,&tam,&f);
3955 FrameToBodyTAM(&f,vu,&s);
3956 NormPoint3d(&s,&s,&d);
3957 ToPolar3d(&s,&a);
3958 if(a.x<az->x) az->x=a.x;
3959 if(a.x>az->y) az->y=a.x;
3960 if(a.y<el->x) el->x=a.y;
3961 if(a.y>el->y) el->y=a.y;
3962 }
3963}
3964
3965/********************************************************************
3966 CThreeD.cp
3967********************************************************************/
3968
3969void CThreeD::Draw()
3970{
3971 Line3d s;
3972 double aEye,dEye,rEye,xTgt,yTgt,zTgt;
3973 double w,h,d,f,ang1,ang2;
3974 Point2d p;
3975 Point3d x,y,z;
3976 double theta;
3977 double phi;
3978 double psi;
3979 double azi,inc;
3980
3981 // outstr("at the start of CThreeD::Draw()\n");
3982 // flush();
3983 AlignCoord();
3984
3985 // VuControl();
3986 // outstr("at the exit of CThreeD::VuControl()\n");
3987 // flush();
3988
3989 aEye=Azi;
3990 dEye=Ele;
3991 rEye=Dst;
3992
3993 xTgt=Tgx;
3994 yTgt=Tgy;
3995 zTgt=Tgz;
3996
3997 if(!contAll){
3998 f=rEye*cos(d2r*dEye);
3999 w=f*cos(d2r*aEye)+xTgt;
4000 h=f*sin(d2r*aEye)+yTgt;
4001 d=rEye*sin(d2r*dEye)+zTgt;
4002 rEye=sqrt(w*w+h*h+d*d);
4003 aEye=atan2(h,w)/d2r;
4004 dEye=atan2(d,sqrt(w*w+h*h))/d2r;
4005 }
4006
4007 s.sx=rEye*cos(d2r*dEye)*cos(d2r*aEye);
4008 s.sy=rEye*cos(d2r*dEye)*sin(d2r*aEye);
4009 s.sz=rEye*sin(d2r*dEye);
4010
4011 s.ex=xTgt;
4012 s.ey=yTgt;
4013 s.ez=zTgt;
4014 MakeThePolar(&s,&thePolar); // Perspective
4015
4016 // convert to double and prepare for conversion to polar coordinate
4017 w=0.5*Wid;
4018 h=0.5*Hei;
4019 d=sqrt(w*w+h*h);
4020 f=Foc;
4021 focus=Foc;
4022
4023 // outstr("at the start of processing the sides\n");
4024 // flush();
4025
4026 // start with right side of the frame
4027 azi=atan2(w,-h); // azimuth
4028 inc=atan2(d,f); // inclination
4029 RtBm.x=azi;
4030 RtBm.y=1.5707963267-inc;
4031 ClipAngleMatrix(azi,inc,&frameRt,&angFrameRt);
4032
4033 // top side of the frame
4034 azi=atan2(w,h); // azimuth
4035 inc=atan2(d,f); // inclination
4036 TpRt.x=azi;
4037 TpRt.y=1.5707963267-inc;
4038 ClipAngleMatrix(azi,inc,&frameTp,&angFrameTp);
4039
4040 // left side of the frame
4041 azi=atan2(-w,h); // azimuth
4042 inc=atan2(d,f); // inclination
4043 LtTp.x=azi;
4044 LtTp.y=1.5707963267-inc;
4045 ClipAngleMatrix(azi,inc,&frameLt,&angFrameLt);
4046
4047 // bottom side of the frame
4048 azi=atan2(-w,-h); // azimuth
4049 inc=atan2(d,f); // inclination
4050 BmLt.x=azi;
4051 BmLt.y=1.5707963267-inc;
4052 ClipAngleMatrix(azi,inc,&frameBm,&angFrameBm);
4053
4054 // outstr("at the start of MemorySetUp()\n");
4055 // flush();
4056
4057 MemorySetUp();
4058
4059 // outstr("at the start of MakePart2()\n");
4060 // flush();
4061
4062 MakePart2(); // part2 is surfaces to be used in the far-near comp
4063
4064 // outstr("at the start of NearFarRelations()\n");
4065 // flush();
4066
4067 NearFarRelations();
4068
4069 // outstr("at the start of DrawView()\n");
4070 // flush();
4071
4072 DrawView();
4073 part.clear();
4074 npart=0;
4075 message=0;
4076}
4077
4078/******************************************************************************
4079 AlignCoord
4080
4081 calculate general coordinate
4082******************************************************************************/
4083void CThreeD::AlignCoord(void)
4084{
4085 CMPpart *p1;
4086 CMPpart *p2;
4087 TransAngleEuler a,b,c;
4088 int i;
4089
4090 for(i=0;i<npart;i++){
4091 p1=part[i];
4092 a.x=p1->x0;
4093 a.y=p1->y0;
4094 a.z=p1->z0;
4095 a.theta=0.01745329252*p1->theta0;
4096 a.phi=0.01745329252*p1->phi0;
4097 a.psi=0.01745329252*p1->psi0;
4098 if(i==0) c=a;
4099 else{
4100 p2=part[p1->refPart];
4101 b.x=p2->xg;
4102 b.y=p2->yg;
4103 b.z=p2->zg;
4104 b.theta=p2->thetag;
4105 b.phi=p2->phig;
4106 b.psi=p2->psig;
4107 MoveTwiceTAE(&b,&a,&c);
4108 }
4109 p1->xg=c.x;
4110 p1->yg=c.y;
4111 p1->zg=c.z;
4112 p1->thetag=c.theta;
4113 p1->phig=c.phi;
4114 p1->psig=c.psi;
4115 }
4116}
4117
4118
4119void CThreeD::VuControl (void)
4120
4121{
4122 int i;
4123
4124 if(contAll){
4125 TransAngleEuler tae;
4126 TransAngleMatrix tam;
4127 Point2d az,azm,el,elm;
4128 double a,b,e,d;
4129 int i,j;
4130 CMPpart *p;
4131 Point3d los,tgt;
4132
4133 a=0.017453293*Azi;
4134 e=0.017453293*Ele;
4135 d=Dst;
4136 tae.x=d*cos(e)*cos(a);
4137 tae.y=d*cos(e)*sin(a);
4138 tae.z=d*sin(e);
4139 if(e>=0.){
4140 tae.theta=e;
4141 tae.phi=a-1.5707963267;
4142 tae.psi=-1.5707963267;
4143 }
4144 else{
4145 tae.theta=-e;
4146 tae.phi=a+1.5707963267;
4147 tae.psi=1.5707963267;
4148 }
4149 TAEtoTAM(&tae,&tam);
4150 azm.x=10.;
4151 azm.y=-10.;
4152 elm.x=10.;
4153 elm.y=-10.;
4154 // outstr("in CThreeD::VuControl()\n");
4155 // sprintf(msg,"npart=%i\n",npart);
4156 // outstr(msg);
4157 // flush();
4158 for(i=0;i<npart;i++){
4159 p=part[i];
4160 if(p->showDrawing){
4161 switch(p->kind){
4162 case MPlines:
4163 reinterpret_cast<CMPlines *>(p)->FieldOfView(&tam,&az,&el);
4164 break;
4165 case MPplane:
4166 reinterpret_cast<CMPplane *>(p)->FieldOfView(&tam,&az,&el);
4167 break;
4168 case MPbox:
4169 reinterpret_cast<CMPbox *>(p)->FieldOfView(&tam,&az,&el);
4170 break;
4171 case MPsphere:
4172 reinterpret_cast<CMPsphere *>(p)->FieldOfView(&tam,&az,&el);
4173 break;
4174 case MPbodyOfRot:
4175 reinterpret_cast<CMPbodyOfRot *>(p)->FieldOfView(&tam,&az,&el);
4176 break;
4177 default:
4178 az.x=10.;
4179 az.y=-10.;
4180 el.x=10.;
4181 el.y=-10.;
4182 break;
4183 }
4184 if(az.x<azm.x) azm.x=az.x;
4185 if(az.y>azm.y) azm.y=az.y;
4186 if(el.x<elm.x) elm.x=el.x;
4187 if(el.y>elm.y) elm.y=el.y;
4188 }
4189 }
4190 los.x=d*cos(0.5*(elm.x+elm.y))*cos(0.5*(azm.x+azm.y));
4191 los.y=d*cos(0.5*(elm.x+elm.y))*sin(0.5*(azm.x+azm.y));
4192 los.z=d*sin(0.5*(elm.x+elm.y));
4193 BodyToFrameTAM(&los,&tam,&tgt);
4194 Tgx=tgt.x;
4195 Tgy=tgt.y;
4196 Tgz=tgt.z;
4197 a=Wid/2-5;
4198 a=a/tan(0.5*(azm.y-azm.x));
4199 b=Hei/2-5;
4200 b=b/tan(0.5*(elm.y-elm.x));
4201 Foc=(int)((a<b)? a:b);
4202 }
4203}
4204
4205
4206/********************************************************************
4207 MemorySetUp
4208
4209 Analize itsList and allocate memory for following handles
4210
4211 part1part obtain the 'part1' number for a given 'part' number
4212 partpart1 obtain the 'part' number for a given 'part1' number
4213 part2part1 obtain the 'part2' number for a given 'part1' number
4214 part2 array of surfaces indexed by 'part2' number
4215 local array of surface points referred by part2
4216 viewed local converted to polar coordinate
4217 sorted part 1 sorted to the farthest order
4218
4219 part1part and partpart1 are defined in this routine
4220********************************************************************/
4221void CThreeD::MemorySetUp( void)
4222{
4223 int i,j,k,l,m;
4224 int ptesti;
4225 CMPpart *p;
4226 CMPpart *p0; // the first CMPplane of the consecutive group planes
4227 CMPpart *p1;
4228
4229
4230 part1part=new StartPTE[npart]; // (StartPTE *)NewPtr(sizeof(StartPTE)*npart);
4231
4232 npart1=0; // to be incremented by j
4233 npart2m=0; // to be incremented by k
4234 npoinm=0; // to be incremented by l
4235
4236 p0=NULL;
4237
4238 for(i=0;i<npart;i++){
4239 p=part[i]; // itsList->NthItem(i+1)
4240 if(p->showDrawing){
4241 switch(p->kind){
4242 case MPpoint:
4243 j=0; // points are not drawn
4244 k=0; // no near-far comparison
4245 l=0; // no surface points
4246 break;
4247 case MPlines:
4248 j=reinterpret_cast<CMPlines *>(p)->npoint;
4249 j--; // number of segments is number of points minus one
4250 k=j; // one line segment is one generalized surface
4251 l=j*2; // each generalized surface has two points
4252 break;
4253 case MPplane:
4254 j=1; // a plane is a drawing unit
4255 k=1; // one unit in the far-near comparison
4256 l=reinterpret_cast<CMPplane *>(p)->npoint; // points defining the plane
4257
4258 if(reinterpret_cast<CMPplane *>(p)->isGroup){
4259 if(p0==NULL) p0=p;
4260 p1=part[i+1];
4261 if(p1->kind==MPplane){
4262 if(reinterpret_cast<CMPplane *>(p1)->isGroup)
4263 reinterpret_cast<CMPplane *>(p)->otherHalf=reinterpret_cast<CMPplane *>(p1);
4264 else{
4265 reinterpret_cast<CMPplane *>(p)->otherHalf=reinterpret_cast<CMPplane *>(p0);
4266 p0=NULL;
4267 }
4268 }
4269 else{
4270 reinterpret_cast<CMPplane *>(p)->otherHalf=reinterpret_cast<CMPplane *>(p0);
4271 p0=NULL;
4272 }
4273 }
4274 if(reinterpret_cast<CMPplane *>(p)->isPolyhed)
4275 if(!reinterpret_cast<CMPplane *>(p)->FacingViewer((Point3d *)&thePolar)){
4276 j=0;
4277 k=0;
4278 l=0;
4279 }
4280 break;
4281 case MPbox:
4282 j=1; // a box is a drawing unit
4283 k=3; // 3 surfaces max
4284 l=12; // 4 points * 3 surfaces
4285 break;
4286 case MPsphere:
4287 j=1; // a sphere is a drawing unit
4288 k=1; // represented by a surface
4289 l=12; // represented by a 12 sided polygon
4290 break;
4291 case MPbodyOfRot:
4292 if(reinterpret_cast<CMPbodyOfRot *>(p)->isTube){
4293 j=2; // near and far sides
4294 k=12; // 12 surfaces combined, 6 surfaces each
4295 l=48; // 4 points * 12 surfaces
4296 }
4297 else{
4298 j=1; // represented by a drawing unit
4299 k=13; // 12 surfaces + top
4300 l=60; // 4 points * 12 surfaces + 12 points
4301 }
4302 break;
4303 }
4304 }
4305 else{ // if showDrawing is false skip data
4306 j=0;
4307 k=0;
4308 l=0;
4309 }
4310 part1part[i].start=npart1;
4311 npart1+=j;
4312 part1part[i].pte=npart1;
4313 npart2m+=k;
4314 npoinm+=l;
4315 }
4316 npoinm+=5; // alloance for increase by clipping
4317 partpart1=new StartPTE[npart1]; // (StartPTE *)NewPtr(sizeof(StartPTE)*npart1);
4318 for(i=0;i<npart;i++){
4319 j=part1part[i].start;
4320 k=part1part[i].pte;
4321 m=0;
4322 for(l=j;l<k;l++){
4323 partpart1[l].start=i;
4324 partpart1[l].pte=m++;
4325 }
4326 }
4327
4328 part2part1=new StartPTE[npart1]; // (StartPTE *)NewPtr(sizeof(StartPTE)*npart1);
4329 part2=new Surface[npart2m]; // (Surface *)NewPtr(sizeof(Surface)*npart2m);
4330 part1part2=new int[npart2m]; // (int *)NewPtr(sizeof(short)*npart2m);
4331 local=new Point2d[npoinm]; // (Point2d *)NewPtr(sizeof(Point2d)*npoinm);
4332 viewed=new Point2d[npoinm]; //(Point2d *)NewPtr(sizeof(Point2d)*npoinm);
4333 // sorted=(int *)NewPtr(sizeof(short)*npart1);
4334
4335
4336}
4337
4338
4339/********************************************************************
4340 MakePart2
4341********************************************************************/
4342void CThreeD::MakePart2( void)
4343{
4344 int i,j,k,l,m,n;
4345 int ptesti;
4346 CMPpart *p;
4347 double x,y,z,x1,y1,z1;
4348 Surface surf1;
4349 Surface surf2;
4350 Point2d *surfPt1;
4351 Point2d *surfPt2;
4352 Point2d a;
4353 TransAngleMatrix tam;
4354
4355 npart2=0; // number of parts after no 2 decomposition
4356 npoin=0;
4357 surfPt1=new Point2d[50]; // (Point2d *)NewPtr(sizeof(Point2d)*50);
4358 surfPt2=new Point2d[50]; // (Point2d *)NewPtr(sizeof(Point2d)*50);
4359
4360 // outstr("in MakePart2()\n");
4361 // sprintf(msg,"npart=%i\n",npart);
4362 // outstr(msg);
4363 // flush();
4364
4365 for(i=0;i<npart;i++){
4366 j=part1part[i].start;
4367 k=part1part[i].pte;
4368 // sprintf(msg,"i=%i j=%i k=%i\n",i,j,k);
4369 // outstr(msg);
4370 // flush();
4371 if(j<k){
4372 p=part[i]; // (CMPpart *)itsList->NthItem(i+1);
4373 switch(p->kind){
4374 case MPpoint:
4375 part2part1[j].start=-1; // part2 not generated
4376 part2part1[j].pte=-1;
4377 break;
4378 case MPlines:
4379 l=reinterpret_cast<CMPlines *>(p)->npoint;
4380 for(m=0;m<l-1;m++){
4381 reinterpret_cast<CMPlines *>(p)->GetSurface(m,&thePolar,&surf1,surfPt1);
4382 if(surf1.start<0){ // degenerate to a point
4383 surf2.start=-1;
4384 surf2.pte=-1;
4385 }
4386 else ClipSurface(&surf1,surfPt1,&surf2,surfPt2);
4387 if(surf2.start!=surf2.pte){
4388 part2[npart2]=surf2;
4389 part2[npart2].start=npoin;
4390 local[npoin++]=surfPt2[0];
4391 local[npoin++]=surfPt2[1];
4392 part2[npart2].pte=npoin;
4393 part1part2[npart2]=j;
4394 part2part1[j].start=npart2;
4395 npart2++;
4396 part2part1[j].pte=npart2;
4397 }
4398 else{
4399 part2part1[j].start=-1; // part2 not generated
4400 part2part1[j].pte=-1;
4401 }
4402 j++;
4403 }
4404 break;
4405 case MPplane:
4406 reinterpret_cast<CMPplane *>(p)->GetSurface(&thePolar,&surf1,surfPt1);
4407 ClipSurface(&surf1,surfPt1,&surf2,surfPt2);
4408 if(surf2.start!=surf2.pte){
4409 part2[npart2]=surf2;
4410 part2[npart2].start=npoin;
4411 for(n=surf2.start;n<surf2.pte;n++)
4412 local[npoin++]=surfPt2[n];
4413 part2[npart2].pte=npoin;
4414 part1part2[npart2]=j;
4415 part2part1[j].start=npart2;
4416 npart2++;
4417 part2part1[j].pte=npart2;
4418 }
4419 else{
4420 part2part1[j].start=-1; // part2 not generated
4421 part2part1[j].pte=-1;
4422 }
4423 break;
4424 case MPbox:
4425 // outstr("in MakePart2() at MPbox\n");
4426 // sprintf(msg,"npart2=%i\n",npart2);
4427 // outstr(msg);
4428 // flush();
4429
4430 part1part2[npart2]=j;
4431 part2part1[j].start=npart2;
4432 reinterpret_cast<CMPbox *>(p)->ClearVisSurface();
4433 for(m=0;m<3;m++){
4434
4435 if(reinterpret_cast<CMPbox *>(p)->GetSurface(m,&thePolar,&surf1,surfPt1)){
4436 // outstr("at the exit of GetSurface()\n");
4437 // sprintf(msg,"m=%i surf1.x0=%8.3f surf1.y0=%8.3f surf1.z0=%8.3f \n",m,surf1.x0,surf1.y0,surf1.z0);
4438 // outstr(msg);
4439 // sprintf(msg," surf1.theta0=%8.3f surf1.phi0=%8.3f surf1.psi0=%8.3f \n",surf1.theta0,surf1.phi0,surf1.psi0);
4440 // outstr(msg);
4441 // flush();
4442
4443 ClipSurface(&surf1,surfPt1,&surf2,surfPt2);
4444 // outstr("at the exit of ClipSurface()\n");
4445 // sprintf(msg," surf2.start=%i surf2.pte=%i npart2=%i npoin=%i j=%i\n",surf2.start,surf2.pte,npart2,npoin,j);
4446 // outstr(msg);
4447 // flush();
4448 if(surf2.start!=surf2.pte){
4449 part2[npart2]=surf2;
4450 part2[npart2].start=npoin;
4451 for(n=surf2.start;n<surf2.pte;n++)
4452 local[npoin++]=surfPt2[n];
4453 part2[npart2].pte=npoin;
4454 part1part2[npart2]=j;
4455 npart2++;
4456 }
4457 }
4458
4459 }
4460 if(part2part1[j].start!=npart2)
4461 part2part1[j].pte=npart2;
4462 else{
4463 part2part1[j].start=-1; // part2 not generated
4464 part2part1[j].pte=-1;
4465 }
4466 break;
4467 case MPsphere:
4468 reinterpret_cast<CMPsphere *>(p)->GetSurface(&thePolar,&surf1,surfPt1);
4469 ClipSurface(&surf1,surfPt1,&surf2,surfPt2);
4470 if(surf2.start!=surf2.pte){
4471 part2[npart2]=surf2;
4472 part2[npart2].start=npoin;
4473 for(n=surf2.start;n<surf2.pte;n++)
4474 local[npoin++]=surfPt2[n];
4475 part2[npart2].pte=npoin;
4476 part1part2[npart2]=j;
4477 part2part1[j].start=npart2;
4478 npart2++;
4479 part2part1[j].pte=npart2;
4480 }
4481 else{
4482 part2part1[j].start=-1; // part2 not generated
4483 part2part1[j].pte=-1;
4484 }
4485 break;
4486 case MPbodyOfRot:
4487 part1part2[npart2]=j;
4488 part2part1[j].start=npart2;
4489 reinterpret_cast<CMPbodyOfRot *>(p)->ClearVisSurface();
4490 for(m=0;m<13;m++){
4491 if(reinterpret_cast<CMPbodyOfRot *>(p)->GetSurface(m,true, // near side
4492 &thePolar,&surf1,surfPt1)){
4493 ClipSurface(&surf1,surfPt1,&surf2,surfPt2);
4494 if(surf2.start!=surf2.pte){
4495 part2[npart2]=surf2;
4496 part2[npart2].start=npoin;
4497 for(n=surf2.start;n<surf2.pte;n++)
4498 local[npoin++]=surfPt2[n];
4499 part2[npart2].pte=npoin;
4500 part1part2[npart2]=j;
4501 npart2++;
4502 }
4503 }
4504 }
4505 if(part2part1[j].start!=npart2)
4506 part2part1[j].pte=npart2;
4507 else{
4508 part2part1[j].start=-1; // part2 not generated
4509 part2part1[j].pte=-1;
4510 }
4511 if(reinterpret_cast<CMPbodyOfRot *>(p)->isTube){
4512 j++;
4513 part1part2[npart2]=j;
4514 part2part1[j].start=npart2;
4515 for(m=0;m<10;m++){
4516 if(reinterpret_cast<CMPbodyOfRot *>(p)->GetSurface(m,false,
4517 &thePolar,&surf1,surfPt1)){
4518 ClipSurface(&surf1,surfPt1,&surf2,surfPt2);
4519 if(surf2.start!=surf2.pte){
4520 part2[npart2]=surf2;
4521 part2[npart2].start=npoin;
4522 for(n=surf2.start;n<surf2.pte;n++)
4523 local[npoin++]=surfPt2[n];
4524 part1part2[npart2]=j;
4525 part2[npart2++].pte=npoin;
4526 }
4527 }
4528 }
4529 if(part2part1[j].start!=npart2)
4530 part2part1[j].pte=npart2;
4531 else{
4532 part2part1[j].start=-1; // part2 not generated
4533 part2part1[j].pte=-1;
4534 }
4535 }
4536 break;
4537 }
4538 }
4539 }
4540 delete[] surfPt1;
4541 delete[] surfPt2;
4542 // convert local to viewed
4543 for(i=0;i<npart2;i++){
4544 surf1=part2[i];
4545 SurfthePolarTAM((TransAngleEuler *)&surf1,&thePolar,&tam);
4546 for(j=surf1.start;j<surf1.pte;j++){
4547 SurfToPolar(&local[j],&tam,&a);
4548 PolarToGnomo(&a,&viewed[j]);
4549 }
4550 }
4551 delete[] local; // allocated at MemorySetUp()
4552}
4553
4554
4555/********************************************************************
4556 NearFarRelations
4557
4558 establish near-far relationship
4559********************************************************************/
4560
4561void CThreeD::NearFarRelations(void)
4562{
4563 int i,j,k,l,m;
4564 CMPpart *p;
4565 Surface s1,s2;
4566 bool s1isLine;
4567 bool doit;
4568 Point2d p0,p1;
4569 TransAngleMatrix m1,m2;
4570 double d;
4571 char ss[100];
4572 int kind1,kind2;
4573 Point3d center;
4574 Point3d s1z,s2z; // normal to the surface vector in view coordinate
4575 double s1m,s2m; // multiplier in the expression calculating z
4576 long prog,progm;
4577 int progs;
4578 CFarNear *farN;
4579 FILE *fp;
4580
4581 farN=new CFarNear(npart1);
4582
4583 // fp=fopen("/tmp/gcs3DDATA","w");
4584 // fprintf(fp,"npart=%i npart1=%i npart2=%i\n",npart,npart1,npart2);
4585 // fclose(fp);
4586
4587 // enter far-near relationship of tubes
4588 for(i=0;i<npart;i++){
4589 p=part[i];
4590 if(p->showDrawing&&p->kind==MPbodyOfRot){
4591 if(reinterpret_cast<CMPbodyOfRot *>(p)->isTube){
4592 j=part1part[i].start;
4593 farN->AddFarNear(j+1,j);
4594 // outstr("in NearFarRelations()\n");
4595 // sprintf(msg,"i=%i j=%i\n",i,j);
4596 // outstr(msg);
4597 // flush();
4598
4599 }
4600 }
4601 }
4602
4603 // if a line coinsides a side of a plane, put the line in farther position
4604 for(i=0;i<npart;i++){
4605 p=part[i];
4606 if(p->showDrawing&&p->kind==MPplane){
4607 double s1z[3],s20[3];
4608 k=part1part[i].start;
4609 s1=part2[part2part1[k].start];
4610 s1z[0]=sin(s1.theta0)*sin(s1.phi0); // axis normal to the plane
4611 s1z[1]=-sin(s1.theta0)*cos(s1.phi0);
4612 s1z[2]=cos(s1.theta0);
4613 for(j=0;j<npart2;j++){
4614 s2=part2[j];
4615 if(s2.pte-s2.start==2){ // line
4616 s20[0]=s2.x0-s1.x0;
4617 s20[1]=s2.y0-s1.y0;
4618 s20[2]=s2.z0-s1.z0;
4619 d=s1z[0]*s20[0]+s1z[0]*s20[0]+s1z[0]*s20[0];
4620 if(fabs(d)<1.e-10){
4621 m=0;
4622 for(l=s1.start;l<s1.pte;l++){
4623 if(fabs(viewed[i].x-viewed[s2.start].x)<1.e-5
4624 &&fabs(viewed[i].y-viewed[s2.start].y)<1.e-5) m++;
4625 if(fabs(viewed[i].x-viewed[s2.start+1].x)<1.e-5
4626 &&fabs(viewed[i].y-viewed[s2.start+1].y)<1.e-5) m++;
4627 }
4628 if(m==2){
4629 farN->AddFarNear(part1part2[j],k); // line plane
4630 }
4631 }
4632 }
4633 }
4634 }
4635 }
4636
4637 // outstr("in NearFarRelations()\n");
4638 // sprintf(msg,"npart2=%i\n",npart2);
4639 // outstr(msg);
4640 // flush();
4641
4642 // compare each surfaces in the combination triangle
4643 prog=0; // for progress status (long)
4644 progm=npart2;
4645 progm=progm*(progm-1)/2;
4646 for(i=0;i<npart2-1;i++){
4647 k=part1part2[i];
4648 s1=part2[i];
4649 if(s1.pte-s1.start==2) s1isLine=true;
4650 else s1isLine=false;
4651
4652 // sprintf(msg,"i=%i k=%i s1.start=%i s1.pte=%i partpart1[k].start=%i\n",i,k,s1.start,s1.pte,partpart1[k].start);
4653 // outstr(msg);
4654 // flush();
4655
4656 p=part[partpart1[k].start];
4657 kind1=p->kind;
4658 if(kind1==MPbox) reinterpret_cast<CMPbox *>(p)->GetCenter(¢er);
4659 if(kind1==MPbodyOfRot) reinterpret_cast<CMPbodyOfRot *>(p)->GetCenter(¢er);
4660 SurfthePolarTAM((TransAngleEuler *)&s1,&thePolar,&m1);
4661 if(kind1==MPplane){ // used when both parts are planes
4662 Point3d p3;
4663 p3.x=0.;
4664 p3.y=0.;
4665 p3.z=1.; // a point along body z axis
4666 BodyToFrameTAM(&p3,&m1,&s1z);
4667 s1z.x-=m1.x;
4668 s1z.y-=m1.y;
4669 s1z.z-=m1.z;
4670 s1m=s1z.x*m1.x+s1z.y*m1.y+s1z.z*m1.z; // multiplier
4671 }
4672 for(j=i+1;j<npart2;j++){
4673 prog++;
4674 l=part1part2[j];
4675 s2=part2[j];
4676 doit=true;
4677 if(k==l) doit=false;
4678 else if(s1isLine&&(s2.pte-s2.start==2)) doit=false; // dont do it for lines
4679 else doit=!(farN->InferFarNear(k,l,&m));
4680 // sprintf(msg,"j=%i k=%i l=%i doit=%i\n",j,k,l,doit);
4681 // outstr(msg);
4682 //flush();
4683 // fp=fopen("/tmp/gcs3DDATA","w");
4684 // fprintf(fp,"i=%i j=%i k=%i l=%i doit=%i\n",i,j,k,l,doit);
4685 // fclose(fp);
4686
4687 if(doit){
4688 // sprintf(msg,"part2[i].start=%i part2[i].pte=%i part2[j].start=%i part2[j].pte=%i\n",
4689 // part2[i].start,part2[i].pte,part2[j].start,part2[j].pte);
4690 // outstr(msg);
4691 // flush();
4692
4693 if(Overlap(viewed,(StartPTE *)&part2[i].start,
4694 (StartPTE *)&part2[j].start,&p0)){
4695 // outstr("Overlap\n");
4696 // flush();
4697 SurfthePolarTAM((TransAngleEuler *)&s2,&thePolar,&m2);
4698 GnomoToPolar(&p0,&p1);
4699 FarNearDist(&m1,&m2,&p1,&d);
4700 if(d>0.001){
4701 farN->AddFarNear(k,l);
4702 doit=false;
4703 }
4704 if(d<-0.001){
4705 farN->AddFarNear(l,k);
4706 doit=false;
4707 }
4708 if(doit){
4709 if(kind1==MPplane||kind1==MPbox||kind1==MPbodyOfRot){
4710 p=part[partpart1[l].start];
4711 kind2=p->kind;
4712 if(kind1==MPplane){
4713 if(kind2==MPplane){ // process the planes with common side
4714 Point3d p3;
4715 double z1,z2;
4716 int m,n10,n1p,n1n,n20,n2p,n2n; // zero, plus or negative
4717 p3.x=0.;
4718 p3.y=0.;
4719 p3.z=1.;
4720 BodyToFrameTAM(&p3,&m2,&s2z);
4721 s2z.x-=m2.x;
4722 s2z.y-=m2.y;
4723 s2z.z-=m2.z;
4724 s2m=s2z.x*m2.x+s2z.y*m2.y+s2z.z*m2.z;
4725 n10=0;
4726 n1p=0;
4727 n1n=0;
4728 n20=0;
4729 n2p=0;
4730 n2n=0;
4731 if(fabs(s1z.x*s2z.x+s1z.y*s2z.y+s1z.z*s2z.z)<0.98){ // not parallel
4732 for(m=s1.start;m<s1.pte;m++){
4733 z1=s1m/(s1z.x*viewed[m].y+s1z.y*viewed[m].x+s1z.z);
4734 z2=s2m/(s2z.x*viewed[m].y+s2z.y*viewed[m].x+s2z.z);
4735 if(z2>0.){
4736 if(fabs(z1-z2)<0.001) n10++;
4737 else if(z1>z2) n1p++;
4738 else n1n++;
4739 }
4740 }
4741 for(m=s2.start;m<s2.pte;m++){
4742 z1=s1m/(s1z.x*viewed[m].y+s1z.y*viewed[m].x+s1z.z);
4743 z2=s2m/(s2z.x*viewed[m].y+s2z.y*viewed[m].x+s2z.z);
4744 if(z1>0.){
4745 if(fabs(z1-z2)<0.001) n20++;
4746 else if(z1>z2) n2p++;
4747 else n2n++;
4748 }
4749 }
4750 if(n10>1&&n20>1){
4751 if(n1p>0&&n2p>0){ // s1 is farther
4752 farN->AddFarNear(k,l);
4753 doit=false;
4754 }
4755 if(n1n>0&&n2n>0){ // s2 is farther
4756 farN->AddFarNear(l,k);
4757 doit=false;
4758 }
4759 } // if(n10>1&&n20>1){
4760 } // if(fabs(s1z.x*s2z.x+s1z.y*s2z.y+s1z.z*s2z.z)<0.98){
4761 } // if(kind2==MPplane){
4762 else if(kind2==MPbox||kind2==MPbodyOfRot){
4763 if(kind2==MPbox) reinterpret_cast<CMPbox *>(p)->GetCenter(¢er);
4764 if(kind2==MPbodyOfRot) reinterpret_cast<CMPbodyOfRot *>(p)->GetCenter(¢er);
4765 if(SameSideOfSurface(&s1,¢er,(Point3d *)&thePolar)){
4766 farN->AddFarNear(k,l);
4767 doit=false;
4768 }
4769 else{
4770 farN->AddFarNear(l,k);
4771 doit=false;
4772 }
4773 } // else if(kind2==MPbox||kind2==MPbodyOfRot){
4774 } // if(kind1==MPplane){
4775 else{ // kind1==MPbox or MPbodyOfRot
4776 if(kind2==MPplane){
4777 if(SameSideOfSurface(&s2,¢er,(Point3d *)&thePolar)){
4778 farN->AddFarNear(l,k);
4779 doit=false;
4780 }
4781 else{
4782 farN->AddFarNear(k,l);
4783 doit=false;
4784 }
4785 } // if(kind2==MPplane){
4786 } // else
4787 } // if(kind1==MPplane||kind1==MPbox||kind1==MPbodyOfRot){
4788 } // if(doit){
4789 /* if(doit){
4790
4791 GnomoToPolar(&p0,&p1);
4792 FarNearDist(&m1,&m2,&p1,&d);
4793 if(d>0.){
4794 farN->AddFarNear(k,l);
4795 }
4796 if(d<0.){
4797 farN->AddFarNear(l,k);
4798 }
4799 }*/
4800 } // if(Overlap(viewed,(StartPTE *)&part2[i].start,(StartPTE *)&part2[j].start,&p0)){
4801 } // if(doit){
4802
4803 } // for(j=i+1;j<npart2;j++){
4804 } // for(i=0;i<npart2-1;i++){
4805
4806 delete[] part2part1; // allocated in MemorySetUp
4807 delete[] part1part2; // allocated in MemorySetUp
4808 delete[] part2; // allocated in MemorySetUp
4809 delete[] viewed; // allocated in MemorySetUp
4810 delete[] part1part; // allocated in MemorySetUp at the beginning
4811
4812 sorted=new int[npart1]; // (int *)NewPtr(sizeof(short)*npart1);
4813 fp=fopen("/tmp/gcs3DDATA","w");
4814 fprintf(fp,"before SetSorted npart1=%i\n",npart1);
4815 fclose(fp);
4816
4817 m=farN->SetSorted(sorted);
4818 if(m!=0){
4819 // sprintf(ss,"\rloop found in farNear m=%i",m);
4820 // Monitor(90,(Ptr)ss,true);
4821 }
4822 delete farN;
4823 fp=fopen("/tmp/gcs3DDATA","w");
4824 fprintf(fp,"at the end of NearFarRelations m=%i\n",m);
4825 fclose(fp);
4826
4827}
4828
4829
4830/********************************************************************
4831 DrawView
4832
4833 draw in the view
4834********************************************************************/
4835void CThreeD::DrawView(void)
4836{
4837 int i,j,k,l,m,n,o,q;
4838 StartPTE pt;
4839 CMPpart *p;
4840 Line2d *seg;
4841 StartPTE blk;
4842 StartPTE cont;
4843 double x,y;
4844 int ix,iy;
4845 double maxang;
4846 int red,green,blue;
4847 bool finished[200];
4848 bool isLines;
4849 FILE *fp;
4850 char *buff;
4851 int repeat;
4852 CMPdeco *pld;
4853
4854
4855 maxang=atan(10000./focus);
4856
4857 seg=new Line2d[200]; // (Line2d *)NewPtr(sizeof(Line2d)*200);
4858
4859 // MakeView(2*Wid,2*Hei);
4860 // SetColor(100,100,100);
4861 // OpenPoly();
4862 // MoveTo(0,0);
4863 // LineTo(0,2*Hei);
4864 // LineTo(2*Wid,2*Hei);
4865 // LineTo(2*Wid,0);
4866 // ClosePoly();
4867 fp=fopen("/tmp/gcs3DDATA","w");
4868 fwrite(&npart,sizeof(int),1,fp);
4869 fwrite(&npart1,sizeof(int),1,fp);
4870 for(i=0;i<npart1;i++){
4871 j=sorted[i]; // part 1 index
4872 pt=partpart1[j]; // part index
4873 k=pt.start;
4874 p=part[k]; // (CMPpart *)itsList->NthItem(k+1);
4875 isLines=false;
4876 repeat=1;
4877 switch (p->kind){
4878 case MPlines:
4879 reinterpret_cast<CMPlines *>(p)->GetDrawData(pt.pte,&thePolar,seg,&blk,&cont);
4880 isLines=true;
4881 break;
4882 case MPplane:
4883 reinterpret_cast<CMPplane *>(p)->GetDrawData(&thePolar,seg,&blk,&cont);
4884 l=reinterpret_cast<CMPplane *>(p)->ndeco;
4885 if(l!=0) repeat+=l;
4886 break;
4887 case MPbox:
4888 reinterpret_cast<CMPbox *>(p)->GetDrawData(&thePolar,seg,&blk,&cont);
4889 break;
4890 case MPsphere:
4891 reinterpret_cast<CMPsphere *>(p)->GetDrawData(&thePolar,seg,&blk,&cont);
4892 l=reinterpret_cast<CMPsphere *>(p)->ndeco;
4893 if(l!=0) repeat+=l;
4894 break;
4895 case MPbodyOfRot:
4896 reinterpret_cast<CMPbodyOfRot *>(p)->GetDrawData(pt.pte,&thePolar,seg,&blk,&cont);
4897 l=reinterpret_cast<CMPbodyOfRot *>(p)->ndeco;
4898 if(l!=0) repeat+=l;
4899 break;
4900 }
4901 for(m=0;m<repeat;m++){
4902 if(repeat!=1&&m>0){
4903 if(p->kind==MPplane)
4904 pld= reinterpret_cast<CMPplane *>(p)->deco[m-1];
4905 else if(p->kind==MPsphere)
4906 pld= reinterpret_cast<CMPsphere *>(p)->deco[m-1];
4907 else
4908 pld= reinterpret_cast<CMPbodyOfRot *>(p)->deco[m-1];
4909 pld->GetDrawData(&thePolar,seg,&blk,&cont);
4910 j=pld->color;
4911 }
4912 else j=p->color;
4913 l=blk.pte;
4914 k=4+4+2*4+2*4+l*4*8+10;
4915 buff=(char *)malloc(sizeof(char)*k);
4916 *(int *)buff=j;
4917 *(int *)(buff+4)=isLines;
4918 *(int *)(buff+8)=blk.start;
4919 *(int *)(buff+12)=blk.pte;
4920 *(int *)(buff+16)=cont.start;
4921 *(int *)(buff+20)=cont.pte;
4922 for(j=0;j<l;j++){
4923 *(double *)(buff+24+j*32)=seg[j].sx;
4924 *(double *)(buff+24+j*32+8)=seg[j].sy;
4925 *(double *)(buff+24+j*32+16)=seg[j].ex;
4926 *(double *)(buff+24+j*32+24)=seg[j].ey;
4927 }
4928 fwrite(&k,sizeof(int),1,fp);
4929 fwrite(buff,sizeof(char),k,fp);
4930 free(buff);
4931 }
4932 if(repeat!=1){
4933 if(p->kind==MPplane) reinterpret_cast<CMPplane *>(p)->GetDrawData(&thePolar,seg,&blk,&cont);
4934 else if(p->kind==MPsphere) reinterpret_cast<CMPsphere *>(p)->GetDrawData(&thePolar,seg,&blk,&cont);
4935 else reinterpret_cast<CMPbodyOfRot *>(p)->GetDrawData(pt.pte,&thePolar,seg,&blk,&cont);
4936 j=p->color;
4937 l=blk.pte;
4938 k=4+4+2*4+2*4+l*4*8+10;
4939 buff=(char *)malloc(sizeof(char)*k);
4940 *(int *)buff=j;
4941 *(int *)(buff+4)=isLines;
4942 *(int *)(buff+8)=blk.start;
4943 *(int *)(buff+12)=blk.pte;
4944 *(int *)(buff+16)=0;
4945 *(int *)(buff+20)=0;
4946 for(j=0;j<l;j++){
4947 *(double *)(buff+24+j*32)=seg[j].sx;
4948 *(double *)(buff+24+j*32+8)=seg[j].sy;
4949 *(double *)(buff+24+j*32+16)=seg[j].ex;
4950 *(double *)(buff+24+j*32+24)=seg[j].ey;
4951 }
4952 fwrite(&k,sizeof(int),1,fp);
4953 fwrite(buff,sizeof(char),k,fp);
4954 free(buff);
4955 }
4956 }
4957 k=0; /* end of data */
4958 fwrite(&k,sizeof(int),1,fp);
4959 k=90;
4960 buff=(char *)malloc(sizeof(char)*k);
4961 *(double *)buff=thePolar.x;
4962 *(double *)(buff+8)=thePolar.y;
4963 *(double *)(buff+8*2)=thePolar.z;
4964 *(double *)(buff+8*3)=thePolar.c[0][0];
4965 *(double *)(buff+8*4)=thePolar.c[0][1];
4966 *(double *)(buff+8*5)=thePolar.c[0][2];
4967 *(int *)(buff+8*6)=p->kind;
4968 *(double *)(buff+52)=p->x0;
4969 *(double *)(buff+52+8)=p->y0;
4970 *(double *)(buff+52+8*2)=p->z0;
4971 *(double *)(buff+52+8*3)=p->xg;
4972 fwrite(buff,sizeof(char),k,fp);
4973 free(buff);
4974 fclose(fp);
4975#if 0
4976 k=cont.start; // supposed to cover all segments
4977 l=blk.pte;
4978 for(j=k;j<l;j++){
4979 x=seg[j].sx;
4980 y=seg[j].sy;
4981 if(y>maxang) y=maxang;
4982 y=2*focus*tan(y);
4983 seg[j].sx=y*sin(x);
4984 seg[j].sy=y*cos(x);
4985 x=seg[j].ex;
4986 y=seg[j].ey;
4987 if(y>maxang) y=maxang;
4988 y=2*focus*tan(y);
4989 seg[j].ex=y*sin(x);
4990 seg[j].ey=y*cos(x);
4991 }
4992
4993 if(l-k>1){
4994 switch(p->color){
4995 case 0:
4996 red=100;
4997 green=100;
4998 blue=100;
4999 break;
5000 case 1:
5001 red=75;
5002 green=75;
5003 blue=75;
5004 break;
5005 case 2:
5006 red=100;
5007 green=75;
5008 blue=75;
5009 break;
5010 case 3:
5011 red=100;
5012 green=100;
5013 blue=75;
5014 break;
5015 case 4:
5016 red=75;
5017 green=100;
5018 blue=75;
5019 break;
5020 case 5:
5021 red=75;
5022 green=100;
5023 blue=100;
5024 break;
5025 case 6:
5026 red=75;
5027 green=75;
5028 blue=100;
5029 break;
5030 case 7:
5031 red=100;
5032 green=75;
5033 blue=100;
5034 break;
5035 }
5036 // SetColor(red,green,blue);
5037 // OpenPoly();
5038 // PenSize(1,1);
5039 k=cont.start; // start of contour
5040 l=cont.pte-cont.start; // number of contour segments
5041 q=k;
5042 ix=(int)(seg[q].sx+Wid);
5043 iy=(int)(Hei-seg[q].sy);
5044 // MoveTo(ix,iy);
5045 for(n=0;n<l;n++) finished[n]=false;
5046 for(m=0;m<l;m++){
5047 ix=(int)(seg[q].ex+Wid);
5048 iy=(int)(Hei-seg[q].ey);
5049 // LineTo(ix,iy);
5050 for(n=0;n<l;n++){
5051 if(!finished[n]){
5052 o=k+n;
5053 if(seg[q].ex==seg[o].sx && seg[q].ey==seg[o].sy){
5054 q=o;
5055 finished[n]=true;
5056 break;
5057 }
5058 }
5059 }
5060 }
5061 // ClosePoly();
5062 }
5063 // PenSize(ContPen,ContPen);
5064 // SetColor(0,0,0);
5065 if(isLines){
5066 switch(p->color){
5067 case 1:
5068 // PenSize(2,2);
5069 break;
5070 case 2:
5071 // PenSize(4,4);
5072 break;
5073 case 3:
5074 // PenSize(8,8);
5075 break;
5076 case 4:
5077 red=100;
5078 green=0;
5079 blue=0;
5080 // SetColor(red,green,blue);
5081 // PenSize(4,4);
5082 break;
5083 case 5:
5084 red=100;
5085 green=0;
5086 blue=0;
5087 // SetColor(red,green,blue);
5088 // PenSize(8,8);
5089 break;
5090 case 6:
5091 red=0;
5092 green=0;
5093 blue=100;
5094 // SetColor(red,green,blue);
5095 // PenSize(4,4);
5096 break;
5097 case 7:
5098 red=0;
5099 green=0;
5100 blue=100;
5101 // SetColor(red,green,blue);
5102 // PenSize(8,8);
5103 break;
5104 }
5105 }
5106 k=blk.start;
5107 l=blk.pte;
5108 for(m=k;m<l;m++){
5109 ix=(int)(seg[m].sx+Wid);
5110 iy=(int)(Hei-seg[m].sy);
5111 // MoveTo(ix,iy);
5112 ix=(int)(seg[m].ex+Wid);
5113 iy=(int)(Hei-seg[m].ey);
5114 // LineTo(ix,iy);
5115 }
5116 if(isLines){
5117 // PenSize(1,1);
5118 red=0;
5119 green=0;
5120 blue=0;
5121 // SetColor(red,green,blue);
5122 }
5123}
5124#endif
5125delete[] seg; // DisposPtr((Ptr)seg);
5126delete[] partpart1; // DisposPtr((Ptr)partpart1);
5127delete[] sorted; // DisposPtr((Ptr)sorted);
5128
5129
5130}
5131
5132
5133/********************************************************************
5134 ClipSurface
5135
5136 clip a surface by the view frame
5137********************************************************************/
5138void CThreeD::ClipSurface(Surface *surf1,Point2d *surfPt1,
5139 Surface *surf2,Point2d *surfPt2)
5140{
5141 TransAngleMatrix s; // body:surface, frame:thePolar
5142 Point2d p0,p1,p2,p3,p4;
5143 int nx; // number of crossing points
5144 Point2d p[8]; // crossing point
5145 int ix[8]; // start point index
5146 int jx[8]; // status 1 : coming-in, 2 : going-out
5147 int kx[8]; // clip-side 1:Rt, 2:Tp, 3:Lt, 4:Bm
5148 double ang[8]; // angle from the corner
5149 int status; // returned status
5150 double an; // returned angle
5151 bool inside;
5152 int i,j,k,l;
5153
5154 SurfthePolarTAM((TransAngleEuler *)surf1,&thePolar,&s); // Perspective
5155 SurfToPolar(&surfPt1[0],&s,&p0); // (x,y) to (alpha,delta)
5156 p2=p0;
5157 nx=0;
5158 // outstr("in ClipSurface()\n");
5159 // sprintf(msg," surf1->start=%i surf1->pte=%i \n",surf1->start,surf1->pte);
5160 // outstr(msg);
5161 // flush();
5162
5163 for(i=surf1->start;i<surf1->pte;i++){
5164 p1=p2;
5165 if(i+1==surf1->pte){
5166 if(surf1->pte-surf1->start==2) break;
5167 else p2=p0;
5168 }
5169 else SurfToPolar(&surfPt1[i+1],&s,&p2);
5170
5171 status=CrossClipSide(&frameRt,angFrameRt,&p1,&p2,&p3,&an); // Perspective
5172
5173 if(status<3){ // crossing or inside
5174 if(status>0){ // crossing
5175 p[nx]=p3; // crossing point in polar coordinate
5176 ix[nx]=i; // start point index
5177 jx[nx]=status; // status 1 or 2
5178 kx[nx]=1; // clip rect side - right
5179 ang[nx]=an; // distance from right-bottom corner
5180 nx++;
5181 }
5182 status=CrossClipSide(&frameTp,angFrameTp,&p1,&p2,&p3,&an);
5183
5184 }
5185 if(status<3){
5186 if(status>0){
5187 p[nx]=p3;
5188 ix[nx]=i;
5189 jx[nx]=status;
5190 kx[nx]=2;
5191 ang[nx]=an;
5192 nx++;
5193 }
5194 status=CrossClipSide(&frameLt,angFrameLt,&p1,&p2,&p3,&an);
5195
5196 }
5197 if(status<3){
5198 if(status>0){
5199 p[nx]=p3;
5200 ix[nx]=i;
5201 jx[nx]=status;
5202 kx[nx]=3;
5203 ang[nx]=an;
5204 nx++;
5205 }
5206 status=CrossClipSide(&frameBm,angFrameBm,&p1,&p2,&p3,&an);
5207
5208 }
5209 if(status<3){
5210 if(status>0){
5211 p[nx]=p3;
5212 ix[nx]=i;
5213 jx[nx]=status;
5214 kx[nx]=4;
5215 ang[nx]=an;
5216 nx++;
5217 }
5218 }
5219
5220
5221 }
5222 // outstr("in ClipSurface()\n");
5223 // sprintf(msg," nx=%i status=%i surf1->start=%i surf1->pte=%i \n",nx,status,surf1->start,surf1->pte);
5224 // outstr(msg);
5225 // flush();
5226
5227
5228 if(nx==0){
5229 if(status==0){ // the whole surface is inside the clip rect
5230 *surf2=*surf1;
5231 for(i=surf1->start;i<surf1->pte;i++) surfPt2[i]=surfPt1[i];
5232 return;
5233 }
5234 else{
5235 *surf2=*surf1;
5236 surf2->pte=surf2->start; // set past-the-end to start
5237 return;
5238 }
5239 }
5240 // if two crossings occur for a segment, 'enter' comes first and then comes 'leave'
5241 for(i=1;i<nx;i++){
5242 if(ix[i]==ix[i-1]){
5243 if(jx[i]==1){ // enter
5244 p3=p[i];
5245 p[i]=p[i-1];
5246 p[i-1]=p3;
5247 j=jx[i];
5248 jx[i]=jx[i-1];
5249 jx[i-1]=j;
5250 j=kx[i];
5251 kx[i]=kx[i-1];
5252 kx[i-1]=j;
5253 an=ang[i];
5254 ang[i]=ang[i-1];
5255 ang[i-1]=an;
5256 }
5257 }
5258 }
5259
5260 // add corner points after 'leave' and before 'enter'
5261 for(i=0;i<nx;i++){
5262 if(jx[i]==2){ // leave
5263 j=i+1;
5264 if(j==nx) j=0; // next enter point
5265 k=kx[i]; // side of leave point
5266 while(k!=kx[j]){
5267 k++;
5268 if(k==5) k=1;
5269 for(l=nx;l>i+1;l--){ // make room for a corner point
5270 p[l]=p[l-1];
5271 ix[l]=ix[l-1];
5272 jx[l]=jx[l-1];
5273 kx[l]=kx[l-1];
5274 ang[l]=ang[l-1];
5275 }
5276 switch(k){
5277 case 1:
5278 p[i+1]=RtBm;
5279 break;
5280 case 2:
5281 p[i+1]=TpRt;
5282 break;
5283 case 3:
5284 p[i+1]=LtTp;
5285 break;
5286 case 4:
5287 p[i+1]=BmLt;
5288 break;
5289 }
5290 ix[i+1]=ix[i];
5291 jx[i+1]=4; // status is corner point
5292 kx[i+1]=k;
5293 ang[i+1]=0.;
5294 nx++;
5295 i++;
5296 }
5297 }
5298 }
5299
5300 // convert polar coordinate to surface coordinate
5301 for(i=0;i<nx;i++) PolarToSurf(&p[i],&s,&p[i]);
5302
5303 // generate new surface
5304 j=0; // index to crossing points
5305 k=0; // index to output points
5306 for(i=surf1->start;i<surf1->pte;i++){
5307 inside=false;
5308 for(j=0;j<nx;j++){
5309 if(i<=ix[j]&&jx[j]==2) {
5310 inside=true;
5311 break;
5312 }
5313 if(i<=ix[j]&&jx[j]==1) {
5314 inside=false;
5315 break;
5316 }
5317 }
5318 if(j==nx&&jx[0]==2) inside=true;
5319 if(inside){
5320 surfPt2[k]=surfPt1[i];
5321 k++;
5322 }
5323 for(j=0;j<nx;j++){
5324 if(i==ix[j]){
5325 surfPt2[k]=p[j];
5326 k++;
5327 }
5328 }
5329 }
5330 *surf2=*surf1;
5331 surf2->start=0;
5332 surf2->pte=k;
5333
5334}
5335
5336/********************************************************************
5337 ToDB
5338
5339 generate stream data
5340********************************************************************/
5341char *CThreeD::ToDB(int *len)
5342{
5343 int i,j,k,l,m,n;
5344 CMPpart *p;
5345 Point2d *p2;
5346 Point3d *p3;
5347 char *e;
5348
5349 n=0;
5350 n+=4; /* npart */
5351 for(i=0;i<npart;i++){
5352 n+=4; /* i */
5353 p=part[i];
5354 n+=4; /* kind */
5355 j=strlen(p->desc.c_str());
5356 n+=4; /* j */
5357 n+=j+1; /* strcpy */
5358 n+=4; /* refPart */
5359 n+=8*6; /* x0,y0,z0,theta0,phi0,psi0 */
5360 n+=4; /* color */
5361 n+=4; /* showDrawing */
5362 switch(p->kind){
5363 case MPpoint:
5364 break;
5365 case MPlines:
5366 k=reinterpret_cast<CMPlines *>(p)->npoint;
5367 n+=4; /* k */
5368 n+=k*24; /* x,y,z */
5369 n+=4; /* line */
5370 break;
5371 case MPplane:
5372 k=reinterpret_cast<CMPplane *>(p)->npoint;
5373 n+=4; /* k */
5374 n+=k*16; /* x,y */
5375 n+=4; /* isPolyhed */
5376 n+=4; /* isGroup */
5377 break;
5378 case MPbox:
5379 n+=24; /* lx,ly,lz */
5380 n+=4; /* visibleSurface */
5381 break;
5382 case MPsphere:
5383 n+=8; /* r */
5384 break;
5385 case MPbodyOfRot:
5386 n+=4; /* npoint */
5387 n+=32; /* p0.x,p0.y,p1.x,p1.y */
5388 n+=16; /* isTube,oblong,appear,visibleSurfac */
5389 break;
5390 }
5391 }
5392 *len=n;
5393 e=(char *)malloc(sizeof(char)*n);
5394 n=0;
5395 *(int *)(e+n)=npart;
5396 n+=4;
5397 for(i=0;i<npart;i++){
5398 *(int *)(e+n)=i;
5399 n+=4;
5400 p=part[i];
5401 *(int *)(e+n)=p->kind;
5402 n+=4;
5403 j=strlen(p->desc.c_str());
5404 *(int *)(e+n)=j;
5405 n+=4;
5406 strcpy(e+n,p->desc.c_str());
5407 n+=j+1;
5408 *(int *)(e+n)=p->refPart;
5409 n+=4;
5410 *(double *)(e+n)=p->x0;
5411 n+=8;
5412 *(double *)(e+n)=p->y0;
5413 n+=8;
5414 *(double *)(e+n)=p->z0;
5415 n+=8;
5416 *(double *)(e+n)=p->theta0;
5417 n+=8;
5418 *(double *)(e+n)=p->phi0;
5419 n+=8;
5420 *(double *)(e+n)=p->psi0;
5421 n+=8;
5422 *(int *)(e+n)=p->color;
5423 n+=4;
5424 *(int *)(e+n)=p->showDrawing;
5425 n+=4;
5426 switch(p->kind){
5427 case MPpoint:
5428 break;
5429 case MPlines:
5430 k=reinterpret_cast<CMPlines *>(p)->npoint;
5431 *(int *)(e+n)=k;
5432 n+=4;
5433 for(j=0;j<k;j++){
5434 p3=&reinterpret_cast<CMPlines *>(p)->pt[j];
5435 *(double *)(e+n)=p3->x;
5436 n+=8;
5437 *(double *)(e+n)=p3->y;
5438 n+=8;
5439 *(double *)(e+n)=p3->z;
5440 n+=8;
5441 }
5442 *(int *)(e+n)=reinterpret_cast<CMPlines *>(p)->line;
5443 n+=4;
5444 break;
5445 case MPplane:
5446 k=reinterpret_cast<CMPplane *>(p)->npoint;
5447 *(int *)(e+n)=k;
5448 n+=4;
5449 for(j=0;j<k;j++){
5450 p2=&reinterpret_cast<CMPplane *>(p)->pt[j];
5451 *(double *)(e+n)=p2->x;
5452 n+=8;
5453 *(double *)(e+n)=p2->y;
5454 n+=8;
5455 }
5456 *(int *)(e+n)=reinterpret_cast<CMPplane *>(p)->isPolyhed;
5457 n+=4;
5458 *(int *)(e+n)=reinterpret_cast<CMPplane *>(p)->isGroup;
5459 n+=4;
5460 break;
5461 case MPbox:
5462 *(double *)(e+n)=reinterpret_cast<CMPbox *>(p)->lx;
5463 n+=8;
5464 *(double *)(e+n)=reinterpret_cast<CMPbox *>(p)->ly;
5465 n+=8;
5466 *(double *)(e+n)=reinterpret_cast<CMPbox *>(p)->lz;
5467 n+=8;
5468 *(int *)(e+n)=reinterpret_cast<CMPbox *>(p)->visibleSurface;
5469 n+=4;
5470 break;
5471 case MPsphere:
5472 *(double *)(e+n)=reinterpret_cast<CMPsphere *>(p)->r;
5473 n+=8;
5474 break;
5475 case MPbodyOfRot:
5476 *(int *)(e+n)=reinterpret_cast<CMPbodyOfRot *>(p)->npoint;
5477 n+=4;
5478 *(double *)(e+n)=reinterpret_cast<CMPbodyOfRot *>(p)->p0.x;
5479 n+=8;
5480 *(double *)(e+n)=reinterpret_cast<CMPbodyOfRot *>(p)->p0.y;
5481 n+=8;
5482 *(double *)(e+n)=reinterpret_cast<CMPbodyOfRot *>(p)->p1.x;
5483 n+=8;
5484 *(double *)(e+n)=reinterpret_cast<CMPbodyOfRot *>(p)->p1.y;
5485 n+=8;
5486 *(int *)(e+n)=reinterpret_cast<CMPbodyOfRot *>(p)->isTube;
5487 n+=4;
5488 *(int *)(e+n)=reinterpret_cast<CMPbodyOfRot *>(p)->oblong;
5489 n+=4;
5490 *(int *)(e+n)=reinterpret_cast<CMPbodyOfRot *>(p)->appear;
5491 n+=4;
5492 *(int *)(e+n)=reinterpret_cast<CMPbodyOfRot *>(p)->visibleSurface;
5493 n+=4;
5494 break;
5495 }
5496 }
5497 return e;
5498}
5499
5500/********************************************************************
5501 FromDB
5502
5503 load stream data
5504********************************************************************/
5505void CThreeD::FromDB(int len,char *e,double loc[6])
5506{
5507 int i,j,k,l,m,n,i1,i2;
5508 char s[200];
5509 CMPpart *p;
5510 Point2d *p2;
5511 Point3d *p3;
5512
5513 n=0;
5514 l=npart;
5515 m=*(int *)e;
5516 n+=4;
5517 for(i=0;i<m;i++){
5518 /* *(int *)(e+n)=i */
5519 n+=4;
5520 i1=*(int *)(e+n); // kind
5521 n+=4;
5522 i2=*(int *)(e+n); // strlen
5523 n+=4;
5524 strcpy(s,e+n);
5525 n+=i2+1;
5526 i2=*(int *)(e+n)+l; // refPart
5527 if(i==0) i2=0;
5528 n+=4;
5529 switch(i1){
5530 case MPpoint:
5531 part.push_back(new CMPpoint(s,i2,i1));
5532 break;
5533 case MPlines:
5534 part.push_back(new CMPlines(s,i2,i1));
5535 break;
5536 case MPplane:
5537 part.push_back(new CMPplane(s,i2,i1));
5538 break;
5539 case MPbox:
5540 part.push_back(new CMPbox(s,i2,i1));
5541 break;
5542 case MPsphere:
5543 part.push_back(new CMPsphere(s,i2,i1));
5544 break;
5545 case MPbodyOfRot:
5546 part.push_back(new CMPbodyOfRot(s,i2,i1));
5547 break;
5548 default:
5549 break;
5550 }
5551 npart++;
5552 p=part[npart-1];
5553 if(i==0){
5554 p->x0=loc[0];
5555 p->y0=loc[1];
5556 p->z0=loc[2];
5557 p->theta0=loc[3];
5558 p->phi0=loc[4];
5559 p->psi0=loc[5];
5560 n+=48;
5561 }
5562 else{
5563 p->x0=*(double *)(e+n);
5564 n+=8;
5565 p->y0=*(double *)(e+n);
5566 n+=8;
5567 p->z0=*(double *)(e+n);
5568 n+=8;
5569 p->theta0=*(double *)(e+n);
5570 n+=8;
5571 p->phi0=*(double *)(e+n);
5572 n+=8;
5573 p->psi0=*(double *)(e+n);
5574 n+=8;
5575 }
5576 p->color=*(int *)(e+n);
5577 n+=4;
5578 p->showDrawing=*(int *)(e+n);
5579 n+=4;
5580 switch(p->kind){
5581 case MPpoint:
5582 break;
5583 case MPlines:
5584 k=*(int *)(e+n);
5585 n+=4;
5586 for(j=0;j<k;j++){
5587 reinterpret_cast<CMPlines *>(p)->SetPoint(*(double *)(e+n),*(double *)(e+n+8),*(double *)(e+n+16));
5588 n+=24;
5589 }
5590 reinterpret_cast<CMPlines *>(p)->line=*(int *)(e+n);
5591 n+=4;
5592 break;
5593 case MPplane:
5594 k=*(int *)(e+n);
5595 n+=4;
5596 for(j=0;j<k;j++){
5597 reinterpret_cast<CMPplane *>(p)->SetPoint(*(double *)(e+n),*(double *)(e+n+8));
5598 n+=16;
5599 }
5600 reinterpret_cast<CMPplane *>(p)->isPolyhed=*(int *)(e+n);
5601 n+=4;
5602 reinterpret_cast<CMPplane *>(p)->isGroup=*(int *)(e+n);
5603 n+=4;
5604 break;
5605 case MPbox:
5606 reinterpret_cast<CMPbox *>(p)->SetSize(*(double *)(e+n),*(double *)(e+n+8),*(double *)(e+n+16));
5607 n+=24;
5608 reinterpret_cast<CMPbox *>(p)->visibleSurface=*(int *)(e+n);
5609 n+=4;
5610 break;
5611 case MPsphere:
5612 reinterpret_cast<CMPsphere *>(p)->r=*(double *)(e+n);
5613 n+=8;
5614 break;
5615 case MPbodyOfRot:
5616 reinterpret_cast<CMPbodyOfRot *>(p)->npoint=*(int *)(e+n);
5617 n+=4;
5618 reinterpret_cast<CMPbodyOfRot *>(p)->p0.x=*(double *)(e+n);
5619 n+=8;
5620 reinterpret_cast<CMPbodyOfRot *>(p)->p0.y=*(double *)(e+n);
5621 n+=8;
5622 reinterpret_cast<CMPbodyOfRot *>(p)->p1.x=*(double *)(e+n);
5623 n+=8;
5624 reinterpret_cast<CMPbodyOfRot *>(p)->p1.y=*(double *)(e+n);
5625 n+=8;
5626 reinterpret_cast<CMPbodyOfRot *>(p)->isTube=*(int *)(e+n);
5627 n+=4;
5628 reinterpret_cast<CMPbodyOfRot *>(p)->oblong=*(int *)(e+n);
5629 n+=4;
5630 reinterpret_cast<CMPbodyOfRot *>(p)->appear=*(int *)(e+n);
5631 n+=4;
5632 reinterpret_cast<CMPbodyOfRot *>(p)->visibleSurface=*(int *)(e+n);
5633 n+=4;
5634 break;
5635 }
5636 }
5637}
5638
5639/******************************************************************************
5640 CFarNear Far-Near relationship of 3D Drawing Objects
5641******************************************************************************/
5642
5643
5644CFarNear::CFarNear(int numpart1)
5645 : npart1(numpart1)
5646{
5647 int i;
5648
5649 nmax=npart1*(npart1-1)/2;
5650 farNear=new StartPTE[nmax+1]; // (StartPTE *)NewPtr(sizeof(StartPTE)*nmax);
5651 farNearIndex=new long[npart1+1]; // (long *)NewPtr(sizeof(long)*(npart1+1));
5652 for(i=0;i<=npart1;i++) farNearIndex[i]=0L;
5653 n=0;
5654}
5655
5656/******************************************************************************
5657 Dispose
5658
5659******************************************************************************/
5660
5661CFarNear::~CFarNear()
5662{
5663 delete[] farNear; // DisposPtr((Ptr)farNear);
5664 delete[] farNearIndex; // DisposPtr((Ptr)farNearIndex);
5665}
5666
5667/******************************************************************************
5668 AddFarNear
5669
5670******************************************************************************/
5671
5672void CFarNear::AddFarNear(int far,int near)
5673{
5674 StartPTE a;
5675 long m;
5676 int i;
5677
5678 a.start=far;
5679 a.pte=near;
5680 m=farNearIndex[a.start+1];
5681 for(i=a.start+1;i<=npart1;i++) farNearIndex[i]++;
5682 // BlockMove((Ptr)&farNear[m],(Ptr)&farNear[m+1],(n-m)*sizeof(StartPTE));
5683 for(i=n;i>=m;i--) farNear[i+1]=farNear[i];
5684 farNear[m]=a;
5685 n++;
5686 // outstr("in AddFarNear()\n");
5687 // for(i=0;i<n;i++){
5688 // sprintf(msg," i=%i farNear[i].start=%i farNear[i].pte=%i \n",i,farNear[i].start,farNear[i].pte);
5689 // outstr(msg);
5690 // }
5691 // flush();
5692
5693}
5694
5695/******************************************************************************
5696 inferFarNear
5697
5698******************************************************************************/
5699
5700bool CFarNear::InferFarNear(int p1,int p2,int *near)
5701{
5702 // test if p1 is farther
5703 *near=p2;
5704 if(RecurseFarNear(p1,p2,0)) return true;
5705 // test if p2 is farther
5706 *near=p1;
5707 if(RecurseFarNear(p2,p1,0)) return true;
5708 return false;
5709}
5710
5711/******************************************************************************
5712 recurseFarNear
5713
5714******************************************************************************/
5715
5716bool CFarNear::RecurseFarNear(int p1,int p2,long depth)
5717{
5718 long i,j,k;
5719
5720 depth++;
5721 if(depth>n) return false; // escape loop
5722
5723 j=farNearIndex[p1];
5724 k=farNearIndex[p1+1];
5725 for(i=j;i<k;i++){
5726 if(farNear[i].pte==p2) return true;
5727 if(RecurseFarNear(farNear[i].pte,p2,depth)) return true;
5728 }
5729 return false;
5730}
5731
5732/******************************************************************************
5733 SetSorted
5734
5735******************************************************************************/
5736
5737int CFarNear::SetSorted(int *sorted)
5738{
5739 int i,j,l;
5740 long k,m;
5741 int *status;
5742 FILE *fp;
5743
5744 // outstr("in SetSorted()\n");
5745 // sprintf(msg," npart1=%i n=%i \n",npart1,n);
5746 // outstr(msg);
5747 // flush();
5748 // for(i=0;i<n;i++){
5749 // sprintf(msg," farNear[i].start=%5i farNear[i].pte=%5i\n",farNear[i].start,farNear[i].pte);
5750 // outstr(msg);
5751 // }
5752 // flush();
5753 fp=fopen("/tmp/gcs3DDATA","w");
5754 fprintf(fp,"in SetSorted()\n");
5755 fprintf(fp," npart1=%i n=%i \n",npart1,n);
5756 for(i=0;i<n;i++){
5757 fprintf(fp," farNear[i].start=%5i farNear[i].pte=%5i\n",farNear[i].start,farNear[i].pte);
5758 }
5759 fclose(fp);
5760
5761 status=new int[npart1]; // (int *)NewPtr(sizeof(short)*npart1);
5762 for(i=0;i<npart1;i++) status[i]=0;
5763 j=0;
5764 i=0;
5765 do{
5766 for(k=0;k<n;k++){
5767 l=farNear[k].start;
5768 if(l<10000){
5769 switch (status[l]){
5770 case 0:
5771 for(m=0;m<n;m++) if(l==farNear[m].pte) break;
5772 if(m==n){
5773 sorted[j++]=l;
5774 status[l]=2;
5775 farNear[k].start=10000;
5776 farNear[k].pte=10000;
5777 }
5778 else status[l]=1;
5779 break;
5780 case 1:
5781 break;
5782 case 2:
5783 case 3:
5784 farNear[k].start=10000;
5785 farNear[k].pte=10000;
5786 break;
5787 default:
5788 break;
5789 }
5790 }
5791 }
5792 m=0;
5793 for(k=0;k<npart1;k++){
5794 l=status[k];
5795 if(l==1){
5796 m++;
5797 status[k]=0;
5798 }
5799 if(l==2){
5800 if(m<10000) m+=10000;
5801 status[k]=3;
5802 }
5803 }
5804 if(m<10000) i++;
5805 } while(i<2);
5806 fp=fopen("/tmp/gcs3DDATA","w");
5807 fprintf(fp,"at the end of SetSorted()\n");
5808 fprintf(fp," npart1=%i n=%i j=%i \n",npart1,n,j);
5809 for(i=0;i<n;i++){
5810 fprintf(fp," farNear[i].start=%5i farNear[i].pte=%5i\n",farNear[i].start,farNear[i].pte);
5811 }
5812 for(i=0;i<npart1;i++) fprintf(fp," status[i]=%i sorted[i]=%i\n",status[i],sorted[i]);
5813 fclose(fp);
5814 for(k=0;k<npart1;k++) if(status[k]!=3) sorted[j++]=k;
5815 delete[] status; // DisposPtr((Ptr)status);
5816 fp=fopen("/tmp/gcs3DDATA","w");
5817 fprintf(fp,"before retun of SetSorted()\n");
5818 fprintf(fp," npart1=%i n=%i j=%i \n",npart1,n,j);
5819 for(i=0;i<n;i++){
5820 fprintf(fp," farNear[i].start=%5i farNear[i].pte=%5i\n",farNear[i].start,farNear[i].pte);
5821 }
5822 for(i=0;i<npart1;i++) fprintf(fp," status[i]=%i sorted[i]=%i\n",status[i],sorted[i]);
5823 fclose(fp);
5824 return m;
5825}
5826
5827int CMPdeco::GetDrawData(TransAngleMatrix *po,Line2d *seg,StartPTE *blk,StartPTE *cont)
5828{
5829 TransAngleEuler tae;
5830 TransAngleMatrix tam;
5831 Point3d a,b,c,d;
5832 Point2d e,f,g;
5833 double r;
5834 int i,j,k,l;
5835 AngleEuler ae;
5836 AngleMatrix amb; /* body */
5837 Line3d eye;
5838 double d0,ra,d1,r1;
5839 AngleMatrix ame; /* eye */
5840 AngleMatrix amc; /* combined */
5841 Point2d *p1;
5842 int np1,nl1;
5843
5844 if(p->kind==MPplane){
5845 k=0;
5846 tae.x=p->xg; // Translate Angle Euler for the part
5847 tae.y=p->yg;
5848 tae.z=p->zg;
5849 tae.theta=p->thetag;
5850 tae.phi=p->phig;
5851 tae.psi=p->psig;
5852 TAEtoTAM(&tae,&tam);
5853 if(ncontour!=0){
5854 a.x=contour[0].x;
5855 a.y=contour[0].y;
5856 a.z=0.;
5857 BodyToFrameTAM(&a,&tam,&b);
5858 FrameToBodyTAM(&b,po,&c);
5859 NormPoint3d(&c,&d,&r);
5860 ToPolar3d(&d,&e);
5861 e.y=1.5707963267-e.y;
5862 f=e;
5863 for(i=1;i<=ncontour;i++){
5864 g=f;
5865 if(i<ncontour){
5866 a.x=contour[i].x;
5867 a.y=contour[i].y;
5868 a.z=0.;
5869 BodyToFrameTAM(&a,&tam,&b);
5870 FrameToBodyTAM(&b,po,&c);
5871 NormPoint3d(&c,&d,&r);
5872 ToPolar3d(&d,&f);
5873 f.y=1.5707963267-f.y;
5874 }
5875 else f=e;
5876 seg[k].sx=g.x;
5877 seg[k].sy=g.y;
5878 seg[k].ex=f.x;
5879 seg[k].ey=f.y;
5880 k++;
5881 }
5882 }
5883 cont->start=0;
5884 cont->pte=k;
5885 blk->start=k;
5886 if(nlineseg!=0){
5887 for(i=0;i<nlineseg;i++){
5888 a.x=lineseg[i].sx;
5889 a.y=lineseg[i].sy;
5890 a.z=0.;
5891 BodyToFrameTAM(&a,&tam,&b);
5892 FrameToBodyTAM(&b,po,&c);
5893 NormPoint3d(&c,&d,&r);
5894 ToPolar3d(&d,&e);
5895 e.y=1.5707963267-e.y;
5896 a.x=lineseg[i].ex;
5897 a.y=lineseg[i].ey;
5898 a.z=0.;
5899 BodyToFrameTAM(&a,&tam,&b);
5900 FrameToBodyTAM(&b,po,&c);
5901 NormPoint3d(&c,&d,&r);
5902 ToPolar3d(&d,&f);
5903 f.y=1.5707963267-f.y;
5904 seg[k].sx=e.x;
5905 seg[k].sy=e.y;
5906 seg[k].ex=f.x;
5907 seg[k].ey=f.y;
5908 k++;
5909 }
5910 }
5911 blk->pte=k;
5912 }
5913 else if(p->kind==MPsphere){
5914 ae.theta=p->thetag;
5915 ae.phi=p->phig;
5916 ae.psi=p->psig;
5917 AEtoAM(&ae,&amb);
5918 eye.sx=p->xg; // center of the sphere
5919 eye.sy=p->yg;
5920 eye.sz=p->zg;
5921 eye.ex=po->x; // thePolar of CThreeD - position of the viewer's eye
5922 eye.ey=po->y;
5923 eye.ez=po->z;
5924 NormLine3d(&eye,&a,&d0);
5925 ToPolar3d(&a,&e);
5926 ae.theta=1.5707963268-e.y; // z axis toward viewer's eye
5927 ae.phi=e.x+1.5707963268;
5928 ae.psi=0.;
5929 tae.theta=ae.theta;
5930 tae.phi=ae.phi;
5931 tae.psi=ae.psi;
5932 AEtoAM(&ae,&ame);
5933 amc.c[0][0]=ame.c[0][0]*amb.c[0][0]+ame.c[1][0]*amb.c[1][0]+ame.c[2][0]*amb.c[2][0];
5934 amc.c[0][1]=ame.c[0][0]*amb.c[0][1]+ame.c[1][0]*amb.c[1][1]+ame.c[2][0]*amb.c[2][1];
5935 amc.c[0][2]=ame.c[0][0]*amb.c[0][2]+ame.c[1][0]*amb.c[1][2]+ame.c[2][0]*amb.c[2][2];
5936 amc.c[1][0]=ame.c[0][1]*amb.c[0][0]+ame.c[1][1]*amb.c[1][0]+ame.c[2][1]*amb.c[2][0];
5937 amc.c[1][1]=ame.c[0][1]*amb.c[0][1]+ame.c[1][1]*amb.c[1][1]+ame.c[2][1]*amb.c[2][1];
5938 amc.c[1][2]=ame.c[0][1]*amb.c[0][2]+ame.c[1][1]*amb.c[1][2]+ame.c[2][1]*amb.c[2][2];
5939 amc.c[2][0]=ame.c[0][2]*amb.c[0][0]+ame.c[1][2]*amb.c[1][0]+ame.c[2][2]*amb.c[2][0];
5940 amc.c[2][1]=ame.c[0][2]*amb.c[0][1]+ame.c[1][2]*amb.c[1][1]+ame.c[2][2]*amb.c[2][1];
5941 amc.c[2][2]=ame.c[0][2]*amb.c[0][2]+ame.c[1][2]*amb.c[1][2]+ame.c[2][2]*amb.c[2][2];
5942 p1=(Point2d *)malloc(sizeof(Point2d)*(ncontour+90));
5943 for(i=0;i<ncontour;i++){
5944 f.x=contour[i].x*0.017453293;
5945 f.y=contour[i].y*0.017453293;
5946 BodyToFrameAMP(&f,&amc,&g);
5947 contour[i].x=g.x;
5948 contour[i].y=g.y;
5949 }
5950 r=reinterpret_cast<CMPsphere *>(p)->r;
5951 ra=asin(r/d0);
5952 j=0;
5953 l=0; // if the first point is inbound
5954 for(i=0;i<ncontour;i++){
5955 if(i==0) k=ncontour-1;
5956 else k=i-1;
5957 if((contour[i].y-ra)*(contour[k].y-ra)<0.){
5958 g.x=contour[k].x;
5959 e.x=contour[i].x;
5960 if(g.x>e.x+3.2) e.x=e.x+6.283185307;
5961 else if(e.x>g.x+3.2) g.x=g.x+6.283185307;
5962 f.x=g.x+(e.x-g.x)*(ra-contour[k].y)/(contour[i].y-contour[k].y);
5963 f.y=ra;
5964 if(contour[i].y>ra){ // inbound
5965 if(j!=0){
5966 if(p1[j-1].x>f.x) r1=p1[j-1].x-6.283185307;
5967 else r1=p1[j-1].x;
5968 for(d1=r1+0.06;d1<f.x;d1=d1+0.06){
5969 p1[j].x=d1;
5970 p1[j].y=ra;
5971 j++;
5972 }
5973 }
5974 else l=1;
5975 p1[j].x=f.x;
5976 p1[j].y=f.y;
5977 j++;
5978 p1[j].x=contour[i].x;
5979 p1[j].y=contour[i].y;
5980 j++;
5981 }
5982 else{ // outbound
5983 p1[j].x=f.x;
5984 p1[j].y=f.y;
5985 j++;
5986 }
5987 }
5988 else if(contour[i].y>ra){
5989 p1[j].x=contour[i].x;
5990 p1[j].y=contour[i].y;
5991 j++;
5992 }
5993 }
5994 if(l==1){
5995 if(p1[j-1].x>p1[0].x) r1=p1[j-1].x-6.283185307;
5996 else r1=p1[j-1].x;
5997 for(d1=r1+0.06;d1<p1[0].x;d1=d1+0.06){
5998 p1[j].x=d1;
5999 p1[j].y=ra;
6000 j++;
6001 }
6002 /*
6003 p1[j].x=p1[0].x;
6004 p1[j].y=p1[0].y;
6005 j++;*/
6006 }
6007 np1=j;
6008 k=0;
6009 tae.x=p->xg; // Translate Angle Euler for the part
6010 tae.y=p->yg;
6011 tae.z=p->zg;
6012 TAEtoTAM(&tae,&tam);
6013 if(np1!=0){
6014 a.z=r*cos(p1[0].y);
6015 a.x=a.z*cos(p1[0].x);
6016 a.y=a.z*sin(p1[0].x);
6017 a.z=r*sin(p1[0].y);
6018 BodyToFrameTAM(&a,&tam,&b);
6019 FrameToBodyTAM(&b,po,&c);
6020 NormPoint3d(&c,&d,&r1);
6021 ToPolar3d(&d,&e);
6022 e.y=1.5707963267-e.y;
6023 f=e;
6024 for(i=1;i<=np1;i++){
6025 g=f;
6026 if(i<np1){
6027 a.z=r*cos(p1[i].y);
6028 a.x=a.z*cos(p1[i].x);
6029 a.y=a.z*sin(p1[i].x);
6030 a.z=r*sin(p1[i].y);
6031 BodyToFrameTAM(&a,&tam,&b);
6032 FrameToBodyTAM(&b,po,&c);
6033 NormPoint3d(&c,&d,&r1);
6034 ToPolar3d(&d,&f);
6035 f.y=1.5707963267-f.y;
6036 }
6037 else f=e;
6038 seg[k].sx=g.x;
6039 seg[k].sy=g.y;
6040 seg[k].ex=f.x;
6041 seg[k].ey=f.y;
6042 k++;
6043 }
6044 }
6045 cont->start=0;
6046 cont->pte=k;
6047 free(p1);
6048 blk->start=k;
6049 j=0;
6050 for(i=0;i<nlineseg;i++){
6051 f.x=lineseg[i].sx*0.017453293;
6052 f.y=lineseg[i].sy*0.017453293;
6053 BodyToFrameAMP(&f,&amc,&e);
6054 f.x=lineseg[i].ex*0.017453293;
6055 f.y=lineseg[i].ey*0.017453293;
6056 BodyToFrameAMP(&f,&amc,&g);
6057 if(e.y<ra&&g.y<ra);
6058 else{
6059 if(e.y<ra){
6060 e.x=e.x+(g.x-e.x)*(ra-e.y)/(g.y-e.y);
6061 e.y=ra;
6062 }
6063 else if(g.y<ra){
6064 g.x=g.x+(e.x-g.x)*(ra-g.y)/(e.y-g.y);
6065 g.y=ra;
6066 }
6067 lineseg[j].sx=e.x;
6068 lineseg[j].sy=e.y;
6069 lineseg[j].ex=g.x;
6070 lineseg[j].ey=g.y;
6071 j++;
6072 }
6073 }
6074 nl1=j;
6075 for(i=0;i<nl1;i++){
6076 a.z=r*cos(lineseg[i].sy);
6077 a.x=a.z*cos(lineseg[i].sx);
6078 a.y=a.z*sin(lineseg[i].sx);
6079 a.z=r*sin(lineseg[i].sy);
6080 BodyToFrameTAM(&a,&tam,&b);
6081 FrameToBodyTAM(&b,po,&c);
6082 NormPoint3d(&c,&d,&r1);
6083 ToPolar3d(&d,&e);
6084 e.y=1.5707963267-e.y;
6085 a.z=r*cos(lineseg[i].ey);
6086 a.x=a.z*cos(lineseg[i].ex);
6087 a.y=a.z*sin(lineseg[i].ex);
6088 a.z=r*sin(lineseg[i].ey);
6089 BodyToFrameTAM(&a,&tam,&b);
6090 FrameToBodyTAM(&b,po,&c);
6091 NormPoint3d(&c,&d,&r1);
6092 ToPolar3d(&d,&f);
6093 f.y=1.5707963267-f.y;
6094 seg[k].sx=e.x;
6095 seg[k].sy=e.y;
6096 seg[k].ex=f.x;
6097 seg[k].ey=f.y;
6098 k++;
6099 }
6100 blk->pte=k;
6101 }
6102 else{ /* MPbodyOfRot */
6103 double z0,x,y,z,ang,xe,ye,ze,ange;
6104 /* first, obtain the visible arc of the bottom circle */
6105 x=reinterpret_cast<CMPbodyOfRot *>(p)->p0.x; // bottom radius
6106 z=reinterpret_cast<CMPbodyOfRot *>(p)->p0.y;
6107 xe=reinterpret_cast<CMPbodyOfRot *>(p)->p1.x; // top radius
6108 ze=reinterpret_cast<CMPbodyOfRot *>(p)->p1.y;
6109 ang=x*(x-xe)/(ze-z); // offset of center of tangential sphere
6110 z0=z-ang; // position of center of tangential sphere
6111 r=sqrt(ang*ang+x*x); // raius of tangential sphere
6112 z=ang;
6113
6114 ae.theta=p->thetag;
6115 ae.phi=p->phig;
6116 ae.psi=p->psig;
6117 AEtoAM(&ae,&amb); // angle-matrix of body coord
6118 eye.sx=p->xg; // origin of the bodyOfRot
6119 eye.sy=p->yg;
6120 eye.sz=p->zg;
6121 eye.ex=po->x; // thePolar of CThreeD - position of the viewer's eye
6122 eye.ey=po->y;
6123 eye.ez=po->z;
6124 a.x=eye.ex-eye.sx;
6125 a.y=eye.ey-eye.sy;
6126 a.z=eye.ez-eye.sz;
6127 FrameToBodyAM(&a,&amb,&b);
6128 b.z=b.z-z0; // sphere centric coord
6129 NormPoint3d(&b,&a,&d0); // unit vector and distance
6130 ToPolar3d(&a,&e);
6131 xe=sqrt(b.x*b.x+b.y*b.y);
6132 ze=b.z;
6133 /*
6134 (x,y,z):tangential point on the bottom circle viewed from observer
6135 (xe,ye,ze):viewer's position
6136 x*x+y*y+z*z=r*r -- (1)
6137 r=defined as shown above
6138 z=defined as shown above
6139 x*xe+y*ye+z*ze=r*d0*cos(ang)=r*r -- (2)
6140 ang=acos(r/d0);
6141 r=defined as shown above
6142 xe=defined as shown above
6143 ye=0
6144 ze=defined as shown above
6145 x=(r*r-z*ze)/xe
6146 y=(+/-)sqrt(r*r-z*z-x*x)
6147 */
6148 x=(r*r-z*ze)/xe;
6149 y=sqrt(r*r-z*z-x*x);
6150 ange=atan2(y,x); /* to be used for clipping */
6151 /* next, obtain the visible arc of the top circle */
6152 x=reinterpret_cast<CMPbodyOfRot *>(p)->p0.x; // bottom radius
6153 z=reinterpret_cast<CMPbodyOfRot *>(p)->p0.y;
6154 xe=reinterpret_cast<CMPbodyOfRot *>(p)->p1.x; // top radius
6155 ze=reinterpret_cast<CMPbodyOfRot *>(p)->p1.y;
6156 ang=xe*(x-xe)/(ze-z); // offset of center of tangential sphere
6157 z0=ze-ang; // position of center of tangential sphere
6158 r=sqrt(ang*ang+xe*xe); // raius of tangential sphere
6159 z=ang;
6160
6161 ae.theta=p->thetag;
6162 ae.phi=p->phig;
6163 ae.psi=p->psig;
6164 AEtoAM(&ae,&amb); // angle-matrix of body coord
6165 eye.sx=p->xg; // origin of the bodyOfRot
6166 eye.sy=p->yg;
6167 eye.sz=p->zg;
6168 eye.ex=po->x; // thePolar of CThreeD - position of the viewer's eye
6169 eye.ey=po->y;
6170 eye.ez=po->z;
6171 a.x=eye.ex-eye.sx;
6172 a.y=eye.ey-eye.sy;
6173 a.z=eye.ez-eye.sz;
6174 FrameToBodyAM(&a,&amb,&b);
6175 b.z=b.z-z0; // sphere centric coord
6176 NormPoint3d(&b,&a,&d0); // unit vector and distance
6177 ToPolar3d(&a,&e);
6178 xe=sqrt(b.x*b.x+b.y*b.y);
6179 ze=b.z;
6180 /*
6181 (x,y,z):tangential point on the top circle viewed from observer
6182 (xe,ye,ze):viewer's position
6183 x*x+y*y+z*z=r*r -- (1)
6184 r=defined as shown above
6185 z=defined as shown above
6186 x*xe+y*ye+z*ze=r*d0*cos(ang)=r*r -- (2)
6187 ang=acos(r/d0);
6188 r=defined as shown above
6189 xe=defined as shown above
6190 ye=0
6191 ze=defined as shown above
6192 x=(r*r-z*ze)/xe
6193 y=(+/-)sqrt(r*r-z*z-x*x)
6194 */
6195 x=(r*r-z*ze)/xe;
6196 y=sqrt(r*r-z*z-x*x);
6197 ang=atan2(y,x); /* to be used for clipping */
6198 /*
6199 parameters obtained so far are:
6200 e.x : bias to be subtracted from input circular coord
6201 ange : clipping angle from center along bottom ring
6202 ang : clipping angle from center along top ring
6203 */
6204 x=reinterpret_cast<CMPbodyOfRot *>(p)->p0.x; // bottom radius
6205 z=reinterpret_cast<CMPbodyOfRot *>(p)->p0.y;
6206 xe=reinterpret_cast<CMPbodyOfRot *>(p)->p1.x; // top radius
6207 ze=reinterpret_cast<CMPbodyOfRot *>(p)->p1.y;
6208 p1=(Point2d *)malloc(sizeof(Point2d)*(ncontour+10));
6209 for(i=0;i<ncontour;i++){
6210 contour[i].x=contour[i].x*0.017453293-e.x;
6211 if(contour[i].x>3.14159265) contour[i].x=contour[i].x-6.283185307;
6212 else if(contour[i].x<-3.14159265) contour[i].x=contour[i].x+6.283185307;
6213 }
6214 j=0;
6215 for(i=0;i<ncontour;i++){
6216 if(i==0) k=ncontour-1;
6217 else k=i-1;
6218 if((contour[i].x-ang)*(contour[k].x-ang)<0.){
6219 /* (f.x-ange)/f.y=(ang-ange)/1, (f.x-ci.x)/(f.y-ci.y)=(ck.x-ci.x)/(ck.y-ci.y) */
6220 /* f.x + (ange-ang)*f.y=ange */
6221 /* (ck.y-ci.y)*f.x + (ci.x-ck.x)*f.y=ci.x*(ck.y-ci.y)-ci.y*(ck.x-ci.x) */
6222 /* will take a simplified method by substituting f.x by ange */
6223 f.y=((contour[k].y-contour[i].y)*(contour[i].x-ange)-contour[i].y*(contour[k].x-contour[i].x));
6224 f.y=f.y/(contour[i].x-contour[k].x);
6225 f.x=ange-(ange-ang)*f.y;
6226 if(contour[i].x<ang){ // inbound
6227 p1[j].x=f.x;
6228 p1[j].y=f.y;
6229 j++;
6230 p1[j].x=contour[i].x;
6231 p1[j].y=contour[i].y;
6232 j++;
6233 }
6234 else{ // outbound
6235 p1[j].x=f.x;
6236 p1[j].y=f.y;
6237 j++;
6238 }
6239 }
6240 else if((contour[i].x+ang)*(contour[k].x+ang)<0.){
6241 /* (f.x+ange)/f.y=(ange-ang)/1, (f.x-ci.x)/(f.y-ci.y)=(ck.x-ci.x)/(ck.y-ci.y) */
6242 /* f.x + (ang-ange)*f.y=-ange */
6243 /* (ck.y-ci.y)*f.x + (ci.x-ck.x)*f.y=ci.x*(ck.y-ci.y)-ci.y*(ck.x-ci.x) */
6244 /* will take a simplified method by substituting f.x by ange */
6245 f.y=((contour[k].y-contour[i].y)*(contour[i].x+ange)-contour[i].y*(contour[k].x-contour[i].x));
6246 f.y=f.y/(contour[i].x-contour[k].x);
6247 f.x=-ange-(ang-ange)*f.y;
6248 if(contour[i].x>-ang){ // inbound
6249 p1[j].x=f.x;
6250 p1[j].y=f.y;
6251 j++;
6252 p1[j].x=contour[i].x;
6253 p1[j].y=contour[i].y;
6254 j++;
6255 }
6256 else{ // outbound
6257 p1[j].x=f.x;
6258 p1[j].y=f.y;
6259 j++;
6260 }
6261 }
6262 else if(contour[i].x>-ang&&contour[i].x<ang){
6263 p1[j].x=contour[i].x;
6264 p1[j].y=contour[i].y;
6265 j++;
6266 }
6267 }
6268 np1=j;
6269 k=0;
6270 tae.x=p->xg; // Translate Angle Euler for the part
6271 tae.y=p->yg;
6272 tae.z=p->zg;
6273 tae.theta=p->thetag;
6274 tae.phi=p->phig;
6275 tae.psi=p->psig+e.x;
6276 TAEtoTAM(&tae,&tam);
6277 if(np1!=0){
6278 a.z=p1[0].y*ze+(1.-p1[0].y)*z;
6279 r=p1[0].y*xe+(1.-p1[0].y)*x;
6280 a.x=r*cos(p1[0].x);
6281 a.y=r*sin(p1[0].x);
6282 BodyToFrameTAM(&a,&tam,&b);
6283 FrameToBodyTAM(&b,po,&c);
6284 NormPoint3d(&c,&d,&r1);
6285 ToPolar3d(&d,&e);
6286 e.y=1.5707963267-e.y;
6287 f=e;
6288 for(i=1;i<=np1;i++){
6289 g=f;
6290 if(i<np1){
6291 a.z=p1[i].y*ze+(1.-p1[i].y)*z;
6292 r=p1[i].y*xe+(1.-p1[i].y)*x;
6293 a.x=r*cos(p1[i].x);
6294 a.y=r*sin(p1[i].x);
6295 BodyToFrameTAM(&a,&tam,&b);
6296 FrameToBodyTAM(&b,po,&c);
6297 NormPoint3d(&c,&d,&r1);
6298 ToPolar3d(&d,&f);
6299 f.y=1.5707963267-f.y;
6300 }
6301 else f=e;
6302 seg[k].sx=g.x;
6303 seg[k].sy=g.y;
6304 seg[k].ex=f.x;
6305 seg[k].ey=f.y;
6306 k++;
6307 }
6308 }
6309 cont->start=0;
6310 cont->pte=k;
6311 free(p1);
6312 blk->start=k;
6313 for(i=0;i<nlineseg;i++){
6314 lineseg[i].sx=lineseg[i].sx*0.017453293-e.x;
6315 if(lineseg[i].sx>3.14159265) lineseg[i].sx=lineseg[i].sx-6.283185307;
6316 else if(lineseg[i].sx<-3.14159265) lineseg[i].sx=lineseg[i].sx+6.283185307;
6317 lineseg[i].ex=lineseg[i].ex*0.017453293-e.x;
6318 if(lineseg[i].ex>3.14159265) lineseg[i].ex=lineseg[i].ex-6.283185307;
6319 else if(lineseg[i].ex<-3.14159265) lineseg[i].ex=lineseg[i].ex+6.283185307;
6320 }
6321 j=0;
6322 for(i=0;i<nlineseg;i++){
6323 if(lineseg[i].sx>ang&&lineseg[i].ex>ang);
6324 else if(lineseg[i].sx<-ang&&lineseg[i].ex<-ang);
6325 else if((lineseg[i].sx-ang)*(lineseg[i].ex-ang)<0.){
6326 f.y=((lineseg[i].sy-lineseg[i].ey)*(lineseg[i].ex-ange)-lineseg[i].ey*(lineseg[i].sx-lineseg[i].ex));
6327 f.y=f.y/(lineseg[i].ex-lineseg[i].sx);
6328 f.x=ange-(ange-ang)*f.y;
6329 if(lineseg[i].sx>ang){
6330 lineseg[j].sx=f.x;
6331 lineseg[j].sy=f.y;
6332 lineseg[j].ex=lineseg[i].ex;
6333 lineseg[j].ey=lineseg[i].ey;
6334 }
6335 else{
6336 lineseg[j].sx=lineseg[i].sx;
6337 lineseg[j].sy=lineseg[i].sy;
6338 lineseg[j].ex=f.x;
6339 lineseg[j].ey=f.y;
6340 }
6341 j++;
6342 }
6343 else if((lineseg[i].sx+ang)*(lineseg[i].ex+ang)<0.){
6344 f.y=((lineseg[i].sy-lineseg[i].ey)*(lineseg[i].ex+ange)-lineseg[i].ey*(lineseg[i].sx-lineseg[i].ex));
6345 f.y=f.y/(lineseg[i].ex-lineseg[i].sx);
6346 f.x=-ange-(ang-ange)*f.y;
6347 if(lineseg[i].sx<-ang){
6348 lineseg[j].sx=f.x;
6349 lineseg[j].sy=f.y;
6350 lineseg[j].ex=lineseg[i].ex;
6351 lineseg[j].ey=lineseg[i].ey;
6352 }
6353 else{
6354 lineseg[j].sx=lineseg[i].sx;
6355 lineseg[j].sy=lineseg[i].sy;
6356 lineseg[j].ex=f.x;
6357 lineseg[j].ey=f.y;
6358 }
6359 j++;
6360 }
6361 else{
6362 lineseg[j].sx=lineseg[i].sx;
6363 lineseg[j].sy=lineseg[i].sy;
6364 lineseg[j].ex=lineseg[i].ex;
6365 lineseg[j].ey=lineseg[i].ey;
6366 j++;
6367 }
6368 }
6369 nl1=j;
6370 for(i=0;i<nl1;i++){
6371 a.z=lineseg[i].sy*ze+(1.-lineseg[i].sy)*z;
6372 r=lineseg[i].sy*xe+(1.-lineseg[i].sy)*x;
6373 a.x=r*cos(lineseg[i].sx);
6374 a.y=r*sin(lineseg[i].sx);
6375 BodyToFrameTAM(&a,&tam,&b);
6376 FrameToBodyTAM(&b,po,&c);
6377 NormPoint3d(&c,&d,&r1);
6378 ToPolar3d(&d,&e);
6379 e.y=1.5707963267-e.y;
6380 a.z=lineseg[i].ey*ze+(1.-lineseg[i].ey)*z;
6381 r=lineseg[i].ey*xe+(1.-lineseg[i].ey)*x;
6382 a.x=r*cos(lineseg[i].ex);
6383 a.y=r*sin(lineseg[i].ex);
6384 BodyToFrameTAM(&a,&tam,&b);
6385 FrameToBodyTAM(&b,po,&c);
6386 NormPoint3d(&c,&d,&r1);
6387 ToPolar3d(&d,&f);
6388 f.y=1.5707963267-f.y;
6389 seg[k].sx=e.x;
6390 seg[k].sy=e.y;
6391 seg[k].ex=f.x;
6392 seg[k].ey=f.y;
6393 k++;
6394 }
6395 blk->pte=k;
6396 }
6397}
6398
6399
6400/**************************************************
6401
6402 Initiate adding a rigid body element
6403
6404 should be followed by following calls as required
6405 setlocal
6406 setweight
6407 setdensity
6408 setnpoint
6409 set1Dpoint
6410 set2Dpoint
6411 set3Dpoint
6412
6413***************************************************/
6414
6415int addelm(char *s,int ref,int knd1)
6416{
6417 int knd,piggy;
6418 piggy=knd1/100;
6419 knd=knd1-piggy*100;
6420 switch(knd){
6421 case MPpoint:
6422 thd.part.push_back(new CMPpoint(s,ref,knd));
6423 break;
6424 case MPlines:
6425 thd.part.push_back(new CMPlines(s,ref,knd));
6426 break;
6427 case MPplane:
6428 thd.part.push_back(new CMPplane(s,ref,knd));
6429 if(piggy) (reinterpret_cast<CMPplane *>(thd.part[thd.npart]))->isGroup=1;
6430 break;
6431 case MPbox:
6432 thd.part.push_back(new CMPbox(s,ref,knd));
6433 break;
6434 case MPsphere:
6435 thd.part.push_back(new CMPsphere(s,ref,knd));
6436 break;
6437 case MPbodyOfRot:
6438 thd.part.push_back(new CMPbodyOfRot(s,ref,knd));
6439 break;
6440 default:
6441 return -1;
6442 break;
6443 }
6444 thd.npart++;
6445 return thd.npart-1;
6446}
6447
6448/****************************************************
6449
6450 set local coordinate
6451
6452****************************************************/
6453void setlocal(double x,double y,double z,double theta,double phi,double psi)
6454{
6455 thd.part[thd.npart-1]->x0=x;
6456 thd.part[thd.npart-1]->y0=y;
6457 thd.part[thd.npart-1]->z0=z;
6458 thd.part[thd.npart-1]->theta0=theta;
6459 thd.part[thd.npart-1]->phi0=phi;
6460 thd.part[thd.npart-1]->psi0=psi;
6461}
6462
6463/***************************************************
6464
6465 set element color
6466
6467***************************************************/
6468void setcolor(int c)
6469{
6470 thd.part[thd.npart-1]->color=c;
6471}
6472
6473/***************************************************
6474
6475 set line appearance
6476
6477***************************************************/
6478int setline(int c)
6479{
6480 if(thd.part[thd.npart-1]->kind!=MPlines) return -1;
6481 (reinterpret_cast<CMPlines *>(thd.part[thd.npart-1]))->line=c;
6482 return 0;
6483}
6484
6485/***************************************************
6486
6487 set if tube for a body of rotation
6488
6489***************************************************/
6490int isTube()
6491{
6492 if(thd.part[thd.npart-1]->kind!=MPbodyOfRot) return -1;
6493 (reinterpret_cast<CMPbodyOfRot *>(thd.part[thd.npart-1]))->isTube=true;
6494 return 0;
6495}
6496
6497/***************************************************
6498
6499 set if oblong for a body of rotation
6500
6501***************************************************/
6502int oblong()
6503{
6504 if(thd.part[thd.npart-1]->kind!=MPbodyOfRot) return -1;
6505 (reinterpret_cast<CMPbodyOfRot *>(thd.part[thd.npart-1]))->oblong=true;
6506 return 0;
6507}
6508
6509/***************************************************
6510
6511 set one dimensional point as in sphere radius
6512
6513***************************************************/
6514int set1Dpoint(double x)
6515{
6516 switch(thd.part[thd.npart-1]->kind){
6517 case MPsphere:
6518 (reinterpret_cast<CMPsphere *>(thd.part[thd.npart-1]))->SetRadius(x);
6519 break;
6520 default:
6521 return -1;
6522 break;
6523 }
6524 return 0;
6525}
6526
6527/***************************************************
6528
6529 set two dimensional point as in an apex of plane
6530
6531***************************************************/
6532int set2Dpoint(double x,double y)
6533{
6534 switch(thd.part[thd.npart-1]->kind){
6535 case MPplane:
6536 (reinterpret_cast<CMPplane *>(thd.part[thd.npart-1]))->SetPoint(x,y);
6537 break;
6538 case MPbodyOfRot:
6539 (reinterpret_cast<CMPbodyOfRot *>(thd.part[thd.npart-1]))->SetPoint(x,y);
6540 break;
6541 default:
6542 return -1;
6543 break;
6544 }
6545 return 0;
6546}
6547
6548/***************************************************
6549
6550 set three dimensional point as in a point in line
6551
6552***************************************************/
6553int set3Dpoint(double x,double y,double z)
6554{
6555 switch(thd.part[thd.npart-1]->kind){
6556 case MPlines:
6557 (reinterpret_cast<CMPlines *>(thd.part[thd.npart-1]))->SetPoint(x,y,z);
6558 break;
6559 case MPbox:
6560 (reinterpret_cast<CMPbox *>(thd.part[thd.npart-1]))->SetSize(x,y,z);
6561 break;
6562 default:
6563 return -1;
6564 break;
6565 }
6566 return 0;
6567}
6568
6569void adddeco()
6570{
6571 CMPplane *pla;
6572 CMPsphere *psp;
6573 CMPbodyOfRot *pbr;
6574 CMPdeco *pld;
6575
6576 if(thd.part[thd.npart-1]->kind==MPplane){
6577 pla=reinterpret_cast<CMPplane *>(thd.part[thd.npart-1]);
6578 pld=new CMPdeco(pla);
6579 pla->deco.push_back(pld);
6580 pla->ndeco++;
6581 }
6582 else if(thd.part[thd.npart-1]->kind==MPsphere){
6583 psp=reinterpret_cast<CMPsphere *>(thd.part[thd.npart-1]);
6584 pld=new CMPdeco(psp);
6585 psp->deco.push_back(pld);
6586 psp->ndeco++;
6587 }
6588 else{
6589 pbr=reinterpret_cast<CMPbodyOfRot *>(thd.part[thd.npart-1]);
6590 pld=new CMPdeco(pbr);
6591 pbr->deco.push_back(pld);
6592 pbr->ndeco++;
6593 }
6594}
6595
6596void decocontxy(double x,double y)
6597{
6598 CMPplane *pla;
6599 CMPsphere *psp;
6600 CMPbodyOfRot *pbr;
6601 CMPdeco *pld;
6602 Point2d p;
6603
6604 if(thd.part[thd.npart-1]->kind==MPplane){
6605 pla=reinterpret_cast<CMPplane *>(thd.part[thd.npart-1]);
6606 pld=pla->deco[pla->ndeco-1];
6607 }
6608 else if(thd.part[thd.npart-1]->kind==MPsphere){
6609 psp=reinterpret_cast<CMPsphere *>(thd.part[thd.npart-1]);
6610 pld=psp->deco[psp->ndeco-1];
6611 }
6612 else{
6613 pbr=reinterpret_cast<CMPbodyOfRot *>(thd.part[thd.npart-1]);
6614 pld=pbr->deco[pbr->ndeco-1];
6615 }
6616 p.x=x;
6617 p.y=y;
6618 pld->contour.push_back(p);
6619 pld->ncontour++;
6620}
6621
6622void decocontco(int color)
6623{
6624 CMPplane *pla;
6625 CMPsphere *psp;
6626 CMPbodyOfRot *pbr;
6627 CMPdeco *pld;
6628
6629 if(thd.part[thd.npart-1]->kind==MPplane){
6630 pla=reinterpret_cast<CMPplane *>(thd.part[thd.npart-1]);
6631 pld=pla->deco[pla->ndeco-1];
6632 }
6633 else if(thd.part[thd.npart-1]->kind==MPsphere){
6634 psp=reinterpret_cast<CMPsphere *>(thd.part[thd.npart-1]);
6635 pld=psp->deco[psp->ndeco-1];
6636 }
6637 else{
6638 pbr=reinterpret_cast<CMPbodyOfRot *>(thd.part[thd.npart-1]);
6639 pld=pbr->deco[pbr->ndeco-1];
6640 }
6641 pld->color=color;
6642}
6643
6644void decolinexy(double x0,double y0,double x1, double y1)
6645{
6646 CMPplane *pla;
6647 CMPsphere *psp;
6648 CMPbodyOfRot *pbr;
6649 CMPdeco *pld;
6650 Line2d l;
6651
6652 if(thd.part[thd.npart-1]->kind==MPplane){
6653 pla=reinterpret_cast<CMPplane *>(thd.part[thd.npart-1]);
6654 pld=pla->deco[pla->ndeco-1];
6655 }
6656 else if(thd.part[thd.npart-1]->kind==MPsphere){
6657 psp=reinterpret_cast<CMPsphere *>(thd.part[thd.npart-1]);
6658 pld=psp->deco[psp->ndeco-1];
6659 }
6660 else{
6661 pbr=reinterpret_cast<CMPbodyOfRot *>(thd.part[thd.npart-1]);
6662 pld=pbr->deco[pbr->ndeco-1];
6663 }
6664 l.sx=x0;
6665 l.sy=y0;
6666 l.ex=x1;
6667 l.ey=y1;
6668 pld->lineseg.push_back(l);
6669 pld->nlineseg++;
6670}
6671
6672void viewFrom(double a,double b,double r)
6673{
6674 thd.Azi=a;
6675 thd.Ele=b;
6676 thd.Dst=r;
6677}
6678
6679void drawMessage(int i)
6680{
6681 thd.message=i;
6682}
6683
6684
6685void viewFixed(double tgx,double tgy,double tgz,int foc)
6686{
6687 thd.contAll=false;
6688 thd.Tgx=tgx;
6689 thd.Tgy=tgy;
6690 thd.Tgz=tgz;
6691 thd.Foc=foc/2;
6692}
6693
6694int draw3D(int wid,int hei)
6695{
6696 thd.Wid=wid/2;
6697 thd.Hei=hei/2;
6698 thd.Draw();
6699 return 0;
6700}
6701
6702void getCoord(int ref,double x,double y,double z,int *ix,int *iy)
6703{
6704 /* to be implemented some time later */
6705}
6706
6707char *insert3D(int *len)
6708{
6709 char *s;
6710 s=thd.ToDB(len);
6711}
6712
6713void select3D(int len,char *d,double x,double y,double z,double theta,double phi,double psi)
6714{
6715 double loc[6];
6716 loc[0]=x;
6717 loc[1]=y;
6718 loc[2]=z;
6719 loc[3]=theta;
6720 loc[4]=phi;
6721 loc[5]=psi;
6722 thd.FromDB(len,d,loc);
6723}
6724
6725void parts3D(int style)
6726{
6727 char s[200];
6728 int i,j,k,l;
6729 CMPpart *p;
6730 Point2d *p2;
6731 Point3d *p3;
6732 sprintf(s,"thd.npart=%i\n",thd.npart);
6733 outstr(s);
6734 for(i=0;i<thd.npart;i++){
6735 sprintf(s,"part index=%i ",i);
6736 outstr(s);
6737 p=thd.part[i];
6738 sprintf(s,"kind=%i ",p->kind);
6739 outstr(s);
6740 strcpy(s,p->desc.c_str());
6741 outstr(s);
6742 sprintf(s,"\n refPart=%i ",p->refPart);
6743 outstr(s);
6744 sprintf(s,"x0=%g ",p->x0);
6745 outstr(s);
6746 sprintf(s,"y0=%g ",p->y0);
6747 outstr(s);
6748 sprintf(s,"z0=%g ",p->z0);
6749 outstr(s);
6750 sprintf(s,"theta0=%g ",p->theta0);
6751 outstr(s);
6752 sprintf(s,"phi0=%g ",p->phi0);
6753 outstr(s);
6754 sprintf(s,"psi0=%g ",p->psi0);
6755 outstr(s);
6756 sprintf(s,"color=%i ",p->color);
6757 outstr(s);
6758 sprintf(s,"showDrawing=%i\n",p->showDrawing);
6759 outstr(s);
6760 switch(p->kind){
6761 case MPpoint:
6762 break;
6763 case MPlines:
6764 k=reinterpret_cast<CMPlines *>(p)->npoint;
6765 sprintf(s,"npoint=%i\n",k);
6766 for(j=0;j<k;j++){
6767 p3=&reinterpret_cast<CMPlines *>(p)->pt[j];
6768 sprintf(s,"x=%g y=%g z=%g\n",p3->x,p3->y,p3->z);
6769 outstr(s);
6770 }
6771 sprintf(s,"line=%i\n",reinterpret_cast<CMPlines *>(p)->line);
6772 outstr(s);
6773 break;
6774 case MPplane:
6775 k=reinterpret_cast<CMPplane *>(p)->npoint;
6776 sprintf(s,"npoint=%i\n",k);
6777 for(j=0;j<k;j++){
6778 p2=&reinterpret_cast<CMPplane *>(p)->pt[j];
6779 sprintf(s,"x=%g y=%g\n",p2->x,p2->y);
6780 outstr(s);
6781 }
6782 sprintf(s,"isPolyhed=%i isGroup=%i\n",reinterpret_cast<CMPplane *>(p)->isPolyhed,
6783 reinterpret_cast<CMPplane *>(p)->isGroup);
6784 break;
6785 case MPbox:
6786 sprintf(s,"lx=%g ly=%g lz=%g visibleSurface=%i\n",
6787 reinterpret_cast<CMPbox *>(p)->lx,reinterpret_cast<CMPbox *>(p)->ly,
6788 reinterpret_cast<CMPbox *>(p)->lz,
6789 reinterpret_cast<CMPbox *>(p)->visibleSurface);
6790 outstr(s);
6791 break;
6792 case MPsphere:
6793 sprintf(s,"r=%g\n",reinterpret_cast<CMPsphere *>(p)->r);
6794 outstr(s);
6795 break;
6796 case MPbodyOfRot:
6797 sprintf(s,"npoint=%i ",reinterpret_cast<CMPbodyOfRot *>(p)->npoint);
6798 outstr(s);
6799 sprintf(s,"p0.x=%g p0.y=%g ",reinterpret_cast<CMPbodyOfRot *>(p)->p0.x,
6800 reinterpret_cast<CMPbodyOfRot *>(p)->p0.y);
6801 outstr(s);
6802 sprintf(s,"p1.x=%g p1.y=%g\n",reinterpret_cast<CMPbodyOfRot *>(p)->p1.x,
6803 reinterpret_cast<CMPbodyOfRot *>(p)->p1.y);
6804 outstr(s);
6805 sprintf(s,"isTube=%i ",reinterpret_cast<CMPbodyOfRot *>(p)->isTube);
6806 outstr(s);
6807 sprintf(s,"oblong=%i ",reinterpret_cast<CMPbodyOfRot *>(p)->oblong);
6808 outstr(s);
6809 sprintf(s,"appear=%i ",reinterpret_cast<CMPbodyOfRot *>(p)->appear);
6810 outstr(s);
6811 sprintf(s,"visibleSurface=%i\n",reinterpret_cast<CMPbodyOfRot *>(p)->visibleSurface);
6812 outstr(s);
6813 }
6814 }
6815 flush();
6816}
6817
6818int setting(double a[],int b[])
6819{
6820 thd.Azi=a[0];
6821 thd.Ele=a[1];
6822 thd.Dst=a[2];
6823 thd.Tgx=a[3];
6824 thd.Tgy=a[4];
6825 thd.Tgz=a[5];
6826 thd.Wid=b[0];
6827 thd.Hei=b[1];
6828 thd.Foc=b[2];
6829 thd.contAll=b[3];
6830 return 0;
6831}
6832
6833/*
6834 cross point of two line segments
6835*/
6836int cross(double x0,double y0,double x1,double y1,double x2,double y2,double x3,double y3,
6837 double *tx,double *ux,double *xx,double *yx)
6838{
6839 double d,t,u,x,y;
6840 d=(x0<x1)? x0:x1;
6841 t=(x2<x3)? x2:x3;
6842 u=(x0<x1)? x1:x0;
6843 x=(x2<x3)? x3:x2;
6844 if(d>x) return 1;
6845 if(t>u) return 1;
6846 d=(y0<y1)? y0:y1;
6847 t=(y2<y3)? y2:y3;
6848 u=(y0<y1)? y1:y0;
6849 x=(y2<y3)? y3:y2;
6850 if(d>x) return 1;
6851 if(t>u) return 1;
6852 /*
6853 x0*t+x1*(1-t)=x2*u+x3*(1-u)
6854 y0*t+y1*(1-t)=y2*u+y3*(1-u)
6855 (x0-x1)*t+(x3-x2)*u=x3-x1
6856 (y0-y1)*t+(y3-y2)*u=y3-y1
6857 */
6858 d=(x0-x1)*(y3-y2)-(y0-y1)*(x3-x2);
6859 if(d==0.) return 1;
6860 t=((x3-x1)*(y3-y2)-(y3-y1)*(x3-x2))/d;
6861 if(t<0.||t>1.) return 1;
6862 u=((x0-x1)*(y3-y1)-(y0-y1)*(x3-x1))/d;
6863 if(u<0.||u>1.) return 1;
6864 x=x0*t+x1*(1.-t);
6865 y=y0*t+y1*(1.-t);
6866 *tx=t;
6867 *ux=u;
6868 *xx=x;
6869 *yx=y;
6870 return 0;
6871}
6872
6873/*
6874 get closed contour of a region that contains two given regions
6875*/
6876int outline(Point2d *p0,int np0,Point2d *p1,int np1,Point2d *p2,int *np2)
6877{
6878 int i0[200],i1[200];
6879 int i,j,k,l,m,n;
6880 double ang;
6881 double x0,y0,x1,y1,d0,d1;
6882 double x2,y2,x3,y3,d,t,u;
6883 int nx;
6884 double xx[20],yx[20],f0x[20],f1x[20];
6885 int i0x[20],i1x[20];
6886 int i0a[200],i0b[200],i1a[200],i1b[200];
6887 int n0 ,n1;
6888
6889 for(i=0;i<200;i++) i0[i]=i1[i]=0; /* 2:in region, 1:on contour, 0:out of region */
6890 for(i=0;i<np0;i++){
6891 ang=0.;
6892 for(j=1;j<np1;j++) ang=ang+ArgDif(p0[i].x,p0[i].y,p1[j-1].x,p1[j-1].y,p1[j].x,p1[j].y);
6893 ang=ang+ArgDif(p0[i].x,p0[i].y,p1[np1-1].x,p1[np1-1].y,p1[0].x,p1[0].y);
6894 if(ang>6.) i0[i]=2;
6895 else if(ang>1.) i0[i]=1; /* on a line */
6896 }
6897 k=0;
6898 for(i=0;i<np0;i++) if(i0[i]==0) k++;
6899 if(k==0){ /* p0 enclosed in p1 */
6900 for(i=0;i<np1;i++) p2[i]=p1[i];
6901 *np2=np1;
6902 return 0;
6903 }
6904 for(i=0;i<np1;i++){
6905 ang=0.;
6906 for(j=1;j<np0;j++) ang=ang+ArgDif(p1[i].x,p1[i].y,p0[j-1].x,p0[j-1].y,p0[j].x,p0[j].y);
6907 ang=ang+ArgDif(p1[i].x,p1[i].y,p0[np1-1].x,p0[np1-1].y,p0[0].x,p0[0].y);
6908 if(ang>6.) i1[i]=2;
6909 else if(ang>1.) i1[i]=1;
6910 }
6911 j=0;
6912 for(i=0;i<np1;i++) if(i1[i]==0) j++;
6913 if(j==0){
6914 for(i=0;i<np1;i++) p2[i]=p0[i];
6915 *np2=np0;
6916 return 0;
6917 }
6918 nx=0;
6919 for(i=0;i<np0;i++){
6920 j=i+1;
6921 if(j==np0) j=0;
6922 for(k=0;k<np1;k++){
6923 l=k+1;
6924 if(l==np1) l=0;
6925 m=cross(p0[i].x,p0[i].y,p0[j].x,p0[j].y,p1[k].x,p1[k].y,p1[l].x,p1[l].y,&t,&u,&x0,&y0);
6926 if(m==0){
6927 i0x[nx]=i;
6928 i1x[nx]=k;
6929 f0x[nx]=t;
6930 f1x[nx]=u;
6931 xx[nx]=x0;
6932 yx[nx]=y0;
6933 nx++;
6934 }
6935 }
6936 }
6937 if(nx==0){ /* disjoint - return connected region */
6938 x0=y0=0.;
6939 for(i=0;i<np0;i++){
6940 x0=x0+p0[i].x;
6941 y0=y0+p0[i].y;
6942 }
6943 x0=x0/np0; /* median */
6944 y0=y0/np0;
6945 l=0;
6946 d0=1.e20;
6947 for(i=0;i<np1;i++){
6948 d1=(x0-p1[i].x)*(x0-p1[i].x)+(y0-p1[i].y)*(y0-p1[i].y);
6949 if(d1<d0){
6950 l=i;
6951 d0=d1;
6952 }
6953 }
6954 x0=p1[l].x; /* nearest point */
6955 y0=p1[l].y;
6956 d0=1.e20;
6957 k=0;
6958 for(i=0;i<np0;i++){
6959 d1=(x0-p0[i].x)*(x0-p0[i].x)+(y0-p0[i].y)*(y0-p0[i].y);
6960 if(d1<d0){
6961 k=i;
6962 d0=d1;
6963 }
6964 }
6965 j=0;
6966 for(i=0;i<=np0;i++){
6967 m=i+k;
6968 if(m>=np0) m-=np0;
6969 p2[j]=p0[m];
6970 j++;
6971 }
6972 for(i=0;i<=np1;i++){
6973 m=i+l;
6974 if(m>=np1) m-=np1;
6975 p2[j]=p1[m];
6976 j++;
6977 }
6978 *np2=j;
6979 return 0;
6980 }
6981 n0=0;
6982 for(i=0;i<np0;i++){
6983 if(i0[i]==0){
6984 i0a[n0]=1;
6985 i0b[n0]=i;
6986 n0++;
6987 }
6988 k=0;
6989 for(j=0;j<nx;j++){
6990 if(i0x[j]==i){
6991 i0a[n0]=2;
6992 i0b[n0]=j;
6993 n0++;
6994 k++;
6995 }
6996 }
6997 if(k>1){ /* sort to the fractional order */
6998 for(j=1;j<k;j++)
6999 for(l=0;l<k-j;l++){
7000 m=n0-1-l;
7001 if(f0x[i0b[m]]>f0x[i0b[m-1]]){
7002 n=i0b[m];
7003 i0b[m]=i0b[m-1];
7004 i0b[m-1]=n;
7005 }
7006 }
7007 }
7008 }
7009 n1=0;
7010 for(i=0;i<np1;i++){
7011 if(i1[i]==0){
7012 i1a[n1]=1;
7013 i1b[n1]=i;
7014 n1++;
7015 }
7016 k=0;
7017 for(j=0;j<nx;j++){
7018 if(i1x[j]==i){
7019 i1a[n1]=2;
7020 i1b[n1]=j;
7021 n1++;
7022 k++;
7023 }
7024 }
7025 if(k>1){ /* sort to the fractional order */
7026 for(j=1;j<k;j++)
7027 for(l=0;l<k-j;l++){
7028 m=n1-1-l;
7029 if(f1x[i1b[m]]>f1x[i1b[m-1]]){
7030 n=i1b[m];
7031 i1b[m]=i1b[m-1];
7032 i1b[m-1]=n;
7033 }
7034 }
7035 }
7036 }
7037 m=0;
7038 for(n=0;n<n0;n++) if(i0a[n]==1) break;
7039 for(i=0;i<n0;i++){
7040 j=i+n;
7041 if(j>=n0) j-=n0;
7042 if(i0a[j]==1){
7043 p2[m]=p0[i0b[j]];
7044 m++;
7045 }
7046 else{
7047 p2[m].x=xx[i0b[j]];
7048 p2[m].y=yx[i0b[j]];
7049 m++;
7050 for(l=0;l<n1;l++) if(i1a[l]==2&&i1b[l]==i0b[j]) break;
7051 l++;
7052 for(k=0;k<n1;k++){
7053 j=k+l;
7054 if(j>=n1) j-=n1;
7055 if(i1a[j]==1){
7056 p2[m]=p1[i1b[j]];
7057 m++;
7058 }
7059 else{
7060 p2[m].x=xx[i1b[j]];
7061 p2[m].y=yx[i1b[j]];
7062 m++;
7063 break;
7064 }
7065 }
7066 i++;
7067 }
7068 }
7069 *np2=m;
7070 return 0;
7071}
7072
7073/*
7074 projecton to a plane perpendicular to the line of sight
7075 (x,y,z) : coordinate of a point in the body coordinate system
7076 tamb : translation and orientation of the body coordinate system
7077 tame :
7078*/
7079int projxy(double x,double y,double z,TransAngleMatrix *tamb,TransAngleMatrix *tame,double re,Point2d *xy)
7080{
7081 Point3d a,b;
7082 double c;
7083 a.x=x;
7084 a.y=y;
7085 a.z=z;
7086 BodyToFrameTAM(&a,tamb,&b);
7087 FrameToBodyTAM(&b,tame,&a);
7088 /*
7089 projection on x-y plane of an object viewed from the eye.
7090 (x-eye.x)/(obj.x-eye.x)=(y-eye.y)/(obj.y-eye.y)=(z-eye.z)/(obj.z-eye.z)
7091 x/a.x=y/a.y=-r/(a.z-r)
7092 */
7093 c=-re/(a.z-re);
7094 xy->x=a.x*c;
7095 xy->y=a.y*c;
7096 return 0;
7097}
7098
7099/*
7100 contour of a foot
7101 p : azimuth and elevation to the observer (rad)
7102 f : contour data Point2d f[7]
7103*/
7104int cntrfoot(Point2d *p,Point2d *f)
7105{
7106 int side,i,j,k;
7107 double d[3][3][12]={ /* [el][az][point] */
7108 0.04,0., 0.04,0.1, -0.04,0.1, -0.04,0., -0.06,-0.12, 0.06,-0.12,
7109 0.04,0., 0.04,0.1, -0.04,0.1, -0.06,0.04, -0.18,-0.06, -0.09,-0.11,
7110 0.04,-0.02, 0.04,0.1, -0.04,0.1, -0.06,0.06, -0.2,0.04, -0.2,-0.04,
7111
7112 0.04,0.06, 0.04,0.1, -0.04,0.1, -0.04,0.06, -0.06,0., 0.06,0.,
7113 0.04,0., 0.04,0.1, -0.04,0.1, -0.06,0.06, -0.18,0.03, -0.18,0.,
7114 0.04,0., 0.04,0.1, -0.04,0.1, -0.06,0.06, -0.2,0.03, -0.2,0.,
7115
7116 0.04,0.04, 0.06,0.12, -0.06,0.12, -0.04,0.04, -0.04,0., 0.04,0.,
7117 0.04,0.02, 0.04,0.1, -0.06,0.11, -0.1,0.12, -0.18,0.06, -0.02,-0.03,
7118 0.04,-0.02, 0.04,0.1, -0.04,0.1, -0.06,0.06, -0.2,0.04, -0.2,-0.04
7119 };
7120 double e;
7121 double az,el;
7122 double d1,d2;
7123 double g;
7124 az=p->x*1.273239545; /* 4/pi */
7125 el=p->y*1.273239545;
7126 if(el>1.) el=1.;
7127 else if(el<-1.) el=-1.;
7128 if(az<0.){
7129 az=-az;
7130 side=1;
7131 }
7132 else side=0;
7133 if(az>2.){
7134 az=4.-az;
7135 el=-el;
7136 }
7137 el=1.-el;
7138 i=(int)floor(el);
7139 if(i==2) i=1;
7140 el=el-i;
7141 j=(int)floor(az);
7142 if(j==2) j=1;
7143 az=az-j;
7144 for(k=0;k<12;k++){
7145 d1=d[i][j][k]*(1.-az)+d[i][j+1][k]*az;
7146 d2=d[i+1][j][k]*(1.-az)+d[i+1][j+1][k]*az;
7147 e=d1*(1.-el)+d2*el;
7148 if(k%2) f[k/2].y=e;
7149 else f[k/2].x=e;
7150 }
7151 f[k/2]=f[0];
7152 if(side==1){
7153 for(i=0;i<4;i++){
7154 g=f[i].x;
7155 f[i].x=-f[6-i].x;
7156 f[6-i].x=-g;
7157 g=f[i].y;
7158 f[i].y=f[6-i].y;
7159 f[6-i].y=g;
7160 }
7161 }
7162 return 0;
7163}
7164
7165/*
7166 get a point that is separated from a terminal point of a line
7167 segment by a specified offset both perpendicular to and along the line.
7168 s : source of the line segment
7169 d : destination of the line segment
7170 r.x : offset from source, perpendicular to the line segment, + if to the right facing destination
7171 r,y : offset from source, along the line segment, + if receeding facing destination
7172 p : point at offset
7173*/
7174int fatline(Point2d *s,Point2d *d,Point2d *r,Point2d *p)
7175{
7176 double l,x,y;
7177 x=d->x-s->x;
7178 y=d->y-s->y;
7179 l=sqrt(x*x+y*y);
7180 p->x=s->x+r->x*y/l-r->y*x/l;
7181 p->y=s->y-r->x*x/l-r->y*y/l;
7182 return 0;
7183}
7184
7185/*
7186 contour of trousers
7187 p[0] az,el, direction in rad toward the observer
7188 p[1] right hip, p[2] right kneecap, p[3] right ankle
7189 p[4] left hip, p[5] left kneecap, p[6] left ankle
7190 q[] *n contour points (q[0]-q[n-1])
7191 *m extra line points (q[n]-q[n+m-1])
7192
7193 radius from a bone is given as a function of az as follows.
7194 (rmin,rmax,az at rmin)
7195 right hip left (0,0.20,pi/4) 0.1-0.1*cos(az-pi/4)
7196 right hip right (0,0.20,5*pi/4) 0.1-0.1*cos(az-5*pi/4)
7197 right kneecap left (0.07,0.10,0/pi) 0.085-0.015*cos(2*az)
7198 right kneecap right same as left
7199 right ankle left (0.06,0.10,0/pi) 0.08-0.02*cos(2*az)
7200 right ankle right same as left
7201*/
7202int cntrpants(Point2d *p,Point2d *q,int *n,int *m)
7203{
7204 int i,j,k;
7205 double az,el;
7206 double pi=3.1415926536;
7207 Point2d p0[6]; /* six-point contour of the right leg, starting at upper left */
7208 Point2d p1[6]; /* six-point contour of the left leg, starting at upper left */
7209 Point2d p3; /* offset */
7210
7211 az=p[0].x;
7212 el=p[0].y;
7213 p3.x=0.1-0.1*cos(az-pi/4.);
7214 p3.y=0.01;
7215 fatline(&p[1],&p[2],&p3,&p0[0]);
7216 p3.x=-(0.085-0.015*cos(2.*az));
7217 p3.y=0.;
7218 fatline(&p[2],&p[1],&p3,&p0[1]);
7219 p3.x=-p3.x;
7220 fatline(&p[2],&p[3],&p3,&p0[2]);
7221 p0[1].x=0.5*(p0[1].x+p0[2].x);
7222 p0[1].y=0.5*(p0[1].y+p0[2].y);
7223 p3.x=-(0.08-0.02*cos(2.*az));
7224 fatline(&p[3],&p[2],&p3,&p0[2]);
7225 p3.x=-p3.x;
7226 fatline(&p[3],&p[2],&p3,&p0[3]);
7227 p3.x=-(0.085-0.015*cos(2.*az));
7228 fatline(&p[2],&p[3],&p3,&p0[4]);
7229 p3.x=-p3.x;
7230 fatline(&p[2],&p[1],&p3,&p0[5]);
7231 p0[4].x=0.5*(p0[4].x+p0[5].x);
7232 p0[4].y=0.5*(p0[4].y+p0[5].y);
7233 p3.x=-(0.1-0.1*cos(az-5.*pi/4.));
7234 p3.y=0.01;
7235 fatline(&p[1],&p[2],&p3,&p0[5]);
7236 p3.x=0.1-0.1*cos(az+5.*pi/4.);
7237 p3.y=0.01;
7238 fatline(&p[4],&p[5],&p3,&p1[0]);
7239 p3.x=-(0.085-0.015*cos(2.*az));
7240 p3.y=0.;
7241 fatline(&p[5],&p[4],&p3,&p1[1]);
7242 p3.x=-p3.x;
7243 fatline(&p[5],&p[6],&p3,&p1[2]);
7244 p1[1].x=0.5*(p1[1].x+p1[2].x);
7245 p1[1].y=0.5*(p1[1].y+p1[2].y);
7246 p3.x=-(0.08-0.02*cos(2.*az));
7247 fatline(&p[6],&p[5],&p3,&p1[2]);
7248 p3.x=-p3.x;
7249 fatline(&p[6],&p[5],&p3,&p1[3]);
7250 p3.x=-(0.085-0.015*cos(2.*az));
7251 fatline(&p[5],&p[6],&p3,&p1[4]);
7252 p3.x=-p3.x;
7253 fatline(&p[5],&p[4],&p3,&p1[5]);
7254 p1[4].x=0.5*(p1[4].x+p1[5].x);
7255 p1[4].y=0.5*(p1[4].y+p1[5].y);
7256 p3.x=-(0.1-0.1*cos(az+pi/4.));
7257 p3.y=0.01;
7258 fatline(&p[4],&p[5],&p3,&p1[5]);
7259 if(p0[0].x<p1[0].x){
7260 p3.y=0.5*(p0[5].y+p1[0].y);
7261 p0[5].y=p3.y;
7262 p1[0].y=p3.y;
7263 }
7264 else{
7265 p3.y=0.5*(p0[0].y+p1[5].y);
7266 p0[0].y=p3.y;
7267 p1[5].y=p3.y;
7268 }
7269 outline(p0,6,p1,6,q,n);
7270 *m=0;
7271}
7272
7273/*
7274 contour of an arm
7275 p[0] az,el, direction in rad toward the observer
7276 p[1] body junction, p[2] shoulder, p[3] elbow
7277 p[4] wrist
7278 q[] 7 contour points (q[0]-q[6])
7279
7280 radius from a bone is given as a function of az as follows.
7281 (rmin,rmax,az at rmin)
7282 shoulder (0.06,0.05,0/pi) 0.055-0.005*cos(2*az)
7283 elbow (0.06,0.05,0/pi) 0.055-0.005*cos(2*az)
7284 wrist (0.042,0.036,0/pi) 0.039-0.003*cos(2*az)
7285*/
7286int cntrarm(Point2d *p,Point2d *q)
7287{
7288 double az,el;
7289 Point2d p3; /* offset */
7290
7291 az=p[0].x;
7292 el=p[0].y;
7293 p3.x=0.055-0.005*cos(2.*az);
7294 p3.y=-0.1;
7295 fatline(&p[2],&p[3],&p3,&q[0]);
7296 p3.x=-(0.055-0.005*cos(2.*az));
7297 p3.y=0.;
7298 fatline(&p[3],&p[2],&p3,&q[1]);
7299 p3.x=-p3.x;
7300 fatline(&p[3],&p[4],&p3,&q[2]);
7301 q[1].x=0.5*(q[1].x+q[2].x);
7302 q[1].y=0.5*(q[1].y+q[2].y);
7303 p3.x=-(0.039-0.003*cos(2.*az));
7304 fatline(&p[4],&p[3],&p3,&q[2]);
7305 p3.x=-p3.x;
7306 fatline(&p[4],&p[3],&p3,&q[3]);
7307 p3.x=-(0.055-0.005*cos(2.*az));
7308 fatline(&p[3],&p[4],&p3,&q[4]);
7309 p3.x=-p3.x;
7310 fatline(&p[3],&p[2],&p3,&q[5]);
7311 q[4].x=0.5*(q[4].x+q[5].x);
7312 q[4].y=0.5*(q[4].y+q[5].y);
7313 p3.x=-(0.055-0.005*cos(2.*az));
7314 p3.y=-0.1;
7315 fatline(&p[2],&p[3],&p3,&q[5]);
7316 q[6]=p[1];
7317 q[7]=q[0];
7318 return 0;
7319}
7320
7321void bezier(double *d,Point2d *p)
7322{
7323 double t,f0,f1,f2,f3;
7324 int i;
7325 for(i=0;i<=5;i++){
7326 t=0.2*i;
7327 f0=pow(1.-t,3);
7328 f1=3.*t*(1.-t)*(1.-t);
7329 f2=3.*t*t*(1.-t);
7330 f3=pow(t,3);
7331 p[i].x=f0*d[0]+f1*d[2]+f2*d[4]+f3*d[6];
7332 p[i].y=f0*d[1]+f1*d[3]+f2*d[5]+f3*d[7];
7333 p[i].x=0.01*p[i].x; /* cm to m */
7334 p[i].y=0.01*p[i].y;
7335 }
7336}
7337
7338/*
7339 contour of a hand
7340 p[0] : projected position of wrist
7341 p[1] : projected position of finger tip
7342 p[2] : azimuth and elevation to the observer (rad)
7343 f : contour data Point2d f[13]
7344*/
7345int cntrhand(Point2d *p,Point2d *f)
7346{
7347 double a0[]={-0.03,0., -0.023,-0.022, -0.014,-0.047,
7348 0.,-0.05, 0.014,-0.047, 0.023,-0.022,
7349 0.03,0., 0.023,0.022, 0.014,0.047,
7350 0.,0.05, -0.014,0.047, -0.023,0.022};
7351 double a1[24];
7352 double a2[]={0.,0., -0.02,0., -0.02,-0.03,
7353 -0.03,-0.05, -0.03,-0.1, -0.02,-0.13,
7354 0.,-0.13, 0.02,-0.13, 0.03,-0.1,
7355 0.03,-0.05, 0.02,-0.03, 0.02,0.};
7356 double a3[]={0.,0., -0.03,0., -0.03,-0.03,
7357 -0.05,-0.07, -0.05,-0.11, -0.03,-0.13,
7358 0.,-0.13, 0.03,-0.13, 0.05,-0.11,
7359 0.05,-0.07, 0.03,-0.03, 0.03,0.};
7360 double a4[24],a5[24];
7361 int side,i,j,k;
7362 double az,el,sa,ca,d;
7363 double g;
7364 az=p[2].x*0.6366198; /* 2/pi */
7365 el=p[2].y*0.6366198;
7366 side=1;
7367 if(el<0.){
7368 el=-el;
7369 side*=-1;
7370 }
7371 if(az<0.){
7372 az=-az;
7373 side*=-1;
7374 }
7375 if(az>1.){
7376 az=2.-az;
7377 side*=-1;
7378 }
7379 sa=sin(1.570796327*az);
7380 ca=cos(1.570796327*az);
7381 d=100.;
7382 for(i=0;i<24;i+=2){
7383 a1[i]=a0[i]*ca+a0[i+1]*sa;
7384 a1[i+1]=-a0[i]*sa+a0[i+1]*ca;
7385 if(a1[i]<d){
7386 j=i;
7387 d=a1[i];
7388 }
7389 }
7390 for(i=0;i<24;i+=2){
7391 k=j+i;
7392 if(k>=24) k-=24;
7393 a4[i]=a1[k];
7394 a4[i+1]=a1[k+1];
7395 }
7396 for(i=0;i<24;i++) a1[i]=a2[i]*(1.-az)+a3[i]*az;
7397 for(i=0;i<24;i++) a5[i]=a1[i]*(1.-el)+a4[i]*el;
7398 if(side==-1){
7399 for(i=0;i<12;i+=2){
7400 g=a5[i];
7401 a5[i]=-a5[22-i];
7402 a5[22-i]=-g;
7403 g=a5[i+1];
7404 a5[i+1]=a5[23-i];
7405 a5[23-i]=g;
7406 }
7407 }
7408 az=atan2(p[0].y-p[1].y,p[0].x-p[1].x)-1.570796327; /* z-axis counter-clockwise deviation */
7409 sa=sin(az);
7410 ca=cos(az);
7411 for(i=0;i<24;i+=2){
7412 f[i/2].x=a5[i]*ca-a5[i+1]*sa;
7413 f[i/2].y=a5[i]*sa+a5[i+1]*ca;
7414 }
7415 f[12]=f[0];
7416 return 0;
7417}
7418
7419/*
7420 contour of body
7421 p[0] : projected position of waist bottom
7422 p[1] : projected position of waist top
7423 p[2] : azimuth and elevation to the observer (rad)
7424 f : contour data Point2d f[13]
7425*/
7426int cntrbody(Point2d *p,Point2d *f)
7427{
7428 double a0[]={-0.17,0., -0.15,-0.05, -0.1,-0.08,
7429 0.,-0.1, 0.1,-0.08, 0.15,-0.05,
7430 0.17,0., 0.15,0.05, 0.1,0.08,
7431 0.,0.1, -0.1,0.08, -0.15,0.05};
7432 double a1[24],a2[24],a3[24];
7433 double a21[]={-0.14,-0.2, -0.1,-0.2, -0.05,-0.2,
7434 0.,-0.2, 0.05,-0.2, 0.1,-0.2,
7435 0.14,-0.2, 0.16,0.3, 0.12,0.38,
7436 0.,0.4, -0.12,0.38, -0.16,0.3};
7437 double a31[]={-0.11,-0.2, -0.07,-0.2, -0.03,-0.2,
7438 0.,-0.2, 0.03,-0.2, 0.07,-0.2,
7439 0.11,-0.2, 0.1,0.3, 0.07,0.38,
7440 0.,0.4, -0.07,0.38, -0.1,0.3};
7441 double a4[24],a5[24];
7442 int side,i,j,k;
7443 double az,el,sa,ca,d;
7444 double g;
7445 for(i=0;i<24;i+=2){
7446 a2[i]=a21[i]*0.8;
7447 a2[i+1]=a21[i+1];
7448 a3[i]=a31[i]*0.8;
7449 a3[i+1]=a31[i+1];
7450 }
7451 az=p[2].x*0.6366198; /* 2/pi */
7452 el=p[2].y*0.6366198;
7453 side=1;
7454 if(el<0.){
7455 el=-el;
7456 side*=-1;
7457 }
7458 if(az<0.){
7459 az=-az;
7460 side*=-1;
7461 }
7462 if(az>1.){
7463 az=2.-az;
7464 side*=-1;
7465 }
7466 sa=sin(1.570796327*az);
7467 ca=cos(1.570796327*az);
7468 d=100.;
7469 for(i=0;i<24;i+=2){
7470 a1[i]=a0[i]*ca+a0[i+1]*sa;
7471 a1[i+1]=-a0[i]*sa+a0[i+1]*ca;
7472 if(a1[i]<d){
7473 j=i;
7474 d=a1[i];
7475 }
7476 }
7477 for(i=0;i<24;i+=2){
7478 k=j+i;
7479 if(k>=24) k-=24;
7480 a4[i]=a1[k];
7481 a4[i+1]=a1[k+1];
7482 }
7483 for(i=0;i<24;i++) a1[i]=a2[i]*(1.-az)+a3[i]*az;
7484 for(i=0;i<24;i++) a5[i]=a1[i]*cos(el/0.6366198)+a4[i]*sin(el/0.6366198);
7485 if(side==-1){
7486 for(i=0;i<12;i+=2){
7487 g=a5[i];
7488 a5[i]=-a5[22-i];
7489 a5[22-i]=-g;
7490 g=a5[i+1];
7491 a5[i+1]=a5[23-i];
7492 a5[23-i]=g;
7493 }
7494 }
7495 az=atan2(p[1].y-p[0].y,p[1].x-p[0].x)-1.570796327; /* z-axis counter-clockwise deviation */
7496 sa=sin(az);
7497 ca=cos(az);
7498 for(i=0;i<24;i+=2){
7499 f[i/2].x=a5[i]*ca-a5[i+1]*sa;
7500 f[i/2].y=a5[i]*sa+a5[i+1]*ca;
7501 }
7502 f[12]=f[0];
7503 return 0;
7504}
7505
7506/*
7507 contour of a head
7508 p : azimuth and elevation to the observer (rad)
7509 f : contour data (cubic bezier) Point2d f[21]
7510*/
7511int cntrhead(Point2d *p,Point2d *f)
7512{
7513 int side,i,j,k;
7514 double d[3][3][26]={ /* [el][az][point] */
7515 4.,-2.,8.,3.,8.,5., 8.,10.,8.,15.,5.,20., 0.,20.,-5.,20.,-8.,15., -8.,10.,-8.,5.,-8.,3.,-4.,-2.,
7516 4.,0.,6.,4.,8.,4., 8.,10.,8.,15.,5.,20., 0.,20.,-5.,20.,-9.,15., -10.,8.,-9.,3.,-8.,1.,-4.,0.,
7517 5.,0.,7.,4.,7.5,6., 7.5,10.,7.5,17.,4.,20., 0.,20.,-7.,20.,-9.5,16., -9.5,10.,-9.,3.,-8.,2.,-5.,0.,
7518
7519 6.,0.,4.,7.,6.,5.,8.,15.,9.,21.,4.,25.,0.,25.,-4.,25.,-9.,21.,-8.,15.,-6.,5.,-4.,7.,-6.,0.,
7520 5.,0.,4.,7.,8.,5., 8.,15.,9.,22.,4.,25., 0.,25.,-4.,25.,-9.,21., -10.,15.,-9.5,-3.,-5.,4.,-4.,0.,
7521 6.,0.,5.,6.,8.,5.,7.5,15.,7.,23.,2.,25.,-3.,24.,-9.,22.,-10.,19.,-9.,7.,-7.,-3.,-3.,4.,-3.,0.,
7522
7523 5.,0.,7.,4.,7.,4., 8.,10.,8.,18.,6.,21., 0.,21.,-6.,21.,-8.,18., -8.,10.,-7.,4.,-7.,4.,-5.,0.,
7524 5.,0.,7.,4.,8.,5., 8.,10.,8.,15.,5.,21., 1.,21.,-6.,21.,-10.,18., -10.,12.,-10.,5.,-8.,3.,-5.,0.,
7525 5.,0.,7.,4.,7.5,6., 7.5,10.,7.5,17.,4.,20., 0.,20.,-7.,20.,-9.5,16., -9.5,10.,-9.,3.,-8.,2.,-5.,0.};
7526 double e[26];
7527 double az,el;
7528 double d1,d2;
7529 double g;
7530 az=p->x*1.273239545; /* 4/pi */
7531 el=p->y*1.273239545;
7532 if(el>1.) el=1.;
7533 else if(el<-1.) el=-1.;
7534 if(az<0.){
7535 az=-az;
7536 side=1;
7537 }
7538 else side=0;
7539 if(az>2.){
7540 az=4.-az;
7541 el=-el;
7542 }
7543 el=1.-el;
7544 i=(int)floor(el);
7545 if(i==2) i=1;
7546 el=el-i;
7547 j=(int)floor(az);
7548 if(j==2) j=1;
7549 az=az-j;
7550 for(k=0;k<26;k++){
7551 d1=d[i][j][k]*(1.-az)+d[i][j+1][k]*az;
7552 d2=d[i+1][j][k]*(1.-az)+d[i+1][j+1][k]*az;
7553 e[k]=d1*(1.-el)+d2*el;
7554 }
7555 bezier(&e[0],&f[0]);
7556 bezier(&e[6],&f[5]);
7557 bezier(&e[12],&f[10]);
7558 bezier(&e[18],&f[15]);
7559 if(side==1){
7560 for(i=0;i<10;i++){
7561 g=f[i].x;
7562 f[i].x=-f[20-i].x;
7563 f[20-i].x=-g;
7564 g=f[i].y;
7565 f[i].y=f[20-i].y;
7566 f[20-i].y=g;
7567 }
7568 }
7569 return 0;
7570}
7571
7572int drawobj(int id,double a[])
7573{
7574 int i,j,ig,il,ip;
7575 int k,m,n;
7576 double rh,lb,wb,la,wa,ll,wl,xc,yc,zc;
7577 double l,w,h;
7578 double r;
7579 double e;
7580 double ang;
7581 char str[10];
7582 TransAngleEuler tae[16];
7583 TransAngleEuler tae1,tae2;
7584 TransAngleMatrix tam,tam1;
7585 Point3d eyez,eyex;
7586 AngleEuler eyeae;
7587 Point2d p0[100],p1[100],p2[100];
7588 Point2d p3[100],p4[10];
7589 double az,el;
7590
7591 switch(id/10){
7592 case 50: /* star of david */
7593 ig=addelm("global",0,MPpoint); /* global */
7594 setlocal(0.,0.,0.,0.,0.,0.);
7595 il=addelm("ref point",0,MPpoint); /* local */
7596 setlocal(a[0],a[1],a[2],a[3],a[4],a[5]); /* center and orientation of the approximating box */
7597 eyez.x=thd.Tgx+thd.Dst*cos(d2r*thd.Azi)*cos(d2r*thd.Ele)-a[0];
7598 eyez.y=thd.Tgy+thd.Dst*sin(d2r*thd.Azi)*cos(d2r*thd.Ele)-a[1];
7599 eyez.z=thd.Tgz+thd.Dst*sin(d2r*thd.Ele)-a[2];
7600#if 0
7601 eyex.x=-eyez.y;
7602 eyex.y=eyez.x;
7603 eyex.z=0.;
7604 PointsToAE(&eyez,&eyex,zxAxis,&eyeae);
7605 ip=addelm("screen",ig,MPplane);
7606 setlocal(a[0],a[1],a[2],eyeae.theta,eyeae.phi,eyeae.psi);
7607#endif
7608 ip=addelm("screen",ig,MPplane);
7609 NormPoint3d(&eyez,&eyex,&r);
7610 setlocal(a[0],a[1],a[2],r2d*acos(eyex.z),r2d*atan2(eyex.y,eyex.x)+90.,0.);
7611 r=0.3;
7612 p0[0].x=-1.73205*r;
7613 p0[0].y=-r;
7614 p0[1].x=1.73205*r;
7615 p0[1].y=-r;
7616 p0[2].x=0.;
7617 p0[2].y=2.*r;
7618 p1[0].x=1.73205*r;
7619 p1[0].y=r;
7620 p1[1].x=-1.73205*r;
7621 p1[1].y=r;
7622 p1[2].x=0.;
7623 p1[2].y=-2.*r;
7624 outline(p0,3,p1,3,p2,&ig);
7625 setcolor(MPyellow);
7626 for(i=0;i<ig;i++) set2Dpoint(p2[i].x,p2[i].y);
7627 adddeco();
7628 for(i=0;i<3;i++) decocontxy(p0[i].x,p0[i].y);
7629 decocontco(MPred);
7630 adddeco();
7631 for(i=0;i<3;i++) decocontxy(p1[i].x,p1[i].y);
7632 decocontco(MPblue);
7633 // adddeco();
7634 //for(i=0;i<3;i++) decocontxy(p2[i].x,p2[i].y);
7635 //decocontco(MPgreen);
7636 break;
7637 case 51: /* skeleton */
7638 /* approximated by a box with sides (0.2,0.4,1.64) */
7639 DegreeToTAE(0.,0.,0.,&tae[0]); /* right foot */
7640 DegreeToTAE(0.,0.,0.,&tae[1]); /* right shin */
7641 DegreeToTAE(0.,0.,0.,&tae[2]); /* right thigh */
7642 DegreeToTAE(0.,0.,0.,&tae[3]); /* left foot */
7643 DegreeToTAE(0.,0.,0.,&tae[4]); /* left shin */
7644 DegreeToTAE(0.,0.,0.,&tae[5]); /* left thigh */
7645 DegreeToTAE(0.,0.,0.,&tae[6]); /* hip */
7646 DegreeToTAE(0.,0.,0.,&tae[7]); /* waist */
7647 DegreeToTAE(0.,0.,0.,&tae[8]); /* shoulder */
7648 DegreeToTAE(0.,0.,0.,&tae[9]); /* right arm */
7649 DegreeToTAE(0.,0.,0.,&tae[10]); /* right forearm */
7650 DegreeToTAE(0.,0.,0.,&tae[11]); /* right hand */
7651 DegreeToTAE(0.,0.,0.,&tae[12]); /* left arm */
7652 DegreeToTAE(0.,0.,0.,&tae[13]); /* left forearm */
7653 DegreeToTAE(0.,0.,0.,&tae[14]); /* left hand */
7654 DegreeToTAE(0.,0.,0.,&tae[15]); /* head */
7655 tae[7].x=0.; /* measure from lower waist */
7656 tae[7].y=0.;
7657 tae[7].z=0.;
7658 tae[6].x=tae[7].x; /* hip */
7659 tae[6].y=tae[7].y;
7660 tae[6].z=tae[7].y;
7661 TAEtoTAM(&tae[6],&tam);
7662 eyez.x=0.;
7663 eyez.y=0.14;
7664 eyez.z=-0.2;
7665 BodyToFrameTAM(&eyez,&tam,&eyex);
7666 tae[5].x=eyex.x; /* left thigh */
7667 tae[5].y=eyex.y;
7668 tae[5].z=eyex.z;
7669 eyez.x=0.;
7670 eyez.y=-0.14;
7671 eyez.z=-0.2;
7672 BodyToFrameTAM(&eyez,&tam,&eyex);
7673 tae[2].x=eyex.x; /* right thigh */
7674 tae[2].y=eyex.y;
7675 tae[2].z=eyex.z;
7676 TAEtoTAM(&tae[5],&tam);
7677 eyez.x=0.;
7678 eyez.y=0.;
7679 eyez.z=-0.4;
7680 BodyToFrameTAM(&eyez,&tam,&eyex);
7681 tae[4].x=eyex.x; /* left shin */
7682 tae[4].y=eyex.y;
7683 tae[4].z=eyex.z;
7684 TAEtoTAM(&tae[4],&tam);
7685 eyez.x=0.;
7686 eyez.y=0.;
7687 eyez.z=-0.4;
7688 BodyToFrameTAM(&eyez,&tam,&eyex);
7689 tae[3].x=eyex.x; /* left foot */
7690 tae[3].y=eyex.y;
7691 tae[3].z=eyex.z;
7692 TAEtoTAM(&tae[2],&tam);
7693 eyez.x=0.;
7694 eyez.y=0.;
7695 eyez.z=-0.4;
7696 BodyToFrameTAM(&eyez,&tam,&eyex);
7697 tae[1].x=eyex.x; /* right shin */
7698 tae[1].y=eyex.y;
7699 tae[1].z=eyex.z;
7700 TAEtoTAM(&tae[1],&tam);
7701 eyez.x=0.;
7702 eyez.y=0.;
7703 eyez.z=-0.4;
7704 BodyToFrameTAM(&eyez,&tam,&eyex);
7705 tae[0].x=eyex.x; /* right foot */
7706 tae[0].y=eyex.y;
7707 tae[0].z=eyex.z;
7708 TAEtoTAM(&tae[7],&tam);
7709 eyez.x=0.;
7710 eyez.y=0.;
7711 eyez.z=0.2;
7712 BodyToFrameTAM(&eyez,&tam,&eyex);
7713 tae[8].x=eyex.x; /* shoulder */
7714 tae[8].y=eyex.y;
7715 tae[8].z=eyex.z;
7716 TAEtoTAM(&tae[8],&tam);
7717 eyez.x=0.;
7718 eyez.y=0.;
7719 eyez.z=0.2;
7720 BodyToFrameTAM(&eyez,&tam,&eyex);
7721 tae[15].x=eyex.x; /* head */
7722 tae[15].y=eyex.y;
7723 tae[15].z=eyex.z;
7724 eyez.x=0.;
7725 eyez.y=-0.17;
7726 eyez.z=0.2;
7727 BodyToFrameTAM(&eyez,&tam,&eyex);
7728 tae[9].x=eyex.x; /* right arm */
7729 tae[9].y=eyex.y;
7730 tae[9].z=eyex.z;
7731 eyez.x=0.;
7732 eyez.y=0.17;
7733 eyez.z=0.2;
7734 BodyToFrameTAM(&eyez,&tam,&eyex);
7735 tae[12].x=eyex.x; /* left arm */
7736 tae[12].y=eyex.y;
7737 tae[12].z=eyex.z;
7738 TAEtoTAM(&tae[9],&tam);
7739 eyez.x=0.;
7740 eyez.y=0.;
7741 eyez.z=-0.29;
7742 BodyToFrameTAM(&eyez,&tam,&eyex);
7743 tae[10].x=eyex.x; /* right forearm */
7744 tae[10].y=eyex.y;
7745 tae[10].z=eyex.z;
7746 TAEtoTAM(&tae[10],&tam);
7747 eyez.x=0.;
7748 eyez.y=0.;
7749 eyez.z=-0.23;
7750 BodyToFrameTAM(&eyez,&tam,&eyex);
7751 tae[11].x=eyex.x; /* right hand */
7752 tae[11].y=eyex.y;
7753 tae[11].z=eyex.z;
7754 TAEtoTAM(&tae[12],&tam);
7755 eyez.x=0.;
7756 eyez.y=0.;
7757 eyez.z=-0.29;
7758 BodyToFrameTAM(&eyez,&tam,&eyex);
7759 tae[13].x=eyex.x; /* left forearm */
7760 tae[13].y=eyex.y;
7761 tae[13].z=eyex.z;
7762 TAEtoTAM(&tae[13],&tam);
7763 eyez.x=0.;
7764 eyez.y=0.;
7765 eyez.z=-0.23;
7766 BodyToFrameTAM(&eyez,&tam,&eyex);
7767 tae[14].x=eyex.x; /* left hand */
7768 tae[14].y=eyex.y;
7769 tae[14].z=eyex.z;
7770 il=addelm("ref point",0,MPpoint); /* local */
7771 setlocal(a[0],a[1],a[2],a[3],a[4],a[5]); /* center and orientation of the approximating box */
7772 addelm("right foot",il,MPlines);
7773 setlocal(tae[0].x,tae[0].y,tae[0].z,r2d*tae[0].theta,r2d*tae[0].phi,r2d*tae[0].psi);
7774 set3Dpoint(0.,0.,0.);
7775 set3Dpoint(0.18,0.,0.);
7776 addelm("right shin",il,MPlines);
7777 setlocal(tae[1].x,tae[1].y,tae[1].z,r2d*tae[1].theta,r2d*tae[1].phi,r2d*tae[1].psi);
7778 set3Dpoint(0.,0.,0.);
7779 set3Dpoint(0.,0.,-0.4);
7780 addelm("right thigh",il,MPlines);
7781 setlocal(tae[2].x,tae[2].y,tae[2].z,r2d*tae[2].theta,r2d*tae[2].phi,r2d*tae[2].psi);
7782 set3Dpoint(0.,0.,0.);
7783 set3Dpoint(0.,0.,-0.4);
7784 addelm("left foot",il,MPlines);
7785 setlocal(tae[3].x,tae[3].y,tae[3].z,r2d*tae[3].theta,r2d*tae[3].phi,r2d*tae[3].psi);
7786 set3Dpoint(0.,0.,0.);
7787 set3Dpoint(0.18,0.,0.);
7788 addelm("left shin",il,MPlines);
7789 setlocal(tae[4].x,tae[4].y,tae[4].z,r2d*tae[4].theta,r2d*tae[4].phi,r2d*tae[4].psi);
7790 set3Dpoint(0.,0.,0.);
7791 set3Dpoint(0.,0.,-0.4);
7792 addelm("left thigh",il,MPlines);
7793 setlocal(tae[5].x,tae[5].y,tae[5].z,r2d*tae[5].theta,r2d*tae[5].phi,r2d*tae[5].psi);
7794 set3Dpoint(0.,0.,0.);
7795 set3Dpoint(0.,0.,-0.4);
7796 addelm("hip",il,MPlines);
7797 setlocal(tae[6].x,tae[6].y,tae[6].z,r2d*tae[6].theta,r2d*tae[6].phi,r2d*tae[6].psi);
7798 set3Dpoint(0.,0.,0.);
7799 set3Dpoint(0.,-0.14,-0.2);
7800 set3Dpoint(0.,0.14,-0.2);
7801 set3Dpoint(0.,0.,0.);
7802 addelm("waist",il,MPlines);
7803 setlocal(tae[7].x,tae[7].y,tae[7].z,r2d*tae[7].theta,r2d*tae[7].phi,r2d*tae[7].psi);
7804 set3Dpoint(0.,0.,0.);
7805 set3Dpoint(0.,0.,0.2);
7806 addelm("shoulder",il,MPlines);
7807 setlocal(tae[8].x,tae[8].y,tae[8].z,r2d*tae[8].theta,r2d*tae[8].phi,r2d*tae[8].psi);
7808 set3Dpoint(0.,0.,0.);
7809 set3Dpoint(0.,0.17,0.2);
7810 set3Dpoint(0.,0.,0.2);
7811 set3Dpoint(0.,-0.17,0.2);
7812 set3Dpoint(0.,0.,0.);
7813 addelm("right arm",il,MPlines);
7814 setlocal(tae[9].x,tae[9].y,tae[9].z,r2d*tae[9].theta,r2d*tae[9].phi,r2d*tae[9].psi);
7815 set3Dpoint(0.,0.,0.);
7816 set3Dpoint(0.,0.,-0.29);
7817 addelm("right forearm",il,MPlines);
7818 setlocal(tae[10].x,tae[10].y,tae[10].z,r2d*tae[10].theta,r2d*tae[10].phi,r2d*tae[10].psi);
7819 set3Dpoint(0.,0.,0.);
7820 set3Dpoint(0.,0.,-0.23);
7821 addelm("right hand",il,MPlines);
7822 setlocal(tae[11].x,tae[11].y,tae[11].z,r2d*tae[11].theta,r2d*tae[11].phi,r2d*tae[11].psi);
7823 set3Dpoint(0.,0.,0.);
7824 set3Dpoint(0.,0.,-0.18);
7825 addelm("left arm",il,MPlines);
7826 setlocal(tae[12].x,tae[12].y,tae[12].z,r2d*tae[12].theta,r2d*tae[12].phi,r2d*tae[12].psi);
7827 set3Dpoint(0.,0.,0.);
7828 set3Dpoint(0.,0.,-0.29);
7829 addelm("left forearm",il,MPlines);
7830 setlocal(tae[13].x,tae[13].y,tae[13].z,r2d*tae[13].theta,r2d*tae[13].phi,r2d*tae[13].psi);
7831 set3Dpoint(0.,0.,0.);
7832 set3Dpoint(0.,0.,-0.23);
7833 addelm("left hand",il,MPlines);
7834 setlocal(tae[14].x,tae[14].y,tae[14].z,r2d*tae[14].theta,r2d*tae[14].phi,r2d*tae[14].psi);
7835 set3Dpoint(0.,0.,0.);
7836 set3Dpoint(0.,0.,-0.18);
7837 addelm("head",il,MPlines);
7838 setlocal(tae[15].x,tae[15].y,tae[15].z,r2d*tae[15].theta,r2d*tae[15].phi,r2d*tae[15].psi);
7839 set3Dpoint(0.,0.,0.);
7840 set3Dpoint(0.,0.,0.2);
7841 break;
7842 case 52: /* skeleton - projected to a plane */
7843 /* approximated by a box with sides (0.2,0.4,1.64) */
7844 DegreeToTAE(0.,0.,0.,&tae[0]); /* right foot */
7845 DegreeToTAE(0.,0.,0.,&tae[1]); /* right shin */
7846 DegreeToTAE(0.,0.,0.,&tae[2]); /* right thigh */
7847 DegreeToTAE(0.,0.,0.,&tae[3]); /* left foot */
7848 DegreeToTAE(0.,0.,0.,&tae[4]); /* left shin */
7849 DegreeToTAE(0.,0.,0.,&tae[5]); /* left thigh */
7850 DegreeToTAE(0.,0.,0.,&tae[6]); /* hip */
7851 DegreeToTAE(0.,0.,0.,&tae[7]); /* waist */
7852 DegreeToTAE(0.,0.,0.,&tae[8]); /* shoulder */
7853 DegreeToTAE(0.,0.,0.,&tae[9]); /* right arm */
7854 DegreeToTAE(0.,0.,0.,&tae[10]); /* right forearm */
7855 DegreeToTAE(0.,0.,0.,&tae[11]); /* right hand */
7856 DegreeToTAE(0.,0.,0.,&tae[12]); /* left arm */
7857 DegreeToTAE(0.,0.,0.,&tae[13]); /* left forearm */
7858 DegreeToTAE(0.,0.,0.,&tae[14]); /* left hand */
7859 DegreeToTAE(0.,0.,0.,&tae[15]); /* head */
7860 tae[7].x=0.; /* measure from lower waist */
7861 tae[7].y=0.;
7862 tae[7].z=0.;
7863 tae[6].x=tae[7].x; /* hip */
7864 tae[6].y=tae[7].y;
7865 tae[6].z=tae[7].y;
7866 TAEtoTAM(&tae[6],&tam);
7867 eyez.x=0.;
7868 eyez.y=0.14;
7869 eyez.z=-0.2;
7870 BodyToFrameTAM(&eyez,&tam,&eyex);
7871 tae[5].x=eyex.x; /* left thigh */
7872 tae[5].y=eyex.y;
7873 tae[5].z=eyex.z;
7874 eyez.x=0.;
7875 eyez.y=-0.14;
7876 eyez.z=-0.2;
7877 BodyToFrameTAM(&eyez,&tam,&eyex);
7878 tae[2].x=eyex.x; /* right thigh */
7879 tae[2].y=eyex.y;
7880 tae[2].z=eyex.z;
7881 TAEtoTAM(&tae[5],&tam);
7882 eyez.x=0.;
7883 eyez.y=0.;
7884 eyez.z=-0.4;
7885 BodyToFrameTAM(&eyez,&tam,&eyex);
7886 tae[4].x=eyex.x; /* left shin */
7887 tae[4].y=eyex.y;
7888 tae[4].z=eyex.z;
7889 TAEtoTAM(&tae[4],&tam);
7890 eyez.x=0.;
7891 eyez.y=0.;
7892 eyez.z=-0.4;
7893 BodyToFrameTAM(&eyez,&tam,&eyex);
7894 tae[3].x=eyex.x; /* left foot */
7895 tae[3].y=eyex.y;
7896 tae[3].z=eyex.z;
7897 TAEtoTAM(&tae[2],&tam);
7898 eyez.x=0.;
7899 eyez.y=0.;
7900 eyez.z=-0.4;
7901 BodyToFrameTAM(&eyez,&tam,&eyex);
7902 tae[1].x=eyex.x; /* right shin */
7903 tae[1].y=eyex.y;
7904 tae[1].z=eyex.z;
7905 TAEtoTAM(&tae[1],&tam);
7906 eyez.x=0.;
7907 eyez.y=0.;
7908 eyez.z=-0.4;
7909 BodyToFrameTAM(&eyez,&tam,&eyex);
7910 tae[0].x=eyex.x; /* right foot */
7911 tae[0].y=eyex.y;
7912 tae[0].z=eyex.z;
7913 TAEtoTAM(&tae[7],&tam);
7914 eyez.x=0.;
7915 eyez.y=0.;
7916 eyez.z=0.2;
7917 BodyToFrameTAM(&eyez,&tam,&eyex);
7918 tae[8].x=eyex.x; /* shoulder */
7919 tae[8].y=eyex.y;
7920 tae[8].z=eyex.z;
7921 TAEtoTAM(&tae[8],&tam);
7922 eyez.x=0.;
7923 eyez.y=0.;
7924 eyez.z=0.2;
7925 BodyToFrameTAM(&eyez,&tam,&eyex);
7926 tae[15].x=eyex.x; /* head */
7927 tae[15].y=eyex.y;
7928 tae[15].z=eyex.z;
7929 eyez.x=0.;
7930 eyez.y=-0.17;
7931 eyez.z=0.2;
7932 BodyToFrameTAM(&eyez,&tam,&eyex);
7933 tae[9].x=eyex.x; /* right arm */
7934 tae[9].y=eyex.y;
7935 tae[9].z=eyex.z;
7936 eyez.x=0.;
7937 eyez.y=0.17;
7938 eyez.z=0.2;
7939 BodyToFrameTAM(&eyez,&tam,&eyex);
7940 tae[12].x=eyex.x; /* left arm */
7941 tae[12].y=eyex.y;
7942 tae[12].z=eyex.z;
7943 TAEtoTAM(&tae[9],&tam);
7944 eyez.x=0.;
7945 eyez.y=0.;
7946 eyez.z=-0.29;
7947 BodyToFrameTAM(&eyez,&tam,&eyex);
7948 tae[10].x=eyex.x; /* right forearm */
7949 tae[10].y=eyex.y;
7950 tae[10].z=eyex.z;
7951 TAEtoTAM(&tae[10],&tam);
7952 eyez.x=0.;
7953 eyez.y=0.;
7954 eyez.z=-0.23;
7955 BodyToFrameTAM(&eyez,&tam,&eyex);
7956 tae[11].x=eyex.x; /* right hand */
7957 tae[11].y=eyex.y;
7958 tae[11].z=eyex.z;
7959 TAEtoTAM(&tae[12],&tam);
7960 eyez.x=0.;
7961 eyez.y=0.;
7962 eyez.z=-0.29;
7963 BodyToFrameTAM(&eyez,&tam,&eyex);
7964 tae[13].x=eyex.x; /* left forearm */
7965 tae[13].y=eyex.y;
7966 tae[13].z=eyex.z;
7967 TAEtoTAM(&tae[13],&tam);
7968 eyez.x=0.;
7969 eyez.y=0.;
7970 eyez.z=-0.23;
7971 BodyToFrameTAM(&eyez,&tam,&eyex);
7972 tae[14].x=eyex.x; /* left hand */
7973 tae[14].y=eyex.y;
7974 tae[14].z=eyex.z;
7975 ig=addelm("global",0,MPpoint); /* global */
7976 setlocal(0.,0.,0.,0.,0.,0.);
7977 il=addelm("ref point",0,MPpoint); /* local */
7978 setlocal(a[0],a[1],a[2],a[3],a[4],a[5]); /* center and orientation of the approximating box */
7979 tae1.x=a[0];
7980 tae1.y=a[1];
7981 tae1.z=a[2];
7982 tae1.theta=d2r*a[3];
7983 tae1.phi=d2r*a[4];
7984 tae1.psi=d2r*a[5];
7985 for(i=0;i<16;i++){
7986 MoveTwiceTAE(&tae1,&tae[i],&tae2);
7987 tae[i].x=tae2.x;
7988 tae[i].y=tae2.y;
7989 tae[i].z=tae2.z;
7990 tae[i].theta=tae2.theta;
7991 tae[i].phi=tae2.phi;
7992 tae[i].psi=tae2.psi;
7993 }
7994 eyez.x=thd.Tgx+thd.Dst*cos(d2r*thd.Azi)*cos(d2r*thd.Ele)-a[0];
7995 eyez.y=thd.Tgy+thd.Dst*sin(d2r*thd.Azi)*cos(d2r*thd.Ele)-a[1];
7996 eyez.z=thd.Tgz+thd.Dst*sin(d2r*thd.Ele)-a[2];
7997 NormPoint3d(&eyez,&eyex,&r);
7998 eyex.x=-eyez.y;
7999 eyex.y=eyez.x;
8000 eyex.z=0.;
8001 PointsToAE(&eyez,&eyex,zxAxis,&eyeae);
8002 ip=addelm("screen",ig,MPplane);
8003 setlocal(a[0],a[1],a[2],r2d*eyeae.theta,r2d*eyeae.phi,r2d*eyeae.psi);
8004 tae1.x=a[0];
8005 tae1.y=a[1];
8006 tae1.z=a[2];
8007 tae1.theta=eyeae.theta;
8008 tae1.phi=eyeae.phi;
8009 tae1.psi=eyeae.psi;
8010 /*
8011 Now, we have 16 bone coordinate systems and an eye coordinate system.
8012 */
8013 TAEtoTAM(&tae1,&tam1);
8014 TAEtoTAM(&tae[0],&tam); /* right foot */
8015 projxy(0.,0.,0.,&tam,&tam1,r,&p3[0]);
8016 projxy(0.18,0.,0.,&tam,&tam1,r,&p3[1]);
8017 adddeco();
8018 decolinexy(p3[0].x,p3[0].y,p3[1].x,p3[1].y);
8019 TAEtoTAM(&tae[1],&tam); /*right shin */
8020 projxy(0.,0.,0.,&tam,&tam1,r,&p3[2]);
8021 projxy(0.,0.,-0.4,&tam,&tam1,r,&p3[3]);
8022 adddeco();
8023 decolinexy(p3[2].x,p3[2].y,p3[3].x,p3[3].y);
8024 TAEtoTAM(&tae[2],&tam); /* right thigh */
8025 projxy(0.,0.,0.,&tam,&tam1,r,&p3[4]);
8026 projxy(0.,0.,-0.4,&tam,&tam1,r,&p3[5]);
8027 adddeco();
8028 decolinexy(p3[4].x,p3[4].y,p3[5].x,p3[5].y);
8029 TAEtoTAM(&tae[3],&tam); /* left foot */
8030 projxy(0.,0.,0.,&tam,&tam1,r,&p3[6]);
8031 projxy(0.18,0.,0.,&tam,&tam1,r,&p3[7]);
8032 adddeco();
8033 decolinexy(p3[6].x,p3[6].y,p3[7].x,p3[7].y);
8034 TAEtoTAM(&tae[4],&tam); /* left shin */
8035 projxy(0.,0.,0.,&tam,&tam1,r,&p3[8]);
8036 projxy(0.,0.,-0.4,&tam,&tam1,r,&p3[9]);
8037 adddeco();
8038 decolinexy(p3[8].x,p3[8].y,p3[9].x,p3[9].y);
8039 TAEtoTAM(&tae[5],&tam); /* left thigh */
8040 projxy(0.,0.,0.,&tam,&tam1,r,&p3[10]);
8041 projxy(0.,0.,-0.4,&tam,&tam1,r,&p3[11]);
8042 adddeco();
8043 decolinexy(p3[10].x,p3[10].y,p3[11].x,p3[11].y);
8044 TAEtoTAM(&tae[6],&tam); /* hip */
8045 projxy(0.,0.,0.,&tam,&tam1,r,&p3[12]);
8046 projxy(0.,-0.14,-0.2,&tam,&tam1,r,&p3[13]);
8047 projxy(0.,0.14,-0.2,&tam,&tam1,r,&p3[14]);
8048 adddeco();
8049 decolinexy(p3[12].x,p3[12].y,p3[13].x,p3[13].y);
8050 decolinexy(p3[13].x,p3[13].y,p3[14].x,p3[14].y);
8051 decolinexy(p3[14].x,p3[14].y,p3[12].x,p3[12].y);
8052 TAEtoTAM(&tae[7],&tam); /* waist */
8053 projxy(0.,0.,0.,&tam,&tam1,r,&p3[15]);
8054 projxy(0.,0.,0.2,&tam,&tam1,r,&p3[16]);
8055 adddeco();
8056 decolinexy(p3[15].x,p3[15].y,p3[16].x,p3[16].y);
8057 TAEtoTAM(&tae[8],&tam); /* shoulder */
8058 projxy(0.,0.,0.,&tam,&tam1,r,&p3[17]);
8059 projxy(0.,0.17,0.2,&tam,&tam1,r,&p3[18]);
8060 projxy(0.,0.,0.2,&tam,&tam1,r,&p3[19]);
8061 projxy(0.,-0.17,0.2,&tam,&tam1,r,&p3[20]);
8062 adddeco();
8063 decolinexy(p3[17].x,p3[17].y,p3[18].x,p3[18].y);
8064 decolinexy(p3[18].x,p3[18].y,p3[19].x,p3[19].y);
8065 decolinexy(p3[19].x,p3[19].y,p3[20].x,p3[20].y);
8066 decolinexy(p3[20].x,p3[20].y,p3[17].x,p3[17].y);
8067 TAEtoTAM(&tae[9],&tam); /* right arm */
8068 projxy(0.,0.,0.,&tam,&tam1,r,&p3[21]);
8069 projxy(0.,0.,-0.29,&tam,&tam1,r,&p3[22]);
8070 adddeco();
8071 decolinexy(p3[21].x,p3[21].y,p3[22].x,p3[22].y);
8072 TAEtoTAM(&tae[10],&tam); /* right forearm */
8073 projxy(0.,0.,0.,&tam,&tam1,r,&p3[23]);
8074 projxy(0.,0.,-0.23,&tam,&tam1,r,&p3[24]);
8075 adddeco();
8076 decolinexy(p3[23].x,p3[23].y,p3[24].x,p3[24].y);
8077 TAEtoTAM(&tae[11],&tam); /* right hand */
8078 projxy(0.,0.,0.,&tam,&tam1,r,&p3[25]);
8079 projxy(0.,0.,-0.18,&tam,&tam1,r,&p3[26]);
8080 adddeco();
8081 decolinexy(p3[25].x,p3[25].y,p3[26].x,p3[26].y);
8082 TAEtoTAM(&tae[12],&tam); /* left arm */
8083 projxy(0.,0.,0.,&tam,&tam1,r,&p3[27]);
8084 projxy(0.,0.,-0.29,&tam,&tam1,r,&p3[28]);
8085 adddeco();
8086 decolinexy(p3[27].x,p3[27].y,p3[28].x,p3[28].y);
8087 TAEtoTAM(&tae[13],&tam); /* left forearm */
8088 projxy(0.,0.,0.,&tam,&tam1,r,&p3[29]);
8089 projxy(0.,0.,-0.23,&tam,&tam1,r,&p3[30]);
8090 adddeco();
8091 decolinexy(p3[29].x,p3[29].y,p3[30].x,p3[30].y);
8092 TAEtoTAM(&tae[14],&tam); /* left hand */
8093 projxy(0.,0.,0.,&tam,&tam1,r,&p3[31]);
8094 projxy(0.,0.,-0.18,&tam,&tam1,r,&p3[32]);
8095 adddeco();
8096 decolinexy(p3[31].x,p3[31].y,p3[32].x,p3[32].y);
8097 TAEtoTAM(&tae[15],&tam); /* head */
8098 projxy(0.,0.,0.,&tam,&tam1,r,&p3[33]);
8099 projxy(0.,0.,0.2,&tam,&tam1,r,&p3[34]);
8100 adddeco();
8101 decolinexy(p3[33].x,p3[33].y,p3[34].x,p3[34].y);
8102 p3[35].x=100.;
8103 p3[35].y=100.;
8104 p3[36].x=-100.;
8105 p3[36].y=-100.;
8106 for(i=0;i<35;i++){
8107 if(p3[i].x<p3[35].x) p3[35].x=p3[i].x;
8108 if(p3[i].y<p3[35].y) p3[35].y=p3[i].y;
8109 if(p3[i].x>p3[36].x) p3[36].x=p3[i].x;
8110 if(p3[i].y>p3[36].y) p3[36].y=p3[i].y;
8111 }
8112 r=p3[36].x-p3[35].x;
8113 p3[35].x=p3[35].x-0.08*r;
8114 p3[36].x=p3[36].x+0.08*r;
8115 r=p3[36].y-p3[35].y;
8116 p3[35].y=p3[35].y-0.08*r;
8117 p3[36].y=p3[36].y+0.08*r;
8118 setcolor(MPyellow);
8119 set2Dpoint(p3[35].x,p3[35].y);
8120 set2Dpoint(p3[36].x,p3[35].y);
8121 set2Dpoint(p3[36].x,p3[36].y);
8122 set2Dpoint(p3[35].x,p3[36].y);
8123 break;
8124 case 53: /* decorated skeleton - projected to a plane - head added */
8125 /* approximated by a box with sides (0.2,0.4,1.64) */
8126 DegreeToTAE(0.,0.,0.,&tae[0]); /* right foot */
8127 DegreeToTAE(0.,0.,0.,&tae[1]); /* right shin */
8128 DegreeToTAE(0.,0.,0.,&tae[2]); /* right thigh */
8129 DegreeToTAE(0.,0.,0.,&tae[3]); /* left foot */
8130 DegreeToTAE(0.,0.,0.,&tae[4]); /* left shin */
8131 DegreeToTAE(0.,0.,0.,&tae[5]); /* left thigh */
8132 DegreeToTAE(0.,0.,0.,&tae[6]); /* hip */
8133 DegreeToTAE(0.,0.,0.,&tae[7]); /* waist */
8134 DegreeToTAE(0.,0.,0.,&tae[8]); /* shoulder */
8135 DegreeToTAE(0.,0.,0.,&tae[9]); /* right arm */
8136 DegreeToTAE(0.,0.,0.,&tae[10]); /* right forearm */
8137 DegreeToTAE(0.,0.,0.,&tae[11]); /* right hand */
8138 DegreeToTAE(0.,0.,0.,&tae[12]); /* left arm */
8139 DegreeToTAE(0.,0.,0.,&tae[13]); /* left forearm */
8140 DegreeToTAE(0.,0.,0.,&tae[14]); /* left hand */
8141 DegreeToTAE(0.,0.,0.,&tae[15]); /* head */
8142 tae[7].x=0.; /* measure from lower waist */
8143 tae[7].y=0.;
8144 tae[7].z=0.;
8145 tae[6].x=tae[7].x; /* hip */
8146 tae[6].y=tae[7].y;
8147 tae[6].z=tae[7].y;
8148 TAEtoTAM(&tae[6],&tam);
8149 eyez.x=0.;
8150 eyez.y=0.14;
8151 eyez.z=-0.2;
8152 BodyToFrameTAM(&eyez,&tam,&eyex);
8153 tae[5].x=eyex.x; /* left thigh */
8154 tae[5].y=eyex.y;
8155 tae[5].z=eyex.z;
8156 eyez.x=0.;
8157 eyez.y=-0.14;
8158 eyez.z=-0.2;
8159 BodyToFrameTAM(&eyez,&tam,&eyex);
8160 tae[2].x=eyex.x; /* right thigh */
8161 tae[2].y=eyex.y;
8162 tae[2].z=eyex.z;
8163 TAEtoTAM(&tae[5],&tam);
8164 eyez.x=0.;
8165 eyez.y=0.;
8166 eyez.z=-0.4;
8167 BodyToFrameTAM(&eyez,&tam,&eyex);
8168 tae[4].x=eyex.x; /* left shin */
8169 tae[4].y=eyex.y;
8170 tae[4].z=eyex.z;
8171 TAEtoTAM(&tae[4],&tam);
8172 eyez.x=0.;
8173 eyez.y=0.;
8174 eyez.z=-0.4;
8175 BodyToFrameTAM(&eyez,&tam,&eyex);
8176 tae[3].x=eyex.x; /* left foot */
8177 tae[3].y=eyex.y;
8178 tae[3].z=eyex.z;
8179 TAEtoTAM(&tae[2],&tam);
8180 eyez.x=0.;
8181 eyez.y=0.;
8182 eyez.z=-0.4;
8183 BodyToFrameTAM(&eyez,&tam,&eyex);
8184 tae[1].x=eyex.x; /* right shin */
8185 tae[1].y=eyex.y;
8186 tae[1].z=eyex.z;
8187 TAEtoTAM(&tae[1],&tam);
8188 eyez.x=0.;
8189 eyez.y=0.;
8190 eyez.z=-0.4;
8191 BodyToFrameTAM(&eyez,&tam,&eyex);
8192 tae[0].x=eyex.x; /* right foot */
8193 tae[0].y=eyex.y;
8194 tae[0].z=eyex.z;
8195 TAEtoTAM(&tae[7],&tam);
8196 eyez.x=0.;
8197 eyez.y=0.;
8198 eyez.z=0.2;
8199 BodyToFrameTAM(&eyez,&tam,&eyex);
8200 tae[8].x=eyex.x; /* shoulder */
8201 tae[8].y=eyex.y;
8202 tae[8].z=eyex.z;
8203 TAEtoTAM(&tae[8],&tam);
8204 eyez.x=0.;
8205 eyez.y=0.;
8206 eyez.z=0.2;
8207 BodyToFrameTAM(&eyez,&tam,&eyex);
8208 tae[15].x=eyex.x; /* head */
8209 tae[15].y=eyex.y;
8210 tae[15].z=eyex.z;
8211 eyez.x=0.;
8212 eyez.y=-0.17;
8213 eyez.z=0.2;
8214 BodyToFrameTAM(&eyez,&tam,&eyex);
8215 tae[9].x=eyex.x; /* right arm */
8216 tae[9].y=eyex.y;
8217 tae[9].z=eyex.z;
8218 eyez.x=0.;
8219 eyez.y=0.17;
8220 eyez.z=0.2;
8221 BodyToFrameTAM(&eyez,&tam,&eyex);
8222 tae[12].x=eyex.x; /* left arm */
8223 tae[12].y=eyex.y;
8224 tae[12].z=eyex.z;
8225 TAEtoTAM(&tae[9],&tam);
8226 eyez.x=0.;
8227 eyez.y=0.;
8228 eyez.z=-0.29;
8229 BodyToFrameTAM(&eyez,&tam,&eyex);
8230 tae[10].x=eyex.x; /* right forearm */
8231 tae[10].y=eyex.y;
8232 tae[10].z=eyex.z;
8233 TAEtoTAM(&tae[10],&tam);
8234 eyez.x=0.;
8235 eyez.y=0.;
8236 eyez.z=-0.23;
8237 BodyToFrameTAM(&eyez,&tam,&eyex);
8238 tae[11].x=eyex.x; /* right hand */
8239 tae[11].y=eyex.y;
8240 tae[11].z=eyex.z;
8241 TAEtoTAM(&tae[12],&tam);
8242 eyez.x=0.;
8243 eyez.y=0.;
8244 eyez.z=-0.29;
8245 BodyToFrameTAM(&eyez,&tam,&eyex);
8246 tae[13].x=eyex.x; /* left forearm */
8247 tae[13].y=eyex.y;
8248 tae[13].z=eyex.z;
8249 TAEtoTAM(&tae[13],&tam);
8250 eyez.x=0.;
8251 eyez.y=0.;
8252 eyez.z=-0.23;
8253 BodyToFrameTAM(&eyez,&tam,&eyex);
8254 tae[14].x=eyex.x; /* left hand */
8255 tae[14].y=eyex.y;
8256 tae[14].z=eyex.z;
8257 ig=addelm("global",0,MPpoint); /* global */
8258 setlocal(0.,0.,0.,0.,0.,0.);
8259 il=addelm("ref point",0,MPpoint); /* local */
8260 setlocal(a[0],a[1],a[2],a[3],a[4],a[5]); /* center and orientation of the approximating box */
8261 tae1.x=a[0];
8262 tae1.y=a[1];
8263 tae1.z=a[2];
8264 tae1.theta=d2r*a[3];
8265 tae1.phi=d2r*a[4];
8266 tae1.psi=d2r*a[5];
8267 for(i=0;i<16;i++){
8268 MoveTwiceTAE(&tae1,&tae[i],&tae2);
8269 tae[i].x=tae2.x;
8270 tae[i].y=tae2.y;
8271 tae[i].z=tae2.z;
8272 tae[i].theta=tae2.theta;
8273 tae[i].phi=tae2.phi;
8274 tae[i].psi=tae2.psi;
8275 }
8276 eyez.x=thd.Tgx+thd.Dst*cos(d2r*thd.Azi)*cos(d2r*thd.Ele);
8277 eyez.y=thd.Tgy+thd.Dst*sin(d2r*thd.Azi)*cos(d2r*thd.Ele);
8278 eyez.z=thd.Tgz+thd.Dst*sin(d2r*thd.Ele);
8279 TAEtoTAM(&tae1,&tam1);
8280 FrameToBodyTAM(&eyez,&tam1,&eyex);
8281 NormPoint3d(&eyex,&eyez,&r);
8282 ToPolar3d(&eyez,&p3[0]); /* az.el */
8283 az=p3[0].x;
8284 el=p3[0].y;
8285 eyez.x=thd.Tgx+thd.Dst*cos(d2r*thd.Azi)*cos(d2r*thd.Ele)-a[0];
8286 eyez.y=thd.Tgy+thd.Dst*sin(d2r*thd.Azi)*cos(d2r*thd.Ele)-a[1];
8287 eyez.z=thd.Tgz+thd.Dst*sin(d2r*thd.Ele)-a[2];
8288 NormPoint3d(&eyez,&eyex,&r);
8289 eyex.x=-eyez.y;
8290 eyex.y=eyez.x;
8291 eyex.z=0.;
8292 PointsToAE(&eyez,&eyex,zxAxis,&eyeae);
8293 ip=addelm("screen",ig,MPplane);
8294 setlocal(a[0],a[1],a[2],r2d*eyeae.theta,r2d*eyeae.phi,r2d*eyeae.psi);
8295 tae1.x=a[0];
8296 tae1.y=a[1];
8297 tae1.z=a[2];
8298 tae1.theta=eyeae.theta;
8299 tae1.phi=eyeae.phi;
8300 tae1.psi=eyeae.psi;
8301 /*
8302 Now, we have 16 bone coordinate systems and an eye coordinate system.
8303 */
8304 e=atan(-1.7/tan(az));
8305 if(az<0.) e=e-3.14159265;
8306 TAEtoTAM(&tae1,&tam1);
8307 TAEtoTAM(&tae[7],&tam); /* waist */
8308 for(i=0;i<4;i++){
8309 projxy(0.1*cos(e),0.17*sin(e),0.,&tam,&tam1,r,&p4[i]);
8310 e=e+1.047197551;
8311 }
8312 for(j=0;j<2;j++){
8313 if((az>=0.&&j==0)||(az<0.&&j==1)){
8314 TAEtoTAM(&tae[0],&tam); /* right foot */
8315 projxy(0.,0.,0.,&tam,&tam1,r,&p3[0]);
8316 projxy(0.18,0.,0.,&tam,&tam1,r,&p3[1]);
8317 adddeco();
8318 decolinexy(p3[0].x,p3[0].y,p3[1].x,p3[1].y);
8319 p3[37]=p3[0];
8320 }
8321 else{
8322 TAEtoTAM(&tae[3],&tam); /* left foot */
8323 projxy(0.,0.,0.,&tam,&tam1,r,&p3[6]);
8324 projxy(0.18,0.,0.,&tam,&tam1,r,&p3[7]);
8325 adddeco();
8326 decolinexy(p3[6].x,p3[6].y,p3[7].x,p3[7].y);
8327 p3[37]=p3[6];
8328 }
8329 eyez.x=thd.Tgx+thd.Dst*cos(d2r*thd.Azi)*cos(d2r*thd.Ele);
8330 eyez.y=thd.Tgy+thd.Dst*sin(d2r*thd.Azi)*cos(d2r*thd.Ele);
8331 eyez.z=thd.Tgz+thd.Dst*sin(d2r*thd.Ele);
8332 FrameToBodyTAM(&eyez,&tam,&eyex);
8333 NormPoint3d(&eyex,&eyez,&r);
8334 ToPolar3d(&eyez,&p3[38]); /* az.el */
8335 cntrfoot(&p3[38],&p3[39]); /* contour of foot in 6 points + terminating point */
8336 adddeco();
8337 for(i=0;i<6;i++)
8338 decocontxy(p3[37].x+p3[39+i].x,p3[37].y+p3[39+i].y);
8339 decocontco(MPred);
8340 adddeco();
8341 for(i=0;i<6;i++) decolinexy(p3[37].x+p3[39+i].x,p3[37].y+p3[39+i].y,p3[37].x+p3[39+i+1].x,p3[37].y+p3[39+i+1].y);
8342 }
8343 TAEtoTAM(&tae[1],&tam); /*right shin */
8344 projxy(0.,0.,0.,&tam,&tam1,r,&p3[2]);
8345 projxy(0.,0.,-0.4,&tam,&tam1,r,&p3[3]);
8346 adddeco();
8347 decolinexy(p3[2].x,p3[2].y,p3[3].x,p3[3].y);
8348 TAEtoTAM(&tae[2],&tam); /* right thigh */
8349 projxy(0.,0.,0.,&tam,&tam1,r,&p3[4]);
8350 projxy(0.,0.,-0.4,&tam,&tam1,r,&p3[5]);
8351 adddeco();
8352 decolinexy(p3[4].x,p3[4].y,p3[5].x,p3[5].y);
8353 TAEtoTAM(&tae[4],&tam); /* left shin */
8354 projxy(0.,0.,0.,&tam,&tam1,r,&p3[8]);
8355 projxy(0.,0.,-0.4,&tam,&tam1,r,&p3[9]);
8356 adddeco();
8357 decolinexy(p3[8].x,p3[8].y,p3[9].x,p3[9].y);
8358 TAEtoTAM(&tae[5],&tam); /* left thigh */
8359 projxy(0.,0.,0.,&tam,&tam1,r,&p3[10]);
8360 projxy(0.,0.,-0.4,&tam,&tam1,r,&p3[11]);
8361 adddeco();
8362 decolinexy(p3[10].x,p3[10].y,p3[11].x,p3[11].y);
8363 TAEtoTAM(&tae[6],&tam); /* hip */
8364 projxy(0.,0.,0.,&tam,&tam1,r,&p3[12]);
8365 projxy(0.,-0.14,-0.2,&tam,&tam1,r,&p3[13]);
8366 projxy(0.,0.14,-0.2,&tam,&tam1,r,&p3[14]);
8367 adddeco();
8368 decolinexy(p3[12].x,p3[12].y,p3[13].x,p3[13].y);
8369 decolinexy(p3[13].x,p3[13].y,p3[14].x,p3[14].y);
8370 decolinexy(p3[14].x,p3[14].y,p3[12].x,p3[12].y);
8371 p3[37].x=az;
8372 p3[37].y=el;
8373 p3[38]=p3[4]; /* right hip */
8374 p3[39]=p3[5];
8375 p3[40]=p3[3];
8376 p3[41]=p3[10]; /* left hip */
8377 p3[42]=p3[11];
8378 p3[43]=p3[9];
8379 cntrpants(&p3[37],&p3[44],&i,&j);
8380 e=-10.;
8381 k=0;
8382 for(j=0;j<i;j++){
8383 if((-p3[44+j].x+p3[44+j].y)>e){
8384 k=j;
8385 e=-p3[44+j].x+p3[44+j].y;
8386 }
8387 }
8388 e=-10.;
8389 n=0;
8390 for(j=0;j<i;j++){
8391 if((p3[44+j].x+p3[44+j].y)>e){
8392 n=j;
8393 e=p3[44+j].x+p3[44+j].y;
8394 }
8395 }
8396 if(k<n){
8397 m=0;
8398 for(j=k+1;j<n;j++,m++) p3[70+m]=p3[44+j];
8399 for(j=3;j>=0;j--,m++) p3[70+m]=p4[j];
8400 adddeco();
8401 for(j=0;j<m;j++) decocontxy(p3[70+j].x,p3[70+j].y);
8402 decocontco(MPblue);
8403 adddeco();
8404 for(j=0;j<m-1;j++) decolinexy(p3[70+j].x,p3[70+j].y,p3[70+j+1].x,p3[70+j+1].y);
8405 decolinexy(p3[70+j].x,p3[70+j].y,p3[70].x,p3[70].y);
8406 }
8407 else{
8408 adddeco();
8409 for(j=0;j<i;j++) decocontxy(p3[44+j].x,p3[44+j].y);
8410 decocontco(MPblue);
8411 adddeco();
8412 for(j=0;j<i-1;j++) decolinexy(p3[44+j].x,p3[44+j].y,p3[44+j+1].x,p3[44+j+1].y);
8413 decolinexy(p3[44+j].x,p3[44+j].y,p3[44].x,p3[44].y);
8414 }
8415 TAEtoTAM(&tae[7],&tam); /* waist */
8416 projxy(0.,0.,0.,&tam,&tam1,r,&p3[15]);
8417 projxy(0.,0.,0.2,&tam,&tam1,r,&p3[16]);
8418 adddeco();
8419 decolinexy(p3[15].x,p3[15].y,p3[16].x,p3[16].y);
8420 TAEtoTAM(&tae[8],&tam); /* shoulder */
8421 projxy(0.,0.,0.,&tam,&tam1,r,&p3[17]);
8422 projxy(0.,0.17,0.2,&tam,&tam1,r,&p3[18]);
8423 projxy(0.,0.,0.2,&tam,&tam1,r,&p3[19]);
8424 projxy(0.,-0.17,0.2,&tam,&tam1,r,&p3[20]);
8425 adddeco();
8426 decolinexy(p3[17].x,p3[17].y,p3[18].x,p3[18].y);
8427 decolinexy(p3[18].x,p3[18].y,p3[19].x,p3[19].y);
8428 decolinexy(p3[19].x,p3[19].y,p3[20].x,p3[20].y);
8429 decolinexy(p3[20].x,p3[20].y,p3[17].x,p3[17].y);
8430 TAEtoTAM(&tae[9],&tam); /* right arm */
8431 projxy(0.,0.,0.,&tam,&tam1,r,&p3[21]);
8432 projxy(0.,0.,-0.29,&tam,&tam1,r,&p3[22]);
8433 adddeco();
8434 decolinexy(p3[21].x,p3[21].y,p3[22].x,p3[22].y);
8435 TAEtoTAM(&tae[10],&tam); /* right forearm */
8436 projxy(0.,0.,0.,&tam,&tam1,r,&p3[23]);
8437 projxy(0.,0.,-0.23,&tam,&tam1,r,&p3[24]);
8438 adddeco();
8439 decolinexy(p3[23].x,p3[23].y,p3[24].x,p3[24].y);
8440 TAEtoTAM(&tae[11],&tam); /* right hand */
8441 projxy(0.,0.,0.,&tam,&tam1,r,&p3[25]);
8442 projxy(0.,0.,-0.18,&tam,&tam1,r,&p3[26]);
8443 adddeco();
8444 decolinexy(p3[25].x,p3[25].y,p3[26].x,p3[26].y);
8445 TAEtoTAM(&tae[12],&tam); /* left arm */
8446 projxy(0.,0.,0.,&tam,&tam1,r,&p3[27]);
8447 projxy(0.,0.,-0.29,&tam,&tam1,r,&p3[28]);
8448 adddeco();
8449 decolinexy(p3[27].x,p3[27].y,p3[28].x,p3[28].y);
8450 TAEtoTAM(&tae[13],&tam); /* left forearm */
8451 projxy(0.,0.,0.,&tam,&tam1,r,&p3[29]);
8452 projxy(0.,0.,-0.23,&tam,&tam1,r,&p3[30]);
8453 adddeco();
8454 decolinexy(p3[29].x,p3[29].y,p3[30].x,p3[30].y);
8455 TAEtoTAM(&tae[14],&tam); /* left hand */
8456 projxy(0.,0.,0.,&tam,&tam1,r,&p3[31]);
8457 projxy(0.,0.,-0.18,&tam,&tam1,r,&p3[32]);
8458 adddeco();
8459 decolinexy(p3[31].x,p3[31].y,p3[32].x,p3[32].y);
8460 for(j=0;j<5;j++){
8461 if((az>=0.&&j==0)||(az<0.&&j==3)){
8462 TAEtoTAM(&tae[11],&tam); /* right hand */
8463 projxy(0.,0.,0.,&tam,&tam1,r,&p3[25]);
8464 projxy(0.,0.,-0.18,&tam,&tam1,r,&p3[26]);
8465 adddeco();
8466 decolinexy(p3[25].x,p3[25].y,p3[26].x,p3[26].y);
8467 p3[37]=p3[25];
8468 p3[38]=p3[26];
8469 }
8470 else if((az>=0.&&j==1)||(az<0.&&j==4)){ /* right arm */
8471 p3[37].x=az;
8472 p3[37].y=el;
8473 p3[38].x=p3[20].x;
8474 p3[38].y=p3[20].y-0.03;
8475 p3[39]=p3[21];
8476 p3[40]=p3[22];
8477 p3[41]=p3[24];
8478 }
8479 else if(j==2){
8480 TAEtoTAM(&tae[7],&tam); /* waist */
8481 projxy(0.,0.,0.,&tam,&tam1,r,&p3[15]);
8482 projxy(0.,0.,0.2,&tam,&tam1,r,&p3[16]);
8483 p3[37]=p3[15];
8484 p3[38]=p3[16];
8485 }
8486 else if((az>=0.&&j==3)||(az<0.&&j==0)){
8487 TAEtoTAM(&tae[14],&tam); /* left hand */
8488 projxy(0.,0.,0.,&tam,&tam1,r,&p3[31]);
8489 projxy(0.,0.,-0.18,&tam,&tam1,r,&p3[32]);
8490 adddeco();
8491 decolinexy(p3[31].x,p3[31].y,p3[32].x,p3[32].y);
8492 p3[37]=p3[31];
8493 p3[38]=p3[32];
8494 }
8495 else{ /* (az<=0.&&j==4)||(az<0.&&j==1) left arm */
8496 p3[37].x=az;
8497 p3[37].y=el;
8498 p3[38].x=p3[18].x;
8499 p3[38].y=p3[18].y-0.03;
8500 p3[39]=p3[27];
8501 p3[40]=p3[28];
8502 p3[41]=p3[30];
8503 }
8504 if(j==1||j==4){
8505 cntrarm(&p3[37],&p3[42]);
8506 for(i=0;i<7;i++)
8507 decocontxy(p3[42+i].x,p3[42+i].y);
8508 decocontco(MPgreen);
8509 adddeco();
8510 for(i=0;i<7;i++) decolinexy(p3[42+i].x,p3[42+i].y,p3[42+i+1].x,p3[42+i+1].y);
8511 }
8512 else{
8513 eyez.x=thd.Tgx+thd.Dst*cos(d2r*thd.Azi)*cos(d2r*thd.Ele);
8514 eyez.y=thd.Tgy+thd.Dst*sin(d2r*thd.Azi)*cos(d2r*thd.Ele);
8515 eyez.z=thd.Tgz+thd.Dst*sin(d2r*thd.Ele);
8516 FrameToBodyTAM(&eyez,&tam,&eyex);
8517 NormPoint3d(&eyex,&eyez,&r);
8518 ToPolar3d(&eyez,&p3[39]); /* az.el */
8519 if(j==2) cntrbody(&p3[37],&p3[40]); /* contour of body in 12 points + terminating point */
8520 else cntrhand(&p3[37],&p3[40]); /* contour of hand in 12 points + terminating point */
8521 adddeco();
8522 for(i=0;i<12;i++)
8523 decocontxy(p3[37].x+p3[40+i].x,p3[37].y+p3[40+i].y);
8524 if(j==2) decocontco(MPgreen);
8525 else decocontco(MPred);
8526 adddeco();
8527 for(i=0;i<12;i++) decolinexy(p3[37].x+p3[40+i].x,p3[37].y+p3[40+i].y,p3[37].x+p3[40+i+1].x,p3[37].y+p3[40+i+1].y);
8528 }
8529 }
8530 TAEtoTAM(&tae[15],&tam); /* head */
8531 projxy(0.,0.,0.,&tam,&tam1,r,&p3[33]);
8532 projxy(0.,0.,0.2,&tam,&tam1,r,&p3[34]);
8533 adddeco();
8534 decolinexy(p3[33].x,p3[33].y,p3[34].x,p3[34].y);
8535 eyez.x=thd.Tgx+thd.Dst*cos(d2r*thd.Azi)*cos(d2r*thd.Ele);
8536 eyez.y=thd.Tgy+thd.Dst*sin(d2r*thd.Azi)*cos(d2r*thd.Ele);
8537 eyez.z=thd.Tgz+thd.Dst*sin(d2r*thd.Ele);
8538 FrameToBodyTAM(&eyez,&tam,&eyex);
8539 NormPoint3d(&eyex,&eyez,&r);
8540 ToPolar3d(&eyez,&p3[37]); /* az.el */
8541 cntrhead(&p3[37],&p3[38]); /* contour of head in 21 points */
8542 adddeco();
8543 for(i=0;i<21;i++)
8544 decocontxy(p3[33].x+p3[38+i].x,p3[33].y+p3[38+i].y);
8545 decocontco(MPred);
8546 adddeco();
8547 for(i=0;i<20;i++) decolinexy(p3[33].x+p3[38+i].x,p3[33].y+p3[38+i].y,p3[33].x+p3[38+i+1].x,p3[33].y+p3[38+i+1].y);
8548 p3[35].x=100.;
8549 p3[35].y=100.;
8550 p3[36].x=-100.;
8551 p3[36].y=-100.;
8552 for(i=0;i<35;i++){
8553 if(p3[i].x<p3[35].x) p3[35].x=p3[i].x;
8554 if(p3[i].y<p3[35].y) p3[35].y=p3[i].y;
8555 if(p3[i].x>p3[36].x) p3[36].x=p3[i].x;
8556 if(p3[i].y>p3[36].y) p3[36].y=p3[i].y;
8557 }
8558 r=p3[36].x-p3[35].x;
8559 p3[35].x=p3[35].x-0.08*r;
8560 p3[36].x=p3[36].x+0.08*r;
8561 r=p3[36].y-p3[35].y;
8562 p3[35].y=p3[35].y-0.08*r;
8563 p3[36].y=p3[36].y+0.08*r;
8564 /*
8565 setcolor(MPyellow);
8566 set2Dpoint(p3[35].x,p3[35].y);
8567 set2Dpoint(p3[36].x,p3[35].y);
8568 set2Dpoint(p3[36].x,p3[36].y);
8569 set2Dpoint(p3[35].x,p3[36].y);*/
8570 break;
8571 case 40:
8572 addelm("ref point",0,MPpoint);
8573 setlocal(a[0],a[1],a[2],a[3],a[4],a[5]);
8574 addelm("tube",0,MPbodyOfRot);
8575 setlocal(0.,0.,0.,0.,0.,0.);
8576 setcolor(MPyellow);
8577 set2Dpoint(0.2,-0.25);
8578 set2Dpoint(0.25,0.25);
8579 adddeco();
8580 decocontxy(-20.,0.1);
8581 decocontxy(20.,0.1);
8582 decocontxy(25.,0.3);
8583 decocontxy(-25.,0.3);
8584 decocontco(MPblue);
8585 adddeco();
8586 decocontxy(-20.,0.4);
8587 decocontxy(20.,0.4);
8588 decocontxy(25.,0.6);
8589 decocontxy(-25.,0.6);
8590 decocontco(MPred);
8591 adddeco();
8592 decolinexy(-20.,0.8,20.,0.8);
8593 decolinexy(-20.,0.7,20.,0.7);
8594 break;
8595 case 41:
8596 addelm("ref point",0,MPpoint);
8597 setlocal(a[0],a[1],a[2],a[3],a[4],a[5]);
8598 addelm("tube",0,MPbodyOfRot);
8599 setlocal(0.,0.,0.,0.,0.,0.);
8600 setcolor(MPyellow);
8601 set2Dpoint(0.2,-0.25);
8602 set2Dpoint(0.25,0.25);
8603 adddeco();
8604 decocontxy(-20.,0.1);
8605 decocontxy(20.,0.1);
8606 decocontxy(30.,0.3);
8607 decocontxy(-30.,0.3);
8608 decocontco(MPblue);
8609 adddeco();
8610 decocontxy(-20.,0.4);
8611 decocontxy(20.,0.4);
8612 decocontxy(25.,0.6);
8613 decocontxy(-25.,0.6);
8614 decocontco(MPred);
8615 adddeco();
8616 decolinexy(-20.,0.8,20.,0.8);
8617 decolinexy(-20.,0.7,20.,0.7);
8618 break;
8619 case 60:
8620 case 61:
8621 case 62:
8622 case 63:
8623 switch(id){
8624 case 60:
8625 strcpy(str,"SOUTH");
8626 break;
8627 case 61:
8628 strcpy(str,"WEST");
8629 break;
8630 case 62:
8631 strcpy(str,"NORTH");
8632 break;
8633 case 63:
8634 strcpy(str,"EAST");
8635 break;
8636 }
8637 addelm("point",0,0);
8638 setlocal(a[0],a[1],a[2],a[3],a[4],a[5]);
8639 addelm("placard",0,2);
8640 setlocal(0.,0.,-0.01,0.,0.,0.);
8641 h=0.2;
8642 w=0.84;
8643 set2Dpoint(-0.5*w,-0.5*h);
8644 set2Dpoint(0.5*w,-0.5*h);
8645 set2Dpoint(0.5*w,0.5*h);
8646 set2Dpoint(-0.5*w,0.5*h);
8647 setcolor(MPwhite);
8648 adddeco();
8649 // decolinexy(-0.5*w,-0.5*h,0.5*w,0.5*h);
8650 // decolinexy(-0.5*w,0.5*h,0.5*w,-0.5*h);
8651 styleVF16(0.6*h/16.,0.6*h/16.,0.);
8652 drawVF16(-0.45*w,-0.3*h,str);
8653 decocontco(MPred);
8654 break;
8655 default:
8656 break;
8657 }
8658 thd.Draw();
8659 return 0;
8660}
8661
8662int styleVF16(double scx,double scy,double ang)
8663{
8664 scalex=scx;
8665 scaley=scy;
8666 angle=ang; /* degree */
8667 if(angle!=0.){
8668 cosa=cos(0.01745329252*angle);
8669 sina=sin(0.01745329252*angle);
8670 }
8671}
8672
8673int drawVF16(double xs,double ys,char *st)
8674{
8675 int i,j;
8676 char s[200];
8677 char s1[200];
8678 char *sp;
8679 char *sp1;
8680 iconv_t cd;
8681 size_t inleft,outleft;
8682 int ch;
8683 unsigned char a[200];
8684 double x,y,x0,y0,x1,y1,x2,y2,x3,y3;
8685 int penUp;
8686
8687 strcpy(s,st);
8688 sp=&s[0];
8689 sp1=&s1[0];
8690 inleft=strlen(s);
8691 outleft=199;
8692 cd=iconv_open("SHIFT-JIS","UTF-8");
8693 iconv(cd,&sp,&inleft,&sp1,&outleft);
8694 iconv_close(cd);
8695 s1[199-outleft]='\0';
8696 outleft=strlen(s1);
8697 x0=xs;
8698 y0=ys;
8699 x1=x0;
8700 y1=y0;
8701 for(i=0;i<outleft;i++){
8702 if((s1[i]&0x80)==0){
8703 s[0]=s1[i];
8704 s[1]='\0';
8705 }
8706 else{
8707 s[1]=s1[i];
8708 i++;
8709 s[0]=s1[i];
8710 s[2]='\0';
8711 }
8712 j=getVF16s(s,a);
8713 penUp=1;
8714 for(j=0;j<100;j++){
8715 ch=a[j];
8716 ch&=0xff;
8717 if(ch==0xff) break;
8718 if(ch!=0xfe){
8719 x=ch/16;
8720 y=ch-16*x;
8721 x=x*scalex;
8722 y=y*scaley;
8723 x2=x1+x;
8724 y2=y1+y;
8725 if(angle!=0.){
8726 x2=x0+(x1+x-x0)*cosa-(y1+y-y0)*sina;
8727 y2=y0+(x1+x-x0)*sina+(y1+y-y0)*cosa;
8728 }
8729 if(penUp);
8730 else decolinexy(x3,y3,x2,y2);
8731 x3=x2;
8732 y3=y2;
8733 penUp=0;
8734 }
8735 else penUp=1;
8736 }
8737 x1=x1+16*scalex;
8738 }
8739 return 0;
8740}
8741