qmath.c

Go to the documentation of this file.
00001 /*                         Q M A T H . C
00002  * BRL-CAD
00003  *
00004  * Copyright (c) 2004-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/qmath.c
00023  *
00024  * @brief
00025  *  Quaternion math routines.
00026  *
00027  *  Unit Quaternions:
00028  *      Q = [ r, a ]    where r = cos(theta/2) = rotation amount
00029  *                          |a| = sin(theta/2) = rotation axis
00030  *
00031  *      If a = 0 we have the reals; if one coord is zero we have
00032  *       complex numbers (2D rotations).
00033  *
00034  *  [r, a][s, b] = [rs - a.b, rb + sa + axb]
00035  *
00036  *       -1
00037  *  [r, a]   = (r - a) / (r^2 + a.a)
00038  *
00039  *  Powers of quaternions yield incremental rotations,
00040  *   e.g. Q^3 is rotated three times as far as Q.
00041  *
00042  *  Some operations on quaternions:
00043  *            -1
00044  *   [0, P'] = Q  [0, P]Q               Rotate a point P by quaternion Q
00045  *                     -1  a
00046  *   slerp(Q, R, a) = Q(Q  R)   Spherical linear interp: 0 < a < 1
00047  *
00048  *   bisect(P, Q) = (P + Q) / |P + Q|   Great circle bisector
00049  *
00050  *  Additions inspired by "Quaternion Calculus For Animation" by Ken Shoemake,
00051  *  SIGGRAPH '89 course notes for "Math for SIGGRAPH", May 1989.
00052  *
00053  */
00054 
00055 #include "common.h"
00056 
00057 #include <stdio.h>              /* DEBUG need stderr for now... */
00058 #include <math.h>
00059 #include "bu.h"
00060 #include "vmath.h"
00061 #include "bn.h"
00062 
00063 
00064 /**
00065  *                      Q U A T _ M A T 2 Q U A T
00066  *@brief
00067  *  Convert Matrix to Quaternion.
00068  */
00069 void
00070 quat_mat2quat(register fastf_t *quat, register const fastf_t *mat)
00071 {
00072     fastf_t             tr;
00073     fastf_t     s;
00074 
00075 #define XX      0
00076 #define YY      5
00077 #define ZZ      10
00078 #define MMM(a, b)               mat[4*(a)+(b)]
00079 
00080     tr = mat[XX] + mat[YY] + mat[ZZ];
00081     if ( tr > 0.0 )  {
00082         s = sqrt( tr + 1.0 );
00083         quat[W] = s * 0.5;
00084         s = 0.5 / s;
00085         quat[X] = ( mat[6] - mat[9] ) * s;
00086         quat[Y] = ( mat[8] - mat[2] ) * s;
00087         quat[Z] = ( mat[1] - mat[4] ) * s;
00088         return;
00089     }
00090 
00091     /* Find dominant element of primary diagonal */
00092     if ( mat[YY] > mat[XX] )  {
00093         if ( mat[ZZ] > mat[YY] )  {
00094             s = sqrt( MMM(Z, Z) - (MMM(X, X)+MMM(Y, Y)) + 1.0 );
00095             quat[Z] = s * 0.5;
00096             s = 0.5 / s;
00097             quat[W] = (MMM(X, Y) - MMM(Y, X)) * s;
00098             quat[X] = (MMM(Z, X) + MMM(X, Z)) * s;
00099             quat[Y] = (MMM(Z, Y) + MMM(Y, Z)) * s;
00100         } else {
00101             s = sqrt( MMM(Y, Y) - (MMM(Z, Z)+MMM(X, X)) + 1.0 );
00102             quat[Y] = s * 0.5;
00103             s = 0.5 / s;
00104             quat[W] = (MMM(Z, X) - MMM(X, Z)) * s;
00105             quat[Z] = (MMM(Y, Z) + MMM(Z, Y)) * s;
00106             quat[X] = (MMM(Y, X) + MMM(X, Y)) * s;
00107         }
00108     } else {
00109         if ( mat[ZZ] > mat[XX] )  {
00110             s = sqrt( MMM(Z, Z) - (MMM(X, X)+MMM(Y, Y)) + 1.0 );
00111             quat[Z] = s * 0.5;
00112             s = 0.5 / s;
00113             quat[W] = (MMM(X, Y) - MMM(Y, X)) * s;
00114             quat[X] = (MMM(Z, X) + MMM(X, Z)) * s;
00115             quat[Y] = (MMM(Z, Y) + MMM(Y, Z)) * s;
00116         } else {
00117             s = sqrt( MMM(X, X) - (MMM(Y, Y)+MMM(Z, Z)) + 1.0 );
00118             quat[X] = s * 0.5;
00119             s = 0.5 / s;
00120             quat[W] = (MMM(Y, Z) - MMM(Z, Y)) * s;
00121             quat[Y] = (MMM(X, Y) + MMM(Y, X)) * s;
00122             quat[Z] = (MMM(X, Z) + MMM(Z, X)) * s;
00123         }
00124     }
00125 #undef MMM
00126 }
00127 
00128 /**
00129  *                      Q U A T _ Q U A T 2 M A T
00130  *@brief
00131  *  Convert Quaternion to Matrix.
00132  *
00133  * NB: This only works for UNIT quaternions.  We may get imaginary results
00134  *   otherwise.  We should normalize first (still yields same rotation).
00135  */
00136 void
00137 quat_quat2mat(register fastf_t *mat, register const fastf_t *quat)
00138 {
00139     quat_t      q;
00140 
00141     QMOVE( q, quat );   /* private copy */
00142     QUNITIZE( q );
00143 
00144     mat[0] = 1.0 - 2.0*q[Y]*q[Y] - 2.0*q[Z]*q[Z];
00145     mat[1] = 2.0*q[X]*q[Y] + 2.0*q[W]*q[Z];
00146     mat[2] = 2.0*q[X]*q[Z] - 2.0*q[W]*q[Y];
00147     mat[3] = 0.0;
00148     mat[4] = 2.0*q[X]*q[Y] - 2.0*q[W]*q[Z];
00149     mat[5] = 1.0 - 2.0*q[X]*q[X] - 2.0*q[Z]*q[Z];
00150     mat[6] = 2.0*q[Y]*q[Z] + 2.0*q[W]*q[X];
00151     mat[7] = 0.0;
00152     mat[8] = 2.0*q[X]*q[Z] + 2.0*q[W]*q[Y];
00153     mat[9] = 2.0*q[Y]*q[Z] - 2.0*q[W]*q[X];
00154     mat[10] = 1.0 - 2.0*q[X]*q[X] - 2.0*q[Y]*q[Y];
00155     mat[11] = 0.0;
00156     mat[12] = 0.0;
00157     mat[13] = 0.0;
00158     mat[14] = 0.0;
00159     mat[15] = 1.0;
00160 }
00161 
00162 /**
00163  *                      Q U A T _ D I S T A N C E
00164  *@brief
00165  * Gives the euclidean distance between two quaternions.
00166  */
00167 double
00168 quat_distance(const fastf_t *q1, const fastf_t *q2)
00169 {
00170     quat_t      qtemp;
00171 
00172     QSUB2( qtemp, q1, q2 );
00173     return QMAGNITUDE( qtemp );
00174 }
00175 
00176 /**
00177  *                      Q U A T _ D O U B L E
00178  *@brief
00179  * Gives the quaternion point representing twice the rotation
00180  *   from q1 to q2.
00181  *   Needed for patching Bezier curves together.
00182  *   A rather poor name admittedly.
00183  */
00184 void
00185 quat_double(fastf_t *qout, const fastf_t *q1, const fastf_t *q2)
00186 {
00187     quat_t      qtemp;
00188     double      scale;
00189 
00190     scale = 2.0 * QDOT( q1, q2 );
00191     QSCALE( qtemp, q2, scale );
00192     QSUB2( qout, qtemp, q1 );
00193     QUNITIZE( qout );
00194 }
00195 
00196 /**
00197  *                      Q U A T _ B I S E C T
00198  *@brief
00199  * Gives the bisector of quaternions q1 and q2.
00200  * (Could be done with quat_slerp and factor 0.5)
00201  * [I believe they must be unit quaternions this to work]
00202  */
00203 void
00204 quat_bisect(fastf_t *qout, const fastf_t *q1, const fastf_t *q2)
00205 {
00206     QADD2( qout, q1, q2 );
00207     QUNITIZE( qout );
00208 }
00209 
00210 /**
00211  *                      Q U A T _ S L E R P
00212  *@brief
00213  * Do Spherical Linear Interpolation between two unit quaternions
00214  *  by the given factor.
00215  *
00216  * As f goes from 0 to 1, qout goes from q1 to q2.
00217  * Code based on code by Ken Shoemake
00218  */
00219 void
00220 quat_slerp(fastf_t *qout, const fastf_t *q1, const fastf_t *q2, double f)
00221 {
00222     double              omega;
00223     double              cos_omega;
00224     double              invsin;
00225     register double     s1, s2;
00226 
00227     cos_omega = QDOT( q1, q2 );
00228     if ( (1.0 + cos_omega) > 1.0e-5 )  {
00229         /* cos_omega > -0.99999 */
00230         if ( (1.0 - cos_omega) > 1.0e-5 )  {
00231             /* usual case */
00232             omega = acos(cos_omega);    /* XXX atan2? */
00233             invsin = 1.0 / sin(omega);
00234             s1 = sin( (1.0-f)*omega ) * invsin;
00235             s2 = sin( f*omega ) * invsin;
00236         } else {
00237             /*
00238              *  cos_omega > 0.99999
00239              * The ends are very close to each other,
00240              * use linear interpolation, to avoid divide-by-zero
00241              */
00242             s1 = 1.0 - f;
00243             s2 = f;
00244         }
00245         QBLEND2( qout, s1, q1, s2, q2 );
00246     } else {
00247         /*
00248          *  cos_omega == -1, omega = M_PI.
00249          *  The ends are nearly opposite, 180 degrees (M_PI) apart.
00250          */
00251         /* (I have no idea what permuting the elements accomplishes,
00252          * perhaps it creates a perpendicular? */
00253         qout[X] = -q1[Y];
00254         qout[Y] =  q1[X];
00255         qout[Z] = -q1[W];
00256         s1 = sin( (0.5-f) * M_PI );
00257         s2 = sin( f * M_PI );
00258         VBLEND2( qout, s1, q1, s2, qout );
00259         qout[W] =  q1[Z];
00260     }
00261 }
00262 
00263 /**
00264  *                      Q U A T _ S B E R P
00265  *@brief
00266  * Spherical Bezier Interpolate between four quaternions by amount f.
00267  * These are intended to be used as start and stop quaternions along
00268  *   with two control quaternions chosen to match spline segments with
00269  *   first order continuity.
00270  *
00271  *  Uses the method of successive bisection.
00272  */
00273 void
00274 quat_sberp(fastf_t *qout, const fastf_t *q1, const fastf_t *qa, const fastf_t *qb, const fastf_t *q2, double f)
00275 {
00276     quat_t      p1, p2, p3, p4, p5;
00277 
00278     /* Interp down the three segments */
00279     quat_slerp( p1, q1, qa, f );
00280     quat_slerp( p2, qa, qb, f );
00281     quat_slerp( p3, qb, q2, f );
00282 
00283     /* Interp down the resulting two */
00284     quat_slerp( p4, p1, p2, f );
00285     quat_slerp( p5, p2, p3, f );
00286 
00287     /* Interp this segment for final quaternion */
00288     quat_slerp( qout, p4, p5, f );
00289 }
00290 
00291 /**
00292  *                      Q U A T _ M A K E _ N E A R E S T
00293  *@brief
00294  *  Set the quaternion q1 to the quaternion which yields the
00295  *   smallest rotation from q2 (of the two versions of q1 which
00296  *   produce the same orientation).
00297  *
00298  * Note that smallest euclidian distance implies smallest great
00299  *   circle distance as well (since surface is convex).
00300  */
00301 void
00302 quat_make_nearest(fastf_t *q1, const fastf_t *q2)
00303 {
00304     quat_t      qtemp;
00305     double      d1, d2;
00306 
00307     QSCALE( qtemp, q1, -1.0 );
00308     d1 = quat_distance( q1, q2 );
00309     d2 = quat_distance( qtemp, q2 );
00310 
00311     /* Choose smallest distance */
00312     if ( d2 < d1 ) {
00313         QMOVE( q1, qtemp );
00314     }
00315 }
00316 
00317 /**
00318  *                      Q U A T _ P R I N T
00319  */
00320 /* DEBUG ROUTINE */
00321 void
00322 quat_print(const char *title, const fastf_t *quat)
00323 {
00324     int i;
00325     vect_t      axis;
00326 
00327     fprintf( stderr, "QUATERNION: %s\n", title );
00328     for ( i = 0; i < 4; i++ )
00329         fprintf( stderr, "%8f  ", quat[i] );
00330     fprintf( stderr, "\n" );
00331 
00332     fprintf( stderr, "rot_angle = %8f deg", RAD2DEG * 2.0 * acos( quat[W] ) );
00333     VMOVE( axis, quat );
00334     VUNITIZE( axis );
00335     fprintf( stderr, ", Axis = (%f, %f, %f)\n",
00336              axis[X], axis[Y], axis[Z] );
00337 }
00338 
00339 /**
00340  *                      Q U A T _ E X P
00341  *@brief
00342  *  Exponentiate a quaternion, assuming that the scalar part is 0.
00343  *  Code by Ken Shoemake.
00344  */
00345 void
00346 quat_exp(fastf_t *out, const fastf_t *in)
00347 {
00348     fastf_t     theta;
00349     fastf_t     scale;
00350 
00351     if ( (theta = MAGNITUDE( in )) > VDIVIDE_TOL )
00352         scale = sin(theta)/theta;
00353     else
00354         scale = 1.0;
00355 
00356     VSCALE( out, in, scale );
00357     out[W] = cos(theta);
00358 }
00359 
00360 /**
00361  *                      Q U A T _ L O G
00362  *@brief
00363  *  Take the natural logarithm of a unit quaternion.
00364  *  Code by Ken Shoemake.
00365  */
00366 void
00367 quat_log(fastf_t *out, const fastf_t *in)
00368 {
00369     fastf_t     theta;
00370     fastf_t     scale;
00371 
00372     if ( (scale = MAGNITUDE(in)) > VDIVIDE_TOL )  {
00373         theta = atan2( scale, in[W] );
00374         scale = theta/scale;
00375     }
00376 
00377     VSCALE( out, in, scale );
00378     out[W] = 0.0;
00379 }
00380 
00381 /** @} */
00382 /*
00383  * Local Variables:
00384  * mode: C
00385  * tab-width: 8
00386  * indent-tabs-mode: t
00387  * c-file-style: "stroustrup"
00388  * End:
00389  * ex: shiftwidth=4 tabstop=8
00390  */
Generated on Tue Dec 11 13:14:28 2012 for LIBBN by  doxygen 1.6.3