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