1 /* 2 Copyright (C) 2010-2014 Kristian Duske 3 4 This file is part of TrenchBroom. 5 6 TrenchBroom is free software: you can redistribute it and/or modify 7 it under the terms of the GNU General Public License as published by 8 the Free Software Foundation, either version 3 of the License, or 9 (at your option) any later version. 10 11 TrenchBroom is distributed in the hope that it will be useful, 12 but WITHOUT ANY WARRANTY; without even the implied warranty of 13 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 GNU General Public License for more details. 15 16 You should have received a copy of the GNU General Public License 17 along with TrenchBroom. If not, see <http://www.gnu.org/licenses/>. 18 */ 19 20 #ifndef TrenchBroom_Quat_h 21 #define TrenchBroom_Quat_h 22 23 #include "MathUtils.h" 24 #include "Vec.h" 25 26 #include <cassert> 27 28 template <typename T> 29 class Quat { 30 public: 31 T r; 32 Vec<T,3> v; 33 Quat()34 Quat() : 35 r(static_cast<float>(0.0)), 36 v(Vec<T,3>::Null) {} 37 Quat(const T i_r,const Vec<T,3> & i_v)38 Quat(const T i_r, const Vec<T,3>& i_v) : 39 r(i_r), 40 v(i_v) {} 41 42 /** 43 * Creates a new quaternion that represent a clounter-clockwise rotation by the given angle (in radians) about 44 * the given axis. 45 */ Quat(const Vec<T,3> & axis,const T angle)46 Quat(const Vec<T,3>& axis, const T angle) { 47 setRotation(axis, angle); 48 } 49 50 /** 51 * Creates a new quaternion that rotates the 1st given vector onto the 2nd given vector. Both vectors are 52 * expected to be normalized. 53 */ Quat(const Vec<T,3> & from,const Vec<T,3> & to)54 Quat(const Vec<T,3>& from, const Vec<T,3>& to) { 55 assert(from.isNormalized()); 56 assert(to.isNormalized()); 57 58 const T cos = from.dot(to); 59 if (Math::eq(std::abs(cos), 1.0)) { 60 setRotation(Vec<T,3>::PosZ, 0.0); 61 } else { 62 const Vec<T,3> axis = crossed(from, to).normalized(); 63 const T angle = std::acos(cos); 64 setRotation(axis, angle); 65 } 66 } 67 68 template <typename U> Quat(const Quat<U> & other)69 Quat(const Quat<U>& other) : 70 r(static_cast<T>(other.r)), 71 v(other.v) {} 72 73 const Quat<T> operator-() const { 74 return Quat(-r, v); 75 } 76 77 const Quat<T> operator*(const T right) const { 78 return Quat(r * right, v); 79 } 80 81 Quat<T>& operator*= (const T right) { 82 r *= right; 83 return *this; 84 } 85 86 const Quat<T> operator*(const Quat<T>& right) const { 87 Quat<T> result = *this; 88 return result *= right; 89 } 90 91 Quat<T>& operator*= (const Quat<T>& right) { 92 const T& t = right.r; 93 const Vec<T,3>& w = right.v; 94 95 const T nx = r * w.x() + t * v.x() + v.y() * w.z() - v.z() * w.y(); 96 const T ny = r * w.y() + t * v.y() + v.z() * w.x() - v.x() * w.z(); 97 const T nz = r * w.z() + t * v.z() + v.x() * w.y() - v.y() * w.x(); 98 99 r = r * t - v.dot(w); 100 v[0] = nx; 101 v[1] = ny; 102 v[2] = nz; 103 return *this; 104 } 105 106 const Vec<T,3> operator*(const Vec<T,3>& right) const { 107 Quat<T> p; 108 p.r = 0.0; 109 p.v = right; 110 p = *this * p * conjugated(); 111 return p.v; 112 } 113 setRotation(const Vec<T,3> & axis,const T angle)114 Quat<T>& setRotation(const Vec<T,3>& axis, const T angle) { 115 assert(axis.isNormalized()); 116 r = std::cos(angle / static_cast<T>(2.0)); 117 v = axis * std::sin(angle / static_cast<T>(2.0)); 118 return *this; 119 } 120 angle()121 float angle() const { 122 return static_cast<T>(2.0) * std::acos(r); 123 } 124 axis()125 Vec<T,3> axis() const { 126 if (v.null()) 127 return v; 128 return v / std::sin(angle() / static_cast<T>(2.0)); 129 } 130 conjugate()131 Quat<T>& conjugate() { 132 v = -v; 133 return *this; 134 } 135 conjugated()136 const Quat conjugated() const { 137 Quat<T> result; 138 result.r = r; 139 result.v = -v; 140 return result; 141 } 142 }; 143 144 typedef Quat<float> Quatf; 145 typedef Quat<double> Quatd; 146 147 template <typename T> 148 Quat<T> operator*(const T left, const Quat<T>& right) { 149 return Quat<T>(left * right.r, right.v); 150 } 151 152 #endif 153