00001 #include "htransform.h"
00002
00003 #include "error.h"
00004
00005 #include <math.h>
00006 #include <string.h>
00007
00018
00019
00020
00026 const THTransform matrix_identity={{1.0, 0.0, 0.0, 0.0},
00027 {0.0, 1.0, 0.0, 0.0},
00028 {0.0, 0.0, 1.0, 0.0},
00029 {0.0, 0.0, 0.0, 1.0}};
00030
00039 #define MATRIX_INT_COPY(td,to) memcpy((td),(to),sizeof(THTransform))
00040
00041
00042
00043
00044
00045
00046 void HTransformIdentity(THTransform *t
00047 )
00048 {
00049 MATRIX_INT_COPY((*t),&matrix_identity);
00050 }
00051
00052
00053
00054
00055 void HTransformCopy(THTransform *t_dst,
00056 THTransform *t_src
00057 )
00058 {
00059 MATRIX_INT_COPY((*t_dst),(*t_src));
00060 }
00061
00062
00063
00064
00065 void HTransformTx(double tx,
00066 THTransform *t
00067 )
00068 {
00069 MATRIX_INT_COPY(*t,&matrix_identity);
00070 (*t)[AXIS_X][AXIS_H]=tx;
00071 }
00072
00073
00074
00075
00076 void HTransformTy(double ty,
00077 THTransform *t
00078 )
00079 {
00080 MATRIX_INT_COPY(*t,&matrix_identity);
00081 (*t)[AXIS_Y][AXIS_H]=ty;
00082 }
00083
00084
00085
00086
00087 void HTransformTz(double tz,
00088 THTransform *t
00089 )
00090 {
00091 MATRIX_INT_COPY(*t,&matrix_identity);
00092 (*t)[AXIS_Z][AXIS_H]=tz;
00093 }
00094
00095
00096
00097
00098
00099 void HTransformTxyz(double tx,
00100 double ty,
00101 double tz,
00102 THTransform *t
00103 )
00104 {
00105 MATRIX_INT_COPY(*t,&matrix_identity);
00106 (*t)[AXIS_X][AXIS_H]=tx;
00107 (*t)[AXIS_Y][AXIS_H]=ty;
00108 (*t)[AXIS_Z][AXIS_H]=tz;
00109 }
00110
00111
00112
00113
00114 void HTransformRx(double rx,
00115 THTransform *t
00116 )
00117 {
00118 double s,c;
00119
00120 s=sin(rx);
00121 c=cos(rx);
00122
00123 MATRIX_INT_COPY(*t,&matrix_identity);
00124 (*t)[AXIS_Y][AXIS_Y]=c; (*t)[AXIS_Y][AXIS_Z]=-s;
00125 (*t)[AXIS_Z][AXIS_Y]=s; (*t)[AXIS_Z][AXIS_Z]= c;
00126 }
00127
00128
00129
00130
00131 void HTransformRy(double ry,
00132 THTransform *t
00133 )
00134 {
00135 double s,c;
00136
00137 s=sin(ry);
00138 c=cos(ry);
00139
00140 MATRIX_INT_COPY(*t,&matrix_identity);
00141 (*t)[AXIS_X][AXIS_X]= c; (*t)[AXIS_X][AXIS_Z]=s;
00142 (*t)[AXIS_Z][AXIS_X]=-s; (*t)[AXIS_Z][AXIS_Z]=c;
00143 }
00144
00145
00146
00147
00148 void HTransformRz(double rz,
00149 THTransform *t
00150 )
00151 {
00152 double s,c;
00153
00154 s=sin(rz);
00155 c=cos(rz);
00156
00157 MATRIX_INT_COPY(*t,&matrix_identity);
00158 (*t)[AXIS_X][AXIS_X]=c; (*t)[AXIS_X][AXIS_Y]=-s;
00159 (*t)[AXIS_Y][AXIS_X]=s; (*t)[AXIS_Y][AXIS_Y]=c;
00160 }
00161
00162
00163
00164
00165 void HTransformRx2(double s,double c,THTransform *t)
00166 {
00167 MATRIX_INT_COPY(*t,&matrix_identity);
00168 (*t)[AXIS_Y][AXIS_Y]=c; (*t)[AXIS_Y][AXIS_Z]=-s;
00169 (*t)[AXIS_Z][AXIS_Y]=s; (*t)[AXIS_Z][AXIS_Z]= c;
00170 }
00171
00172
00173
00174
00175 void HTransformRy2(double s,double c,THTransform *t)
00176 {
00177 MATRIX_INT_COPY(*t,&matrix_identity);
00178 (*t)[AXIS_X][AXIS_X]= c; (*t)[AXIS_X][AXIS_Z]=s;
00179 (*t)[AXIS_Z][AXIS_X]=-s; (*t)[AXIS_Z][AXIS_Z]=c;
00180 }
00181
00182
00183
00184
00185 void HTransformRz2(double s,double c,THTransform *t)
00186 {
00187 MATRIX_INT_COPY(*t,&matrix_identity);
00188 (*t)[AXIS_X][AXIS_X]=c; (*t)[AXIS_X][AXIS_Y]=-s;
00189 (*t)[AXIS_Y][AXIS_X]=s; (*t)[AXIS_Y][AXIS_Y]=c;
00190 }
00191
00192
00193
00194
00195
00196
00197 void HTransformCreate(unsigned int dof_r3,
00198 double v,
00199 THTransform *t
00200 )
00201 {
00202 switch (dof_r3)
00203 {
00204 case TX:
00205 HTransformTx(v,t);
00206 break;
00207 case TY:
00208 HTransformTy(v,t);
00209 break;
00210 case TZ:
00211 HTransformTz(v,t);
00212 break;
00213 case RX:
00214 HTransformRx(v,t);
00215 break;
00216 case RY:
00217 HTransformRy(v,t);
00218 break;
00219 case RZ:
00220 HTransformRz(v,t);
00221 break;
00222 }
00223 }
00224
00225
00226
00227
00228
00229 void HTransformSetElement(unsigned int i,
00230 unsigned int j,
00231 double v,
00232 THTransform *t
00233 )
00234 {
00235 if ((i<AXIS_H)&&(j<=AXIS_H))
00236 (*t)[i][j]=v;
00237 else
00238 Error("Element out of range in HTransformSetElement");
00239 }
00240
00241
00242
00243
00244
00245 double HTransformGetElement(unsigned int i,
00246 unsigned int j,
00247 THTransform *t
00248 )
00249 {
00250 if ((i<AXIS_H)&&(j<=AXIS_H))
00251 return((*t)[i][j]);
00252 else
00253 Error("Element out of range in HTransformGetElement");
00254 return(0.0);
00255 }
00256
00257
00258
00259
00260
00261
00262
00263 void HTransformProduct(THTransform *t1,
00264 THTransform *t2,
00265 THTransform *t3
00266 )
00267 {
00268 THTransform t4;
00269 unsigned int i,j,k;
00270
00271 for(i=0;i<(DIM_SP+1);i++)
00272 {
00273 for(j=0;j<(DIM_SP+1);j++)
00274 {
00275 t4[i][j]=0.0;
00276 for(k=0;k<(DIM_SP+1);k++)
00277 {
00278 t4[i][j]+=(*t1)[i][k]*(*t2)[k][j];
00279 }
00280 }
00281 }
00282
00283 MATRIX_INT_COPY(*t3,&t4);
00284 }
00285
00286
00287
00288
00289
00290
00291
00292 void HTransformAdd(THTransform *t1,
00293 THTransform *t2,
00294 THTransform *t3
00295 )
00296 {
00297 THTransform t4;
00298 unsigned int i,j;
00299
00300 for(i=0;i<(DIM_SP+1);i++)
00301 {
00302 for(j=0;j<(DIM_SP+1);j++)
00303 {
00304 t4[i][j]=(*t1)[i][j]+(*t2)[i][j];
00305 }
00306 }
00307
00308 MATRIX_INT_COPY(*t3,&t4);
00309 }
00310
00311
00312
00313
00314
00315
00316
00317
00318
00319 void HTransformInverse(THTransform *t,
00320 THTransform *ti
00321 )
00322 {
00323 THTransform t_trans;
00324 THTransform t_rot;
00325 unsigned int i,j;
00326
00327 MATRIX_INT_COPY(&t_trans,&matrix_identity);
00328 MATRIX_INT_COPY(&t_rot,&matrix_identity);
00329
00330 t_trans[AXIS_X][AXIS_H]=-(*t)[AXIS_X][AXIS_H];
00331 t_trans[AXIS_Y][AXIS_H]=-(*t)[AXIS_Y][AXIS_H];
00332 t_trans[AXIS_Z][AXIS_H]=-(*t)[AXIS_Z][AXIS_H];
00333
00334 for(i=0;i<AXIS_H;i++)
00335 {
00336 for(j=0;j<AXIS_H;j++)
00337 t_rot[i][j]=(*t)[j][i];
00338 }
00339
00340 HTransformProduct(&t_rot,&t_trans,ti);
00341 }
00342
00343 void HTransformOrthonormalize(THTransform *t,THTransform *ta)
00344 {
00345 THTransform t_new;
00346 double c,n;
00347 unsigned int i;
00348
00349 MATRIX_INT_COPY(&t_new,&matrix_identity);
00350
00351
00352 n=0.0;
00353 for(i=0;i<3;i++)
00354 n+=((*t)[i][0]*(*t)[i][0]);
00355 n=sqrt(n);
00356 for(i=0;i<3;i++)
00357 t_new[i][0]=(*t)[i][0]/n;
00358
00359
00360
00361
00362 c=0.0;
00363 for(i=0;i<3;i++)
00364 c+=(t_new[i][0]*(*t)[i][1]);
00365 for(i=0;i<3;i++)
00366 t_new[i][1]=(*t)[i][1]-c*t_new[i][0];
00367
00368
00369 n=0.0;
00370 for(i=0;i<3;i++)
00371 n+=(t_new[i][1]*t_new[i][1]);
00372 n=sqrt(n);
00373 for(i=0;i<3;i++)
00374 t_new[i][1]=t_new[i][1]/n;
00375
00376
00377 t_new[0][2]= t_new[1][0]*t_new[2][1]-t_new[1][1]*t_new[2][0];
00378 t_new[1][2]=-t_new[0][0]*t_new[2][1]+t_new[0][1]*t_new[2][0];
00379 t_new[2][2]= t_new[0][0]*t_new[1][1]-t_new[0][1]*t_new[1][0];
00380
00381
00382 for(i=0;i<3;i++)
00383 t_new[i][AXIS_H]=(*t)[i][AXIS_H];
00384
00385 MATRIX_INT_COPY(*ta,&t_new);
00386 }
00387
00388 void HTransformX2Vect(double ry,double rz,double *p1,double *p2,THTransform *t)
00389 {
00390 double x,y,z,l,theta,phi,h;
00391 THTransform s,r1,r2;
00392
00393 x=p2[0]-p1[0];
00394 y=p2[1]-p1[1];
00395 z=p2[2]-p1[2];
00396
00397 l=sqrt(x*x+y*y+z*z);
00398
00399 theta=atan2(y,x);
00400 h=sqrt(x*x+y*y);
00401 phi=-atan2(z,h);
00402
00403
00404 HTransformIdentity(&s);
00405 HTransformSetElement(0,0,l,&s);
00406 HTransformSetElement(1,1,ry,&s);
00407 HTransformSetElement(2,2,rz,&s);
00408
00409
00410
00411 HTransformRy(phi,&r1);
00412
00413
00414 HTransformRz(theta,&r2);
00415
00416
00417 HTransformTxyz(p1[0],p1[1],p1[2],t);
00418
00419
00420 HTransformProduct(t,&r2,t);
00421 HTransformProduct(t,&r1,t);
00422 HTransformProduct(t,&s,t);
00423 }
00424
00425
00426
00427
00428 void HTransformTranspose(THTransform *t,
00429 THTransform *tt
00430 )
00431 {
00432 THTransform t4;
00433 unsigned int i,j;
00434
00435 for(i=0;i<(DIM_SP+1);i++)
00436 {
00437 for(j=0;j<(DIM_SP+1);j++)
00438 t4[i][j]=(*t)[j][i];
00439 }
00440
00441 MATRIX_INT_COPY(*tt,&t4);
00442 }
00443
00444
00445
00446
00447
00448
00449
00450
00451
00452 void HTransformAcumTrans(double tx,
00453 double ty,
00454 double tz,
00455 THTransform *t
00456 )
00457 {
00458 unsigned int i;
00459
00460 for(i=0;i<AXIS_H;i++)
00461 (*t)[i][AXIS_H]=(*t)[i][AXIS_X]*tx+(*t)[i][AXIS_Y]*ty+(*t)[i][AXIS_Z]*tz+(*t)[i][AXIS_H];
00462 }
00463
00464
00465
00466
00467 void HTransformAcumTrans2(THTransform *t_in,
00468 double tx,
00469 double ty,
00470 double tz,
00471 THTransform *t
00472 )
00473 {
00474 if (t!=t_in)
00475 MATRIX_INT_COPY(t,t_in);
00476 HTransformAcumTrans(tx,ty,tz,t);
00477 }
00478
00479
00480
00481
00482
00483
00484 void HTransformAcumRot(unsigned int type,
00485 double s,
00486 double c,
00487 THTransform *t
00488 )
00489 {
00490 unsigned int i;
00491 double aux;
00492
00493 switch(type)
00494 {
00495 case RX:
00496 for(i=0;i<AXIS_H;i++)
00497 {
00498 aux=(*t)[i][AXIS_Y];
00499 (*t)[i][AXIS_Y]= aux*c+(*t)[i][AXIS_Z]*s;
00500 (*t)[i][AXIS_Z]=-aux*s+(*t)[i][AXIS_Z]*c;
00501 }
00502 break;
00503 case RY:
00504 for(i=0;i<AXIS_H;i++)
00505 {
00506 aux=(*t)[i][AXIS_X];
00507 (*t)[i][AXIS_X]=aux*c-(*t)[i][AXIS_Z]*s;
00508 (*t)[i][AXIS_Z]=aux*s+(*t)[i][AXIS_Z]*c;
00509 }
00510 break;
00511 case RZ:
00512 for(i=0;i<AXIS_H;i++)
00513 {
00514 aux=(*t)[i][AXIS_X];
00515 (*t)[i][AXIS_X]= aux*c+(*t)[i][AXIS_Y]*s;
00516 (*t)[i][AXIS_Y]=-aux*s+(*t)[i][AXIS_Y]*c;
00517 }
00518 break;
00519 default:
00520 Error("Non rotation matrix type in function HTransformAcumRot");
00521 break;
00522 }
00523 }
00524
00525
00526
00527 void HTransformAcumRot2(THTransform *t_in,
00528 unsigned int type,
00529 double s,
00530 double c,
00531 THTransform *t
00532 )
00533 {
00534 if (t!=t_in)
00535 MATRIX_INT_COPY(t,t_in);
00536 HTransformAcumRot(type,s,c,t);
00537 }
00538
00539 void HTransformApply(double *p_in,double *p_out,THTransform *t)
00540 {
00541 unsigned int i,j;
00542 double pAux[3];
00543
00544 for(i=0;i<DIM_SP;i++)
00545 {
00546 pAux[i]=(*t)[i][AXIS_H];
00547
00548 for(j=0;j<DIM_SP;j++)
00549 pAux[i]+=((*t)[i][j]*p_in[j]);
00550 }
00551
00552 p_out[0]=pAux[0];
00553 p_out[1]=pAux[1];
00554 p_out[2]=pAux[2];
00555 }
00556
00557
00558
00559
00560 void HTransformPrint(FILE *f,
00561 THTransform *t
00562 )
00563 {
00564 unsigned int i,j;
00565
00566 for(i=0;i<DIM_SP+1;i++)
00567 {
00568 for(j=0;j<(DIM_SP+1);j++)
00569 {
00570 fprintf(f,"%f ",(*t)[i][j]);
00571 }
00572 fprintf(f,"\n");
00573 }
00574 }
00575
00576
00577
00578
00579
00580
00581
00582
00583
00584
00585
00586 void HTransformPrintT(FILE *f,
00587 THTransform *t
00588 )
00589 {
00590 unsigned int i,j;
00591
00592 for(i=0;i<(DIM_SP+1);i++)
00593 {
00594 for(j=0;j<DIM_SP+1;j++)
00595 {
00596 fprintf(f,"%f ",(*t)[j][i]);
00597 }
00598 }
00599 }
00600
00601
00602
00603
00604
00605 void HTransformDelete(THTransform *t
00606 )
00607 {}