src/base/quat.h

   1 /************************************************************************
   2     This file is part of NE.
   3
   4     NE is free software: you can redistribute it and/or modify
   5     it under the terms of the GNU General Public License as published by
   6     the Free Software Foundation, either version 3 of the License, or
   7     (at your option) any later version.
   8
   9     NE is distributed in the hope that it will be useful,
  10     but WITHOUT ANY WARRANTY; without even the implied warranty of
  11     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12     GNU General Public License for more details.
  13
  14     You should have received a copy of the GNU General Public License
  15     along with NE.  If not, see <http://www.gnu.org/licenses/>.
  16 ************************************************************************/
  17
  18 #ifndef QUAT_H_
  19 #define QUAT_H_
  20
  21 #include <GL/gl.h>
  22 #include <ode/ode.h>
  23 #include <math.h>
  24 #include "vec.h"
  25
  26 //namespace Base {
  27
  28 #define SLERP_DELTA 0.001
  29
  30 template<class T>
  31 class Quat
  32 {
  33 public:
  34         Quat() : m_w(0.0) {}
  35         Quat(T w, const Vec<T> &v) : m_v(v), m_w(w) {}
  36         Quat(T w, T x, T y, T z) : m_v(x, y, z), m_w(w) {}
  37
  38         ///////////////////////
  39         // Set/Get functions //
  40         ///////////////////////
  41         inline const Quat<T> set(T w, const Vec<T> &v)
  42         {
  43                 m_v = v;
  44                 m_w = w;
  45                 return *this;
  46         }
  47         inline const Quat<T> set(T x, T y, T z, T w)
  48         {
  49                 m_v = Vec<T>(x, y, z);
  50                 m_w = w;
  51                 return *this;
  52         }
  53
  54         inline void get(T v[4]) const
  55         {
  56                 v[0] = x();
  57                 v[1] = y();
  58                 v[2] = z();
  59                 v[3] = w();
  60         }
  61
  62         inline const Vec<T> v() const {return m_v;}
  63         inline T x() const {return m_v.x();}
  64         inline T y() const {return m_v.y();}
  65         inline T z() const {return m_v.z();}
  66         inline T w() const {return m_w;}
  67
  68         ////////////////
  69         // Conversion //
  70         ////////////////
  71         inline T angle() const
  72         {
  73                 return acos(w())*2.0;
  74         }
  75
  76         inline static const Quat<T> FromAxis(T angle, const Vec<T> &v)
  77         {
  78                 T a = angle * 0.5;
  79
  80                 return Quat<T>(cos(a), v * sin(a));
  81         }
  82
  83         inline static const Quat<T> FromODE(const dReal *q)
  84         {
  85                 return Quat<T>(q[0], q[1], q[2], q[3]);
  86         }
  87
  88         inline const dReal *toODE()
  89         {
  90                 static dReal q[4];
  91                 q[0] = w();
  92                 q[1] = x();
  93                 q[2] = y();
  94                 q[3] = z();
  95                 return q;
  96         }
  97
  98         inline void toMatrixGL(T m[16])
  99         {
 100                 T x2 = x() + x();
 101                 T y2 = y() + y();
 102                 T z2 = z() + z();
 103
 104                 T xx = x() * x2;
 105                 T xy = x() * y2;
 106                 T xz = x() * z2;
 107
 108                 T yy = y() * y2;
 109                 T yz = y() * z2;
 110
 111                 T zz = z() * z2;
 112
 113                 T wx = w() * x2;
 114                 T wy = w() * y2;
 115                 T wz = w() * z2;
 116
 117                 m[ 0] = 1.0 - (yy + zz);
 118                 m[ 1] = xy + wz;
 119                 m[ 2] = xz - wy;
 120                 m[ 3] = 0.0;
 121
 122                 m[ 4] = xy - wz;
 123                 m[ 5] = 1.0 - (xx + zz);
 124                 m[ 6] = yz + wx;
 125                 m[ 7] = 0.0;
 126
 127                 m[ 8] = xz + wy;
 128                 m[ 9] = yz - wx;
 129                 m[10] = 1.0 - (xx + yy);
 130                 m[11] = 0.0;
 131
 132                 m[12] = 0.0;
 133                 m[13] = 0.0;
 134                 m[14] = 0.0;
 135                 m[15] = 1.0;
 136         }
 137
 138         inline const Vec<T> toEuler() const
 139         {
 140                 T nx, ny, nz;
 141                 double test = x() * y() + z() * w();
 142
 143                 if (test > 0.5 - SLERP_DELTA) {
 144                         nx = 2.0 * atan2(x(), w());
 145                         ny = M_PI * 0.5;
 146                         nz = 0.0;
 147                 }
 148                 else if (test < -0.5 + SLERP_DELTA) {
 149                         nx = -2.0 * atan2(x(), w());
 150                         ny = -M_PI * 0.5;
 151                         nz = 0.0;
 152                 }
 153                 else {
 154                         T xx, yy, zz, x2, y2, z2, w2;
 155
 156                         x2 = x() * 2.0;
 157                         y2 = y() * 2.0;
 158                         z2 = z() * 2.0;
 159                         w2 = w() * 2.0;
 160
 161                         xx = x2 * x();
 162                         yy = y2 * y();
 163                         zz = z2 * z();
 164
 165                         nx = atan2(y2 * w() - x2 * z(), 1.0 - yy - zz);
 166                         ny = asin(2.0 * test);
 167                         nz = atan2(x2 * w() - y2 * z(), 1.0 - xx - zz);
 168                 }
 169
 170                 return Vec<T>(nx, ny, nz);
 171         }
 172
 173         /////////////
 174         // 3D Math //
 175         /////////////
 176
 177         inline T norm() const
 178         {
 179                 return w() * w() + v().dot(v());
 180         }
 181
 182         inline const Quat<T> unit() const
 183         {
 184                 return Quat<T>(*this / norm());
 185         }
 186
 187         inline const Quat<T> inverse() const
 188         {
 189                 return conjugate() / norm();
 190         }
 191
 192         inline const Quat<T> conjugate() const
 193         {
 194                 return Quat<T>(w(), -v());
 195         }
 196
 197         inline const Quat<T> operator * (const Quat<T> &q) const
 198         {
 199                 //T w = m_w * q.m_w - m_v.dot(q.m_v);
 200                 //Vec<T> v(m_v.cross(q.m_v) + m_w * q.m_v + q.m_w * m_v);
 201                 //return Quat<T>(w, v);
 202
 203                 return Quat<T>(*this) *= q;
 204         }
 205
 206         inline const Quat<T> &operator *= (const Quat<T> &q)
 207         {
 208                 T a, b, c, d, e, f, g, h;
 209                 a = (w() + x()) * (q.w() + q.x());
 210                 b = (z() - y()) * (q.y() - q.z());
 211                 c = (w() - x()) * (q.y() + q.z());
 212                 d = (y() + z()) * (q.w() - q.x());
 213                 e = (x() + z()) * (q.x() + q.y());
 214                 f = (x() - z()) * (q.x() - q.y());
 215                 g = (w() + y()) * (q.w() - q.z());
 216                 h = (w() - y()) * (q.w() + q.z());
 217
 218                 T w = b + (-e - f + g + h) * 0.5;
 219                 T x = a - ( e + f + g + h) * 0.5;
 220                 T y = c + ( e - f + g - h) * 0.5;
 221                 T z = d + ( e - f - g + h) * 0.5;
 222
 223                 m_w = w;
 224                 m_v = Vec<T>(x, y, z);
 225
 226                 return *this;
 227         }
 228
 229         inline const Quat<T> slerp(const Quat<T> &to, T t)
 230         {
 231                 double om, cosom, sinom, s1, s2;
 232
 233                 Quat<T> tmp(to);
 234
 235                 cosom = v().dot(to.v()) + w() * to.w();
 236
 237                 if (cosom < 0.0) {
 238                         cosom = -cosom;
 239                         tmp.m_w = -tmp.m_w;
 240                         tmp.m_v = -tmp.m_v;
 241                 }
 242
 243                 if ((1.0 - cosom) > SLERP_DELTA) {
 244                         om = acos(cosom);
 245                         sinom = sin(om);
 246                         s1 = sin((1.0 - t) * om) / sinom;
 247                         s2 = sin(t * om) / sinom;
 248                 }
 249                 else {
 250                         // do linear because they are very close
 251                         s1 = 1.0 - t;
 252                         s2 = t;
 253                 }
 254
 255                 return Quat<T>(s1 * x() + s2 * tmp.x(),
 256                                            s1 * y() + s2 * tmp.y(),
 257                                            s1 * z() + s2 * tmp.z(),
 258                                            s1 * w() + s2 * tmp.w());
 259         }
 260
 261         ///////////////////////
 262         // Equality overload //
 263         ///////////////////////
 264
 265         inline const Quat<T> &operator = (const Quat<T> &q)
 266         {
 267                 m_w = q.m_w;
 268                 m_v = q.m_v;
 269                 return *this;
 270         }
 271         inline bool operator == (const Quat<T> &q) const
 272         {
 273                 return m_w == q.m_w && m_v == q.m_v;
 274         }
 275
 276         /////////////////////
 277         // Scalar overload //
 278         /////////////////////
 279
 280         inline const Quat<T> operator / (T s) const
 281         {
 282                 return Quat<T> (m_w / s, m_v / s);
 283         }
 284
 285 protected:
 286         Vec<T> m_v;
 287         T m_w;
 288
 289 };
 290
 291 typedef Quat<float> Quatf;
 292 typedef Quat<double> Quatd;
 293
 294 //}
 295
 296 #endif /*QUAT_H_*/