Dogcows Code - chaz/yoink/blob - src/quaternion.hh

   1
   2 /*******************************************************************************
   3
   4  Copyright (c) 2009, Charles McGarvey
   5  All rights reserved.
   6
   7  Redistribution   and   use  in  source  and  binary  forms,  with  or  without
   8  modification, are permitted provided that the following conditions are met:
   9
  10    * Redistributions  of  source  code  must retain the above copyright notice,
  11      this list of conditions and the following disclaimer.
  12    * Redistributions  in binary form must reproduce the above copyright notice,
  13      this  list of conditions and the following disclaimer in the documentation
  14      and/or other materials provided with the distribution.
  15
  16  THIS  SOFTWARE  IS  PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
  17  AND  ANY  EXPRESS  OR  IMPLIED  WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  18  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
  19  DISCLAIMED.  IN  NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
  20  FOR  ANY  DIRECT,  INDIRECT,  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  21  DAMAGES  (INCLUDING,  BUT  NOT  LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
  22  SERVICES;  LOSS  OF  USE,  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
  23  CAUSED  AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
  24  OR  TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
  25  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  26
  27 *******************************************************************************/
  28
  29 #ifndef _QUATERNION_HH_
  30 #define _QUATERNION_HH_
  31
  32 #include <cassert>
  33
  34 #include "math.hh"
  35 #include "matrix.hh"
  36 #include "vector.hh"
  37
  38
  39 //
  40 // Quaternion -> 3x3 Matrix
  41 //
  42 // | w^2 + x^2 - y^2 - z^2         2xy - 2wz               2xz + 2yw       |
  43 // |       2xy + 2wz         w^2 - x^2 + y^2 - z^2         2yz - 2wx       |
  44 // |       2xz - 2wy               2yz - 2wx         w^2 - x^2 - y^2 + z^2 |
  45 //
  46
  47 namespace dc {
  48
  49
  50 class quaternion
  51 {
  52 public:
  53         // Constructors.
  54         quaternion() {}
  55         quaternion(scalar X, scalar Y, scalar Z, scalar W) {
  56                 vec.x = X; vec.y = Y; vec.z = Z; w = W;
  57         }
  58         quaternion(vector3 v, scalar W = 1.0) {
  59                 vec = v; w = W;
  60         }
  61         quaternion(scalar q[4]) {
  62                 vec.x = q[0]; vec.y = q[1]; vec.z = q[2]; w = q[3];
  63         }
  64
  65         // Accessible by index.
  66         const scalar& operator [] (int i) const {
  67                 assert(i >= 0 && i <= 3 && "Index into quaternion out of bounds.");
  68                 return *((&vec.x) + i);
  69         }
  70         scalar& operator [] (int i) {
  71                 assert(i >= 0 && i <= 3 && "Index into quaternion out of bounds.");
  72                 //return vec[i];
  73                 return *((&vec.x) + i);
  74         }
  75
  76         // Basic maths.
  77         quaternion operator + (const quaternion& q) const {
  78                 return quaternion(vec + q.vec, w + q.w);
  79         }
  80         quaternion operator - (const quaternion& q) const {
  81                 return quaternion(vec - q.vec, w - q.w);
  82         }
  83         quaternion operator * (const quaternion& q) const {
  84                 return quaternion(q.w * vec + w * q.vec + q.vec.cross(vec),
  85                                                   w * q.w - q.vec.dot(vec));
  86         }
  87         quaternion operator * (scalar s) const {
  88                 return quaternion(vec * s, w * s);
  89         }
  90         quaternion operator / (scalar s) const {
  91                 return quaternion(vec / s, w / s);
  92         }
  93
  94         quaternion& operator += (const quaternion& q) {
  95                 vec += q.vec; w += q.w;
  96                 return *this;
  97         }
  98         quaternion& operator -= (const quaternion& q) {
  99                 vec -= q.vec; w -= q.w;
 100                 return *this;
 101         }
 102         quaternion& operator *= (const quaternion& q) {
 103                 scalar W = w;
 104                 w = W * q.w - q.vec.dot(vec);
 105                 vec = q.w * vec + W * q.vec + q.vec.cross(vec);
 106                 return *this;
 107         }
 108         quaternion& operator *= (scalar s) {
 109                 vec *= s; w *= s;
 110                 return *this;
 111         }
 112         quaternion& operator /= (scalar s) {
 113                 vec /= s; w /= s;
 114                 return *this;
 115         }
 116
 117         // Unary operators.
 118         quaternion operator - () const {
 119                 return quaternion(-vec, -w);
 120         }
 121
 122         // Quaternion operations.
 123         quaternion conjugate() const {
 124                 return quaternion(-vec, w);
 125         }
 126         void normalize(const vector3& v)
 127         {
 128                 scalar len = length();
 129                 if (len != 0.0) { *this /= len; }
 130         }
 131
 132         scalar length2() const {
 133                 return vec.x * vec.x + vec.y * vec.y + vec.z * vec.z + w * w;
 134         }
 135         scalar length() const {
 136                 return std::sqrt(length2());
 137         }
 138
 139         // Converting to other types.
 140         matrix3 matrix() const {
 141                 scalar x2 = vec.x * vec.x;
 142                 scalar y2 = vec.y * vec.y;
 143                 scalar z2 = vec.z * vec.z;
 144                 scalar w2 = w * w;
 145                 scalar xy = vec.x * vec.y;
 146                 scalar xz = vec.x * vec.z;
 147                 scalar yzwx = 2 * (vec.y * vec.z - w * vec.x);
 148                 scalar wy = w * vec.y;
 149                 scalar wz = w * vec.z;
 150                 return matrix3(x2 - y2 - z2 + w2, 2 * (xy - wz), 2 * (xz + wy),
 151                                            2 * (xy + wz), w2 - x2 + y2 - z2, yzwx,
 152                                            2 * (xz - wy), yzwx, w2 - x2 - y2 + z2);
 153         }
 154         vector3 axis(scalar& angle) { // Axis of rotation, w/ angle of rotation.
 155                 vector3 axisVec = axis();
 156                 angle = 2 * std::acos(w);
 157                 return axisVec;
 158         }
 159         vector3 axis() {
 160                 scalar sa = std::sqrt(1 - w * w);
 161                 return vec / sa;
 162         }
 163
 164         // Checking equality.
 165         bool operator == (const quaternion& q) const {
 166                 return vec == q.vec && equals(w,q.w);
 167         }
 168         bool operator != (const quaternion& q) const {
 169                 return !(*this == q);
 170         }
 171
 172         // Data.
 173         vector3 vec;
 174         scalar w;
 175 };
 176
 177 inline quaternion operator * (scalar s, const quaternion& q) {
 178         return q * s;
 179 }
 180 inline quaternion operator / (scalar s, const quaternion& q) {
 181         return quaternion(s / q.vec, s / q.w);
 182 }
 183
 184 } // namespace dc
 185
 186
 187 #endif // _QUATERNION_HH_
 188