misc/mediasource/extra/netradiant-src/libs/math/quaternion.h

   1 /*
   2 Copyright (C) 2001-2006, William Joseph.
   3 All Rights Reserved.
   4
   5 This file is part of GtkRadiant.
   6
   7 GtkRadiant is free software; you can redistribute it and/or modify
   8 it under the terms of the GNU General Public License as published by
   9 the Free Software Foundation; either version 2 of the License, or
  10 (at your option) any later version.
  11
  12 GtkRadiant is distributed in the hope that it will be useful,
  13 but WITHOUT ANY WARRANTY; without even the implied warranty of
  14 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  15 GNU General Public License for more details.
  16
  17 You should have received a copy of the GNU General Public License
  18 along with GtkRadiant; if not, write to the Free Software
  19 Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  20 */
  21
  22 #if !defined(INCLUDED_MATH_QUATERNION_H)
  23 #define INCLUDED_MATH_QUATERNION_H
  24
  25 /// \file
  26 /// \brief Quaternion data types and related operations.
  27
  28 #include "math/matrix.h"
  29
  30 /// \brief A quaternion stored in single-precision floating-point.
  31 typedef Vector4 Quaternion;
  32
  33 const Quaternion c_quaternion_identity(0, 0, 0, 1);
  34
  35 inline Quaternion quaternion_multiplied_by_quaternion(const Quaternion& quaternion, const Quaternion& other)
  36 {
  37   return Quaternion(
  38     quaternion[3]*other[0] + quaternion[0]*other[3] + quaternion[1]*other[2] - quaternion[2]*other[1],
  39     quaternion[3]*other[1] + quaternion[1]*other[3] + quaternion[2]*other[0] - quaternion[0]*other[2],
  40     quaternion[3]*other[2] + quaternion[2]*other[3] + quaternion[0]*other[1] - quaternion[1]*other[0],
  41     quaternion[3]*other[3] - quaternion[0]*other[0] - quaternion[1]*other[1] - quaternion[2]*other[2]
  42   );
  43 }
  44
  45 inline void quaternion_multiply_by_quaternion(Quaternion& quaternion, const Quaternion& other)
  46 {
  47   quaternion = quaternion_multiplied_by_quaternion(quaternion, other);
  48 }
  49
  50 /// \brief Constructs a quaternion which rotates between two points on the unit-sphere, \p from and \p to.
  51 inline Quaternion quaternion_for_unit_vectors(const Vector3& from, const Vector3& to)
  52 {
  53   return Quaternion(vector3_cross(from, to), static_cast<float>(vector3_dot(from, to)));
  54 }
  55
  56 inline Quaternion quaternion_for_axisangle(const Vector3& axis, double angle)
  57 {
  58   angle *= 0.5;
  59   float sa = static_cast<float>(sin(angle));
  60   return Quaternion(axis[0] * sa, axis[1] * sa, axis[2] * sa, static_cast<float>(cos(angle)));
  61 }
  62
  63 inline Quaternion quaternion_for_x(double angle)
  64 {
  65   angle *= 0.5;
  66   return Quaternion(static_cast<float>(sin(angle)), 0, 0, static_cast<float>(cos(angle)));
  67 }
  68
  69 inline Quaternion quaternion_for_y(double angle)
  70 {
  71   angle *= 0.5;
  72   return Quaternion(0, static_cast<float>(sin(angle)), 0, static_cast<float>(cos(angle)));
  73 }
  74
  75 inline Quaternion quaternion_for_z(double angle)
  76 {
  77   angle *= 0.5;
  78   return Quaternion(0, 0, static_cast<float>(sin(angle)), static_cast<float>(cos(angle)));
  79 }
  80
  81 inline Quaternion quaternion_inverse(const Quaternion& quaternion)
  82 {
  83   return Quaternion(vector3_negated(vector4_to_vector3(quaternion)), quaternion[3]);
  84 }
  85
  86 inline void quaternion_conjugate(Quaternion& quaternion)
  87 {
  88   quaternion = quaternion_inverse(quaternion);
  89 }
  90
  91 inline Quaternion quaternion_normalised(const Quaternion& quaternion)
  92 {
  93   const double n = (1.0 / (quaternion[0] * quaternion[0] + quaternion[1] * quaternion[1] + quaternion[2] * quaternion[2] + quaternion[3] * quaternion[3]));
  94   return Quaternion(
  95     static_cast<float>(quaternion[0] * n),
  96     static_cast<float>(quaternion[1] * n),
  97     static_cast<float>(quaternion[2] * n),
  98     static_cast<float>(quaternion[3] * n)
  99   );
 100 }
 101
 102 inline void quaternion_normalise(Quaternion& quaternion)
 103 {
 104   quaternion = quaternion_normalised(quaternion);
 105 }
 106
 107 /// \brief Constructs a pure-rotation matrix from \p quaternion.
 108 inline Matrix4 matrix4_rotation_for_quaternion(const Quaternion& quaternion)
 109 {
 110 #if 0
 111   const double xx = quaternion[0] * quaternion[0];
 112   const double xy = quaternion[0] * quaternion[1];
 113   const double xz = quaternion[0] * quaternion[2];
 114   const double xw = quaternion[0] * quaternion[3];
 115
 116   const double yy = quaternion[1] * quaternion[1];
 117   const double yz = quaternion[1] * quaternion[2];
 118   const double yw = quaternion[1] * quaternion[3];
 119
 120   const double zz = quaternion[2] * quaternion[2];
 121   const double zw = quaternion[2] * quaternion[3];
 122
 123   return Matrix4(
 124     static_cast<float>( 1 - 2 * ( yy + zz ) ),
 125     static_cast<float>(     2 * ( xy + zw ) ),
 126     static_cast<float>(     2 * ( xz - yw ) ),
 127     0,
 128     static_cast<float>(     2 * ( xy - zw ) ),
 129     static_cast<float>( 1 - 2 * ( xx + zz ) ),
 130     static_cast<float>(     2 * ( yz + xw ) ),
 131     0,
 132     static_cast<float>(     2 * ( xz + yw ) ),
 133     static_cast<float>(     2 * ( yz - xw ) ),
 134     static_cast<float>( 1 - 2 * ( xx + yy ) ),
 135     0,
 136     0,
 137     0,
 138     0,
 139     1
 140   );
 141
 142 #else
 143   const double x2 = quaternion[0] + quaternion[0];
 144   const double y2 = quaternion[1] + quaternion[1];
 145   const double z2 = quaternion[2] + quaternion[2];
 146   const double xx = quaternion[0] * x2;
 147   const double xy = quaternion[0] * y2;
 148   const double xz = quaternion[0] * z2;
 149   const double yy = quaternion[1] * y2;
 150   const double yz = quaternion[1] * z2;
 151   const double zz = quaternion[2] * z2;
 152   const double wx = quaternion[3] * x2;
 153   const double wy = quaternion[3] * y2;
 154   const double wz = quaternion[3] * z2;
 155
 156   return Matrix4(
 157     static_cast<float>( 1.0 - (yy + zz) ),
 158     static_cast<float>(xy + wz),
 159     static_cast<float>(xz - wy),
 160     0,
 161     static_cast<float>(xy - wz),
 162     static_cast<float>( 1.0 - (xx + zz) ),
 163     static_cast<float>(yz + wx),
 164     0,
 165     static_cast<float>(xz + wy),
 166     static_cast<float>(yz - wx),
 167     static_cast<float>( 1.0 - (xx + yy) ),
 168     0,
 169     0,
 170     0,
 171     0,
 172     1
 173   );
 174
 175 #endif
 176 }
 177
 178 const double c_half_sqrt2 = 0.70710678118654752440084436210485;
 179 const float c_half_sqrt2f = static_cast<float>(c_half_sqrt2);
 180
 181 inline bool quaternion_component_is_90(float component)
 182 {
 183   return (fabs(component) - c_half_sqrt2) < 0.001;
 184 }
 185
 186 inline Matrix4 matrix4_rotation_for_quaternion_quantised(const Quaternion& quaternion)
 187 {
 188   if(quaternion.y() == 0
 189     && quaternion.z() == 0
 190     && quaternion_component_is_90(quaternion.x())
 191     && quaternion_component_is_90(quaternion.w()))
 192   {
 193     return matrix4_rotation_for_sincos_x((quaternion.x() > 0) ? 1.f : -1.f, 0);
 194   }
 195
 196   if(quaternion.x() == 0
 197     && quaternion.z() == 0
 198     && quaternion_component_is_90(quaternion.y())
 199     && quaternion_component_is_90(quaternion.w()))
 200   {
 201     return matrix4_rotation_for_sincos_y((quaternion.y() > 0) ? 1.f : -1.f, 0);
 202   }
 203
 204   if(quaternion.x() == 0
 205     && quaternion.y() == 0
 206     && quaternion_component_is_90(quaternion.z())
 207     && quaternion_component_is_90(quaternion.w()))
 208   {
 209     return matrix4_rotation_for_sincos_z((quaternion.z() > 0) ? 1.f : -1.f, 0);
 210   }
 211
 212   return matrix4_rotation_for_quaternion(quaternion);
 213 }
 214
 215 inline Quaternion quaternion_for_matrix4_rotation(const Matrix4& matrix4)
 216 {
 217   Matrix4 transposed = matrix4_transposed(matrix4);
 218
 219   double trace = transposed[0] + transposed[5] + transposed[10] + 1.0;
 220
 221   if(trace > 0.0001)
 222   {
 223     double S = 0.5 / sqrt(trace);
 224     return Quaternion(
 225       static_cast<float>((transposed[9] - transposed[6]) * S),
 226       static_cast<float>((transposed[2] - transposed[8]) * S),
 227       static_cast<float>((transposed[4] - transposed[1]) * S),
 228       static_cast<float>(0.25 / S)
 229     );
 230   }
 231
 232   if(transposed[0] >= transposed[5] && transposed[0] >= transposed[10])
 233   {
 234     double S = 2.0 * sqrt(1.0 + transposed[0] - transposed[5] - transposed[10]);
 235     return Quaternion(
 236       static_cast<float>(0.25 / S),
 237       static_cast<float>((transposed[1] + transposed[4]) / S),
 238       static_cast<float>((transposed[2] + transposed[8]) / S),
 239       static_cast<float>((transposed[6] + transposed[9]) / S)
 240     );
 241   }
 242
 243   if(transposed[5] >= transposed[0] && transposed[5] >= transposed[10])
 244   {
 245     double S = 2.0 * sqrt(1.0 + transposed[5] - transposed[0] - transposed[10]);
 246     return Quaternion(
 247       static_cast<float>((transposed[1] + transposed[4]) / S),
 248       static_cast<float>(0.25 / S),
 249       static_cast<float>((transposed[6] + transposed[9]) / S),
 250       static_cast<float>((transposed[2] + transposed[8]) / S)
 251     );
 252   }
 253
 254   double S = 2.0 * sqrt(1.0 + transposed[10] - transposed[0] - transposed[5]);
 255   return Quaternion(
 256     static_cast<float>((transposed[2] + transposed[8]) / S),
 257     static_cast<float>((transposed[6] + transposed[9]) / S),
 258     static_cast<float>(0.25 / S),
 259     static_cast<float>((transposed[1] + transposed[4]) / S)
 260   );
 261 }
 262
 263 /// \brief Returns \p self concatenated with the rotation transform produced by \p rotation.
 264 /// The concatenated rotation occurs before \p self.
 265 inline Matrix4 matrix4_rotated_by_quaternion(const Matrix4& self, const Quaternion& rotation)
 266 {
 267   return matrix4_multiplied_by_matrix4(self, matrix4_rotation_for_quaternion(rotation));
 268 }
 269
 270 /// \brief Concatenates \p self with the rotation transform produced by \p rotation.
 271 /// The concatenated rotation occurs before \p self.
 272 inline void matrix4_rotate_by_quaternion(Matrix4& self, const Quaternion& rotation)
 273 {
 274   self = matrix4_rotated_by_quaternion(self, rotation);
 275 }
 276
 277 /// \brief Rotates \p self by \p rotation, using \p pivotpoint.
 278 inline void matrix4_pivoted_rotate_by_quaternion(Matrix4& self, const Quaternion& rotation, const Vector3& pivotpoint)
 279 {
 280   matrix4_translate_by_vec3(self, pivotpoint);
 281   matrix4_rotate_by_quaternion(self, rotation);
 282   matrix4_translate_by_vec3(self, vector3_negated(pivotpoint));
 283 }
 284
 285 inline Vector3 quaternion_transformed_point(const Quaternion& quaternion, const Vector3& point)
 286 {
 287   double xx = quaternion.x() * quaternion.x();
 288   double yy = quaternion.y() * quaternion.y();
 289   double zz = quaternion.z() * quaternion.z();
 290   double ww = quaternion.w() * quaternion.w();
 291
 292   double xy2 = quaternion.x() * quaternion.y() * 2;
 293   double xz2 = quaternion.x() * quaternion.z() * 2;
 294   double xw2 = quaternion.x() * quaternion.w() * 2;
 295   double yz2 = quaternion.y() * quaternion.z() * 2;
 296   double yw2 = quaternion.y() * quaternion.w() * 2;
 297   double zw2 = quaternion.z() * quaternion.w() * 2;
 298
 299         return Vector3(
 300     static_cast<float>(ww * point.x() + yw2 * point.z() - zw2 * point.y() + xx * point.x() + xy2 * point.y() + xz2 * point.z() - zz * point.x() - yy * point.x()),
 301     static_cast<float>(xy2 * point.x() + yy * point.y() + yz2 * point.z() + zw2 * point.x() - zz * point.y() + ww * point.y() - xw2 * point.z() - xx * point.y()),
 302     static_cast<float>(xz2 * point.x() + yz2 * point.y() + zz * point.z() - yw2 * point.x() - yy * point.z() + xw2 * point.y() - xx * point.z() + ww * point.z())
 303   );
 304 }
 305
 306 /// \brief Constructs a pure-rotation transform from \p axis and \p angle (radians).
 307 inline Matrix4 matrix4_rotation_for_axisangle(const Vector3& axis, double angle)
 308 {
 309   return matrix4_rotation_for_quaternion(quaternion_for_axisangle(axis, angle));
 310 }
 311
 312 /// \brief Rotates \p self about \p axis by \p angle.
 313 inline void matrix4_rotate_by_axisangle(Matrix4& self, const Vector3& axis, double angle)
 314 {
 315   matrix4_multiply_by_matrix4(self, matrix4_rotation_for_axisangle(axis, angle));
 316 }
 317
 318 /// \brief Rotates \p self about \p axis by \p angle using \p pivotpoint.
 319 inline void matrix4_pivoted_rotate_by_axisangle(Matrix4& self, const Vector3& axis, double angle, const Vector3& pivotpoint)
 320 {
 321   matrix4_translate_by_vec3(self, pivotpoint);
 322   matrix4_rotate_by_axisangle(self, axis, angle);
 323   matrix4_translate_by_vec3(self, vector3_negated(pivotpoint));
 324 }
 325
 326
 327 #endif