git.rot13.org Git - simavr/blob - examples/shared/libc3/src/c3quaternion.c

   1 /*
   2         c3quaternion.c
   3
   4         Copyright 2008-2012 Michel Pollet <buserror@gmail.com>
   5
   6         This file is part of libc3.
   7
   8         libc3 is free software: you can redistribute it and/or modify
   9         it under the terms of the GNU General Public License as published by
  10         the Free Software Foundation, either version 3 of the License, or
  11         (at your option) any later version.
  12
  13         libc3 is distributed in the hope that it will be useful,
  14         but WITHOUT ANY WARRANTY; without even the implied warranty of
  15         MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  16         GNU General Public License for more details.
  17
  18         You should have received a copy of the GNU General Public License
  19         along with libc3.  If not, see <http://www.gnu.org/licenses/>.
  20  */
  21
  22 #include <math.h>
  23 #include "c3quaternion.h"
  24
  25 #ifndef DEG2RAD
  26 #define DEG2RAD(x) ((x)/180.0*M_PI)
  27 #define RAD2DEG(x) ((x)/M_PI*180.0)
  28 #endif
  29 #ifndef FUDGE
  30 #define FUDGE .00001
  31 #endif
  32
  33 c3quat
  34 c3quat_new()
  35 {
  36     return c3quat_identity();
  37 }
  38
  39 /************************************************* c3quat_identity() *****/
  40 /* Returns quaternion identity element                                 */
  41
  42 c3quat
  43 c3quat_identity()
  44 {
  45     return c3quat_vec3( c3vec3f( 0.0, 0.0, 0.0 ), 1.0 );
  46 }
  47
  48 c3quat
  49 c3quatf(
  50                 const c3f x,
  51                 const c3f y,
  52                 const c3f z,
  53                 const c3f w)
  54 {
  55         c3quat q = { .v = c3vec3f(x,y,z), .s = w };
  56         return q;
  57 }
  58
  59 c3quat
  60 c3quat_vec3(
  61                 const c3vec3 v,
  62                 const c3f s)
  63 {
  64         c3quat q = { .v = v, .s = s };
  65     return q;
  66 }
  67
  68 c3quat
  69 c3quat_vec4(
  70                 const c3vec4 v)
  71 {
  72         c3quat q = { .v = c3vec3f(v.n[0], v.n[1], v.n[2]), .s = v.n[3] };
  73     return q;
  74 }
  75
  76 c3quat
  77 c3quat_double(
  78                 const double *d)
  79 {
  80         c3quat q;
  81     q.v.n[0] = (c3f) d[0];
  82     q.v.n[1] = (c3f) d[1];
  83     q.v.n[2] = (c3f) d[2];
  84     q.s          = (c3f) d[3];
  85     return q;
  86 }
  87
  88
  89 c3quat
  90 c3quat_add(
  91                 const c3quat a,
  92                 const c3quat b)
  93 {
  94     return c3quat_vec3(c3vec3_add(a.v, b.v), a.s + b.s );
  95 }
  96
  97 c3quat
  98 c3quat_sub(
  99                 const c3quat a,
 100                 const c3quat b)
 101 {
 102     return c3quat_vec3(c3vec3_sub(a.v, b.v), a.s - b.s );
 103 }
 104
 105 c3quat
 106 c3quat_minus(
 107                 const c3quat a )
 108 {
 109     return c3quat_vec3(c3vec3_minus(a.v), -a.s);
 110 }
 111
 112 c3quat
 113 c3quat_mul(
 114                 const c3quat a,
 115                 const c3quat b)
 116 {
 117 //    return c3quat( a.s*b.s - a.v*b.v, a.s*b.v + b.s*a.v + a.v^b.v );
 118     return c3quat_vec3(
 119                 c3vec3_add(c3vec3_mulf(b.v, a.s), c3vec3_add(c3vec3_mulf(a.v, b.s), c3vec3_cross(a.v, b.v))),
 120                 (a.s * b.s) - c3vec3_dot(a.v, b.v));
 121 }
 122
 123 c3quat
 124 c3quat_mulf( const c3quat a, const c3f t)
 125 {
 126     return c3quat_vec3(c3vec3_mulf(a.v, t), a.s * t );
 127 }
 128
 129 c3mat4
 130 c3quat_to_mat4(
 131                 const c3quat a )
 132 {
 133     c3f xs, ys, zs, wx, wy, wz, xx, xy, xz, yy, yz, zz;
 134
 135     c3f t  = 2.0f / (c3vec3_dot(a.v, a.v) + (a.s * a.s));
 136
 137     xs = a.v.n[VX]*t;   ys = a.v.n[VY]*t;   zs = a.v.n[VZ]*t;
 138     wx = a.s*xs;                wy = a.s*ys;            wz = a.s*zs;
 139     xx = a.v.n[VX]*xs;  xy = a.v.n[VX]*ys;  xz = a.v.n[VX]*zs;
 140     yy = a.v.n[VY]*ys;  yz = a.v.n[VY]*zs;  zz = a.v.n[VZ]*zs;
 141
 142     c3mat4 m = c3mat4_vec4(
 143            c3vec4f(1.0f-(yy+zz), xy+wz,        xz-wy,        0.0f),
 144            c3vec4f(xy-wz,        1.0f-(xx+zz), yz+wx,        0.0f),
 145            c3vec4f(xz+wy,        yz-wx,        1.0f-(xx+yy), 0.0f),
 146            c3vec4f(0.0f,         0.0f,         0.0f,         1.0f ));
 147
 148     return m;
 149 }
 150
 151
 152 /************************************************ quat_slerp() ********/
 153 /* Quaternion spherical interpolation                                 */
 154
 155 c3quat
 156 quat_slerp(
 157                 const c3quat from,
 158                 const c3quat to,
 159                 c3f t)
 160 {
 161     c3quat to1;
 162     c3f omega, cosom, sinom, scale0, scale1;
 163
 164     /* calculate cosine */
 165     cosom = c3vec3_dot(from.v, to.v) + from.s + to.s;
 166
 167         /* Adjust signs (if necessary) */
 168         if (cosom < 0.0) {
 169                 cosom = -cosom;
 170                 to1 = c3quat_minus(to);
 171         } else {
 172                 to1 = to;
 173         }
 174
 175     /* Calculate coefficients */
 176     if ((1.0 - cosom) > FUDGE ) {
 177         /* standard case (slerp) */
 178         omega =  (c3f) acos( cosom );
 179         sinom =  (c3f) sin( omega );
 180         scale0 = (c3f) sin((1.0 - t) * omega) / sinom;
 181         scale1 = (c3f) sin(t * omega) / sinom;
 182     } else {
 183         /* 'from' and 'to' are very close - just do linear interpolation */
 184         scale0 = 1.0f - t;
 185         scale1 = t;
 186     }
 187
 188     return c3quat_add(c3quat_mulf(from, scale0), c3quat_mulf(to1, scale1));
 189 }
 190
 191 /********************************************** set_angle() ************/
 192 /* set rot angle (degrees)                                             */
 193
 194 c3quatp
 195 c3quat_set_angle(
 196                 c3quatp a,
 197                 c3f f)
 198 {
 199     c3vec3 axis = c3quat_get_axis(a);
 200
 201     a->s = (c3f) cos( DEG2RAD( f ) / 2.0 );
 202
 203     a->v = c3vec3_mulf(axis, (c3f) sin(DEG2RAD(f) / 2.0));
 204     return a;
 205 }
 206
 207 /********************************************** scale_angle() ************/
 208 /* scale rot angle (degrees)                                             */
 209
 210 c3quatp
 211 c3quat_scale_angle(
 212                 c3quatp a,
 213                 c3f f)
 214 {
 215         return c3quat_set_angle(a, f * c3quat_get_angle(a) );
 216 }
 217
 218 /********************************************** get_angle() ************/
 219 /* get rot angle (degrees).  Assumes s is between -1 and 1             */
 220
 221 c3f
 222 c3quat_get_angle(
 223                 const c3quatp a)
 224 {
 225     return (c3f) RAD2DEG( 2.0 * acos( a->s ) );
 226 }
 227
 228 /********************************************* get_axis() **************/
 229
 230 c3vec3
 231 c3quat_get_axis(
 232                 c3quatp a)
 233 {
 234     c3f scale = (c3f) sin( acos( a->s ) );
 235
 236     if ( scale < FUDGE && scale > -FUDGE )
 237         return c3vec3f( 0.0, 0.0, 0.0 );
 238     else
 239         return  c3vec3_divf(a->v, scale);
 240 }
 241
 242 /******************************************* c3quat_print() ************/
 243 #if 0
 244 void c3quat_print(FILE *dest, const char *name) const
 245 {
 246     fprintf( dest, "%s:   v:<%3.2f %3.2f %3.2f>  s:%3.2f\n",
 247         name, v[0], v[1], v[2], s );
 248 }
 249 #endif