git.mutantstargoat.com Git - summerhack/blob - src/3dengfx/libs/lib3ds/quat.c

   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
   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.
  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
  13  * or  FITNESS FOR A  PARTICULAR PURPOSE.  See the  GNU Lesser General Public
  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: quat.c,v 1.6 2004/11/20 08:31:31 efalk Exp $
  21  */
  22 #define LIB3DS_EXPORT
  23 #include <lib3ds/quat.h>
  24 #include <math.h>
  25
  26
  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
  37
  38 /*!
  39  * Clear a quaternion.
  40  * \ingroup quat
  41  */
  42 void
  43 lib3ds_quat_zero(Lib3dsQuat c)
  44 {
  45   c[0]=c[1]=c[2]=c[3]=0.0f;
  46 }
  47
  48
  49 /*!
  50  * Set a quaternion to Identity
  51  * \ingroup quat
  52  */
  53 void
  54 lib3ds_quat_identity(Lib3dsQuat c)
  55 {
  56   c[0]=c[1]=c[2]=0.0f;
  57   c[3]=1.0f;
  58 }
  59
  60
  61 /*!
  62  * Copy a quaternion.
  63  * \ingroup quat
  64  */
  65 void
  66 lib3ds_quat_copy(Lib3dsQuat dest, Lib3dsQuat src)
  67 {
  68   int i;
  69   for (i=0; i<4; ++i) {
  70     dest[i]=src[i];
  71   }
  72 }
  73
  74
  75 /*!
  76  * Compute a quaternion from axis and angle.
  77  *
  78  * \param c Computed quaternion
  79  * \param axis Rotation axis
  80  * \param angle Angle of rotation, radians.
  81  *
  82  * \ingroup quat
  83  */
  84 void
  85 lib3ds_quat_axis_angle(Lib3dsQuat c, Lib3dsVector axis, Lib3dsFloat angle)
  86 {
  87   Lib3dsDouble omega,s;
  88   Lib3dsDouble l;
  89
  90   l=sqrt(axis[0]*axis[0] + axis[1]*axis[1] + axis[2]*axis[2]);
  91   if (l<LIB3DS_EPSILON) {
  92     c[0]=c[1]=c[2]=0.0f;
  93     c[3]=1.0f;
  94   }
  95   else {
  96     omega=-0.5*angle;
  97     s=sin(omega)/l;
  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);
 102   }
 103 }
 104
 105
 106 /*!
 107  * Negate a quaternion
 108  *
 109  * \ingroup quat
 110  */
 111 void
 112 lib3ds_quat_neg(Lib3dsQuat c)
 113 {
 114   int i;
 115   for (i=0; i<4; ++i) {
 116     c[i]=-c[i];
 117   }
 118 }
 119
 120
 121 /*!
 122  * Compute the absolute value of a quaternion
 123  *
 124  * \ingroup quat
 125  */
 126 void
 127 lib3ds_quat_abs(Lib3dsQuat c)
 128 {
 129   int i;
 130   for (i=0; i<4; ++i) {
 131     c[i]=(Lib3dsFloat)fabs(c[i]);
 132   }
 133 }
 134
 135
 136 /*!
 137  * Compute the conjugate of a quaternion
 138  *
 139  * \ingroup quat
 140  */
 141 void
 142 lib3ds_quat_cnj(Lib3dsQuat c)
 143 {
 144   int i;
 145   for (i=0; i<3; ++i) {
 146     c[i]=-c[i];
 147   }
 148 }
 149
 150
 151 /*!
 152  * Multiply two quaternions.
 153  *
 154  * \param c Result
 155  * \param a,b Inputs
 156  * \ingroup quat
 157  */
 158 void
 159 lib3ds_quat_mul(Lib3dsQuat c, Lib3dsQuat a, Lib3dsQuat b)
 160 {
 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];
 165 }
 166
 167
 168 /*!
 169  * Multiply a quaternion by a scalar.
 170  *
 171  * \ingroup quat
 172  */
 173 void
 174 lib3ds_quat_scalar(Lib3dsQuat c, Lib3dsFloat k)
 175 {
 176   int i;
 177   for (i=0; i<4; ++i) {
 178     c[i]*=k;
 179   }
 180 }
 181
 182
 183 /*!
 184  * Normalize a quaternion.
 185  *
 186  * \ingroup quat
 187  */
 188 void
 189 lib3ds_quat_normalize(Lib3dsQuat c)
 190 {
 191   Lib3dsDouble l,m;
 192
 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     int i;
 200     m=1.0f/l;
 201     for (i=0; i<4; ++i) {
 202       c[i]=(Lib3dsFloat)(c[i]*m);
 203     }
 204   }
 205 }
 206
 207
 208 /*!
 209  * Compute the inverse of a quaternion.
 210  *
 211  * \ingroup quat
 212  */
 213 void
 214 lib3ds_quat_inv(Lib3dsQuat c)
 215 {
 216   Lib3dsDouble l,m;
 217
 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) {
 220     c[0]=c[1]=c[2]=0.0f;
 221     c[3]=1.0f;
 222   }
 223   else {
 224     m=1.0f/l;
 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);
 229   }
 230 }
 231
 232
 233 /*!
 234  * Compute the dot-product of a quaternion.
 235  *
 236  * \ingroup quat
 237  */
 238 Lib3dsFloat
 239 lib3ds_quat_dot(Lib3dsQuat a, Lib3dsQuat b)
 240 {
 241   return(a[0]*b[0] + a[1]*b[1] + a[2]*b[2] + a[3]*b[3]);
 242 }
 243
 244
 245 /*!
 246  * \ingroup quat
 247  */
 248 Lib3dsFloat
 249 lib3ds_quat_squared(Lib3dsQuat c)
 250 {
 251   return(c[0]*c[0] + c[1]*c[1] + c[2]*c[2] + c[3]*c[3]);
 252 }
 253
 254
 255 /*!
 256  * \ingroup quat
 257  */
 258 Lib3dsFloat
 259 lib3ds_quat_length(Lib3dsQuat c)
 260 {
 261   return((Lib3dsFloat)sqrt(c[0]*c[0] + c[1]*c[1] + c[2]*c[2] + c[3]*c[3]));
 262 }
 263
 264
 265 /*!
 266  * \ingroup quat
 267  */
 268 void
 269 lib3ds_quat_ln(Lib3dsQuat c)
 270 {
 271   Lib3dsDouble om,s,t;
 272
 273   s=sqrt(c[0]*c[0] + c[1]*c[1] + c[2]*c[2]);
 274   om=atan2(s,c[3]);
 275   if (fabs(s)<LIB3DS_EPSILON) {
 276     t=0.0f;
 277   }
 278   else {
 279     t=om/s;
 280   }
 281   {
 282     int i;
 283     for (i=0; i<3; ++i) {
 284       c[i]=(Lib3dsFloat)(c[i]*t);
 285     }
 286     c[3]=0.0f;
 287   }
 288 }
 289
 290
 291 /*!
 292  * \ingroup quat
 293  */
 294 void
 295 lib3ds_quat_ln_dif(Lib3dsQuat c, Lib3dsQuat a, Lib3dsQuat b)
 296 {
 297   Lib3dsQuat invp;
 298
 299   lib3ds_quat_copy(invp, a);
 300   lib3ds_quat_inv(invp);
 301   lib3ds_quat_mul(c, invp, b);
 302   lib3ds_quat_ln(c);
 303 }
 304
 305
 306 /*!
 307  * \ingroup quat
 308  */
 309 void
 310 lib3ds_quat_exp(Lib3dsQuat c)
 311 {
 312   Lib3dsDouble om,sinom;
 313
 314   om=sqrt(c[0]*c[0] + c[1]*c[1] + c[2]*c[2]);
 315   if (fabs(om)<LIB3DS_EPSILON) {
 316     sinom=1.0f;
 317   }
 318   else {
 319     sinom=sin(om)/om;
 320   }
 321   {
 322     int i;
 323     for (i=0; i<3; ++i) {
 324       c[i]=(Lib3dsFloat)(c[i]*sinom);
 325     }
 326     c[3]=(Lib3dsFloat)cos(om);
 327   }
 328 }
 329
 330
 331 /*!
 332  * \ingroup quat
 333  */
 334 void
 335 lib3ds_quat_slerp(Lib3dsQuat c, Lib3dsQuat a, Lib3dsQuat b, Lib3dsFloat t)
 336 {
 337   Lib3dsDouble l;
 338   Lib3dsDouble om,sinom;
 339   Lib3dsDouble sp,sq;
 340   Lib3dsQuat q;
 341
 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);
 345     om=acos(l);
 346     sinom=sin(om);
 347     if (fabs(sinom)>LIB3DS_EPSILON) {
 348       sp=sin((1.0f-t)*om)/sinom;
 349       sq=sin(t*om)/sinom;
 350     }
 351     else {
 352       sp=1.0f-t;
 353       sq=t;
 354     }
 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]);
 359   }
 360   else {
 361     q[0]=-a[1];
 362     q[1]=a[0];
 363     q[2]=-a[3];
 364     q[3]=a[2];
 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]);
 371   }
 372 }
 373
 374
 375 /*!
 376  * \ingroup quat
 377  */
 378 void
 379 lib3ds_quat_squad(Lib3dsQuat c, Lib3dsQuat a, Lib3dsQuat p, Lib3dsQuat q,
 380   Lib3dsQuat b, Lib3dsFloat t)
 381 {
 382   Lib3dsQuat ab;
 383   Lib3dsQuat pq;
 384
 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));
 388 }
 389
 390
 391 /*!
 392  * \ingroup quat
 393  */
 394 void
 395 lib3ds_quat_tangent(Lib3dsQuat c, Lib3dsQuat p, Lib3dsQuat q, Lib3dsQuat n)
 396 {
 397   Lib3dsQuat dn,dp,x;
 398   int i;
 399
 400   lib3ds_quat_ln_dif(dn, q, n);
 401   lib3ds_quat_ln_dif(dp, q, p);
 402
 403   for (i=0; i<4; i++) {
 404     x[i]=-1.0f/4.0f*(dn[i]+dp[i]);
 405   }
 406   lib3ds_quat_exp(x);
 407   lib3ds_quat_mul(c,q,x);
 408 }
 409
 410
 411 /*!
 412  * \ingroup quat
 413  */
 414 void
 415 lib3ds_quat_dump(Lib3dsQuat q)
 416 {
 417   printf("%f %f %f %f\n", q[0], q[1], q[2], q[3]);
 418 }
 419