00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021 #include "common.h"
00022
00023 #include <stdio.h>
00024 #include <math.h>
00025
00026 #include "bu.h"
00027 #include "vmath.h"
00028 #include "bn.h"
00029 #include "anim.h"
00030
00031
00032 #define NORMAL 0
00033 #define ERROR1 1
00034 #define ERROR2 2
00035
00036
00037 void
00038 anim_v_permute(mat_t m)
00039 {
00040 int i;
00041 fastf_t store;
00042
00043 for (i=0; i<9; i+=4) {
00044 store = m[i];
00045 m[i] = -m[i+1];
00046 m[i+1] = m[i+2];
00047 m[i+2] = -store;
00048 }
00049 }
00050
00051
00052 void
00053 anim_v_unpermute(mat_t m)
00054 {
00055 int i;
00056 fastf_t store;
00057
00058 for (i=0; i<9; i+=4) {
00059 store = m[i+2];
00060 m[i+2] = m[i+1];
00061 m[i+1] = -m[i];
00062 m[i] = -store;
00063 }
00064 }
00065
00066
00067 void
00068 anim_tran(mat_t m)
00069 {
00070 int i;
00071 fastf_t store;
00072
00073 int src[6] = { 1, 2, 3, 6, 7, 11 };
00074 int dst[6] = { 4, 8, 12, 9, 13, 14};
00075
00076 for (i=0; i<6; i++) {
00077 store = m[dst[i]];
00078 m[dst[i]] = m[src[i]];
00079 m[src[i]] = store;
00080 }
00081 }
00082
00083
00084
00085
00086
00087
00088 int
00089 anim_mat2zyx(const mat_t viewrot, vect_t angle)
00090 {
00091 int i, return_value, id_x, id_z;
00092 fastf_t sin_x, sin_z, cos_x, cos_z, big_x, big_z;
00093 static fastf_t previous[3];
00094
00095 if (ZERO(viewrot[1]) && ZERO(viewrot[0])) {
00096 return_value = ERROR1;
00097 angle[0] = 0.0;
00098 angle[2] = atan2(viewrot[4], viewrot[5]);
00099 } else {
00100 return_value = NORMAL;
00101 angle[2] = atan2(-viewrot[1], viewrot[0]);
00102 angle[0] = atan2(-viewrot[6], viewrot[10]);
00103 }
00104
00105 sin_x = sin(angle[0]);
00106 sin_z = sin(angle[2]);
00107 cos_x = cos(angle[0]);
00108 cos_z = cos(angle[2]);
00109
00110
00111
00112
00113
00114
00115
00116 id_z = (fabs(sin_z) > fabs(cos_z)) ? 1 : 0;
00117 big_z = id_z ? sin_z : cos_z;
00118 id_x = (fabs(sin_x) > fabs(cos_x)) ? 1 : 0;
00119 big_x = id_x ? sin_x : cos_x;
00120
00121 if (fabs(big_x*big_z) < VDIVIDE_TOL) {
00122
00123
00124 return ERROR2;
00125 } else if (id_x && (!id_z))
00126 angle[1]=atan2((viewrot[4] - cos_x*sin_z)/(sin_x*cos_z), -viewrot[6]/sin_x);
00127 else if ((!id_x) && (!id_z))
00128 angle[1]=atan2((-viewrot[8] + sin_x*sin_z)/(cos_x*cos_z), viewrot[0]/cos_z);
00129 else if (id_x && id_z)
00130 angle[1]=atan2((-viewrot[5] + cos_x*cos_z)/(sin_x*sin_z), -viewrot[1]/sin_z);
00131 else if ((!id_x) && id_z)
00132 angle[1]=atan2((viewrot[9] - sin_x*cos_z)/(cos_x*sin_z), viewrot[10]/cos_x);
00133
00134
00135
00136 for (i=0; i<3; i++) {
00137 while ((angle[i] - previous[i]) > M_PI)
00138 angle[i] -= (2.0*M_PI);
00139 while ((previous[i] - angle[i]) > M_PI)
00140 angle[i] += (2.0*M_PI);
00141 previous[i] = angle[i];
00142 }
00143
00144 return return_value;
00145 }
00146
00147
00148 int
00149 anim_mat2ypr(mat_t viewrot, vect_t angle)
00150 {
00151 int i, return_value, id_y, id_r;
00152 fastf_t sin_y, sin_r, cos_y, cos_r, big_y, big_r;
00153 static fastf_t prev_angle[3];
00154
00155 if (ZERO(viewrot[9]) && ZERO(viewrot[10])) {
00156 return_value = ERROR1;
00157 angle[2] = 0.0;
00158 angle[0] = atan2(-viewrot[1], viewrot[5]);
00159 } else {
00160 return_value = NORMAL;
00161 angle[0] = atan2(viewrot[4], viewrot[0]);
00162 angle[2] = atan2(viewrot[9], viewrot[10]);
00163 }
00164
00165 sin_y = sin(angle[0]);
00166 sin_r = sin(angle[2]);
00167 cos_y = cos(angle[0]);
00168 cos_r = cos(angle[2]);
00169
00170
00171
00172
00173
00174
00175
00176 id_y = (fabs(sin_y) > fabs(cos_y)) ? 1 : 0;
00177 big_y = id_y ? sin_y : cos_y;
00178 id_r = (fabs(sin_r) > fabs(cos_r)) ? 1 : 0;
00179 big_r = id_r ? sin_r : cos_r;
00180
00181 if (fabs(big_y*big_r) < VDIVIDE_TOL) {
00182
00183
00184 return ERROR2;
00185 } else if ((!id_y) && id_r)
00186 angle[1] = atan2(-(viewrot[1]+sin_y*cos_r)/(cos_y*sin_r), viewrot[9]/sin_r);
00187 else if (id_y && (!id_r))
00188 angle[1] = atan2(-(viewrot[6]+cos_y*sin_r)/(sin_y*cos_r), viewrot[10]/cos_r);
00189 else if (id_y && id_r)
00190 angle[1] = atan2(-(viewrot[5]-cos_y*cos_r)/(sin_y*sin_r), viewrot[4]/sin_y);
00191 else if ((!id_y) && (!id_r))
00192 angle[1] = atan2(-(viewrot[2]-sin_y*sin_r)/(cos_y*cos_r), viewrot[0]/cos_y);
00193
00194
00195 for (i=0; i<3; i++) {
00196 while ((angle[i] - prev_angle[i]) > M_PI)
00197 angle[i] -= (2.0*M_PI);
00198 while ((prev_angle[i] - angle[i]) > M_PI)
00199 angle[i] += (2.0*M_PI);
00200 prev_angle[i] = angle[i];
00201 }
00202
00203 return return_value;
00204 }
00205
00206
00207 int
00208 anim_mat2quat(quat_t quat, const mat_t viewrot)
00209 {
00210 int i;
00211 fastf_t qdiff[4], square, mag1, mag2;
00212 static fastf_t prev_quat[4];
00213
00214 square = 0.25 * (1 + viewrot[0] + viewrot[5] + viewrot[10]);
00215 if (!ZERO(square)) {
00216 quat[W] = sqrt(square);
00217 quat[X] = 0.25 * (viewrot[9] - viewrot[6])/ quat[W];
00218 quat[Y] = 0.25 * (viewrot[2] - viewrot[8])/ quat[W];
00219 quat[Z] = 0.25 * (viewrot[4] - viewrot[1])/ quat[W];
00220 } else {
00221 quat[W] = 0.0;
00222 square = -0.5 * (viewrot[5] + viewrot[10]);
00223 if (!ZERO(square)) {
00224 quat[X] = sqrt(square);
00225 quat[Y] = 0.5 * viewrot[4] / quat[X];
00226 quat[Z] = 0.5 * viewrot[8] / quat[X];
00227 } else {
00228 quat[X] = 0.0;
00229 square = 0.5 * (1 - viewrot[10]);
00230 if (!ZERO(square)) {
00231 quat[Y] = sqrt(square);
00232 quat[Z] = 0.5 * viewrot[9]/ quat[Y];
00233 } else {
00234 quat[Y] = 0.0;
00235 quat[Z] = 1.0;
00236 }
00237 }
00238 }
00239
00240
00241
00242
00243 for (i=0; i<4; i++)
00244 qdiff[i] = prev_quat[i] - quat[i];
00245 mag1 = QMAGSQ(qdiff);
00246 for (i=0; i<4; i++)
00247 qdiff[i] = prev_quat[i] + quat[i];
00248 mag2 = QMAGSQ(qdiff);
00249
00250 for (i=0; i<4; i++) {
00251 if (mag1 > mag2)
00252 quat[i] = -quat[i];
00253 prev_quat[i] = quat[i];
00254 }
00255
00256 return 1;
00257 }
00258
00259
00260
00261
00262
00263
00264 void
00265 anim_ypr2mat(mat_t m, const vect_t a)
00266 {
00267 fastf_t cos_y, cos_p, cos_r, sin_y, sin_p, sin_r;
00268
00269 cos_y = cos(a[0]);
00270 cos_p = cos(a[1]);
00271 cos_r = cos(a[2]);
00272 sin_y = sin(a[0]);
00273 sin_p = sin(a[1]);
00274 sin_r = sin(a[2]);
00275
00276 m[0] = cos_y*cos_p;
00277 m[1] = -cos_y*sin_p*sin_r-sin_y*cos_r;
00278 m[2] = -cos_y*sin_p*cos_r+sin_y*sin_r;
00279 m[3] = 0;
00280 m[4] = sin_y*cos_p;
00281 m[5] = -sin_y*sin_p*sin_r+cos_y*cos_r;
00282 m[6] = -sin_y*sin_p*cos_r-cos_y*sin_r;
00283 m[7] = 0;
00284 m[8]= sin_p;
00285 m[9] = cos_p*sin_r;
00286 m[10] = cos_p*cos_r;
00287 m[11] = 0.0;
00288 m[12] = 0.0;
00289 m[13] = 0.0;
00290 m[14] = 0.0;
00291 m[15] = 1.0;
00292 }
00293
00294
00295 void
00296 anim_ypr2vmat(mat_t m, const vect_t a)
00297 {
00298 fastf_t cos_y, cos_p, cos_r, sin_y, sin_p, sin_r;
00299
00300 cos_y = cos(a[0]);
00301 cos_p = cos(a[1]);
00302 cos_r = cos(a[2]);
00303 sin_y = sin(a[0]);
00304 sin_p = sin(a[1]);
00305 sin_r = sin(a[2]);
00306
00307 m[0] = -cos_y*sin_p*sin_r-sin_y*cos_r;
00308 m[1] = -sin_y*sin_p*sin_r+cos_y*cos_r;
00309 m[2] = cos_p*sin_r;
00310 m[3] = 0;
00311 m[4] = -cos_y*sin_p*cos_r+sin_y*sin_r;
00312 m[5] = -sin_y*sin_p*cos_r-cos_y*sin_r;
00313 m[6] = cos_p*cos_r;
00314 m[7] = 0;
00315 m[8] = cos_y*cos_p;
00316 m[9] = sin_y*cos_p;
00317 m[10] = sin_p;
00318 m[11] = 0.0;
00319 m[12] = 0.0;
00320 m[13] = 0.0;
00321 m[14] = 0.0;
00322 m[15] = 1.0;
00323 }
00324
00325
00326 void
00327 anim_y_p_r2mat(mat_t m, double y, double p, double r)
00328 {
00329 fastf_t cos_y = cos(y);
00330 fastf_t sin_y = sin(y);
00331 fastf_t cos_p = cos(p);
00332 fastf_t sin_p = sin(p);
00333 fastf_t cos_r = cos(r);
00334 fastf_t sin_r = sin(r);
00335
00336 m[0] = cos_y*cos_p;
00337 m[1] = -cos_y*sin_p*sin_r-sin_y*cos_r;
00338 m[2] = -cos_y*sin_p*cos_r+sin_y*sin_r;
00339 m[4] = sin_y*cos_p;
00340 m[5] = -sin_y*sin_p*sin_r+cos_y*cos_r;
00341 m[6] = -sin_y*sin_p*cos_r-cos_y*sin_r;
00342 m[8]= sin_p;
00343 m[9] = cos_p*sin_r;
00344 m[10] = cos_p*cos_r;
00345 m[3]=m[7]=m[11]=m[12]=m[13]=m[14]=0;
00346 m[15]=1;
00347 }
00348
00349
00350 void
00351 anim_dy_p_r2mat(mat_t m, double y, double p, double r)
00352 {
00353 fastf_t radian_yaw = y*(M_PI*0.0055555555556);
00354 fastf_t radian_pitch = p*(M_PI*0.0055555555556);
00355 fastf_t radian_roll = r*(M_PI*0.0055555555556);
00356
00357 fastf_t cos_y = cos(radian_yaw);
00358 fastf_t sin_y = sin(radian_yaw);
00359 fastf_t cos_p = cos(radian_pitch);
00360 fastf_t sin_p = sin(radian_pitch);
00361 fastf_t cos_r = cos(radian_roll);
00362 fastf_t sin_r = sin(radian_roll);
00363
00364 m[0] = cos_y*cos_p;
00365 m[1] = -cos_y*sin_p*sin_r-sin_y*cos_r;
00366 m[2] = -cos_y*sin_p*cos_r+sin_y*sin_r;
00367 m[4] = sin_y*cos_p;
00368 m[5] = -sin_y*sin_p*sin_r+cos_y*cos_r;
00369 m[6] = -sin_y*sin_p*cos_r-cos_y*sin_r;
00370 m[8]= sin_p;
00371 m[9] = cos_p*sin_r;
00372 m[10] = cos_p*cos_r;
00373 m[3]=m[7]=m[11]=m[12]=m[13]=m[14]=0;
00374 m[15]=1;
00375 }
00376
00377
00378 void
00379 anim_dy_p_r2vmat(mat_t m, double yaw, double pch, double rll)
00380 {
00381
00382 float ryaw = yaw*(M_PI*0.0055555555556);
00383 float rpch = pch*(M_PI*0.0055555555556);
00384 float rrll = rll*(M_PI*0.0055555555556);
00385
00386 float cos_y = cos(ryaw);
00387 float sin_y = sin(ryaw);
00388 float cos_p = cos(rpch);
00389 float sin_p = sin(rpch);
00390 float cos_r = cos(rrll);
00391 float sin_r = sin(rrll);
00392
00393 m[0] = -cos_y*sin_p*sin_r-sin_y*cos_r;
00394 m[1] = -sin_y*sin_p*sin_r+cos_y*cos_r;
00395 m[2] = cos_p*sin_r;
00396 m[4] = -cos_y*sin_p*cos_r+sin_y*sin_r;
00397 m[5] = -sin_y*sin_p*cos_r-cos_y*sin_r;
00398 m[6] = cos_p*cos_r;
00399 m[8] = cos_y*cos_p;
00400 m[9] = sin_y*cos_p;
00401 m[10]= sin_p;
00402 m[3]=m[7]=m[11]=0;
00403 m[12]=m[13]=m[14]=0;
00404 m[15]=1;
00405
00406 }
00407
00408
00409 void
00410 anim_x_y_z2mat(mat_t m, double x, double y, double z)
00411 {
00412 fastf_t cosx = cos(x);
00413 fastf_t sinx = sin(x);
00414 fastf_t cosy = cos(y);
00415 fastf_t siny = sin(y);
00416 fastf_t cosz = cos(z);
00417 fastf_t sinz = sin(z);
00418
00419 m[0] = cosz*cosy;
00420 m[1] = cosz*siny*sinx-sinz*cosx;
00421 m[2] = cosz*siny*cosx+sinz*sinx;
00422 m[4] = sinz*cosy;
00423 m[5] = sinz*siny*sinx+cosz*cosx;
00424 m[6] = sinz*siny*cosx-cosz*sinx;
00425 m[8] = -siny;
00426 m[9] = cosy*sinx;
00427 m[10] = cosy*cosx;
00428 m[3]=m[7]=m[11]=m[12]=m[13]=m[14]=0;
00429 m[15]=1;
00430 }
00431
00432
00433 void
00434 anim_dx_y_z2mat(mat_t m, double x, double y, double z)
00435 {
00436 fastf_t cosx, cosy, cosz, sinx, siny, sinz;
00437
00438 x *= (M_PI*0.0055555555556);
00439 y *= (M_PI*0.0055555555556);
00440 z *= (M_PI*0.0055555555556);
00441
00442 cosx = cos(x);
00443 sinx = sin(x);
00444 cosy = cos(y);
00445 siny = sin(y);
00446 cosz = cos(z);
00447 sinz = sin(z);
00448
00449 m[0] = cosz*cosy;
00450 m[1] = cosz*siny*sinx-sinz*cosx;
00451 m[2] = cosz*siny*cosx+sinz*sinx;
00452 m[4] = sinz*cosy;
00453 m[5] = sinz*siny*sinx+cosz*cosx;
00454 m[6] = sinz*siny*cosx-cosz*sinx;
00455 m[8] = -siny;
00456 m[9] = cosy*sinx;
00457 m[10] = cosy*cosx;
00458 m[3]=m[7]=m[11]=m[12]=m[13]=m[14]=0.0;
00459 m[15]=1.0;
00460 }
00461
00462
00463 void
00464 anim_zyx2mat(mat_t m, const vect_t a)
00465 {
00466 fastf_t cosX, cosY, cosZ, sinX, sinY, sinZ;
00467
00468 cosX = cos(a[0]);
00469 cosY = cos(a[1]);
00470 cosZ = cos(a[2]);
00471 sinX = sin(a[0]);
00472 sinY = sin(a[1]);
00473 sinZ = sin(a[2]);
00474
00475 m[0] = cosY*cosZ;
00476 m[1] = -cosY*sinZ;
00477 m[2] = sinY;
00478 m[3] = 0;
00479 m[4] = cosX*sinZ + sinX*sinY*cosZ;
00480 m[5] = cosX*cosZ - sinX*sinY*sinZ;
00481 m[6] = -sinX*cosY;
00482 m[7] = 0;
00483 m[8] = sinX*sinZ - cosX*sinY*cosZ;
00484 m[9] = sinX*cosZ + cosX*sinY*sinZ;
00485 m[10] = cosX*cosY;
00486 m[11] = 0.0;
00487 m[12] = 0.0;
00488 m[13] = 0.0;
00489 m[14] = 0.0;
00490 m[15] = 1.0;
00491
00492 }
00493
00494
00495 void
00496 anim_z_y_x2mat(mat_t m, double x, double y, double z)
00497 {
00498 fastf_t cosx = cos(x);
00499 fastf_t sinx = sin(x);
00500 fastf_t cosy = cos(y);
00501 fastf_t siny = sin(y);
00502 fastf_t cosz = cos(z);
00503 fastf_t sinz = sin(z);
00504
00505 m[0] = cosy*cosz;
00506 m[1] = -cosy*sinz;
00507 m[2] = siny;
00508 m[4] = cosx*sinz + sinx*siny*cosz;
00509 m[5] = cosx*cosz - sinx*siny*sinz;
00510 m[6] = -sinx*cosy;
00511 m[8] = sinx*sinz - cosx*siny*cosz;
00512 m[9] = sinx*cosz + cosx*siny*sinz;
00513 m[10]= cosx*cosy;
00514 m[3]=m[7]=m[11]=m[12]=m[13]=m[14]=0.0;
00515 m[15]=1.0;
00516 }
00517
00518
00519 void
00520 anim_dz_y_x2mat(mat_t m, double x, double y, double z)
00521 {
00522 fastf_t cosx, cosy, cosz, sinx, siny, sinz;
00523
00524 x *= (M_PI*0.0055555555556);
00525 y *= (M_PI*0.0055555555556);
00526 z *= (M_PI*0.0055555555556);
00527
00528 cosx = cos(x);
00529 sinx = sin(x);
00530 cosy = cos(y);
00531 siny = sin(y);
00532 cosz = cos(z);
00533 sinz = sin(z);
00534
00535 m[0] = cosy*cosz;
00536 m[1] = -cosy*sinz;
00537 m[2] = siny;
00538 m[4] = cosx*sinz + sinx*siny*cosz;
00539 m[5] = cosx*cosz - sinx*siny*sinz;
00540 m[6] = -sinx*cosy;
00541 m[8] = sinx*sinz - cosx*siny*cosz;
00542 m[9] = sinx*cosz + cosx*siny*sinz;
00543 m[10]= cosx*cosy;
00544 m[3]=m[7]=m[11]=m[12]=m[13]=m[14]=0;
00545 m[15]=1;
00546 }
00547
00548
00549 void
00550 anim_quat2mat(mat_t m, const quat_t qq)
00551 {
00552 fastf_t two_q[4];
00553 quat_t q;
00554
00555 QMOVE(q, qq);
00556 QUNITIZE(q);
00557
00558 VADD2N(two_q, q, q, 4);
00559
00560 m[0] = 1.0 - two_q[Y]*q[Y] - two_q[Z]*q[Z];
00561 m[1] = two_q[X]*q[Y] - two_q[W]*q[Z];
00562 m[2] = two_q[X]*q[Z] + two_q[W]*q[Y];
00563 m[3] = 0.0;
00564 m[4] = two_q[X]*q[Y] + two_q[W]*q[Z];
00565 m[5] = 1.0 - two_q[X]*q[X] - two_q[Z]*q[Z];
00566 m[6] = two_q[Y]*q[Z] - two_q[W]*q[X];
00567 m[7] = 0.0;
00568 m[8] = two_q[X]*q[Z] - two_q[W]*q[Y];
00569 m[9] = two_q[Y]*q[Z] + two_q[W]*q[X];
00570 m[10] = 1.0 - two_q[X]*q[X] - two_q[Y]*q[Y];
00571 m[11] = 0.0;
00572 m[12] = 0.0;
00573 m[13] = 0.0;
00574 m[14] = 0.0;
00575 m[15] = 1.0;
00576 }
00577
00578
00579 void
00580 anim_dir2mat(mat_t m, const vect_t d, const vect_t d2b)
00581 {
00582 fastf_t hypotenuse, sign;
00583 vect_t d2;
00584
00585 VMOVE(d2, d2b);
00586 sign = 1.0;
00587 hypotenuse = sqrt(d[0]*d[0]+d[1]*d[1]);
00588 if (hypotenuse < VDIVIDE_TOL) {
00589
00590 hypotenuse = sqrt(d2[0]*d2[0]+d2[1]*d2[1]);
00591 if (hypotenuse < VDIVIDE_TOL) {
00592
00593 VSET(d2, 1, 0, 0);
00594 hypotenuse = 1;
00595 }
00596 if (d[2] < 0)
00597 sign = -1.0;
00598 m[1] = -d2[1]/hypotenuse;
00599 m[5] = d2[0]/hypotenuse;
00600 m[2] = -sign * d2[0]/hypotenuse;
00601 m[6] = -sign * d2[1]/hypotenuse;
00602 m[8] = sign;
00603 m[0]=m[4]=m[9]=m[10]=0.0;
00604 } else {
00605
00606 m[0] = d[0];
00607 m[1] = -d[1]/hypotenuse;
00608 m[2] = -d[0]*d[2]/hypotenuse;
00609 m[4] = d[1];
00610 m[5] = d[0]/hypotenuse;
00611 m[6] = -d[1]*d[2]/hypotenuse;
00612 m[8] = d[2];
00613 m[9] = 0.0;
00614 m[10] = hypotenuse;
00615 }
00616 m[3]=m[7]=m[11]=0.0;
00617 m[12]=m[13]=m[14]=0.0;
00618 m[15]=1.0;
00619
00620 }
00621
00622
00623 void
00624 anim_dirn2mat(mat_t m, const vect_t dx2, const vect_t dn)
00625 {
00626 vect_t temp;
00627 fastf_t hyp, sign, inv, mag;
00628 vect_t dx;
00629
00630 VMOVE(dx, dx2);
00631 sign = 1.0;
00632 mag = MAGNITUDE(dx);
00633 if (mag < VDIVIDE_TOL) {
00634 bu_log("anim_dirn2mat: Need non-zero vector");
00635 return;
00636 }
00637 inv = 1.0/mag;
00638 dx[0] *= inv;
00639 dx[1] *= inv;
00640 dx[2] *= inv;
00641 hyp = sqrt(dx[0]*dx[0]+dx[1]*dx[1]);
00642 if (hyp < VDIVIDE_TOL) {
00643
00644 sign = (dx[2] < 0) ? -1.0 : 1.0;
00645 VSET(temp, dn[0], dn[1], 0.0);
00646 mag = MAGNITUDE(temp);
00647 if (mag < VDIVIDE_TOL) {
00648
00649 VSET(temp, 0.0, 1.0, 0.0);
00650 mag = 1.0;
00651 } else {
00652 inv = 1.0/mag;
00653 temp[0] *= inv;
00654 temp[1] *= inv;
00655 }
00656 m[0] = 0.0;
00657 m[4] = 0.0;
00658 m[8] = sign;
00659 m[1] = temp[0];
00660 m[5] = temp[1];
00661 m[9] = 0.0;
00662 m[2] = -sign*temp[1];
00663 m[6] = sign*temp[0];
00664 m[10] = 0.0;
00665 m[3]=m[7]=m[11]=0.0;
00666 m[12]=m[13]=m[14]=0.0;
00667 m[15]=1.0;
00668 return;
00669 }
00670
00671
00672 m[0] = dx[0];
00673 m[1] = -dx[1]/hyp;
00674 m[2] = -dx[0]*dx[2]/hyp;
00675 m[4] = dx[1];
00676 m[5] = dx[0]/hyp;
00677 m[6] = -dx[1]*dx[2]/hyp;
00678 m[8] = dx[2];
00679 m[9] = 0.0;
00680 m[10] = hyp;
00681 m[3]=m[7]=m[11]=0.0;
00682 m[12]=m[13]=m[14]=0.0;
00683 m[15]=1.0;
00684
00685 }
00686
00687
00688 #define ASM_EMPTY 0
00689 #define ASM_FIRST 1
00690 #define ASM_FULL 2
00691
00692 int
00693 anim_steer_mat(mat_t mat, vect_t point, int end)
00694 {
00695 static vect_t p1, p2, p3;
00696 vect_t dir = VINIT_ZERO;
00697 static vect_t norm;
00698 static int state = ASM_EMPTY;
00699
00700 VMOVE(p1, p2);
00701 VMOVE(p2, p3);
00702 VMOVE(p3, point);
00703
00704 switch (state) {
00705 case ASM_EMPTY:
00706 if (end) {
00707 state = ASM_EMPTY;
00708 } else {
00709 state = ASM_FIRST;
00710
00711 }
00712 return 0;
00713 case ASM_FIRST:
00714 if (end) {
00715
00716 VSET(dir, 1.0, 0.0, 0.0);
00717 VSET(norm, 0.0, 1.0, 0.0);
00718 state = ASM_EMPTY;
00719 } else {
00720 VSUBUNIT(dir, p3, p2);
00721 VSET(norm, 0.0, 1.0, 0.0);
00722 state = ASM_FULL;
00723 }
00724 break;
00725 case ASM_FULL:
00726 if (end) {
00727 VSUBUNIT(dir, p2, p1);
00728 state = ASM_EMPTY;
00729 } else {
00730 VSUBUNIT(dir, p3, p1);
00731 state = ASM_FULL;
00732 }
00733 }
00734
00735
00736 anim_dirn2mat(mat, dir, norm);
00737 VSET(norm, mat[1], mat[5], 0.0);
00738 VMOVE(point, p2);
00739 return 1;
00740
00741 }
00742
00743
00744
00745
00746
00747
00748
00749 void
00750 anim_add_trans(mat_t m, const vect_t post, const vect_t pre)
00751 {
00752 int i;
00753 for (i=0; i<3; i++)
00754 m[3+i*4] += m[i*4]*pre[0] + m[1+i*4]*pre[1]+m[2+i*4]*pre[2] + post[i];
00755
00756 }
00757
00758
00759 void
00760 anim_rotatez(fastf_t a, vect_t d)
00761 {
00762 fastf_t temp[3];
00763 fastf_t cos_y = cos(a);
00764 fastf_t sin_y = sin(a);
00765 temp[0] = d[0]*cos_y - d[1]*sin_y;
00766 temp[1] = d[0]*sin_y + d[1]*cos_y;
00767 d[0]=temp[0];
00768 d[1]=temp[1];
00769 }
00770
00771
00772 void
00773 anim_mat_print(FILE *fp, const mat_t m, int s_colon)
00774 {
00775 bu_flog(fp, "%.10g %.10g %.10g %.10g\n", m[0], m[1], m[2], m[3]);
00776 bu_flog(fp, "%.10g %.10g %.10g %.10g\n", m[4], m[5], m[6], m[7]);
00777 bu_flog(fp, "%.10g %.10g %.10g %.10g\n", m[8], m[9], m[10], m[11]);
00778 bu_flog(fp, "%.10g %.10g %.10g %.10g", m[12], m[13], m[14], m[15]);
00779 if (s_colon)
00780 bu_flog(fp, ";");
00781 bu_flog(fp, "\n");
00782 }
00783
00784
00785 void
00786 anim_mat_printf(
00787 FILE *fp,
00788 const mat_t m,
00789 const char *formstr,
00790 const char *linestr,
00791 const char *endstr)
00792 {
00793 char mystr[80];
00794 snprintf(mystr, 80, "%s%s%s%s%%s", formstr, formstr, formstr, formstr);
00795 bu_flog(fp, mystr, m[0], m[1], m[2], m[3], linestr);
00796 bu_flog(fp, mystr, m[4], m[5], m[6], m[7], linestr);
00797 bu_flog(fp, mystr, m[8], m[9], m[10], m[11], linestr);
00798 bu_flog(fp, mystr, m[12], m[13], m[14], m[15], endstr);
00799 }
00800
00801
00802 void
00803 anim_view_rev(mat_t m)
00804 {
00805 m[0] = -m[0];
00806 m[1] = -m[1];
00807 m[4] = -m[4];
00808 m[5] = -m[5];
00809 m[8] = -m[8];
00810 m[9] = -m[9];
00811 }
00812
00813
00814
00815
00816
00817
00818
00819
00820
00821
00822