anim.c

Go to the documentation of this file.
00001 /*                          A N I M . C
00002  * BRL-CAD
00003  *
00004  * Copyright (c) 1993-2012 United States Government as represented by
00005  * the U.S. Army Research Laboratory.
00006  *
00007  * This library is free software; you can redistribute it and/or
00008  * modify it under the terms of the GNU Lesser General Public License
00009  * version 2.1 as published by the Free Software Foundation.
00010  *
00011  * This library is distributed in the hope that it will be useful, but
00012  * WITHOUT ANY WARRANTY; without even the implied warranty of
00013  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00014  * Lesser General Public License for more details.
00015  *
00016  * You should have received a copy of the GNU Lesser General Public
00017  * License along with this file; see the file named COPYING for more
00018  * information.
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  *ANIM_MAT2* - Conversions from matrices
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     /* in principle, we can use the sin_x or cos_x with sin_z or cos_z
00111      * to figure out angle[1], as long as they are non-zero. To avoid
00112      * ill-conditioning effects, we choose the two that are greatest
00113      * in absolute value
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         /* this should be impossible*/
00123         /* unable to calculate pitch*/
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     /* assume the smallest possible arc-length from frame to frame */
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     /* in principle, we can use sin_y or cos_y with sin_r or cos_r to
00171      * figure out angle[1], as long as they are non-zero. To avoid
00172      * ill-conditioning effects, we choose the two that are greatest
00173      * in absolute value
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         /* this should not happen */
00183         /* unable to calculate pitch*/
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     /* assume the smallest possible arc-length from frame to frame */
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     /* quaternions on opposite sides of a four-dimensional sphere are
00241        equivalent. Take the quaternion closest to the previous one */
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)  /* inverse of quat would be closer */
00252             quat[i] = -quat[i];
00253         prev_quat[i] = quat[i];
00254     }
00255 
00256     return 1;
00257 }
00258 
00259 
00260 /***************************************
00261  *ANIM_*2MAT - Conversions to matrices
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         /* vertical direction - use d2 to determine roll */
00590         hypotenuse = sqrt(d2[0]*d2[0]+d2[1]*d2[1]);
00591         if (hypotenuse < VDIVIDE_TOL) {
00592             /* use x-axis as default*/
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         /* normal - no roll*/
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         /* vertical - special handling */
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             /* use default */
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     /*else normal*/
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                 /* "don't print yet */
00711             }
00712             return 0;
00713         case ASM_FIRST:
00714             if (end) {
00715                 /* only one point specified, use default direction */
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     /* go for it */
00736     anim_dirn2mat(mat, dir, norm); /* create basic rotation matrix */
00737     VSET(norm, mat[1], mat[5], 0.0); /* save for next time */
00738     VMOVE(point, p2); /* for main's purposes, the current point is p2 */
00739     return 1; /* return signal go ahead and print */
00740 
00741 }
00742 
00743 
00744 /***************************************
00745  * Other animation routines
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  * Local Variables:
00816  * mode: C
00817  * tab-width: 8
00818  * indent-tabs-mode: t
00819  * c-file-style: "stroustrup"
00820  * End:
00821  * ex: shiftwidth=4 tabstop=8
00822  */
Generated on Tue Dec 11 13:14:27 2012 for LIBBN by  doxygen 1.6.3