gcs4.c
0000/**********************************************************************
0001 gcs4.c
0002
0003 3D Geometry Utility
0004
0005**********************************************************************/
0006
0007#include "gcs4.h"
0008#include <math.h>
0009
0010#define d2r 0.01745329252
0011#define r2d 57.295779513
0012#define TRUE 1
0013#define FALSE 0
0014
0015/*********************************************************************
0016
0017 DegreeToTAE
0018
0019 Put angles in degree into TransAngleEuler structure
0020
0021*********************************************************************/
0022void DegreeToTAE(double theta,double phi,double psi,TransAngleEuler *dest)
0023{
0024 dest->theta=d2r*theta;
0025 dest->phi=d2r*phi;
0026 dest->psi=d2r*psi;
0027}
0028
0029
0030/*********************************************************************
0031
0032 LinesToTAM
0033
0034 Construct TransAngleMatrix parameter from 2 Line3D parameters
0035
0036 a line segment on one of the coordinate axis
0037 b line segment on one of the coordinate axis
0038 w a and b are yzAxis, zxAxis or xyAxis
0039 dest TransAngleMatrix constructed
0040*********************************************************************/
0041void LinesToTAM(Line3d *a,Line3d *b,int w,TransAngleMatrix *dest)
0042{
0043 Point3d x,y,z;
0044 double d;
0045
0046 dest->x=a->sx;
0047 dest->y=a->sy;
0048 dest->z=a->sz;
0049
0050 switch (w){
0051 case yzAxis:
0052 NormLine3d(a,&y,&d);
0053 NormLine3d(b,&z,&d);
0054 x.x=y.y*z.z-y.z*z.y;
0055 x.y=y.z*z.x-y.x*z.z;
0056 x.z=y.x*z.y-y.y*z.x;
0057 break;
0058 case zxAxis:
0059 NormLine3d(a,&z,&d);
0060 NormLine3d(b,&x,&d);
0061 y.x=z.y*x.z-z.z*x.y;
0062 y.y=z.z*x.x-z.x*x.z;
0063 y.z=z.x*x.y-z.y*x.x;
0064 break;
0065 case xyAxis:
0066 NormLine3d(a,&x,&d);
0067 NormLine3d(b,&y,&d);
0068 z.x=x.y*y.z-x.z*y.y;
0069 z.y=x.z*y.x-x.x*y.z;
0070 z.z=x.x*y.y-x.y*y.x;
0071 break;
0072 }
0073 dest->c[0][0]=x.x;
0074 dest->c[1][0]=x.y;
0075 dest->c[2][0]=x.z;
0076 dest->c[0][1]=y.x;
0077 dest->c[1][1]=y.y;
0078 dest->c[2][1]=y.z;
0079 dest->c[0][2]=z.x;
0080 dest->c[1][2]=z.y;
0081 dest->c[2][2]=z.z;
0082}
0083
0084/*********************************************************************
0085
0086 LinesToTAE
0087
0088 Construct TransAngleEuler parameter from 2 Line3D parameters
0089
0090 a line segment on one of the coordinate axis
0091 b line segment on one of the coordinate axis
0092 w a and b are yzAxis, zxAxis or xyAxis
0093 dest TransAngleEuler constructed
0094*********************************************************************/
0095void LinesToTAE(Line3d *a,Line3d *b,int w,TransAngleEuler *dest)
0096{
0097 TransAngleMatrix tmp;
0098
0099 LinesToTAM(a,b,w,&tmp);
0100 dest->x=tmp.x;
0101 dest->y=tmp.y;
0102 dest->z=tmp.z;
0103
0104 if(tmp.c[0][2]==0.&&tmp.c[1][2]==0.){
0105 dest->theta=0.;
0106 dest->phi=0.;
0107 dest->psi=atan2(tmp.c[1][0],tmp.c[0][0]);
0108 }
0109 else{
0110 double x1,y1;
0111 dest->theta=acos(tmp.c[2][2]);
0112 dest->phi=atan2(tmp.c[1][2],tmp.c[0][2])+1.5707963268;
0113 x1=cos(dest->phi); // ascending node
0114 y1=sin(dest->phi);
0115
0116// (y1*x.z-z1*x.y)*z.x+(z1*x.x-x1*x.z)*z.y+(x1*x.y-y1*x.x)*z.z
0117// x1*x.x+y1*x.y+z1*x.z
0118// 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);
0119
0120 dest->psi=atan2(y1*tmp.c[2][0]*tmp.c[0][2]-x1*tmp.c[2][0]*tmp.c[2][1]
0121 +(x1*tmp.c[1][0]-y1*tmp.c[0][0])*tmp.c[2][2],
0122 x1*tmp.c[0][0]+y1*tmp.c[1][0]);
0123 }
0124}
0125
0126
0127/*********************************************************************
0128
0129 PointsToAM
0130
0131 Construct AngleMatrix parameter from 2 Point3D parameters
0132
0133 a point on one of the coordinate axis
0134 b point on one of the coordinate axis
0135 w a and b are yzAxis, zxAxis or xyAxis
0136 dest AngleMatrix constructed
0137*********************************************************************/
0138void PointsToAM(Point3d *a,Point3d *b,int w,AngleMatrix *dest)
0139{
0140 Point3d x,y,z;
0141 double d;
0142
0143
0144 switch (w){
0145 case yzAxis:
0146 NormPoint3d(a,&y,&d);
0147 NormPoint3d(b,&z,&d);
0148 x.x=y.y*z.z-y.z*z.y;
0149 x.y=y.z*z.x-y.x*z.z;
0150 x.z=y.x*z.y-y.y*z.x;
0151 break;
0152 case zxAxis:
0153 NormPoint3d(a,&z,&d);
0154 NormPoint3d(b,&x,&d);
0155 y.x=z.y*x.z-z.z*x.y;
0156 y.y=z.z*x.x-z.x*x.z;
0157 y.z=z.x*x.y-z.y*x.x;
0158 break;
0159 case xyAxis:
0160 NormPoint3d(a,&x,&d);
0161 NormPoint3d(b,&y,&d);
0162 z.x=x.y*y.z-x.z*y.y;
0163 z.y=x.z*y.x-x.x*y.z;
0164 z.z=x.x*y.y-x.y*y.x;
0165 break;
0166 }
0167 dest->c[0][0]=x.x;
0168 dest->c[1][0]=x.y;
0169 dest->c[2][0]=x.z;
0170 dest->c[0][1]=y.x;
0171 dest->c[1][1]=y.y;
0172 dest->c[2][1]=y.z;
0173 dest->c[0][2]=z.x;
0174 dest->c[1][2]=z.y;
0175 dest->c[2][2]=z.z;
0176}
0177
0178/*********************************************************************
0179
0180 PointsToAE
0181
0182 Construct AngleEuler parameter from 2 Point3D parameters
0183
0184 a point on one of the coordinate axis
0185 b point on one of the coordinate axis
0186 w a and b are yzAxis, zxAxis or xyAxis
0187 dest TransAngleEuler constructed
0188*********************************************************************/
0189void PointsToAE(Point3d *a,Point3d *b,int w,AngleEuler *dest)
0190{
0191 AngleMatrix tmp;
0192
0193 PointsToAM(a,b,w,&tmp);
0194
0195 if(tmp.c[0][2]==0.&&tmp.c[1][2]==0.){
0196 dest->theta=0.;
0197 dest->phi=0.;
0198 dest->psi=atan2(tmp.c[1][0],tmp.c[0][0]);
0199 }
0200 else{
0201 double x1,y1;
0202 dest->theta=acos(tmp.c[2][2]);
0203 dest->phi=atan2(tmp.c[1][2],tmp.c[0][2])+1.5707963268;
0204 x1=cos(dest->phi); // ascending node
0205 y1=sin(dest->phi);
0206
0207// (y1*x.z-z1*x.y)*z.x+(z1*x.x-x1*x.z)*z.y+(x1*x.y-y1*x.x)*z.z
0208// x1*x.x+y1*x.y+z1*x.z
0209// 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);
0210
0211 dest->psi=atan2(y1*tmp.c[2][0]*tmp.c[0][2]-x1*tmp.c[2][0]*tmp.c[2][1]
0212 +(x1*tmp.c[1][0]-y1*tmp.c[0][0])*tmp.c[2][2],
0213 x1*tmp.c[0][0]+y1*tmp.c[1][0]);
0214 }
0215}
0216
0217
0218/*********************************************************************
0219 NormLine3d
0220
0221 normalize a vector
0222
0223 source line segment to be normalized
0224
0225 dest normalized vector
0226 norm length of the vector
0227
0228*********************************************************************/
0229void NormLine3d(Line3d *source,Point3d *dest,double *norm)
0230{
0231 double x,y,z,d;
0232
0233 x=source->ex-source->sx;
0234 y=source->ey-source->sy;
0235 z=source->ez-source->sz;
0236
0237 d=sqrt(x*x+y*y+z*z);
0238 dest->x=x/d;
0239 dest->y=y/d;
0240 dest->z=z/d;
0241 *norm=d;
0242}
0243
0244/*********************************************************************
0245 NormPoint3d
0246
0247 normalize a vector
0248
0249 source vector to be normalized
0250
0251 dest normalized vector
0252 norm length of the vector
0253
0254*********************************************************************/
0255void NormPoint3d(Point3d *source,Point3d *dest,double *norm)
0256{
0257 double x,y,z,d;
0258
0259 x=source->x;
0260 y=source->y;
0261 z=source->z;
0262
0263 d=sqrt(x*x+y*y+z*z);
0264 dest->x=x/d;
0265 dest->y=y/d;
0266 dest->z=z/d;
0267 *norm=d;
0268}
0269
0270/*********************************************************************
0271 NormLine2d
0272
0273 normalize a vector
0274
0275 source line segment to be normalized
0276
0277 dest normalized vector
0278 norm length of the vector
0279
0280*********************************************************************/
0281void NormLine2d(Line2d *source,Point2d *dest,double *norm)
0282{
0283 double x,y,d;
0284
0285 x=source->ex-source->sx;
0286 y=source->ey-source->sy;
0287
0288 d=sqrt(x*x+y*y);
0289 dest->x=x/d;
0290 dest->y=y/d;
0291 *norm=d;
0292}
0293
0294/*********************************************************************
0295 NormPoint2d
0296
0297 normalize a vector
0298
0299 source vector to be normalized
0300
0301 dest normalized vector
0302 norm length of the vector
0303
0304*********************************************************************/
0305void NormPoint2d(Point2d *source,Point2d *dest,double *norm)
0306{
0307 double x,y,d;
0308
0309 x=source->x;
0310 y=source->y;
0311
0312 d=sqrt(x*x+y*y);
0313 dest->x=x/d;
0314 dest->y=y/d;
0315 *norm=d;
0316}
0317
0318
0319/*********************************************************************
0320 TAEtoTAM
0321
0322 convert TransAngleEuler to TransAngleMatrix
0323
0324 source TransAngleEuler
0325
0326 dest TransAngleMatrix
0327
0328*********************************************************************/
0329void TAEtoTAM(TransAngleEuler *source,TransAngleMatrix *dest)
0330{
0331 double sth,cth,sph,cph,sps,cps;
0332
0333 dest->x=source->x;
0334 dest->y=source->y;
0335 dest->z=source->z;
0336 sth=sin(source->theta);
0337 cth=cos(source->theta);
0338 sph=sin(source->phi);
0339 cph=cos(source->phi);
0340 sps=sin(source->psi);
0341 cps=cos(source->psi);
0342 dest->c[0][0]=cps*cph-sps*cth*sph; // x component of x'
0343 dest->c[0][1]=-sps*cph-cps*cth*sph;
0344 dest->c[0][2]=sth*sph;
0345 dest->c[1][0]=cps*sph+sps*cth*cph; // y component of x'
0346 dest->c[1][1]=-sps*sph+cps*cth*cph;
0347 dest->c[1][2]=-sth*cph;
0348 dest->c[2][0]=sps*sth; // z component of x'
0349 dest->c[2][1]=cps*sth;
0350 dest->c[2][2]=cth;
0351}
0352
0353
0354/*********************************************************************
0355 TAMtoTAE
0356
0357 convert TransAngleMatrix to TransAngleEuler
0358
0359 source TransAngleMatrix
0360
0361 dest TransAngleEuler
0362
0363*********************************************************************/
0364void TAMtoTAE(TransAngleMatrix *source,TransAngleEuler *dest)
0365{
0366 double a,ca,sa;
0367
0368 dest->x=source->x;
0369 dest->y=source->y;
0370 dest->z=source->z;
0371
0372 if(source->c[2][2]==1.){
0373 dest->theta=0.;
0374 dest->phi=0.;
0375 dest->psi=atan2(source->c[0][1],source->c[0][0]);
0376 return;
0377 }
0378 dest->theta=acos(source->c[2][2]);
0379 dest->phi=atan2(source->c[1][2],source->c[0][2])+1.5707963268;
0380 a=dest->phi;
0381 ca=cos(a);
0382 sa=sin(a);
0383 dest->psi=-atan2(source->c[0][1]*ca+source->c[1][1]*sa,
0384 source->c[0][0]*ca+source->c[1][0]*sa);
0385}
0386
0387
0388/*********************************************************************
0389 InverseTAM
0390
0391 Inverse body-frame order
0392
0393 source TransAngleMatrix
0394
0395 dest TransAngleMatrix
0396
0397*********************************************************************/
0398void InverseTAM(TransAngleMatrix *source,TransAngleMatrix *dest)
0399{
0400 Point3d a;
0401
0402 a.x=-source->x;
0403 a.y=-source->y;
0404 a.z=-source->z;
0405
0406 dest->x=source->c[0][0]*a.x+source->c[1][0]*a.y+source->c[2][0]*a.z;
0407 dest->y=source->c[0][1]*a.x+source->c[1][1]*a.y+source->c[2][1]*a.z;
0408 dest->z=source->c[0][2]*a.x+source->c[1][2]*a.y+source->c[2][2]*a.z;
0409
0410 dest->c[0][0]=source->c[0][0];
0411 dest->c[0][1]=source->c[1][0];
0412 dest->c[0][2]=source->c[2][0];
0413 dest->c[1][0]=source->c[0][1];
0414 dest->c[1][1]=source->c[1][1];
0415 dest->c[1][2]=source->c[2][1];
0416 dest->c[2][0]=source->c[0][2];
0417 dest->c[2][1]=source->c[1][2];
0418 dest->c[2][2]=source->c[2][2];
0419
0420}
0421
0422
0423
0424/*********************************************************************
0425 BodyToFrameTAM
0426
0427 convert Body coordinate to Frame coordinate
0428
0429 source Point3d
0430 m TransAngleMatrix
0431
0432 dest Point3d
0433
0434*********************************************************************/
0435void BodyToFrameTAM(Point3d *source,TransAngleMatrix *m,Point3d *dest)
0436{
0437 dest->x=m->x+m->c[0][0]*source->x+m->c[0][1]*source->y+m->c[0][2]*source->z;
0438 dest->y=m->y+m->c[1][0]*source->x+m->c[1][1]*source->y+m->c[1][2]*source->z;
0439 dest->z=m->z+m->c[2][0]*source->x+m->c[2][1]*source->y+m->c[2][2]*source->z;
0440}
0441
0442/*********************************************************************
0443 FrameToBodyTAM
0444
0445 convert Frame coordinate to Body coordinate
0446
0447 source Point3d
0448 m TransAngleMatrix
0449
0450 dest Point3d
0451
0452*********************************************************************/
0453void FrameToBodyTAM(Point3d *source,TransAngleMatrix *m,Point3d *dest)
0454{
0455 dest->x=m->c[0][0]*(source->x-m->x)
0456 +m->c[1][0]*(source->y-m->y)
0457 +m->c[2][0]*(source->z-m->z);
0458 dest->y=m->c[0][1]*(source->x-m->x)
0459 +m->c[1][1]*(source->y-m->y)
0460 +m->c[2][1]*(source->z-m->z);
0461 dest->z=m->c[0][2]*(source->x-m->x)
0462 +m->c[1][2]*(source->y-m->y)
0463 +m->c[2][2]*(source->z-m->z);
0464}
0465
0466
0467/*********************************************************************
0468 MoveTwiceTAE
0469
0470 obtain single conversion parameters from double consecutive
0471 coordinate conversion parameters
0472
0473 src1 TransAngleEuler cood1 - frame cood2 - body
0474 src2 TransAngleEuler cood2 - frame cood3 - body
0475
0476 dest TransAngleEuler cood1 - frame cood3 - body
0477
0478********************************************************************/
0479void MoveTwiceTAE(TransAngleEuler *src1,TransAngleEuler *src2,TransAngleEuler *dest)
0480{
0481 double x1,y1,z1,theta1,phi1,psi1;
0482 double x2,y2,z2,theta2,phi2,psi2;
0483 double x3,y3,z3,theta3,phi3,psi3;
0484
0485 double a[3][3],b[3][3],c[3],d[3],e[3];
0486 double cost,sint,cosf,sinf,cosp,sinp;
0487 int locked;
0488
0489 x1=src1->x;
0490 y1=src1->y;
0491 z1=src1->z;
0492 theta1=src1->theta;
0493 phi1=src1->phi;
0494 psi1=src1->psi;
0495 x2=src2->x;
0496 y2=src2->y;
0497 z2=src2->z;
0498 theta2=src2->theta;
0499 phi2=src2->phi;
0500 psi2=src2->psi;
0501
0502 cost=cos(theta1);
0503 sint=sin(theta1);
0504 cosf=cos(phi1);
0505 sinf=sin(phi1);
0506 cosp=cos(psi1);
0507 sinp=sin(psi1);
0508// if d[i] is a vector in cood2, c[j]=a[j][i]*d[i] is the vector in cood1
0509 a[0][0]=cosp*cosf-sinp*cost*sinf;
0510 a[0][1]=-sinp*cosf-cosp*cost*sinf;
0511 a[0][2]=sint*sinf;
0512
0513 a[1][0]=cosp*sinf+sinp*cost*cosf;
0514 a[1][1]=-sinp*sinf+cosp*cost*cosf;
0515 a[1][2]=-sint*cosf;
0516
0517 a[2][0]=sinp*sint;
0518 a[2][1]=cosp*sint;
0519 a[2][2]=cost;
0520
0521 cost=cos(theta2);
0522 sint=sin(theta2);
0523 cosf=cos(phi2);
0524 sinf=sin(phi2);
0525 cosp=cos(psi2);
0526 sinp=sin(psi2);
0527
0528// if d[i] is a vector in cood3, c[j]=b[j][i]*d[i] is the vector in cood2
0529 b[0][0]=cosp*cosf-sinp*cost*sinf;
0530 b[0][1]=-sinp*cosf-cosp*cost*sinf;
0531 b[0][2]=sint*sinf;
0532
0533 b[1][0]=cosp*sinf+sinp*cost*cosf;
0534 b[1][1]=-sinp*sinf+cosp*cost*cosf;
0535 b[1][2]=-sint*cosf;
0536
0537 b[2][0]=sinp*sint;
0538 b[2][1]=cosp*sint;
0539 b[2][2]=cost;
0540
0541 x3=x1+a[0][0]*x2+a[0][1]*y2+a[0][2]*z2;
0542 y3=y1+a[1][0]*x2+a[1][1]*y2+a[1][2]*z2;
0543 z3=z1+a[2][0]*x2+a[2][1]*y2+a[2][2]*z2;
0544
0545// if c is a unit z vector in coord 3, d=b*c is the vector in coord 2
0546// and a*d is the vector in coord 1
0547 c[0]=0.;
0548 c[1]=0.;
0549 c[2]=1.;
0550 d[0]=b[0][0]*c[0]+b[0][1]*c[1]+b[0][2]*c[2];
0551 d[1]=b[1][0]*c[0]+b[1][1]*c[1]+b[1][2]*c[2];
0552 d[2]=b[2][0]*c[0]+b[2][1]*c[1]+b[2][2]*c[2];
0553 e[0]=a[0][0]*d[0]+a[0][1]*d[1]+a[0][2]*d[2];
0554 e[1]=a[1][0]*d[0]+a[1][1]*d[1]+a[1][2]*d[2];
0555 e[2]=a[2][0]*d[0]+a[2][1]*d[1]+a[2][2]*d[2];
0556
0557// Euler angles of coord3 with reference to coord1 are to be derived
0558// theta is the angle between z vector in coord3 and z vector in coord1
0559// cosine of the theta is e[2] in the above expression
0560
0561 theta3=acos(e[2]);
0562 if(theta3<0.0005) {
0563 phi3=0.; // the case when theta = 0
0564 locked=TRUE;
0565 }
0566 else {
0567 phi3=atan2(e[0],-e[1]);
0568 locked=FALSE;
0569 }
0570
0571// if c is a unit x vector in coord 3, d=b*c is the vector in coord 2
0572// and a*d is the vector in coord 1
0573 c[0]=1.;
0574 c[1]=0.;
0575 c[2]=0.;
0576 d[0]=b[0][0]*c[0]+b[0][1]*c[1]+b[0][2]*c[2];
0577 d[1]=b[1][0]*c[0]+b[1][1]*c[1]+b[1][2]*c[2];
0578 d[2]=b[2][0]*c[0]+b[2][1]*c[1]+b[2][2]*c[2];
0579 e[0]=a[0][0]*d[0]+a[0][1]*d[1]+a[0][2]*d[2];
0580 e[1]=a[1][0]*d[0]+a[1][1]*d[1]+a[1][2]*d[2];
0581 e[2]=a[2][0]*d[0]+a[2][1]*d[1]+a[2][2]*d[2];
0582// if c is a unit y vector in coord 3, d=b*c is the vector in coord 2
0583// and a*d is the vector in coord 1
0584 c[0]=0.;
0585 c[1]=1.;
0586 c[2]=0.;
0587 d[0]=b[0][0]*c[0]+b[0][1]*c[1]+b[0][2]*c[2];
0588 d[1]=b[1][0]*c[0]+b[1][1]*c[1]+b[1][2]*c[2];
0589 d[2]=b[2][0]*c[0]+b[2][1]*c[1]+b[2][2]*c[2];
0590 c[0]=a[0][0]*d[0]+a[0][1]*d[1]+a[0][2]*d[2];
0591 c[1]=a[1][0]*d[0]+a[1][1]*d[1]+a[1][2]*d[2];
0592 c[2]=a[2][0]*d[0]+a[2][1]*d[1]+a[2][2]*d[2];
0593
0594 if(locked) psi3=atan2(e[1],e[0]);
0595 else psi3=atan2(e[2],c[2]);
0596
0597 dest->x=x3;
0598 dest->y=y3;
0599 dest->z=z3;
0600 dest->theta=theta3;
0601 dest->phi=phi3;
0602 dest->psi=psi3;
0603}
0604
0605
0606/*********************************************************************
0607 AEtoAM
0608
0609 convert CoordEuler to CoordMatrix
0610
0611 source AngleEuler
0612
0613 dest AngleMatrix
0614
0615*********************************************************************/
0616void AEtoAM(AngleEuler *source,AngleMatrix *dest)
0617{
0618 double sth,cth,sph,cph,sps,cps;
0619
0620 sth=sin(source->theta);
0621 cth=cos(source->theta);
0622 sph=sin(source->phi);
0623 cph=cos(source->phi);
0624 sps=sin(source->psi);
0625 cps=cos(source->psi);
0626 dest->c[0][0]=cps*cph-sps*cth*sph;
0627 dest->c[0][1]=-sps*cph-cps*cth*sph;
0628 dest->c[0][2]=sth*sph;
0629 dest->c[1][0]=cps*sph+sps*cth*cph;
0630 dest->c[1][1]=-sps*sph+cps*cth*cph;
0631 dest->c[1][2]=-sth*cph;
0632 dest->c[2][0]=sps*sth;
0633 dest->c[2][1]=cps*sth;
0634 dest->c[2][2]=cth;
0635}
0636
0637/*********************************************************************
0638 BodyToFrameAM
0639
0640 convert Body coordinate to Frame coordinate
0641
0642 source Point3d - Cartesian
0643 m AngleMatrix
0644
0645 dest Point3d - Cartesian
0646
0647*********************************************************************/
0648void BodyToFrameAM(Point3d *source,AngleMatrix *m,Point3d *dest)
0649{
0650
0651 dest->x=m->c[0][0]*source->x+m->c[0][1]*source->y+m->c[0][2]*source->z;
0652 dest->y=m->c[1][0]*source->x+m->c[1][1]*source->y+m->c[1][2]*source->z;
0653 dest->z=m->c[2][0]*source->x+m->c[2][1]*source->y+m->c[2][2]*source->z;
0654
0655}
0656
0657/*********************************************************************
0658 FrameToBodyAM
0659
0660 convert Frame coordinate to Body coordinate
0661
0662 source Point3d - Cartesian
0663 m AngleMatrix
0664
0665 dest Point3d - Cartesian
0666
0667*********************************************************************/
0668void FrameToBodyAM(Point3d *source,AngleMatrix *m,Point3d *dest)
0669{
0670 dest->x=m->c[0][0]*source->x+m->c[1][0]*source->y+m->c[2][0]*source->z;
0671 dest->y=m->c[0][1]*source->x+m->c[1][1]*source->y+m->c[2][1]*source->z;
0672 dest->z=m->c[0][2]*source->x+m->c[1][2]*source->y+m->c[2][2]*source->z;
0673}
0674
0675/*********************************************************************
0676 BodyToFrameAMP
0677
0678 convert Body coordinate to Frame coordinate - Polar
0679
0680 source Point2d - azimuth and elevation (rad)
0681 m AngleMatrix
0682
0683 dest Point2d - azimuth and elevation (rad)
0684
0685*********************************************************************/
0686void BodyToFrameAMP(Point2d *source,AngleMatrix *m,Point2d *dest)
0687{
0688 Point3d s,d; // unit vector
0689
0690 ToCartesian3d(source,&s);
0691
0692 d.x=m->c[0][0]*s.x+m->c[0][1]*s.y+m->c[0][2]*s.z;
0693 d.y=m->c[1][0]*s.x+m->c[1][1]*s.y+m->c[1][2]*s.z;
0694 d.z=m->c[2][0]*s.x+m->c[2][1]*s.y+m->c[2][2]*s.z;
0695
0696 ToPolar3d(&d,dest);
0697
0698}
0699
0700/*********************************************************************
0701 FrameToBodyAMP
0702
0703 convert Frame coordinate to Body coordinate - Polar
0704
0705 source Point2d - azimuth and elevation (rad)
0706 m AngleMatrix
0707
0708 dest Point2d - azimuth and elevation (rad)
0709
0710*********************************************************************/
0711void FrameToBodyAMP(Point2d *source,AngleMatrix *m,Point2d *dest)
0712{
0713 Point3d s,d;
0714
0715 ToCartesian3d(source,&s);
0716
0717 d.x=m->c[0][0]*s.x+m->c[1][0]*s.y+m->c[2][0]*s.z;
0718 d.y=m->c[0][1]*s.x+m->c[1][1]*s.y+m->c[2][1]*s.z;
0719 d.z=m->c[0][2]*s.x+m->c[1][2]*s.y+m->c[2][2]*s.z;
0720
0721 ToPolar3d(&d,dest);
0722
0723}
0724
0725
0726/*********************************************************************
0727 ToPolar3d
0728
0729 convert Catesian coordinate to Polar coordinate
0730
0731 source Point3d - Cartesian with unit length
0732
0733 dest Point2d - azimuth and elevation (rad)
0734
0735*********************************************************************/
0736void ToPolar3d(Point3d *source,Point2d *dest)
0737{
0738 dest->y=asin(source->z);
0739 if(source->z==1.||source->z==-1.) dest->x=0.;
0740 else dest->x=atan2(source->y,source->x);
0741}
0742
0743/*********************************************************************
0744 ToCartesian3d
0745
0746 convert Polar coordinate to Cartesian coordinate
0747
0748 source Point2d - azimuth and elevation (rad)
0749
0750 dest Point3d - Cartesian with unit length
0751
0752*********************************************************************/
0753void ToCartesian3d(Point2d *source,Point3d *dest)
0754{
0755 double cel;
0756 cel=cos(source->y); // cosine elevation
0757 dest->x=cel*cos(source->x);
0758 dest->y=cel*sin(source->x);
0759 dest->z=sin(source->y);
0760}
0761
0762/*********************************************************************
0763 VectorProduct
0764
0765 Vector product
0766
0767 u input
0768 v input
0769 w output uxv
0770
0771*********************************************************************/
0772void VectorProduct(Point3d *u,Point3d *v,Point3d *w)
0773{
0774 w->x=u->y*v->z-u->z*v->y;
0775 w->y=u->z*v->x-u->x*v->z;
0776 w->z=u->x*v->y-u->y*v->x;
0777}
0778
0779/*********************************************************************
0780 AcuteAngle
0781
0782 Test if the angle u-v-w is an acute angle
0783
0784 u point1
0785 v point2 - center point
0786 w point3
0787
0788 returned TRUE if the angle is acute angle
0789 FALSE if the angle is obtuse or right angle
0790
0791*********************************************************************/
0792int AcuteAngle(Point3d *u,Point3d *v,Point3d *w)
0793{
0794 if((u->x-v->x)*(w->x-v->x)+(u->y-v->y)*(w->y-v->y)+
0795 (u->z-v->z)*(w->z-v->z)>0.) return TRUE;
0796 else return FALSE;
0797}
0798