2 * The 3D Studio File Format Library
3 * Copyright (C) 1996-2001 by J.E. Hoffmann <je-h@gmx.net>
6 * This program is free software; you can redistribute it and/or modify it
7 * under the terms of the GNU Lesser General Public License as published by
8 * the Free Software Foundation; either version 2.1 of the License, or (at
9 * your option) any later version.
11 * This program is distributed in the hope that it will be useful, but
12 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
13 * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public
14 * License for more details.
16 * You should have received a copy of the GNU Lesser General Public License
17 * along with this program; if not, write to the Free Software Foundation,
18 * Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
20 * $Id: quat.c,v 1.6 2004/11/20 08:31:31 efalk Exp $
23 #include <lib3ds/quat.h>
28 * \defgroup quat Quaternion Mathematics
30 * \author J.E. Hoffmann <je-h@gmx.net>
43 lib3ds_quat_zero(Lib3dsQuat c)
45 c[0]=c[1]=c[2]=c[3]=0.0f;
50 * Set a quaternion to Identity
54 lib3ds_quat_identity(Lib3dsQuat c)
66 lib3ds_quat_copy(Lib3dsQuat dest, Lib3dsQuat src)
76 * Compute a quaternion from axis and angle.
78 * \param c Computed quaternion
79 * \param axis Rotation axis
80 * \param angle Angle of rotation, radians.
85 lib3ds_quat_axis_angle(Lib3dsQuat c, Lib3dsVector axis, Lib3dsFloat angle)
90 l=sqrt(axis[0]*axis[0] + axis[1]*axis[1] + axis[2]*axis[2]);
91 if (l<LIB3DS_EPSILON) {
98 c[0]=(Lib3dsFloat)s*axis[0];
99 c[1]=(Lib3dsFloat)s*axis[1];
100 c[2]=(Lib3dsFloat)s*axis[2];
101 c[3]=(Lib3dsFloat)cos(omega);
107 * Negate a quaternion
112 lib3ds_quat_neg(Lib3dsQuat c)
115 for (i=0; i<4; ++i) {
122 * Compute the absolute value of a quaternion
127 lib3ds_quat_abs(Lib3dsQuat c)
130 for (i=0; i<4; ++i) {
131 c[i]=(Lib3dsFloat)fabs(c[i]);
137 * Compute the conjugate of a quaternion
142 lib3ds_quat_cnj(Lib3dsQuat c)
145 for (i=0; i<3; ++i) {
152 * Multiply two quaternions.
159 lib3ds_quat_mul(Lib3dsQuat c, Lib3dsQuat a, Lib3dsQuat b)
161 c[0]=a[3]*b[0] + a[0]*b[3] + a[1]*b[2] - a[2]*b[1];
162 c[1]=a[3]*b[1] + a[1]*b[3] + a[2]*b[0] - a[0]*b[2];
163 c[2]=a[3]*b[2] + a[2]*b[3] + a[0]*b[1] - a[1]*b[0];
164 c[3]=a[3]*b[3] - a[0]*b[0] - a[1]*b[1] - a[2]*b[2];
169 * Multiply a quaternion by a scalar.
174 lib3ds_quat_scalar(Lib3dsQuat c, Lib3dsFloat k)
177 for (i=0; i<4; ++i) {
184 * Normalize a quaternion.
189 lib3ds_quat_normalize(Lib3dsQuat c)
193 l=sqrt(c[0]*c[0] + c[1]*c[1] + c[2]*c[2] + c[3]*c[3]);
194 if (fabs(l)<LIB3DS_EPSILON) {
201 for (i=0; i<4; ++i) {
202 c[i]=(Lib3dsFloat)(c[i]*m);
209 * Compute the inverse of a quaternion.
214 lib3ds_quat_inv(Lib3dsQuat c)
218 l=sqrt(c[0]*c[0] + c[1]*c[1] + c[2]*c[2] + c[3]*c[3]);
219 if (fabs(l)<LIB3DS_EPSILON) {
225 c[0]=(Lib3dsFloat)(-c[0]*m);
226 c[1]=(Lib3dsFloat)(-c[1]*m);
227 c[2]=(Lib3dsFloat)(-c[2]*m);
228 c[3]=(Lib3dsFloat)(c[3]*m);
234 * Compute the dot-product of a quaternion.
239 lib3ds_quat_dot(Lib3dsQuat a, Lib3dsQuat b)
241 return(a[0]*b[0] + a[1]*b[1] + a[2]*b[2] + a[3]*b[3]);
249 lib3ds_quat_squared(Lib3dsQuat c)
251 return(c[0]*c[0] + c[1]*c[1] + c[2]*c[2] + c[3]*c[3]);
259 lib3ds_quat_length(Lib3dsQuat c)
261 return((Lib3dsFloat)sqrt(c[0]*c[0] + c[1]*c[1] + c[2]*c[2] + c[3]*c[3]));
269 lib3ds_quat_ln(Lib3dsQuat c)
273 s=sqrt(c[0]*c[0] + c[1]*c[1] + c[2]*c[2]);
275 if (fabs(s)<LIB3DS_EPSILON) {
283 for (i=0; i<3; ++i) {
284 c[i]=(Lib3dsFloat)(c[i]*t);
295 lib3ds_quat_ln_dif(Lib3dsQuat c, Lib3dsQuat a, Lib3dsQuat b)
299 lib3ds_quat_copy(invp, a);
300 lib3ds_quat_inv(invp);
301 lib3ds_quat_mul(c, invp, b);
310 lib3ds_quat_exp(Lib3dsQuat c)
312 Lib3dsDouble om,sinom;
314 om=sqrt(c[0]*c[0] + c[1]*c[1] + c[2]*c[2]);
315 if (fabs(om)<LIB3DS_EPSILON) {
323 for (i=0; i<3; ++i) {
324 c[i]=(Lib3dsFloat)(c[i]*sinom);
326 c[3]=(Lib3dsFloat)cos(om);
335 lib3ds_quat_slerp(Lib3dsQuat c, Lib3dsQuat a, Lib3dsQuat b, Lib3dsFloat t)
338 Lib3dsDouble om,sinom;
342 l=a[0]*b[0] + a[1]*b[1] + a[2]*b[2] + a[3]*b[3];
343 if ((1.0+l)>LIB3DS_EPSILON) {
344 if (fabs(l)>1.0f) l/=fabs(l);
347 if (fabs(sinom)>LIB3DS_EPSILON) {
348 sp=sin((1.0f-t)*om)/sinom;
355 c[0]=(Lib3dsFloat)(sp*a[0] + sq*b[0]);
356 c[1]=(Lib3dsFloat)(sp*a[1] + sq*b[1]);
357 c[2]=(Lib3dsFloat)(sp*a[2] + sq*b[2]);
358 c[3]=(Lib3dsFloat)(sp*a[3] + sq*b[3]);
365 sp=sin((1.0-t)*LIB3DS_HALFPI);
366 sq=sin(t*LIB3DS_HALFPI);
367 c[0]=(Lib3dsFloat)(sp*a[0] + sq*q[0]);
368 c[1]=(Lib3dsFloat)(sp*a[1] + sq*q[1]);
369 c[2]=(Lib3dsFloat)(sp*a[2] + sq*q[2]);
370 c[3]=(Lib3dsFloat)(sp*a[3] + sq*q[3]);
379 lib3ds_quat_squad(Lib3dsQuat c, Lib3dsQuat a, Lib3dsQuat p, Lib3dsQuat q,
380 Lib3dsQuat b, Lib3dsFloat t)
385 lib3ds_quat_slerp(ab,a,b,t);
386 lib3ds_quat_slerp(pq,p,q,t);
387 lib3ds_quat_slerp(c,ab,pq,2*t*(1-t));
395 lib3ds_quat_tangent(Lib3dsQuat c, Lib3dsQuat p, Lib3dsQuat q, Lib3dsQuat n)
400 lib3ds_quat_ln_dif(dn, q, n);
401 lib3ds_quat_ln_dif(dp, q, p);
403 for (i=0; i<4; i++) {
404 x[i]=-1.0f/4.0f*(dn[i]+dp[i]);
407 lib3ds_quat_mul(c,q,x);
415 lib3ds_quat_dump(Lib3dsQuat q)
417 printf("%f %f %f %f\n", q[0], q[1], q[2], q[3]);