root/OpenSceneGraph/trunk/src/osgPlugins/lib3ds/quat.cpp @ 1563

Revision 1563, 6.3 kB (checked in by robert, 12 years ago)

Added an #ifdef to osgGLUT/Window.cpp to handle the case of compiling against
older versions of GLUT.

  • Property svn:eol-style set to native
  • Property svn:keywords set to Author Date Id Revision
RevLine 
[8]1/*
2 * The 3D Studio File Format Library
3 * Copyright (C) 1996-2001 by J.E. Hoffmann <je-h@gmx.net>
4 * All rights reserved.
5 *
6 * This program is  free  software;  you can redistribute it and/or modify it
[151]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
[8]9 * your option) any later version.
10 *
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
[151]13 * or  FITNESS FOR A  PARTICULAR PURPOSE.  See the  GNU Lesser General Public 
[8]14 * License for more details.
15 *
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.
19 *
20 * $Id$
21 */
22#define LIB3DS_EXPORT
[1563]23#include "quat.h"
[8]24#include <math.h>
25
[151]26
[8]27/*!
28 * \defgroup quat Quaternion Mathematics
29 *
30 * \author J.E. Hoffmann <je-h@gmx.net>
31 */
32/*!
33 * \typedef Lib3dsQuat
34 *   \ingroup quat
35 */
36
[151]37
[8]38/*!
39 * \ingroup quat
40 */
41void
42lib3ds_quat_zero(Lib3dsQuat c)
43{
[151]44  c[0]=c[1]=c[2]=c[3]=0.0f;
[8]45}
46
47
48/*!
49 * \ingroup quat
50 */
51void
52lib3ds_quat_identity(Lib3dsQuat c)
53{
[151]54  c[0]=c[1]=c[2]=0.0f;
55  c[3]=1.0f;
[8]56}
57
58
59/*!
60 * \ingroup quat
61 */
[151]62void 
[8]63lib3ds_quat_copy(Lib3dsQuat dest, Lib3dsQuat src)
64{
[151]65  int i;
66  for (i=0; i<4; ++i) {
67    dest[i]=src[i];
68  }
[8]69}
70
71
72/*!
73 * \ingroup quat
74 */
75void
76lib3ds_quat_axis_angle(Lib3dsQuat c, Lib3dsVector axis, Lib3dsFloat angle)
77{
[151]78  Lib3dsDouble omega,s;
79  Lib3dsDouble l;
[8]80
[151]81  l=sqrt(axis[0]*axis[0] + axis[1]*axis[1] + axis[2]*axis[2]);
82  if (l<LIB3DS_EPSILON) {
83    c[0]=c[1]=c[2]=0.0f;
84    c[3]=1.0f;
85  }
86  else {
87    omega=-0.5*angle;
88    s=sin(omega)/l;
89    c[0]=(Lib3dsFloat)s*axis[0];
90    c[1]=(Lib3dsFloat)s*axis[1];
91    c[2]=(Lib3dsFloat)s*axis[2];
92    c[3]=(Lib3dsFloat)cos(omega);
93  }
[8]94}
95
96
97/*!
98 * \ingroup quat
99 */
100void
101lib3ds_quat_neg(Lib3dsQuat c)
102{
[151]103  int i;
104  for (i=0; i<4; ++i) {
105    c[i]=-c[i];
106  }
[8]107}
108
109
110/*!
111 * \ingroup quat
112 */
113void
114lib3ds_quat_abs(Lib3dsQuat c)
115{
[151]116  int i;
117  for (i=0; i<4; ++i) {
118    c[i]=(Lib3dsFloat)fabs(c[i]);
119  }
[8]120}
121
122
123/*!
124 * \ingroup quat
125 */
126void
127lib3ds_quat_cnj(Lib3dsQuat c)
128{
[151]129  int i;
130  for (i=0; i<3; ++i) {
131    c[i]=-c[i];
132  }
[8]133}
134
135
136/*!
137 * \ingroup quat
138 */
139void
140lib3ds_quat_mul(Lib3dsQuat c, Lib3dsQuat a, Lib3dsQuat b)
141{
[151]142  c[0]=a[3]*b[0] + a[0]*b[3] + a[1]*b[2] - a[2]*b[1];
143  c[1]=a[3]*b[1] + a[1]*b[3] + a[2]*b[0] - a[0]*b[2];
144  c[2]=a[3]*b[2] + a[2]*b[3] + a[0]*b[1] - a[1]*b[0];
145  c[3]=a[3]*b[3] - a[0]*b[0] - a[1]*b[1] - a[2]*b[2];
[8]146}
147
148
149/*!
150 * \ingroup quat
151 */
152void
153lib3ds_quat_scalar(Lib3dsQuat c, Lib3dsFloat k)
154{
[151]155  int i;
156  for (i=0; i<4; ++i) {
157    c[i]*=k;
158  }
[8]159}
160
161
162/*!
163 * \ingroup quat
164 */
165void
166lib3ds_quat_normalize(Lib3dsQuat c)
167{
[151]168  Lib3dsDouble l,m;
[8]169
[151]170  l=sqrt(c[0]*c[0] + c[1]*c[1] + c[2]*c[2] + c[3]*c[3]);
171  if (fabs(l)<LIB3DS_EPSILON) {
172    c[0]=c[1]=c[2]=0.0f;
173    c[3]=1.0f;
174  }
175  else { 
176    int i;
177    m=1.0f/l;
178    for (i=0; i<4; ++i) {
179      c[i]=(Lib3dsFloat)(c[i]*m);
[8]180    }
[151]181  }
[8]182}
183
184
185/*!
186 * \ingroup quat
187 */
188void
189lib3ds_quat_inv(Lib3dsQuat c)
190{
[151]191  Lib3dsDouble l,m;
[8]192
[151]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) {
195    c[0]=c[1]=c[2]=0.0f;
196    c[3]=1.0f;
197  }
198  else { 
199    m=1.0f/l;
200    c[0]=(Lib3dsFloat)(-c[0]*m);
201    c[1]=(Lib3dsFloat)(-c[1]*m);
202    c[2]=(Lib3dsFloat)(-c[2]*m);
203    c[3]=(Lib3dsFloat)(c[3]*m);
204  }
[8]205}
206
207
208/*!
209 * \ingroup quat
210 */
211Lib3dsFloat
212lib3ds_quat_dot(Lib3dsQuat a, Lib3dsQuat b)
213{
[151]214  return(a[0]*b[0] + a[1]*b[1] + a[2]*b[2] + a[3]*b[3]);
[8]215}
216
217
218/*!
219 * \ingroup quat
220 */
221Lib3dsFloat
222lib3ds_quat_squared(Lib3dsQuat c)
223{
[151]224  return(c[0]*c[0] + c[1]*c[1] + c[2]*c[2] + c[3]*c[3]);
[8]225}
226
227
228/*!
229 * \ingroup quat
230 */
231Lib3dsFloat
232lib3ds_quat_length(Lib3dsQuat c)
233{
[151]234  return((Lib3dsFloat)sqrt(c[0]*c[0] + c[1]*c[1] + c[2]*c[2] + c[3]*c[3]));
[8]235}
236
237
238/*!
239 * \ingroup quat
240 */
241void
242lib3ds_quat_ln(Lib3dsQuat c)
243{
[151]244  Lib3dsDouble om,s,t;
[8]245
[151]246  s=sqrt(c[0]*c[0] + c[1]*c[1] + c[2]*c[2]);
[202]247  om=atan2(s,(Lib3dsDouble)c[3]);
[151]248  if (fabs(s)<LIB3DS_EPSILON) {
249    t=0.0f;
250  }
251  else {
252    t=om/s;
253  }
254  {
255    int i;
256    for (i=0; i<3; ++i) {
257      c[i]=(Lib3dsFloat)(c[i]*t);
[8]258    }
[151]259    c[3]=0.0f;
260  }
[8]261}
262
263
264/*!
265 * \ingroup quat
266 */
267void
268lib3ds_quat_ln_dif(Lib3dsQuat c, Lib3dsQuat a, Lib3dsQuat b)
269{
[151]270  Lib3dsQuat invp;
[8]271
[151]272  lib3ds_quat_copy(invp, a);
273  lib3ds_quat_inv(invp);
274  lib3ds_quat_mul(c, invp, b);
275  lib3ds_quat_ln(c);
[8]276}
277
278
279/*!
280 * \ingroup quat
281 */
282void
283lib3ds_quat_exp(Lib3dsQuat c)
284{
[151]285  Lib3dsDouble om,sinom;
[8]286
[151]287  om=sqrt(c[0]*c[0] + c[1]*c[1] + c[2]*c[2]);
288  if (fabs(om)<LIB3DS_EPSILON) {
289    sinom=1.0f;
290  }
291  else {
292    sinom=sin(om)/om;
293  }
294  {
295    int i;
296    for (i=0; i<3; ++i) {
297      c[i]=(Lib3dsFloat)(c[i]*sinom);
[8]298    }
[151]299    c[3]=(Lib3dsFloat)cos(om);
300  }
[8]301}
302
303
304/*!
305 * \ingroup quat
306 */
307void
308lib3ds_quat_slerp(Lib3dsQuat c, Lib3dsQuat a, Lib3dsQuat b, Lib3dsFloat t)
309{
[151]310  Lib3dsDouble l;
311  Lib3dsDouble om,sinom;
312  Lib3dsDouble sp,sq;
313  Lib3dsQuat q;
[8]314
[151]315  l=a[0]*b[0] + a[1]*b[1] + a[2]*b[2] + a[3]*b[3];
316  if ((1.0+l)>LIB3DS_EPSILON) {
317    if (fabs(l)>1.0f) l/=fabs(l);
318    om=acos(l);
319    sinom=sin(om);
320    if (fabs(sinom)>LIB3DS_EPSILON) {
321      sp=sin((1.0f-t)*om)/sinom;
322      sq=sin(t*om)/sinom;
[8]323    }
[151]324    else {
325      sp=1.0f-t;
326      sq=t;
[8]327    }
[151]328    c[0]=(Lib3dsFloat)(sp*a[0] + sq*b[0]);
329    c[1]=(Lib3dsFloat)(sp*a[1] + sq*b[1]);
330    c[2]=(Lib3dsFloat)(sp*a[2] + sq*b[2]);
331    c[3]=(Lib3dsFloat)(sp*a[3] + sq*b[3]);
332  }
333  else {
334    q[0]=-a[1];
335    q[1]=a[0];
336    q[2]=-a[3];
337    q[3]=a[2];
338    sp=sin((1.0-t)*LIB3DS_HALFPI);
339    sq=sin(t*LIB3DS_HALFPI);
340    c[0]=(Lib3dsFloat)(sp*a[0] + sq*q[0]);
341    c[1]=(Lib3dsFloat)(sp*a[1] + sq*q[1]);
342    c[2]=(Lib3dsFloat)(sp*a[2] + sq*q[2]);
343    c[3]=(Lib3dsFloat)(sp*a[3] + sq*q[3]);
344  }
[8]345}
346
347
348/*!
349 * \ingroup quat
350 */
351void
352lib3ds_quat_squad(Lib3dsQuat c, Lib3dsQuat a, Lib3dsQuat p, Lib3dsQuat q,
[151]353  Lib3dsQuat b, Lib3dsFloat t)
[8]354{
[151]355  Lib3dsQuat ab;
356  Lib3dsQuat pq;
[8]357
[151]358  lib3ds_quat_slerp(ab,a,b,t);
359  lib3ds_quat_slerp(pq,p,q,t);
360  lib3ds_quat_slerp(c,ab,pq,2*t*(1-t));
[8]361}
362
363
364/*!
365 * \ingroup quat
366 */
367void
368lib3ds_quat_tangent(Lib3dsQuat c, Lib3dsQuat p, Lib3dsQuat q, Lib3dsQuat n)
369{
[151]370  Lib3dsQuat dn,dp,x;
371  int i;
[8]372
[151]373  lib3ds_quat_ln_dif(dn, q, n);
374  lib3ds_quat_ln_dif(dp, q, p);
[8]375
[151]376  for (i=0; i<4; i++) {
377    x[i]=-1.0f/4.0f*(dn[i]+dp[i]);
378  }
379  lib3ds_quat_exp(x);
380  lib3ds_quat_mul(c,q,x);
[8]381}
382
383
384/*!
385 * \ingroup quat
386 */
387void
388lib3ds_quat_dump(Lib3dsQuat q)
389{
[151]390  printf("%f %f %f %f\n", q[0], q[1], q[2], q[3]);
[8]391}
Note: See TracBrowser for help on using the browser.