1 /* 2 * Copyright 2013 The Android Open Source Project 3 * 4 * Licensed under the Apache License, Version 2.0 (the "License"); 5 * you may not use this file except in compliance with the License. 6 * You may obtain a copy of the License at 7 * 8 * http://www.apache.org/licenses/LICENSE-2.0 9 * 10 * Unless required by applicable law or agreed to in writing, software 11 * distributed under the License is distributed on an "AS IS" BASIS, 12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 * See the License for the specific language governing permissions and 14 * limitations under the License. 15 */ 16 17 18 #pragma once 19 20 #include <math.h> 21 #include <stdint.h> 22 #include <sys/types.h> 23 24 #include <iostream> 25 26 #include <math/vec3.h> 27 28 #define PURE __attribute__((pure)) 29 30 namespace android { 31 namespace details { 32 // ------------------------------------------------------------------------------------- 33 34 /* 35 * No user serviceable parts here. 36 * 37 * Don't use this file directly, instead include ui/quat.h 38 */ 39 40 41 /* 42 * TQuatProductOperators implements basic arithmetic and basic compound assignment 43 * operators on a quaternion of type BASE<T>. 44 * 45 * BASE only needs to implement operator[] and size(). 46 * By simply inheriting from TQuatProductOperators<BASE, T> BASE will automatically 47 * get all the functionality here. 48 */ 49 50 template <template<typename T> class QUATERNION, typename T> 51 class TQuatProductOperators { 52 public: 53 /* compound assignment from a another quaternion of the same size but different 54 * element type. 55 */ 56 template <typename OTHER> 57 QUATERNION<T>& operator *= (const QUATERNION<OTHER>& r) { 58 QUATERNION<T>& q = static_cast<QUATERNION<T>&>(*this); 59 q = q * r; 60 return q; 61 } 62 63 /* compound assignment products by a scalar 64 */ 65 QUATERNION<T>& operator *= (T v) { 66 QUATERNION<T>& lhs = static_cast<QUATERNION<T>&>(*this); 67 for (size_t i = 0; i < QUATERNION<T>::size(); i++) { 68 lhs[i] *= v; 69 } 70 return lhs; 71 } 72 QUATERNION<T>& operator /= (T v) { 73 QUATERNION<T>& lhs = static_cast<QUATERNION<T>&>(*this); 74 for (size_t i = 0; i < QUATERNION<T>::size(); i++) { 75 lhs[i] /= v; 76 } 77 return lhs; 78 } 79 80 /* 81 * NOTE: the functions below ARE NOT member methods. They are friend functions 82 * with they definition inlined with their declaration. This makes these 83 * template functions available to the compiler when (and only when) this class 84 * is instantiated, at which point they're only templated on the 2nd parameter 85 * (the first one, BASE<T> being known). 86 */ 87 88 /* The operators below handle operation between quaternion of the same size 89 * but of a different element type. 90 */ 91 template<typename RT> 92 friend inline 93 constexpr QUATERNION<T> PURE operator *(const QUATERNION<T>& q, const QUATERNION<RT>& r) { 94 // could be written as: 95 // return QUATERNION<T>( 96 // q.w*r.w - dot(q.xyz, r.xyz), 97 // q.w*r.xyz + r.w*q.xyz + cross(q.xyz, r.xyz)); 98 99 return QUATERNION<T>( 100 q.w*r.w - q.x*r.x - q.y*r.y - q.z*r.z, 101 q.w*r.x + q.x*r.w + q.y*r.z - q.z*r.y, 102 q.w*r.y - q.x*r.z + q.y*r.w + q.z*r.x, 103 q.w*r.z + q.x*r.y - q.y*r.x + q.z*r.w); 104 } 105 106 template<typename RT> 107 friend inline 108 constexpr TVec3<T> PURE operator *(const QUATERNION<T>& q, const TVec3<RT>& v) { 109 // note: if q is known to be a unit quaternion, then this simplifies to: 110 // TVec3<T> t = 2 * cross(q.xyz, v) 111 // return v + (q.w * t) + cross(q.xyz, t) 112 return imaginary(q * QUATERNION<T>(v, 0) * inverse(q)); 113 } 114 115 116 /* For quaternions, we use explicit "by a scalar" products because it's much faster 117 * than going (implicitly) through the quaternion multiplication. 118 * For reference: we could use the code below instead, but it would be a lot slower. 119 * friend inline 120 * constexpr BASE<T> PURE operator *(const BASE<T>& q, const BASE<T>& r) { 121 * return BASE<T>( 122 * q.w*r.w - q.x*r.x - q.y*r.y - q.z*r.z, 123 * q.w*r.x + q.x*r.w + q.y*r.z - q.z*r.y, 124 * q.w*r.y - q.x*r.z + q.y*r.w + q.z*r.x, 125 * q.w*r.z + q.x*r.y - q.y*r.x + q.z*r.w); 126 * 127 */ 128 friend inline 129 constexpr QUATERNION<T> PURE operator *(QUATERNION<T> q, T scalar) { 130 // don't pass q by reference because we need a copy anyways 131 return q *= scalar; 132 } 133 friend inline 134 constexpr QUATERNION<T> PURE operator *(T scalar, QUATERNION<T> q) { 135 // don't pass q by reference because we need a copy anyways 136 return q *= scalar; 137 } 138 139 friend inline 140 constexpr QUATERNION<T> PURE operator /(QUATERNION<T> q, T scalar) { 141 // don't pass q by reference because we need a copy anyways 142 return q /= scalar; 143 } 144 }; 145 146 147 /* 148 * TQuatFunctions implements functions on a quaternion of type BASE<T>. 149 * 150 * BASE only needs to implement operator[] and size(). 151 * By simply inheriting from TQuatFunctions<BASE, T> BASE will automatically 152 * get all the functionality here. 153 */ 154 template <template<typename T> class QUATERNION, typename T> 155 class TQuatFunctions { 156 public: 157 /* 158 * NOTE: the functions below ARE NOT member methods. They are friend functions 159 * with they definition inlined with their declaration. This makes these 160 * template functions available to the compiler when (and only when) this class 161 * is instantiated, at which point they're only templated on the 2nd parameter 162 * (the first one, BASE<T> being known). 163 */ 164 165 template<typename RT> 166 friend inline 167 constexpr T PURE dot(const QUATERNION<T>& p, const QUATERNION<RT>& q) { 168 return p.x * q.x + 169 p.y * q.y + 170 p.z * q.z + 171 p.w * q.w; 172 } 173 174 friend inline 175 constexpr T PURE norm(const QUATERNION<T>& q) { 176 return std::sqrt( dot(q, q) ); 177 } 178 179 friend inline 180 constexpr T PURE length(const QUATERNION<T>& q) { 181 return norm(q); 182 } 183 184 friend inline 185 constexpr T PURE length2(const QUATERNION<T>& q) { 186 return dot(q, q); 187 } 188 189 friend inline 190 constexpr QUATERNION<T> PURE normalize(const QUATERNION<T>& q) { 191 return length(q) ? q / length(q) : QUATERNION<T>(1); 192 } 193 194 friend inline 195 constexpr QUATERNION<T> PURE conj(const QUATERNION<T>& q) { 196 return QUATERNION<T>(q.w, -q.x, -q.y, -q.z); 197 } 198 199 friend inline 200 constexpr QUATERNION<T> PURE inverse(const QUATERNION<T>& q) { 201 return conj(q) * (1 / dot(q, q)); 202 } 203 204 friend inline 205 constexpr T PURE real(const QUATERNION<T>& q) { 206 return q.w; 207 } 208 209 friend inline 210 constexpr TVec3<T> PURE imaginary(const QUATERNION<T>& q) { 211 return q.xyz; 212 } 213 214 friend inline 215 constexpr QUATERNION<T> PURE unreal(const QUATERNION<T>& q) { 216 return QUATERNION<T>(q.xyz, 0); 217 } 218 219 friend inline 220 constexpr QUATERNION<T> PURE cross(const QUATERNION<T>& p, const QUATERNION<T>& q) { 221 return unreal(p*q); 222 } 223 224 friend inline 225 QUATERNION<T> PURE exp(const QUATERNION<T>& q) { 226 const T nq(norm(q.xyz)); 227 return std::exp(q.w)*QUATERNION<T>((sin(nq)/nq)*q.xyz, cos(nq)); 228 } 229 230 friend inline 231 QUATERNION<T> PURE log(const QUATERNION<T>& q) { 232 const T nq(norm(q)); 233 return QUATERNION<T>((std::acos(q.w/nq)/norm(q.xyz))*q.xyz, log(nq)); 234 } 235 236 friend inline 237 QUATERNION<T> PURE pow(const QUATERNION<T>& q, T a) { 238 // could also be computed as: exp(a*log(q)); 239 const T nq(norm(q)); 240 const T theta(a*std::acos(q.w / nq)); 241 return std::pow(nq, a) * QUATERNION<T>(normalize(q.xyz) * std::sin(theta), std::cos(theta)); 242 } 243 244 friend inline 245 QUATERNION<T> PURE slerp(const QUATERNION<T>& p, const QUATERNION<T>& q, T t) { 246 // could also be computed as: pow(q * inverse(p), t) * p; 247 const T d = dot(p, q); 248 const T npq = sqrt(dot(p, p) * dot(q, q)); // ||p|| * ||q|| 249 const T a = std::acos(std::abs(d) / npq); 250 const T a0 = a * (1 - t); 251 const T a1 = a * t; 252 const T isina = 1 / sin(a); 253 const T s0 = std::sin(a0) * isina; 254 const T s1 = std::sin(a1) * isina; 255 // ensure we're taking the "short" side 256 return normalize(s0 * p + ((d < 0) ? (-s1) : (s1)) * q); 257 } 258 259 friend inline 260 constexpr QUATERNION<T> PURE lerp(const QUATERNION<T>& p, const QUATERNION<T>& q, T t) { 261 return ((1 - t) * p) + (t * q); 262 } 263 264 friend inline 265 constexpr QUATERNION<T> PURE nlerp(const QUATERNION<T>& p, const QUATERNION<T>& q, T t) { 266 return normalize(lerp(p, q, t)); 267 } 268 269 friend inline 270 constexpr QUATERNION<T> PURE positive(const QUATERNION<T>& q) { 271 return q.w < 0 ? -q : q; 272 } 273 }; 274 275 /* 276 * TQuatDebug implements functions on a vector of type BASE<T>. 277 * 278 * BASE only needs to implement operator[] and size(). 279 * By simply inheriting from TQuatDebug<BASE, T> BASE will automatically 280 * get all the functionality here. 281 */ 282 template <template<typename T> class QUATERNION, typename T> 283 class TQuatDebug { 284 public: 285 /* 286 * NOTE: the functions below ARE NOT member methods. They are friend functions 287 * with they definition inlined with their declaration. This makes these 288 * template functions available to the compiler when (and only when) this class 289 * is instantiated, at which point they're only templated on the 2nd parameter 290 * (the first one, BASE<T> being known). 291 */ 292 friend std::ostream& operator<< (std::ostream& stream, const QUATERNION<T>& q) { 293 return stream << "< " << q.w << " + " << q.x << "i + " << q.y << "j + " << q.z << "k >"; 294 } 295 }; 296 #undef PURE 297 298 // ------------------------------------------------------------------------------------- 299 } // namespace details 300 } // namespace android 301
