Extras/glui/quaternion.cpp

   1 /***********************************************************************
   2
   3   quaternion.cpp - A quaternion class
   4
   5   -------------------------------------------------------------------
   6
   7   GLUI User Interface Toolkit (LGPL)
   8   Copyright (c) 1998 Paul Rademacher
   9
  10   WWW:    http://sourceforge.net/projects/glui/
  11   Forums: http://sourceforge.net/forum/?group_id=92496
  12
  13   This library is free software; you can redistribute it and/or
  14   modify it under the terms of the GNU Lesser General Public
  15   License as published by the Free Software Foundation; either
  16   version 2.1 of the License, or (at your option) any later version.
  17
  18   This library is distributed in the hope that it will be useful,
  19   but WITHOUT ANY WARRANTY; without even the implied warranty of
  20   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  21   Lesser General Public License for more details.
  22
  23   You should have received a copy of the GNU Lesser General Public
  24   License along with this library; if not, write to the Free Software
  25   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  26
  27 ************************************************************************
  28
  29   Feb 1998, Paul Rademacher (rademach@cs.unc.edu)
  30   Oct 2003, Nigel Stewart - GLUI Code Cleaning
  31
  32 ************************************************************************/
  33 #include "glui_internal_control.h"
  34 #include "quaternion.h"
  35 #include <cmath>
  36 #include "glui_internal.h"
  37
  38 /******************************************* constructors **************/
  39
  40 quat::quat()
  41 {
  42     *this = quat_identity();
  43 }
  44
  45 quat::quat(const float x, const float y, const float z, const float w)
  46 {
  47     v.set( x, y, z );
  48     s = w;
  49 }
  50
  51 quat::quat(const vec3 &_v, const float _s)
  52 {
  53     set( _v, _s );
  54 }
  55
  56 quat::quat(const float _s, const vec3 &_v)
  57 {
  58     set( _v, _s );
  59 }
  60
  61 quat::quat(const float *d)
  62 {
  63     v[0] = d[0];
  64     v[1] = d[1];
  65     v[2] = d[2];
  66     s    = d[3];
  67 }
  68
  69 quat::quat(const double *d)
  70 {
  71     v[0] = (float) d[0];
  72     v[1] = (float) d[1];
  73     v[2] = (float) d[2];
  74     s    = (float) d[3];
  75 }
  76
  77 quat::quat(const quat &q)
  78 {
  79     v = q.v;
  80     s = q.s;
  81 }
  82
  83 void quat::set(const vec3 &_v, const float _s)
  84 {
  85     v = _v;
  86     s = _s;
  87 }
  88
  89 quat &quat::operator=(const quat &q)
  90 {
  91     v = q.v;
  92     s = q.s;
  93     return *this;
  94 }
  95
  96 /******** quat friends ************/
  97
  98 quat operator + (const quat &a, const quat &b)
  99 {
 100     return quat( a.s+b.s, a.v+b.v );
 101 }
 102
 103 quat operator - (const quat &a, const quat &b)
 104 {
 105     return quat( a.s-b.s, a.v-b.v );
 106 }
 107
 108 quat operator - (const quat &a )
 109 {
 110     return quat( -a.s, -a.v );
 111 }
 112
 113 quat operator * ( const quat &a, const quat &b)
 114 {
 115     return quat( a.s*b.s - a.v*b.v, a.s*b.v + b.s*a.v + a.v^b.v );
 116 }
 117
 118 quat operator * ( const quat &a, const float t)
 119 {
 120     return quat( a.v * t, a.s * t );
 121 }
 122
 123 quat operator * ( const float t, const quat &a )
 124 {
 125     return quat( a.v * t, a.s * t );
 126 }
 127
 128 mat4 quat::to_mat4() const
 129 {
 130     float xs, ys, zs, wx, wy, wz, xx, xy, xz, yy, yz, zz;
 131
 132     float t  = 2.0f / (v*v + s*s);
 133
 134     xs = v[VX]*t;   ys = v[VY]*t;   zs = v[VZ]*t;
 135     wx = s*xs;      wy = s*ys;      wz = s*zs;
 136     xx = v[VX]*xs;  xy = v[VX]*ys;  xz = v[VX]*zs;
 137     yy = v[VY]*ys;  yz = v[VY]*zs;  zz = v[VZ]*zs;
 138
 139     mat4 matrix(
 140            1.0f-(yy+zz), xy+wz,        xz-wy,        0.0f,
 141            xy-wz,        1.0f-(xx+zz), yz+wx,        0.0f,
 142            xz+wy,        yz-wx,        1.0f-(xx+yy), 0.0f,
 143            0.0f,         0.0f,         0.0f,         1.0f );
 144
 145     return matrix;
 146 }
 147
 148 /************************************************* quat_identity() *****/
 149 /* Returns quaternion identity element                                 */
 150
 151 quat quat_identity()
 152 {
 153     return quat( vec3( 0.0, 0.0, 0.0 ), 1.0 );
 154 }
 155
 156 /************************************************ quat_slerp() ********/
 157 /* Quaternion spherical interpolation                                 */
 158
 159 quat quat_slerp(const quat &from, const quat &to, float t)
 160 {
 161     quat to1;
 162     float omega, cosom, sinom, scale0, scale1;
 163
 164     /* calculate cosine */
 165     cosom = from.v * to.v + from.s + to.s;
 166
 167     /* Adjust signs (if necessary) */
 168     if ( cosom < 0.0 )
 169     {
 170         cosom = -cosom;
 171         to1 = -to;
 172     }
 173     else
 174     {
 175         to1 = to;
 176     }
 177
 178     /* Calculate coefficients */
 179     if ((1.0 - cosom) > FUDGE )
 180     {
 181         /* standard case (slerp) */
 182         omega =  (float) acos( cosom );
 183         sinom =  (float) sin( omega );
 184         scale0 = (float) sin((1.0 - t) * omega) / sinom;
 185         scale1 = (float) sin(t * omega) / sinom;
 186     }
 187     else
 188     {
 189         /* 'from' and 'to' are very close - just do linear interpolation */
 190         scale0 = 1.0f - t;
 191         scale1 = t;
 192     }
 193
 194     return scale0 * from + scale1 * to1;
 195 }
 196
 197 /********************************************** set_angle() ************/
 198 /* set rot angle (degrees)                                             */
 199
 200 void quat::set_angle(float f)
 201 {
 202     vec3 axis = get_axis();
 203
 204     s = (float) cos( DEG2RAD( f ) / 2.0 );
 205
 206     v = axis * (float) sin(DEG2RAD(f) / 2.0);
 207 }
 208
 209 /********************************************** scale_angle() ************/
 210 /* scale rot angle (degrees)                                             */
 211
 212 void quat::scale_angle(float f)
 213 {
 214     set_angle( f * get_angle() );
 215 }
 216
 217 /********************************************** get_angle() ************/
 218 /* get rot angle (degrees).  Assumes s is between -1 and 1             */
 219
 220 float quat::get_angle() const
 221 {
 222     return (float) RAD2DEG( 2.0 * acos( s ) );
 223 }
 224
 225 /********************************************* get_axis() **************/
 226
 227 vec3 quat::get_axis() const
 228 {
 229     float scale = (float) sin( acos( s ) );
 230
 231     if ( scale < FUDGE AND scale > -FUDGE )
 232         return vec3( 0.0, 0.0, 0.0 );
 233     else
 234         return  v / scale;
 235 }
 236
 237 /******************************************* quat::print() ************/
 238
 239 void quat::print(FILE *dest, const char *name) const
 240 {
 241     fprintf( dest, "%s:   v:<%3.2f %3.2f %3.2f>  s:%3.2f\n",
 242         name, v[0], v[1], v[2], s );
 243 }