qmath.c
Go to the documentation of this file.
1 /* Q M A T H . C
3  *
4  * Copyright (c) 2004-2014 United States Government as represented by
5  * the U.S. Army Research Laboratory.
6  *
7  * This library is free software; you can redistribute it and/or
8  * modify it under the terms of the GNU Lesser General Public License
10  *
11  * This library is distributed in the hope that it will be useful, but
12  * WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
14  * Lesser General Public License for more details.
15  *
16  * You should have received a copy of the GNU Lesser General Public
17  * License along with this file; see the file named COPYING for more
18  * information.
19  */
20
22 /** @{ */
23 /** @file libbn/qmath.c
24  *
25  * @brief
26  * Quaternion math routines.
27  *
28  * Unit Quaternions:
29  * Q = [ r, a ] where r = cos(theta/2) = rotation amount
30  * |a| = sin(theta/2) = rotation axis
31  *
32  * If a = 0 we have the reals; if one coord is zero we have
33  * complex numbers (2D rotations).
34  *
35  * [r, a][s, b] = [rs - a.b, rb + sa + axb]
36  *
37  * -1
38  * [r, a] = (r - a) / (r^2 + a.a)
39  *
40  * Powers of quaternions yield incremental rotations,
41  * e.g. Q^3 is rotated three times as far as Q.
42  *
43  * Some operations on quaternions:
44  * -1
45  * [0, P'] = Q [0, P]Q Rotate a point P by quaternion Q
46  * -1 a
47  * slerp(Q, R, a) = Q(Q R) Spherical linear interp: 0 < a < 1
48  *
49  * bisect(P, Q) = (P + Q) / |P + Q| Great circle bisector
50  *
51  * Additions inspired by "Quaternion Calculus For Animation" by Ken Shoemake,
52  * SIGGRAPH '89 course notes for "Math for SIGGRAPH", May 1989.
53  *
54  */
55
56 #include "common.h"
57
58 #include <stdio.h> /* DEBUG need stderr for now... */
59 #include <math.h>
60 #include "vmath.h"
61 #include "bn/qmath.h"
62
63
64 void
65 quat_mat2quat(register fastf_t *quat, register const fastf_t *mat)
66 {
67  fastf_t tr;
68  fastf_t s;
69
70 #define XX 0
71 #define YY 5
72 #define ZZ 10
73 #define MMM(a, b) mat[4*(a)+(b)]
74
75  tr = mat[XX] + mat[YY] + mat[ZZ];
76  if ( tr > 0.0 ) {
77  s = sqrt( tr + 1.0 );
78  quat[W] = s * 0.5;
79  s = 0.5 / s;
80  quat[X] = ( mat[6] - mat[9] ) * s;
81  quat[Y] = ( mat[8] - mat[2] ) * s;
82  quat[Z] = ( mat[1] - mat[4] ) * s;
83  return;
84  }
85
86  /* Find dominant element of primary diagonal */
87  if ( mat[YY] > mat[XX] ) {
88  if ( mat[ZZ] > mat[YY] ) {
89  s = sqrt( MMM(Z, Z) - (MMM(X, X)+MMM(Y, Y)) + 1.0 );
90  quat[Z] = s * 0.5;
91  s = 0.5 / s;
92  quat[W] = (MMM(X, Y) - MMM(Y, X)) * s;
93  quat[X] = (MMM(Z, X) + MMM(X, Z)) * s;
94  quat[Y] = (MMM(Z, Y) + MMM(Y, Z)) * s;
95  } else {
96  s = sqrt( MMM(Y, Y) - (MMM(Z, Z)+MMM(X, X)) + 1.0 );
97  quat[Y] = s * 0.5;
98  s = 0.5 / s;
99  quat[W] = (MMM(Z, X) - MMM(X, Z)) * s;
100  quat[Z] = (MMM(Y, Z) + MMM(Z, Y)) * s;
101  quat[X] = (MMM(Y, X) + MMM(X, Y)) * s;
102  }
103  } else {
104  if ( mat[ZZ] > mat[XX] ) {
105  s = sqrt( MMM(Z, Z) - (MMM(X, X)+MMM(Y, Y)) + 1.0 );
106  quat[Z] = s * 0.5;
107  s = 0.5 / s;
108  quat[W] = (MMM(X, Y) - MMM(Y, X)) * s;
109  quat[X] = (MMM(Z, X) + MMM(X, Z)) * s;
110  quat[Y] = (MMM(Z, Y) + MMM(Y, Z)) * s;
111  } else {
112  s = sqrt( MMM(X, X) - (MMM(Y, Y)+MMM(Z, Z)) + 1.0 );
113  quat[X] = s * 0.5;
114  s = 0.5 / s;
115  quat[W] = (MMM(Y, Z) - MMM(Z, Y)) * s;
116  quat[Y] = (MMM(X, Y) + MMM(Y, X)) * s;
117  quat[Z] = (MMM(X, Z) + MMM(Z, X)) * s;
118  }
119  }
120 #undef MMM
121 }
122
123
124 void
125 quat_quat2mat(register fastf_t *mat, register const fastf_t *quat)
126 {
127  quat_t q;
128
129  QMOVE( q, quat ); /* private copy */
130  QUNITIZE( q );
131
132  mat[0] = 1.0 - 2.0*q[Y]*q[Y] - 2.0*q[Z]*q[Z];
133  mat[1] = 2.0*q[X]*q[Y] + 2.0*q[W]*q[Z];
134  mat[2] = 2.0*q[X]*q[Z] - 2.0*q[W]*q[Y];
135  mat[3] = 0.0;
136  mat[4] = 2.0*q[X]*q[Y] - 2.0*q[W]*q[Z];
137  mat[5] = 1.0 - 2.0*q[X]*q[X] - 2.0*q[Z]*q[Z];
138  mat[6] = 2.0*q[Y]*q[Z] + 2.0*q[W]*q[X];
139  mat[7] = 0.0;
140  mat[8] = 2.0*q[X]*q[Z] + 2.0*q[W]*q[Y];
141  mat[9] = 2.0*q[Y]*q[Z] - 2.0*q[W]*q[X];
142  mat[10] = 1.0 - 2.0*q[X]*q[X] - 2.0*q[Y]*q[Y];
143  mat[11] = 0.0;
144  mat[12] = 0.0;
145  mat[13] = 0.0;
146  mat[14] = 0.0;
147  mat[15] = 1.0;
148 }
149
150
151 double
152 quat_distance(const fastf_t *q1, const fastf_t *q2)
153 {
154  quat_t qtemp;
155
156  QSUB2( qtemp, q1, q2 );
157  return QMAGNITUDE( qtemp );
158 }
159
160
161 void
162 quat_double(fastf_t *qout, const fastf_t *q1, const fastf_t *q2)
163 {
164  quat_t qtemp;
165  double scale;
166
167  scale = 2.0 * QDOT( q1, q2 );
168  QSCALE( qtemp, q2, scale );
169  QSUB2( qout, qtemp, q1 );
170  QUNITIZE( qout );
171 }
172
173
174 void
175 quat_bisect(fastf_t *qout, const fastf_t *q1, const fastf_t *q2)
176 {
177  QADD2( qout, q1, q2 );
178  QUNITIZE( qout );
179 }
180
181
182 void
183 quat_slerp(fastf_t *qout, const fastf_t *q1, const fastf_t *q2, double f)
184 {
185  double omega;
186  double cos_omega;
187  double invsin;
188  register double s1, s2;
189
190  cos_omega = QDOT( q1, q2 );
191  if ( (1.0 + cos_omega) > 1.0e-5 ) {
192  /* cos_omega > -0.99999 */
193  if ( (1.0 - cos_omega) > 1.0e-5 ) {
194  /* usual case */
195  omega = acos(cos_omega); /* XXX atan2? */
196  invsin = 1.0 / sin(omega);
197  s1 = sin( (1.0-f)*omega ) * invsin;
198  s2 = sin( f*omega ) * invsin;
199  } else {
200  /*
201  * cos_omega > 0.99999
202  * The ends are very close to each other,
203  * use linear interpolation, to avoid divide-by-zero
204  */
205  s1 = 1.0 - f;
206  s2 = f;
207  }
208  QBLEND2( qout, s1, q1, s2, q2 );
209  } else {
210  /*
211  * cos_omega == -1, omega = M_PI.
212  * The ends are nearly opposite, 180 degrees (M_PI) apart.
213  */
214  /* (I have no idea what permuting the elements accomplishes,
215  * perhaps it creates a perpendicular? */
216  qout[X] = -q1[Y];
217  qout[Y] = q1[X];
218  qout[Z] = -q1[W];
219  s1 = sin( (0.5-f) * M_PI );
220  s2 = sin( f * M_PI );
221  VBLEND2( qout, s1, q1, s2, qout );
222  qout[W] = q1[Z];
223  }
224 }
225
226
227 void
228 quat_sberp(fastf_t *qout, const fastf_t *q1, const fastf_t *qa, const fastf_t *qb, const fastf_t *q2, double f)
229 {
230  quat_t p1, p2, p3, p4, p5;
231
232  /* Interp down the three segments */
233  quat_slerp( p1, q1, qa, f );
234  quat_slerp( p2, qa, qb, f );
235  quat_slerp( p3, qb, q2, f );
236
237  /* Interp down the resulting two */
238  quat_slerp( p4, p1, p2, f );
239  quat_slerp( p5, p2, p3, f );
240
241  /* Interp this segment for final quaternion */
242  quat_slerp( qout, p4, p5, f );
243 }
244
245
246 void
248 {
249  quat_t qtemp;
250  double d1, d2;
251
252  QSCALE( qtemp, q1, -1.0 );
253  d1 = quat_distance( q1, q2 );
254  d2 = quat_distance( qtemp, q2 );
255
256  /* Choose smallest distance */
257  if ( d2 < d1 ) {
258  QMOVE( q1, qtemp );
259  }
260 }
261
262
263 /* DEBUG ROUTINE */
264 void
265 quat_print(const char *title, const fastf_t *quat)
266 {
267  int i;
268  vect_t axis;
269
270  fprintf( stderr, "QUATERNION: %s\n", title );
271  for ( i = 0; i < 4; i++ )
272  fprintf( stderr, "%8f ", quat[i] );
273  fprintf( stderr, "\n" );
274
275  fprintf( stderr, "rot_angle = %8f deg", RAD2DEG * 2.0 * acos( quat[W] ) );
276  VMOVE( axis, quat );
277  VUNITIZE( axis );
278  fprintf( stderr, ", Axis = (%f, %f, %f)\n",
279  axis[X], axis[Y], axis[Z] );
280 }
281
282
283 void
285 {
286  fastf_t theta;
287  fastf_t scale;
288
289  if ( (theta = MAGNITUDE( in )) > VDIVIDE_TOL )
290  scale = sin(theta)/theta;
291  else
292  scale = 1.0;
293
294  VSCALE( out, in, scale );
295  out[W] = cos(theta);
296 }
297
298
299 void
301 {
302  fastf_t theta;
303  fastf_t scale;
304
305  if ( (scale = MAGNITUDE(in)) > VDIVIDE_TOL ) {
306  theta = atan2( scale, in[W] );
307  scale = theta/scale;
308  }
309
310  VSCALE( out, in, scale );
311  out[W] = 0.0;
312 }
313
314 /** @} */
315 /*
316  * Local Variables:
317  * mode: C
318  * tab-width: 8
319  * indent-tabs-mode: t
320  * c-file-style: "stroustrup"
321  * End:
322  * ex: shiftwidth=4 tabstop=8
323  */
void quat_print(const char *title, const fastf_t *quat)
Definition: qmath.c:265
void quat_slerp(fastf_t *qout, const fastf_t *q1, const fastf_t *q2, double f)
Definition: qmath.c:183
if lu s
Definition: nmg_mod.c:3860
void quat_exp(fastf_t *out, const fastf_t *in)
Definition: qmath.c:284
#define M_PI
Definition: fft.h:35
#define MMM(a, b)
#define YY
Definition: color.c:49
double quat_distance(const fastf_t *q1, const fastf_t *q2)
Definition: qmath.c:152
#define ZZ
void quat_make_nearest(fastf_t *q1, const fastf_t *q2)
Definition: qmath.c:247
void quat_log(fastf_t *out, const fastf_t *in)
Definition: qmath.c:300
goto out
Definition: nmg_mod.c:3846
void quat_double(fastf_t *qout, const fastf_t *q1, const fastf_t *q2)
Definition: qmath.c:162
void quat_mat2quat(register fastf_t *quat, register const fastf_t *mat)
Definition: qmath.c:65
#define XX
axis
Definition: color.c:48
Definition: color.c:51
void quat_quat2mat(register fastf_t *mat, register const fastf_t *quat)
Definition: qmath.c:125
double fastf_t
Definition: defines.h:300
void quat_bisect(fastf_t *qout, const fastf_t *q1, const fastf_t *q2)
Definition: qmath.c:175
Definition: color.c:50
void quat_sberp(fastf_t *qout, const fastf_t *q1, const fastf_t *qa, const fastf_t *qb, const fastf_t *q2, double f)
Definition: qmath.c:228