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
dot(const QUATERNION<T> & p,const QUATERNION<RT> & q)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
norm(const QUATERNION<T> & q)175     constexpr T PURE norm(const QUATERNION<T>& q) {
176         return std::sqrt( dot(q, q) );
177     }
178 
179     friend inline
length(const QUATERNION<T> & q)180     constexpr T PURE length(const QUATERNION<T>& q) {
181         return norm(q);
182     }
183 
184     friend inline
length2(const QUATERNION<T> & q)185     constexpr T PURE length2(const QUATERNION<T>& q) {
186         return dot(q, q);
187     }
188 
189     friend inline
normalize(const QUATERNION<T> & q)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
conj(const QUATERNION<T> & q)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
inverse(const QUATERNION<T> & q)200     constexpr QUATERNION<T> PURE inverse(const QUATERNION<T>& q) {
201         return conj(q) * (1 / dot(q, q));
202     }
203 
204     friend inline
real(const QUATERNION<T> & q)205     constexpr T PURE real(const QUATERNION<T>& q) {
206         return q.w;
207     }
208 
209     friend inline
imaginary(const QUATERNION<T> & q)210     constexpr TVec3<T> PURE imaginary(const QUATERNION<T>& q) {
211         return q.xyz;
212     }
213 
214     friend inline
unreal(const QUATERNION<T> & q)215     constexpr QUATERNION<T> PURE unreal(const QUATERNION<T>& q) {
216         return QUATERNION<T>(q.xyz, 0);
217     }
218 
219     friend inline
cross(const QUATERNION<T> & p,const QUATERNION<T> & q)220     constexpr QUATERNION<T> PURE cross(const QUATERNION<T>& p, const QUATERNION<T>& q) {
221         return unreal(p*q);
222     }
223 
224     friend inline
exp(const QUATERNION<T> & q)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
log(const QUATERNION<T> & q)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
pow(const QUATERNION<T> & q,T a)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
slerp(const QUATERNION<T> & p,const QUATERNION<T> & q,T t)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
lerp(const QUATERNION<T> & p,const QUATERNION<T> & q,T t)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
nlerp(const QUATERNION<T> & p,const QUATERNION<T> & q,T t)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
positive(const QUATERNION<T> & q)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