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
30namespace android {
31namespace 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
50template <template<typename T> class QUATERNION, typename T>
51class TQuatProductOperators {
52public:
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 */
154template <template<typename T> class QUATERNION, typename T>
155class TQuatFunctions {
156public:
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 */
282template <template<typename T> class QUATERNION, typename T>
283class TQuatDebug {
284public:
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