mat.c

Go to the documentation of this file.
00001 /*                           M A T . C
00002  * BRL-CAD
00003  *
00004  * Copyright (c) 1996-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 /** @addtogroup mat */
00021 /** @{ */
00022 /** @file libbn/mat.c
00023  *
00024  * @brief
00025  * 4 x 4 Matrix manipulation functions...
00026  *
00027  * Matrix array elements have the following positions in the matrix:
00028  @code
00029  *                      |  0  1  2  3 |         | 0 |
00030  *      [ 0 1 2 3 ]     |  4  5  6  7 |         | 1 |
00031  *                      |  8  9 10 11 |         | 2 |
00032  *                      | 12 13 14 15 |         | 3 |
00033  *
00034  * preVector (vect_t) Matrix (mat_t) postVector (vect_t)
00035  @endcode
00036  *
00037  * TODO: need a better way to control tolerancing, either via
00038  * additional tolerance parameters or perhaps providing a global
00039  * tolerance state interface.
00040  */
00041 /** @} */
00042 
00043 #include "common.h"
00044 
00045 #include <math.h>
00046 #include <string.h>
00047 #include "bio.h"
00048 
00049 #include "bu.h"
00050 #include "vmath.h"
00051 #include "bn.h"
00052 
00053 
00054 const mat_t bn_mat_identity = MAT_INIT_IDN;
00055 
00056 
00057 void
00058 bn_mat_print_guts(const char *title,
00059                   const mat_t m,
00060                   char *obuf,
00061                   int len)
00062 {
00063     register int i;
00064     register char *cp;
00065 
00066     snprintf(obuf, len, "MATRIX %s:\n  ", title);
00067     cp = obuf+strlen(obuf);
00068     if (!m) {
00069         bu_strlcat(obuf, "(Identity)", len);
00070     } else {
00071         for (i=0; i<16; i++) {
00072             snprintf(cp, len-(cp-obuf), " %8.3f", m[i]);
00073             cp += strlen(cp);
00074             if (i == 15) {
00075                 break;
00076             } else if ((i&3) == 3) {
00077                 *cp++ = '\n';
00078                 *cp++ = ' ';
00079                 *cp++ = ' ';
00080             }
00081         }
00082         *cp++ = '\0';
00083     }
00084 }
00085 
00086 
00087 /**
00088  * B N _ M A T _ P R I N T
00089  */
00090 void
00091 bn_mat_print(const char *title,
00092              const mat_t m)
00093 {
00094     char obuf[1024];    /* sprintf may be non-PARALLEL */
00095 
00096     bn_mat_print_guts(title, m, obuf, 1024);
00097     bu_log("%s\n", obuf);
00098 }
00099 
00100 
00101 /**
00102  * B N _ M A T _ P R I N T _ V L S
00103  */
00104 void
00105 bn_mat_print_vls(const char *title,
00106                  const mat_t m,
00107                  struct bu_vls *vls)
00108 {
00109     char obuf[1024];
00110 
00111     bn_mat_print_guts(title, m, obuf, 1024);
00112     bu_vls_printf(vls, "%s\n", obuf);
00113 }
00114 
00115 
00116 /**
00117  * B N _ A T A N 2
00118  *
00119  * A wrapper for the system atan2().  On the Silicon Graphics, and
00120  * perhaps on others, x==0 incorrectly returns infinity.
00121  */
00122 double
00123 bn_atan2(double y, double x)
00124 {
00125     if (x > -1.0e-20 && x < 1.0e-20) {
00126         /* X is equal to zero, check Y */
00127         if (y < -1.0e-20)
00128             return -M_PI_2;
00129         if (y > 1.0e-20)
00130             return M_PI_2;
00131         return 0.0;
00132     }
00133     return atan2(y, x);
00134 }
00135 
00136 
00137 /**
00138  * B N _ M A T _ M U L
00139  *
00140  * Multiply matrix "a" by "b" and store the result in "o".
00141  *
00142  * This is different from multiplying "b" by "a" (most of the time!)
00143  * Also, "o" must not be the same as either of the inputs.
00144  */
00145 void
00146 bn_mat_mul(register mat_t o, register const mat_t a, register const mat_t b)
00147 {
00148     o[ 0] = a[ 0]*b[ 0] + a[ 1]*b[ 4] + a[ 2]*b[ 8] + a[ 3]*b[12];
00149     o[ 1] = a[ 0]*b[ 1] + a[ 1]*b[ 5] + a[ 2]*b[ 9] + a[ 3]*b[13];
00150     o[ 2] = a[ 0]*b[ 2] + a[ 1]*b[ 6] + a[ 2]*b[10] + a[ 3]*b[14];
00151     o[ 3] = a[ 0]*b[ 3] + a[ 1]*b[ 7] + a[ 2]*b[11] + a[ 3]*b[15];
00152 
00153     o[ 4] = a[ 4]*b[ 0] + a[ 5]*b[ 4] + a[ 6]*b[ 8] + a[ 7]*b[12];
00154     o[ 5] = a[ 4]*b[ 1] + a[ 5]*b[ 5] + a[ 6]*b[ 9] + a[ 7]*b[13];
00155     o[ 6] = a[ 4]*b[ 2] + a[ 5]*b[ 6] + a[ 6]*b[10] + a[ 7]*b[14];
00156     o[ 7] = a[ 4]*b[ 3] + a[ 5]*b[ 7] + a[ 6]*b[11] + a[ 7]*b[15];
00157 
00158     o[ 8] = a[ 8]*b[ 0] + a[ 9]*b[ 4] + a[10]*b[ 8] + a[11]*b[12];
00159     o[ 9] = a[ 8]*b[ 1] + a[ 9]*b[ 5] + a[10]*b[ 9] + a[11]*b[13];
00160     o[10] = a[ 8]*b[ 2] + a[ 9]*b[ 6] + a[10]*b[10] + a[11]*b[14];
00161     o[11] = a[ 8]*b[ 3] + a[ 9]*b[ 7] + a[10]*b[11] + a[11]*b[15];
00162 
00163     o[12] = a[12]*b[ 0] + a[13]*b[ 4] + a[14]*b[ 8] + a[15]*b[12];
00164     o[13] = a[12]*b[ 1] + a[13]*b[ 5] + a[14]*b[ 9] + a[15]*b[13];
00165     o[14] = a[12]*b[ 2] + a[13]*b[ 6] + a[14]*b[10] + a[15]*b[14];
00166     o[15] = a[12]*b[ 3] + a[13]*b[ 7] + a[14]*b[11] + a[15]*b[15];
00167 }
00168 
00169 
00170 /**
00171  * B N _ M A T _ M U L 2
00172  *
00173  * o = i * o
00174  *
00175  * A convenience wrapper for bn_mat_mul() to update a matrix in place.
00176  * The argument ordering is confusing either way.
00177  */
00178 void
00179 bn_mat_mul2(register const mat_t i, register mat_t o)
00180 {
00181     mat_t temp;
00182 
00183     bn_mat_mul(temp, i, o);
00184     MAT_COPY(o, temp);
00185 }
00186 
00187 
00188 /**
00189  * B N _ M A T _ M U L 3
00190  *
00191  * o = a * b * c
00192  *
00193  * The output matrix may be the same as 'b' or 'c', but may not be
00194  * 'a'.
00195  */
00196 void
00197 bn_mat_mul3(mat_t o, const mat_t a, const mat_t b, const mat_t c)
00198 {
00199     mat_t t;
00200 
00201     bn_mat_mul(t, b, c);
00202     bn_mat_mul(o, a, t);
00203 }
00204 
00205 
00206 /**
00207  * B N _ M A T _ M U L 4
00208  *
00209  * o = a * b * c * d
00210  *
00211  * The output matrix may be the same as any input matrix.
00212  */
00213 void
00214 bn_mat_mul4(mat_t ao, const mat_t a, const mat_t b, const mat_t c, const mat_t d)
00215 {
00216     mat_t t, u;
00217 
00218     bn_mat_mul(u, c, d);
00219     bn_mat_mul(t, a, b);
00220     bn_mat_mul(ao, t, u);
00221 }
00222 
00223 
00224 /**
00225  * B N _ M A T X V E C
00226  *
00227  * Multiply the matrix "im" by the vector "iv" and store the result in
00228  * the vector "ov".  Note this is post-multiply, and operates on
00229  * 4-tuples.  Use MAT4X3VEC() to operate on 3-tuples.
00230  */
00231 void
00232 bn_matXvec(register vect_t ov, register const mat_t im, register const vect_t iv)
00233 {
00234     register int eo = 0;        /* Position in output vector */
00235     register int em = 0;        /* Position in input matrix */
00236     register int ei;            /* Position in input vector */
00237 
00238     /* For each element in the output array... */
00239     for (; eo<4; eo++) {
00240 
00241         ov[eo] = 0;             /* Start with zero in output */
00242 
00243         for (ei=0; ei<4; ei++)
00244             ov[eo] += im[em++] * iv[ei];
00245     }
00246 }
00247 
00248 
00249 /**
00250  * B N _ M A T _ I N V
00251  *
00252  * The matrix pointed at by "input" is inverted and stored in the area
00253  * pointed at by "output".
00254  *
00255  * Calls bu_bomb if matrix is singular.
00256  */
00257 void
00258 bn_mat_inv(register mat_t output, const mat_t input)
00259 {
00260     if (bn_mat_inverse(output, input) == 0) {
00261 
00262         if (bu_debug & BU_DEBUG_MATH) {
00263             bu_log("bn_mat_inv:  error!");
00264             bn_mat_print("singular matrix", input);
00265         }
00266         bu_bomb("bn_mat_inv: singular matrix\n");
00267         /* NOTREACHED */
00268     }
00269 }
00270 
00271 
00272 /**
00273  * B N _ M A T _ I N V E R S E
00274  *
00275  * The matrix pointed at by "input" is inverted and stored in the area
00276  * pointed at by "output".
00277  *
00278  * Invert a 4-by-4 matrix using Algorithm 120 from ACM.  This is a
00279  * modified Gauss-Jordan alogorithm Note: Inversion is done in place,
00280  * with 3 work vectors
00281  *
00282  * @return 1 if OK.
00283  * @return 0 if matrix is singular.
00284  */
00285 int
00286 bn_mat_inverse(register mat_t output, const mat_t input)
00287 {
00288     register int i, j;  /* Indices */
00289     int k;              /* Indices */
00290     int z[4];           /* Temporary */
00291     fastf_t b[4];       /* Temporary */
00292     fastf_t c[4];       /* Temporary */
00293 
00294     MAT_COPY(output, input);    /* Duplicate */
00295 
00296     /* Initialization */
00297     for (j = 0; j < 4; j++)
00298         z[j] = j;
00299 
00300     /* Main Loop */
00301     for (i = 0; i < 4; i++) {
00302         register fastf_t y;             /* local temporary */
00303 
00304         k = i;
00305         y = output[i*4+i];
00306         for (j = i+1; j < 4; j++) {
00307             register fastf_t w;         /* local temporary */
00308 
00309             w = output[i*4+j];
00310             if (fabs(w) > fabs(y)) {
00311                 k = j;
00312                 y = w;
00313             }
00314         }
00315 
00316         if (ZERO(y)) {
00317             /* SINGULAR */
00318             return 0;
00319         }
00320         y = 1.0 / y;
00321 
00322         for (j = 0; j < 4; j++) {
00323             register fastf_t temp;      /* Local */
00324 
00325             c[j] = output[j*4+k];
00326             output[j*4+k] = output[j*4+i];
00327             output[j*4+i] = - c[j] * y;
00328             temp = output[i*4+j] * y;
00329             b[j] = temp;
00330             output[i*4+j] = temp;
00331         }
00332 
00333         output[i*4+i] = y;
00334         j = z[i];
00335         z[i] = z[k];
00336         z[k] = j;
00337         for (k = 0; k < 4; k++) {
00338             if (k == i)
00339                 continue;
00340             for (j = 0; j < 4; j++) {
00341                 if (j == i)
00342                     continue;
00343                 output[k*4+j] = output[k*4+j] - b[j] * c[k];
00344             }
00345         }
00346     }
00347 
00348     /* Second Loop */
00349     for (i = 0; i < 4; i++) {
00350         while ((k = z[i]) != i) {
00351             int p;                      /* Local temp */
00352 
00353             for (j = 0; j < 4; j++) {
00354                 register fastf_t w;     /* Local temp */
00355 
00356                 w = output[i*4+j];
00357                 output[i*4+j] = output[k*4+j];
00358                 output[k*4+j] = w;
00359             }
00360             p = z[i];
00361             z[i] = z[k];
00362             z[k] = p;
00363         }
00364     }
00365 
00366     return 1;
00367 }
00368 
00369 
00370 /**
00371  * B N _ V T O H _ M O V E
00372  *
00373  * Takes a pointer to a [x, y, z] vector, and a pointer to space for a
00374  * homogeneous vector [x, y, z, w], and builds [x, y, z, 1].
00375  */
00376 void
00377 bn_vtoh_move(register vect_t h, register const vect_t v)
00378 {
00379     VMOVE(h, v);
00380     h[W] = 1.0;
00381 }
00382 
00383 
00384 /**
00385  * B N _ H T O V _ M O V E
00386  *
00387  * Takes a pointer to [x, y, z, w], and converts it to an ordinary
00388  * vector [x/w, y/w, z/w].  Optimization for the case of w==1 is
00389  * performed.
00390  *
00391  * FIXME: make tolerance configurable
00392  */
00393 void
00394 bn_htov_move(register vect_t v, register const vect_t h)
00395 {
00396     register fastf_t inv;
00397 
00398     if (ZERO(h[3] - 1.0)) {
00399         VMOVE(v, h);
00400     } else {
00401         if (ZERO(h[W])) {
00402             bu_log("bn_htov_move: divide by %f!\n", h[W]);
00403             return;
00404         }
00405         inv = 1.0 / h[W];
00406         VSCALE(v, h, inv);
00407     }
00408 }
00409 
00410 
00411 /**
00412  * B N _ M A T _ T R N
00413  */
00414 void
00415 bn_mat_trn(mat_t om, register const mat_t im)
00416 {
00417     MAT_TRANSPOSE(om, im);
00418 }
00419 
00420 
00421 /**
00422  * B N _ M A T _ A E
00423  *
00424  * Compute a 4x4 rotation matrix given Azimuth and Elevation.
00425  *
00426  * Azimuth is +X, Elevation is +Z, both in degrees.
00427  *
00428  * Formula due to Doug Gwyn, BRL.
00429  */
00430 void
00431 bn_mat_ae(register fastf_t *m, double azimuth, double elev)
00432 {
00433     double sin_az, sin_el;
00434     double cos_az, cos_el;
00435 
00436     azimuth *= bn_degtorad;
00437     elev *= bn_degtorad;
00438 
00439     sin_az = sin(azimuth);
00440     cos_az = cos(azimuth);
00441     sin_el = sin(elev);
00442     cos_el = cos(elev);
00443 
00444     m[0] = cos_el * cos_az;
00445     m[1] = -sin_az;
00446     m[2] = -sin_el * cos_az;
00447     m[3] = 0;
00448 
00449     m[4] = cos_el * sin_az;
00450     m[5] = cos_az;
00451     m[6] = -sin_el * sin_az;
00452     m[7] = 0;
00453 
00454     m[8] = sin_el;
00455     m[9] = 0;
00456     m[10] = cos_el;
00457     m[11] = 0;
00458 
00459     m[12] = m[13] = m[14] = 0;
00460     m[15] = 1.0;
00461 }
00462 
00463 
00464 /**
00465  * B N _ A E _ V E C
00466  *
00467  * Find the azimuth and elevation angles that correspond to the
00468  * direction (not including twist) given by a direction vector.
00469  */
00470 void
00471 bn_ae_vec(fastf_t *azp, fastf_t *elp, const vect_t v)
00472 {
00473     register fastf_t az;
00474 
00475     if ((az = bn_atan2(v[Y], v[X]) * bn_radtodeg) < 0) {
00476         *azp = 360 + az;
00477     } else if (az >= 360) {
00478         *azp = az - 360;
00479     } else {
00480         *azp = az;
00481     }
00482     *elp = bn_atan2(v[Z], hypot(v[X], v[Y])) * bn_radtodeg;
00483 }
00484 
00485 
00486 /**
00487  * B N _ A E T _ V E C
00488  *
00489  * Find the azimuth, elevation, and twist from two vectors.  Vec_ae is
00490  * in the direction of view (+z in mged view) and vec_twist points to
00491  * the viewers right (+x in mged view).  Accuracy (degrees) is used to
00492  * stabilze flutter between equivalent extremes of atan2(), and to
00493  * snap twist to zero when elevation is near +/- 90
00494  */
00495 void
00496 bn_aet_vec(fastf_t *az, fastf_t *el, fastf_t *twist, fastf_t *vec_ae, fastf_t *vec_twist, fastf_t accuracy)
00497 {
00498     vect_t zero_twist, ninety_twist;
00499     vect_t z_dir;
00500 
00501     /* Get az and el as usual */
00502     bn_ae_vec(az, el, vec_ae);
00503 
00504     /* stabilize fluctuation bewteen 0 and 360
00505      * change azimuth near 360 to 0 */
00506     if (NEAR_EQUAL(*az, 360.0, accuracy))
00507         *az = 0.0;
00508 
00509     /* if elevation is +/-90 set twist to zero and calculate azimuth */
00510     if (NEAR_EQUAL(*el, 90.0, accuracy) || NEAR_ZERO(*el + 90.0, accuracy)) {
00511         *twist = 0.0;
00512         *az = bn_atan2(-vec_twist[X], vec_twist[Y]) * bn_radtodeg;
00513     } else {
00514         /* Calculate twist from vec_twist */
00515         VSET(z_dir, 0, 0, 1);
00516         VCROSS(zero_twist, z_dir, vec_ae);
00517         VUNITIZE(zero_twist);
00518         VCROSS(ninety_twist, vec_ae, zero_twist);
00519         VUNITIZE(ninety_twist);
00520 
00521         *twist = bn_atan2(VDOT(vec_twist, ninety_twist), VDOT(vec_twist, zero_twist)) * bn_radtodeg;
00522 
00523         /* stabilize flutter between +/- 180 */
00524         if (NEAR_EQUAL(*twist, -180.0, accuracy))
00525             *twist = 180.0;
00526     }
00527 }
00528 
00529 
00530 /**
00531  * B N _ V E C _ A E
00532  *
00533  * Find a unit vector from the origin given azimuth and elevation.
00534  */
00535 void
00536 bn_vec_ae(vect_t vect, fastf_t az, fastf_t el)
00537 {
00538     fastf_t vx, vy, vz;
00539     vz = sin(el);
00540     vy = fabs(vz) * sin(az);
00541     vx = fabs(vz) * cos(az);
00542     VSET(vect, vx, vy , vz);
00543     VUNITIZE(vect);
00544 }
00545 
00546 
00547 /**
00548  * B N _ V E C _ A E D
00549  *
00550  * Find a vector from the origin given azimuth, elevation, and distance.
00551  */
00552 void
00553 bn_vec_aed(vect_t vect, fastf_t az, fastf_t el, fastf_t distance)
00554 {
00555     fastf_t vx, vy, vz, rtemp;
00556     vz = distance * sin(el);
00557     rtemp = sqrt((distance * distance) - (vz * vz));
00558     vy = rtemp * sin(az);
00559     vx = rtemp * cos(az);
00560     VSET(vect, vx, vy , vz);
00561 }
00562 
00563 
00564 /**
00565  * B N _ M A T _ A N G L E S
00566  *
00567  * This routine builds a Homogeneous rotation matrix, given alpha,
00568  * beta, and gamma as angles of rotation, in degrees.
00569  *
00570  * Alpha is angle of rotation about the X axis, and is done third.
00571  * Beta is angle of rotation about the Y axis, and is done second.
00572  * Gamma is angle of rotation about Z axis, and is done first.
00573  *
00574  * FIXME: make tolerance configurable
00575  */
00576 void
00577 bn_mat_angles(register fastf_t *mat, double alpha_in, double beta_in, double ggamma_in)
00578 {
00579     double alpha, beta, ggamma;
00580     double calpha, cbeta, cgamma;
00581     double salpha, sbeta, sgamma;
00582 
00583     if (NEAR_ZERO(alpha_in, 0.0) && NEAR_ZERO(beta_in, 0.0) && NEAR_ZERO(ggamma_in, 0.0)) {
00584         MAT_IDN(mat);
00585         return;
00586     }
00587 
00588     alpha = alpha_in * bn_degtorad;
00589     beta = beta_in * bn_degtorad;
00590     ggamma = ggamma_in * bn_degtorad;
00591 
00592     calpha = cos(alpha);
00593     cbeta = cos(beta);
00594     cgamma = cos(ggamma);
00595 
00596     /* sine of "180*bn_degtorad" will not be exactly zero and will
00597      * result in errors when some codes try to convert this back to
00598      * azimuth and elevation.  do_frame() uses this technique!!!
00599      */
00600     if (ZERO(alpha_in - 180.0))
00601         salpha = 0.0;
00602     else
00603         salpha = sin(alpha);
00604 
00605     if (ZERO(beta_in - 180.0))
00606         sbeta = 0.0;
00607     else
00608         sbeta = sin(beta);
00609 
00610     if (ZERO(ggamma_in - 180.0))
00611         sgamma = 0.0;
00612     else
00613         sgamma = sin(ggamma);
00614 
00615     mat[0] = cbeta * cgamma;
00616     mat[1] = -cbeta * sgamma;
00617     mat[2] = sbeta;
00618     mat[3] = 0.0;
00619 
00620     mat[4] = salpha * sbeta * cgamma + calpha * sgamma;
00621     mat[5] = -salpha * sbeta * sgamma + calpha * cgamma;
00622     mat[6] = -salpha * cbeta;
00623     mat[7] = 0.0;
00624 
00625     mat[8] = salpha * sgamma - calpha * sbeta * cgamma;
00626     mat[9] = salpha * cgamma + calpha * sbeta * sgamma;
00627     mat[10] = calpha * cbeta;
00628     mat[11] = 0.0;
00629     mat[12] = mat[13] = mat[14] = 0.0;
00630     mat[15] = 1.0;
00631 }
00632 
00633 
00634 /**
00635  * B N _ M A T _ A N G L E S _ R A D
00636  *
00637  * This routine builds a Homogeneous rotation matrix, given alpha,
00638  * beta, and gamma as angles of rotation, in radians.
00639  *
00640  * Alpha is angle of rotation about the X axis, and is done third.
00641  * Beta is angle of rotation about the Y axis, and is done second.
00642  * Gamma is angle of rotation about Z axis, and is done first.
00643  *
00644  * FIXME: make tolerance configurable
00645  */
00646 void
00647 bn_mat_angles_rad(register mat_t mat,
00648                   double alpha,
00649                   double beta,
00650                   double ggamma)
00651 {
00652     double calpha, cbeta, cgamma;
00653     double salpha, sbeta, sgamma;
00654 
00655     if (ZERO(alpha) && ZERO(beta) && ZERO(ggamma)) {
00656         MAT_IDN(mat);
00657         return;
00658     }
00659 
00660     calpha = cos(alpha);
00661     cbeta = cos(beta);
00662     cgamma = cos(ggamma);
00663 
00664     salpha = sin(alpha);
00665     sbeta = sin(beta);
00666     sgamma = sin(ggamma);
00667 
00668     mat[0] = cbeta * cgamma;
00669     mat[1] = -cbeta * sgamma;
00670     mat[2] = sbeta;
00671     mat[3] = 0.0;
00672 
00673     mat[4] = salpha * sbeta * cgamma + calpha * sgamma;
00674     mat[5] = -salpha * sbeta * sgamma + calpha * cgamma;
00675     mat[6] = -salpha * cbeta;
00676     mat[7] = 0.0;
00677 
00678     mat[8] = salpha * sgamma - calpha * sbeta * cgamma;
00679     mat[9] = salpha * cgamma + calpha * sbeta * sgamma;
00680     mat[10] = calpha * cbeta;
00681     mat[11] = 0.0;
00682     mat[12] = mat[13] = mat[14] = 0.0;
00683     mat[15] = 1.0;
00684 }
00685 
00686 
00687 /**
00688  * B N _ E I G E N 2 X 2
00689  *
00690  * Find the eigenvalues and eigenvectors of a symmetric 2x2 matrix.
00691  * (a b)
00692  * (b c)
00693  *
00694  * The eigenvalue with the smallest absolute value is returned in
00695  * val1, with its eigenvector in vec1.
00696  */
00697 void
00698 bn_eigen2x2(fastf_t *val1, fastf_t *val2, fastf_t *vec1, fastf_t *vec2, fastf_t a, fastf_t b, fastf_t c)
00699 {
00700     fastf_t d, root;
00701     fastf_t v1, v2;
00702 
00703     d = 0.5 * (c - a);
00704 
00705     /* Check for diagonal matrix */
00706     if (NEAR_ZERO(b, 1.0e-10)) {
00707         /* smaller mag first */
00708         if (fabs(c) < fabs(a)) {
00709             *val1 = c;
00710             VSET(vec1, 0.0, 1.0, 0.0);
00711             *val2 = a;
00712             VSET(vec2, -1.0, 0.0, 0.0);
00713         } else {
00714             *val1 = a;
00715             VSET(vec1, 1.0, 0.0, 0.0);
00716             *val2 = c;
00717             VSET(vec2, 0.0, 1.0, 0.0);
00718         }
00719         return;
00720     }
00721 
00722     root = sqrt(d*d + b*b);
00723     v1 = 0.5 * (c + a) - root;
00724     v2 = 0.5 * (c + a) + root;
00725 
00726     /* smaller mag first */
00727     if (fabs(v1) < fabs(v2)) {
00728         *val1 = v1;
00729         *val2 = v2;
00730         VSET(vec1, b, d - root, 0.0);
00731     } else {
00732         *val1 = v2;
00733         *val2 = v1;
00734         VSET(vec1, root - d, b, 0.0);
00735     }
00736     VUNITIZE(vec1);
00737     VSET(vec2, -vec1[Y], vec1[X], 0.0); /* vec1 X vec2 = +Z */
00738 }
00739 
00740 
00741 /**
00742  * B N _ V E C _ P E R P
00743  *
00744  * Given a vector, create another vector which is perpendicular to it.
00745  * The output vector will have unit length only if the input vector
00746  * did.
00747  *
00748  * FIXME: make tolerance configurable
00749  */
00750 void
00751 bn_vec_perp(vect_t new, const vect_t old)
00752 {
00753     register int i;
00754     vect_t another;     /* Another vector, different */
00755 
00756     i = X;
00757     if (fabs(old[Y])<fabs(old[i])) i=Y;
00758     if (fabs(old[Z])<fabs(old[i])) i=Z;
00759     VSETALL(another, 0);
00760     another[i] = 1.0;
00761     if (ZERO(old[X]) && ZERO(old[Y]) && ZERO(old[Z])) {
00762         VMOVE(new, another);
00763     } else {
00764         VCROSS(new, another, old);
00765     }
00766 }
00767 
00768 
00769 /**
00770  * B N _ M A T _ F R O M T O
00771  *
00772  * Given two vectors, compute a rotation matrix that will transform
00773  * space by the angle between the two.  There are many candidate
00774  * matricies.
00775  *
00776  * The input 'from' and 'to' vectors need not be unit length.
00777  * MAT4X3VEC(to, m, from) is the identity that is created.
00778  *
00779  * FIXME: make tolerance configurable
00780  */
00781 void
00782 bn_mat_fromto(mat_t m, const vect_t from, const vect_t to)
00783 {
00784     vect_t test_to;
00785     vect_t unit_from, unit_to;
00786     fastf_t dot;
00787 
00788     /**
00789      * The method used here is from Graphics Gems, A. Glasner, ed.
00790      * page 531, "The Use of Coordinate Frames in Computer Graphics",
00791      * by Ken Turkowski, Example 6.
00792      */
00793     mat_t Q, Qt;
00794     mat_t R;
00795     mat_t A;
00796     mat_t temp;
00797     vect_t N, M;
00798     vect_t w_prime;             /* image of "to" ("w") in Qt */
00799 
00800     VMOVE(unit_from, from);
00801     VUNITIZE(unit_from);                /* aka "v" */
00802     VMOVE(unit_to, to);
00803     VUNITIZE(unit_to);          /* aka "w" */
00804 
00805     /* If from and to are the same or opposite, special handling is
00806      * needed, because the cross product isn't defined.  asin(0.00001)
00807      * = 0.0005729 degrees (1/2000 degree)
00808      */
00809     dot = VDOT(unit_from, unit_to);
00810     if (dot > 1.0 - 0.00001) {
00811         /* dot == 1, return identity matrix */
00812         MAT_IDN(m);
00813         return;
00814     }
00815     if (dot < -1.0 + 0.00001) {
00816         /* dot == -1, select random perpendicular N vector */
00817         bn_vec_perp(N, unit_from);
00818     } else {
00819         VCROSS(N, unit_from, unit_to);
00820         VUNITIZE(N);                    /* should be unnecessary */
00821     }
00822     VCROSS(M, N, unit_from);
00823     VUNITIZE(M);                        /* should be unnecessary */
00824 
00825     /* Almost everything here is done with pre-multiplys:  vector * mat */
00826     MAT_IDN(Q);
00827     VMOVE(&Q[0], unit_from);
00828     VMOVE(&Q[4], M);
00829     VMOVE(&Q[8], N);
00830     bn_mat_trn(Qt, Q);
00831 
00832     /* w_prime = w * Qt */
00833     MAT4X3VEC(w_prime, Q, unit_to);     /* post-multiply by transpose */
00834 
00835     MAT_IDN(R);
00836     VMOVE(&R[0], w_prime);
00837     VSET(&R[4], -w_prime[Y], w_prime[X], w_prime[Z]);
00838     VSET(&R[8], 0, 0, 1);               /* is unnecessary */
00839 
00840     bn_mat_mul(temp, R, Q);
00841     bn_mat_mul(A, Qt, temp);
00842     bn_mat_trn(m, A);           /* back to post-multiply style */
00843 
00844     /* Verify that it worked */
00845     MAT4X3VEC(test_to, m, unit_from);
00846     dot = VDOT(unit_to, test_to);
00847     if (UNLIKELY(dot < 0.98 || dot > 1.02)) {
00848         bu_log("bn_mat_fromto() ERROR!  from (%g, %g, %g) to (%g, %g, %g) went to (%g, %g, %g), dot=%g?\n",
00849                V3ARGS(from),
00850                V3ARGS(to),
00851                V3ARGS(test_to), dot);
00852     }
00853 }
00854 
00855 
00856 /**
00857  * B N _ M A T _ X R O T
00858  *
00859  * Given the sin and cos of an X rotation angle, produce the rotation
00860  * matrix.
00861  */
00862 void
00863 bn_mat_xrot(fastf_t *m, double sinx, double cosx)
00864 {
00865     m[0] = 1.0;
00866     m[1] = 0.0;
00867     m[2] = 0.0;
00868     m[3] = 0.0;
00869 
00870     m[4] = 0.0;
00871     m[5] = cosx;
00872     m[6] = -sinx;
00873     m[7] = 0.0;
00874 
00875     m[8] = 0.0;
00876     m[9] = sinx;
00877     m[10] = cosx;
00878     m[11] = 0.0;
00879 
00880     m[12] = m[13] = m[14] = 0.0;
00881     m[15] = 1.0;
00882 }
00883 
00884 
00885 /**
00886  * B N _ M A T _ Y R O T
00887  *
00888  * Given the sin and cos of a Y rotation angle, produce the rotation
00889  * matrix.
00890  */
00891 void
00892 bn_mat_yrot(fastf_t *m, double siny, double cosy)
00893 {
00894     m[0] = cosy;
00895     m[1] = 0.0;
00896     m[2] = -siny;
00897     m[3] = 0.0;
00898 
00899     m[4] = 0.0;
00900     m[5] = 1.0;
00901     m[6] = 0.0;
00902     m[7] = 0.0;
00903 
00904     m[8] = siny;
00905     m[9] = 0.0;
00906     m[10] = cosy;
00907 
00908     m[11] = 0.0;
00909     m[12] = m[13] = m[14] = 0.0;
00910     m[15] = 1.0;
00911 }
00912 
00913 
00914 /**
00915  * B N _ M A T _ Z R O T
00916  *
00917  * Given the sin and cos of a Z rotation angle, produce the rotation
00918  * matrix.
00919  */
00920 void
00921 bn_mat_zrot(fastf_t *m, double sinz, double cosz)
00922 {
00923     m[0] = cosz;
00924     m[1] = -sinz;
00925     m[2] = 0.0;
00926     m[3] = 0.0;
00927 
00928     m[4] = sinz;
00929     m[5] = cosz;
00930     m[6] = 0.0;
00931     m[7] = 0.0;
00932 
00933     m[8] = 0.0;
00934     m[9] = 0.0;
00935     m[10] = 1.0;
00936     m[11] = 0.0;
00937 
00938     m[12] = m[13] = m[14] = 0.0;
00939     m[15] = 1.0;
00940 }
00941 
00942 
00943 /**
00944  * B N _ M A T _ L O O K A T
00945  *
00946  * Given a direction vector D of unit length, product a matrix which
00947  * rotates that vector D onto the -Z axis.  This matrix will be
00948  * suitable for use as a "model2view" matrix.
00949  *
00950  * XXX This routine will fail if the vector is already more or less
00951  * aligned with the Z axis.
00952  *
00953  * This is done in several steps.
00954  *
00955  @code
00956  1) Rotate D about Z to match +X axis.  Azimuth adjustment.
00957  2) Rotate D about Y to match -Y axis.  Elevation adjustment.
00958  3) Rotate D about Z to make projection of X axis again point
00959  in the +X direction.  Twist adjustment.
00960  4) Optionally, flip sign on Y axis if original Z becomes inverted.
00961  This can be nice for static frames, but is astonishing when
00962  used in animation.
00963  @endcode
00964 */
00965 void
00966 bn_mat_lookat(mat_t rot, const vect_t dir, int yflip)
00967 {
00968     mat_t first;
00969     mat_t second;
00970     mat_t prod12;
00971     mat_t third;
00972     vect_t x;
00973     vect_t z;
00974     vect_t t1;
00975     fastf_t hypot_xy;
00976     vect_t xproj;
00977     vect_t zproj;
00978 
00979     /* First, rotate D around Z axis to match +X axis (azimuth) */
00980     hypot_xy = hypot(dir[X], dir[Y]);
00981     bn_mat_zrot(first, -dir[Y] / hypot_xy, dir[X] / hypot_xy);
00982 
00983     /* Next, rotate D around Y axis to match -Z axis (elevation) */
00984     bn_mat_yrot(second, -hypot_xy, -dir[Z]);
00985     bn_mat_mul(prod12, second, first);
00986 
00987     /* Produce twist correction, by re-orienting projection of X axis */
00988     VSET(x, 1, 0, 0);
00989     MAT4X3VEC(xproj, prod12, x);
00990     hypot_xy = hypot(xproj[X], xproj[Y]);
00991     if (hypot_xy < 1.0e-10) {
00992         bu_log("Warning: bn_mat_lookat:  unable to twist correct, hypot=%g\n", hypot_xy);
00993         VPRINT("xproj", xproj);
00994         MAT_COPY(rot, prod12);
00995         return;
00996     }
00997     bn_mat_zrot(third, -xproj[Y] / hypot_xy, xproj[X] / hypot_xy);
00998     bn_mat_mul(rot, third, prod12);
00999 
01000     if (yflip) {
01001         VSET(z, 0, 0, 1);
01002         MAT4X3VEC(zproj, rot, z);
01003         /* If original Z inverts sign, flip sign on resulting Y */
01004         if (zproj[Y] < 0.0) {
01005             MAT_COPY(prod12, rot);
01006             MAT_IDN(third);
01007             third[5] = -1;
01008             bn_mat_mul(rot, third, prod12);
01009         }
01010     }
01011 
01012     /* Check the final results */
01013     MAT4X3VEC(t1, rot, dir);
01014     if (t1[Z] > -0.98) {
01015         bu_log("Error:  bn_mat_lookat final= (%g, %g, %g)\n", V3ARGS(t1));
01016     }
01017 }
01018 
01019 
01020 /**
01021  * B N _ V E C _ O R T H O
01022  *
01023  * Given a vector, create another vector which is perpendicular to it,
01024  * and with unit length.  This algorithm taken from Gift's arvec.f; a
01025  * faster algorithm may be possible.
01026  *
01027  * FIXME: make tolerance configurable
01028  */
01029 void
01030 bn_vec_ortho(register vect_t out, register const vect_t in)
01031 {
01032     register int j, k;
01033     register fastf_t f;
01034     register int i;
01035 
01036     if (UNLIKELY(NEAR_ZERO(MAGSQ(in), SQRT_SMALL_FASTF))) {
01037         bu_log("bn_vec_ortho(): zero magnitude input vector %g %g %g\n", V3ARGS(in));
01038         VSETALL(out, 0);
01039         return;
01040     }
01041 
01042     /* Find component closest to zero */
01043     f = fabs(in[X]);
01044     i = X;
01045     j = Y;
01046     k = Z;
01047     if (fabs(in[Y]) < f) {
01048         f = fabs(in[Y]);
01049         i = Y;
01050         j = Z;
01051         k = X;
01052     }
01053     if (fabs(in[Z]) < f) {
01054         i = Z;
01055         j = X;
01056         k = Y;
01057     }
01058     f = hypot(in[j], in[k]);
01059     if (UNLIKELY(ZERO(f))) {
01060         bu_log("bn_vec_ortho(): zero hypot on %g %g %g\n", V3ARGS(in));
01061         VSETALL(out, 0);
01062         return;
01063     }
01064     f = 1.0/f;
01065     out[i] = 0.0;
01066     out[j] = -in[k]*f;
01067     out[k] =  in[j]*f;
01068 
01069     return;
01070 }
01071 
01072 
01073 /**
01074  * B N _ M A T _ S C A L E _ A B O U T _ P T
01075  *
01076  * Build a matrix to scale uniformly around a given point.
01077  *
01078  * @return -1 if scale is too small.
01079  * @return  0 if OK.
01080  *
01081  * FIXME: make tolerance configurable
01082  */
01083 int
01084 bn_mat_scale_about_pt(mat_t mat, const point_t pt, const double scale)
01085 {
01086     mat_t xlate;
01087     mat_t s;
01088     mat_t tmp;
01089 
01090     MAT_IDN(xlate);
01091     MAT_DELTAS_VEC_NEG(xlate, pt);
01092 
01093     MAT_IDN(s);
01094     if (ZERO(scale)) {
01095         MAT_ZERO(mat);
01096         return -1;                      /* ERROR */
01097     }
01098     s[15] = 1/scale;
01099 
01100     bn_mat_mul(tmp, s, xlate);
01101 
01102     MAT_DELTAS_VEC(xlate, pt);
01103     bn_mat_mul(mat, xlate, tmp);
01104     return 0;                           /* OK */
01105 }
01106 
01107 
01108 /**
01109  * B N _ M A T _ X F O R M _ A B O U T _ P T
01110  *
01111  * Build a matrix to apply arbitary 4x4 transformation around a given
01112  * point.
01113  */
01114 void
01115 bn_mat_xform_about_pt(mat_t mat, const mat_t xform, const point_t pt)
01116 {
01117     mat_t xlate;
01118     mat_t tmp;
01119 
01120     MAT_IDN(xlate);
01121     MAT_DELTAS_VEC_NEG(xlate, pt);
01122 
01123     bn_mat_mul(tmp, xform, xlate);
01124 
01125     MAT_DELTAS_VEC(xlate, pt);
01126     bn_mat_mul(mat, xlate, tmp);
01127 }
01128 
01129 
01130 /**
01131  * B N _ M A T _ I S _ E Q U A L
01132  *
01133  * @return 0 When matrices are not equal
01134  * @return 1 When matricies are equal
01135  */
01136 int
01137 bn_mat_is_equal(const mat_t a, const mat_t b, const struct bn_tol *tol)
01138 {
01139     register int i;
01140     register double f;
01141     register double tdist, tperp;
01142 
01143     BN_CK_TOL(tol);
01144 
01145     tdist = tol->dist;
01146     tperp = tol->perp;
01147 
01148     /* First, check that the translation part of the matrix (dist) are
01149      * within the distance tolerance.  Because most instancing
01150      * involves translation and no rotation, doing this first should
01151      * detect most non-equal cases rapidly.
01152      */
01153     for (i=3; i<12; i+=4) {
01154         f = a[i] - b[i];
01155         if (!NEAR_ZERO(f, tdist)) return 0;
01156     }
01157 
01158     /* Check that the rotation part of the matrix (cosines) are within
01159      * the perpendicular tolerance.
01160      */
01161     for (i = 0; i < 16; i+=4) {
01162         f = a[i] - b[i];
01163         if (!NEAR_ZERO(f, tperp)) return 0;
01164         f = a[i+1] - b[i+1];
01165         if (!NEAR_ZERO(f, tperp)) return 0;
01166         f = a[i+2] - b[i+2];
01167         if (!NEAR_ZERO(f, tperp)) return 0;
01168     }
01169 
01170     /* Check that the scale part of the matrix (ratio) is within the
01171      * perpendicular tolerance.  There is no ratio tolerance so we use
01172      * the tighter of dist or perp.
01173      */
01174     f = a[15] - b[15];
01175     if (!NEAR_ZERO(f, tperp)) return 0;
01176 
01177     return 1;
01178 }
01179 
01180 
01181 /**
01182  * B N _ M A T _ I S _ I D E N T I T Y
01183  *
01184  * This routine is intended for detecting identity matricies read in
01185  * from ascii or binary files, where the numbers are pure ones or
01186  * zeros.  This routine is *not* intended for tolerance-based
01187  * "near-zero" comparisons; as such, it shouldn't be used on matrices
01188  * which are the result of calculation.
01189  *
01190  *
01191  * @return 0 non-identity
01192  * @return 1 a perfect identity matrix
01193  */
01194 int
01195 bn_mat_is_identity(const mat_t m)
01196 {
01197     return ! memcmp(m, bn_mat_identity, sizeof(mat_t));
01198 }
01199 
01200 
01201 /**
01202  * B N _ M A T _ A R B _ R O T
01203  *
01204  * Construct a transformation matrix for rotation about an arbitrary
01205  * axis.  The axis is defined by a point (pt) and a unit direction
01206  * vector (dir).  The angle of rotation is "ang"
01207  *
01208  * FIXME: make tolerance configurable
01209  */
01210 void
01211 bn_mat_arb_rot(mat_t m, const point_t pt, const vect_t dir, const fastf_t ang)
01212 {
01213     mat_t tran1, tran2, rot;
01214     double cos_ang, sin_ang, one_m_cosang;
01215     double n1_sq, n2_sq, n3_sq;
01216     double n1_n2, n1_n3, n2_n3;
01217 
01218     if (ZERO(ang)) {
01219         MAT_IDN(m);
01220         return;
01221     }
01222 
01223     MAT_IDN(tran1);
01224     MAT_IDN(tran2);
01225 
01226     /* construct translation matrix to pt */
01227     tran1[MDX] = (-pt[X]);
01228     tran1[MDY] = (-pt[Y]);
01229     tran1[MDZ] = (-pt[Z]);
01230 
01231     /* construct translation back from pt */
01232     tran2[MDX] = pt[X];
01233     tran2[MDY] = pt[Y];
01234     tran2[MDZ] = pt[Z];
01235 
01236     /* construct rotation matrix */
01237     cos_ang = cos(ang);
01238     sin_ang = sin(ang);
01239     one_m_cosang = 1.0 - cos_ang;
01240     n1_sq = dir[X]*dir[X];
01241     n2_sq = dir[Y]*dir[Y];
01242     n3_sq = dir[Z]*dir[Z];
01243     n1_n2 = dir[X]*dir[Y];
01244     n1_n3 = dir[X]*dir[Z];
01245     n2_n3 = dir[Y]*dir[Z];
01246 
01247     MAT_IDN(rot);
01248     rot[0] = n1_sq + (1.0 - n1_sq)*cos_ang;
01249     rot[1] = n1_n2 * one_m_cosang - dir[Z]*sin_ang;
01250     rot[2] = n1_n3 * one_m_cosang + dir[Y]*sin_ang;
01251 
01252     rot[4] = n1_n2 * one_m_cosang + dir[Z]*sin_ang;
01253     rot[5] = n2_sq + (1.0 - n2_sq)*cos_ang;
01254     rot[6] = n2_n3 * one_m_cosang - dir[X]*sin_ang;
01255 
01256     rot[8] = n1_n3 * one_m_cosang - dir[Y]*sin_ang;
01257     rot[9] = n2_n3 * one_m_cosang + dir[X]*sin_ang;
01258     rot[10] = n3_sq + (1.0 - n3_sq) * cos_ang;
01259 
01260     bn_mat_mul(m, rot, tran1);
01261     bn_mat_mul2(tran2, m);
01262 }
01263 
01264 
01265 /**
01266  * B N _ M A T _ D U P
01267  *
01268  * Return a pointer to a copy of the matrix in dynamically allocated
01269  * memory.
01270  */
01271 matp_t
01272 bn_mat_dup(const mat_t in)
01273 {
01274     matp_t out;
01275 
01276     out = (matp_t) bu_malloc(sizeof(mat_t), "bn_mat_dup");
01277     memcpy((char *)out, (const char *)in, sizeof(mat_t));
01278     return out;
01279 }
01280 
01281 
01282 /**
01283  * B N _ M A T _ C K
01284  *
01285  * Check to ensure that a rotation matrix preserves axis
01286  * perpendicularily.  Note that not all matricies are rotation
01287  * matricies.
01288  *
01289  *
01290  * @return -1 FAIL
01291  * @return  0 OK
01292  */
01293 int
01294 bn_mat_ck(const char *title, const mat_t m)
01295 {
01296     vect_t A, B, C;
01297     fastf_t fx, fy, fz;
01298 
01299     if (!m) return 0;           /* implies identity matrix */
01300 
01301     /* Validate that matrix preserves perpendicularity of axis by
01302      * checking that A.B == 0, B.C == 0, A.C == 0 XXX these vectors
01303      * should just be grabbed out of the matrix
01304      */
01305     VMOVE(A, &m[0]);
01306     VMOVE(B, &m[4]);
01307     VMOVE(C, &m[8]);
01308 
01309     fx = VDOT(A, B);
01310     fy = VDOT(B, C);
01311     fz = VDOT(A, C);
01312 
01313     /* NOTE: this tolerance cannot be any more tight than 0.00001 due
01314      * to default calculation tolernacing used by models.  Matrices
01315      * exported to disk outside of tolerance and will fail import if
01316      * set too restrictive.
01317      */
01318     if (!NEAR_ZERO(fx, 0.00001)
01319         || !NEAR_ZERO(fy, 0.00001)
01320         || !NEAR_ZERO(fz, 0.00001)
01321         || NEAR_ZERO(m[15], VDIVIDE_TOL))
01322     {
01323         if (bu_debug & BU_DEBUG_MATH) {
01324             bu_log("bn_mat_ck(%s):  bad matrix, does not preserve axis perpendicularity.\n  X.Y=%g, Y.Z=%g, X.Z=%g, s=%g\n", title, fx, fy, fz, m[15]);
01325             bn_mat_print("bn_mat_ck() bad matrix", m);
01326         }
01327 
01328         if (bu_debug & (BU_DEBUG_MATH | BU_DEBUG_COREDUMP)) {
01329             bu_debug |= BU_DEBUG_COREDUMP;
01330             bu_bomb("bn_mat_ck() bad matrix\n");
01331         }
01332         return -1;      /* FAIL */
01333     }
01334     return 0;           /* OK */
01335 }
01336 
01337 
01338 /**
01339  * B N _ M A T _ D E T 3
01340  *
01341  * Calculates the determinant of the 3X3 "rotation" part of the passed
01342  * matrix.
01343  */
01344 fastf_t
01345 bn_mat_det3(const mat_t m)
01346 {
01347     register fastf_t sum;
01348 
01349     sum = m[0] * (m[5]*m[10] - m[6]*m[9])
01350         -m[1] * (m[4]*m[10] - m[6]*m[8])
01351         +m[2] * (m[4]*m[9] - m[5]*m[8]);
01352 
01353     return sum;
01354 }
01355 
01356 
01357 /**
01358  * B N _ M A T _ D E T E R M I N A N T
01359  *
01360  * Calculates the determinant of the 4X4 matrix
01361  */
01362 fastf_t
01363 bn_mat_determinant(const mat_t m)
01364 {
01365     fastf_t det[4];
01366     fastf_t sum;
01367 
01368     det[0] = m[5] * (m[10]*m[15] - m[11]*m[14])
01369         -m[6] * (m[ 9]*m[15] - m[11]*m[13])
01370         +m[7] * (m[ 9]*m[14] - m[10]*m[13]);
01371 
01372     det[1] = m[4] * (m[10]*m[15] - m[11]*m[14])
01373         -m[6] * (m[ 8]*m[15] - m[11]*m[12])
01374         +m[7] * (m[ 8]*m[14] - m[10]*m[12]);
01375 
01376     det[2] = m[4] * (m[ 9]*m[15] - m[11]*m[13])
01377         -m[5] * (m[ 8]*m[15] - m[11]*m[12])
01378         +m[7] * (m[ 8]*m[13] - m[ 9]*m[12]);
01379 
01380     det[3] = m[4] * (m[ 9]*m[14] - m[10]*m[13])
01381         -m[5] * (m[ 8]*m[14] - m[10]*m[12])
01382         +m[6] * (m[ 8]*m[13] - m[ 9]*m[12]);
01383 
01384     sum = m[0]*det[0] - m[1]*det[1] + m[2]*det[2] - m[3]*det[3];
01385 
01386     return sum;
01387 
01388 }
01389 
01390 
01391 /**
01392  * B N _ M A T _ I S _ N O N _ U N I F
01393  *
01394  * FIXME: make tolerance configurable
01395  */
01396 int
01397 bn_mat_is_non_unif (const mat_t m)
01398 {
01399     double mag[3];
01400 
01401     mag[0] = MAGSQ(m);
01402     mag[1] = MAGSQ(&m[4]);
01403     mag[2] = MAGSQ(&m[8]);
01404 
01405     if (fabs(1.0 - (mag[1]/mag[0])) > .0005 ||
01406         fabs(1.0 - (mag[2]/mag[0])) > .0005) {
01407 
01408         return 1;
01409     }
01410 
01411     if (!ZERO(m[12]) || !ZERO(m[13]) || !ZERO(m[14]))
01412         return 2;
01413 
01414     return 0;
01415 }
01416 
01417 
01418 /**
01419  * B N _ W R T _ P O I N T _ D I R E C
01420  *
01421  * Given a model-space transformation matrix "change",
01422  * return a matrix which applies the change with-respect-to
01423  * given "point" and "direc".
01424  */
01425 void
01426 bn_wrt_point_direc(mat_t out, const mat_t change, const mat_t in, const point_t point, const vect_t direc)
01427 {
01428     static mat_t t1;
01429     static mat_t pt_to_origin, origin_to_pt;
01430     static mat_t d_to_zaxis, zaxis_to_d;
01431     static vect_t zaxis;
01432 
01433     /* build "point to origin" matrix */
01434     MAT_IDN(pt_to_origin);
01435     MAT_DELTAS_VEC_NEG(pt_to_origin, point);
01436 
01437     /* build "origin to point" matrix */
01438     MAT_IDN(origin_to_pt);
01439     MAT_DELTAS_VEC_NEG(origin_to_pt, point);
01440 
01441     /* build "direc to zaxis" matrix */
01442     VSET(zaxis, 0.0, 0.0, 1.0);
01443     bn_mat_fromto(d_to_zaxis, direc, zaxis);
01444 
01445     /* build "zaxis to direc" matrix */
01446     bn_mat_inv(zaxis_to_d, d_to_zaxis);
01447 
01448     /* apply change matrix...
01449      * t1 = change * d_to_zaxis * pt_to_origin * in
01450      */
01451     bn_mat_mul4(t1, change, d_to_zaxis, pt_to_origin, in);
01452 
01453     /* apply origin_to_pt matrix:
01454      * out = origin_to_pt * zaxis_to_d *
01455      * change * d_to_zaxis * pt_to_origin * in
01456      */
01457     bn_mat_mul3(out, origin_to_pt, zaxis_to_d, t1);
01458 }
01459 
01460 
01461 /** @} */
01462 /*
01463  * Local Variables:
01464  * mode: C
01465  * tab-width: 8
01466  * indent-tabs-mode: t
01467  * c-file-style: "stroustrup"
01468  * End:
01469  * ex: shiftwidth=4 tabstop=8
01470  */
Generated on Tue Dec 11 13:14:27 2012 for LIBBN by  doxygen 1.6.3