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#pragma once
18
19#include <math/quat.h>
20#include <math/TMatHelpers.h>
21#include <math/vec3.h>
22#include <stdint.h>
23#include <sys/types.h>
24
25#define PURE __attribute__((pure))
26
27#if __cplusplus >= 201402L
28#define CONSTEXPR constexpr
29#else
30#define CONSTEXPR
31#endif
32
33namespace android {
34// -------------------------------------------------------------------------------------
35namespace details {
36
37template<typename T>
38class TQuaternion;
39
40/**
41 * A 3x3 column-major matrix class.
42 *
43 * Conceptually a 3x3 matrix is a an array of 3 column vec3:
44 *
45 * mat3 m =
46 *      \f$
47 *      \left(
48 *      \begin{array}{ccc}
49 *      m[0] & m[1] & m[2] \\
50 *      \end{array}
51 *      \right)
52 *      \f$
53 *      =
54 *      \f$
55 *      \left(
56 *      \begin{array}{ccc}
57 *      m[0][0] & m[1][0] & m[2][0] \\
58 *      m[0][1] & m[1][1] & m[2][1] \\
59 *      m[0][2] & m[1][2] & m[2][2] \\
60 *      \end{array}
61 *      \right)
62 *      \f$
63 *      =
64 *      \f$
65 *      \left(
66 *      \begin{array}{ccc}
67 *      m(0,0) & m(0,1) & m(0,2) \\
68 *      m(1,0) & m(1,1) & m(1,2) \\
69 *      m(2,0) & m(2,1) & m(2,2) \\
70 *      \end{array}
71 *      \right)
72 *      \f$
73 *
74 * m[n] is the \f$ n^{th} \f$ column of the matrix and is a vec3.
75 *
76 */
77template <typename T>
78class TMat33 :  public TVecUnaryOperators<TMat33, T>,
79                public TVecComparisonOperators<TMat33, T>,
80                public TVecAddOperators<TMat33, T>,
81                public TMatProductOperators<TMat33, T>,
82                public TMatSquareFunctions<TMat33, T>,
83                public TMatTransform<TMat33, T>,
84                public TMatHelpers<TMat33, T>,
85                public TMatDebug<TMat33, T> {
86public:
87    enum no_init { NO_INIT };
88    typedef T value_type;
89    typedef T& reference;
90    typedef T const& const_reference;
91    typedef size_t size_type;
92    typedef TVec3<T> col_type;
93    typedef TVec3<T> row_type;
94
95    static constexpr size_t COL_SIZE = col_type::SIZE;  // size of a column (i.e.: number of rows)
96    static constexpr size_t ROW_SIZE = row_type::SIZE;  // size of a row (i.e.: number of columns)
97    static constexpr size_t NUM_ROWS = COL_SIZE;
98    static constexpr size_t NUM_COLS = ROW_SIZE;
99
100private:
101    /*
102     *  <--  N columns  -->
103     *
104     *  a[0][0] a[1][0] a[2][0] ... a[N][0]    ^
105     *  a[0][1] a[1][1] a[2][1] ... a[N][1]    |
106     *  a[0][2] a[1][2] a[2][2] ... a[N][2]  M rows
107     *  ...                                    |
108     *  a[0][M] a[1][M] a[2][M] ... a[N][M]    v
109     *
110     *  COL_SIZE = M
111     *  ROW_SIZE = N
112     *  m[0] = [ a[0][0] a[0][1] a[0][2] ... a[0][M] ]
113     */
114
115    col_type m_value[NUM_COLS];
116
117public:
118    // array access
119    inline constexpr col_type const& operator[](size_t column) const {
120#if __cplusplus >= 201402L
121        // only possible in C++0x14 with constexpr
122        assert(column < NUM_COLS);
123#endif
124        return m_value[column];
125    }
126
127    inline col_type& operator[](size_t column) {
128        assert(column < NUM_COLS);
129        return m_value[column];
130    }
131
132    // -----------------------------------------------------------------------
133    // we want the compiler generated versions for these...
134    TMat33(const TMat33&) = default;
135    ~TMat33() = default;
136    TMat33& operator = (const TMat33&) = default;
137
138    /**
139     *  constructors
140     */
141
142    /**
143     * leaves object uninitialized. use with caution.
144     */
145    explicit constexpr TMat33(no_init)
146            : m_value{ col_type(col_type::NO_INIT),
147                       col_type(col_type::NO_INIT),
148                       col_type(col_type::NO_INIT) } {}
149
150
151    /**
152     * initialize to identity.
153     *
154     *      \f$
155     *      \left(
156     *      \begin{array}{ccc}
157     *      1 & 0 & 0 \\
158     *      0 & 1 & 0 \\
159     *      0 & 0 & 1 \\
160     *      \end{array}
161     *      \right)
162     *      \f$
163     */
164    CONSTEXPR TMat33();
165
166    /**
167     * initialize to Identity*scalar.
168     *
169     *      \f$
170     *      \left(
171     *      \begin{array}{ccc}
172     *      v & 0 & 0 \\
173     *      0 & v & 0 \\
174     *      0 & 0 & v \\
175     *      \end{array}
176     *      \right)
177     *      \f$
178     */
179    template<typename U>
180    explicit CONSTEXPR TMat33(U v);
181
182    /**
183     * sets the diagonal to a vector.
184     *
185     *      \f$
186     *      \left(
187     *      \begin{array}{ccc}
188     *      v[0] & 0 & 0 \\
189     *      0 & v[1] & 0 \\
190     *      0 & 0 & v[2] \\
191     *      \end{array}
192     *      \right)
193     *      \f$
194     */
195    template <typename U>
196    explicit CONSTEXPR TMat33(const TVec3<U>& v);
197
198    /**
199     * construct from another matrix of the same size
200     */
201    template <typename U>
202    explicit CONSTEXPR TMat33(const TMat33<U>& rhs);
203
204    /**
205     * construct from 3 column vectors.
206     *
207     *      \f$
208     *      \left(
209     *      \begin{array}{ccc}
210     *      v0 & v1 & v2 \\
211     *      \end{array}
212     *      \right)
213     *      \f$
214     */
215    template <typename A, typename B, typename C>
216    CONSTEXPR TMat33(const TVec3<A>& v0, const TVec3<B>& v1, const TVec3<C>& v2);
217
218    /** construct from 9 elements in column-major form.
219     *
220     *      \f$
221     *      \left(
222     *      \begin{array}{ccc}
223     *      m[0][0] & m[1][0] & m[2][0] \\
224     *      m[0][1] & m[1][1] & m[2][1] \\
225     *      m[0][2] & m[1][2] & m[2][2] \\
226     *      \end{array}
227     *      \right)
228     *      \f$
229     */
230    template <
231        typename A, typename B, typename C,
232        typename D, typename E, typename F,
233        typename G, typename H, typename I>
234    CONSTEXPR TMat33(
235           A m00, B m01, C m02,
236           D m10, E m11, F m12,
237           G m20, H m21, I m22);
238
239    /**
240     * construct from a quaternion
241     */
242    template <typename U>
243    explicit CONSTEXPR TMat33(const TQuaternion<U>& q);
244
245    /**
246     * construct from a C array in column major form.
247     */
248    template <typename U>
249    explicit CONSTEXPR TMat33(U const* rawArray);
250
251    /**
252     * orthogonalize only works on matrices of size 3x3
253     */
254    friend inline
255    CONSTEXPR TMat33 orthogonalize(const TMat33& m) {
256        TMat33 ret(TMat33::NO_INIT);
257        ret[0] = normalize(m[0]);
258        ret[2] = normalize(cross(ret[0], m[1]));
259        ret[1] = normalize(cross(ret[2], ret[0]));
260        return ret;
261    }
262};
263
264// ----------------------------------------------------------------------------------------
265// Constructors
266// ----------------------------------------------------------------------------------------
267
268// Since the matrix code could become pretty big quickly, we don't inline most
269// operations.
270
271template <typename T>
272CONSTEXPR TMat33<T>::TMat33() {
273    m_value[0] = col_type(1, 0, 0);
274    m_value[1] = col_type(0, 1, 0);
275    m_value[2] = col_type(0, 0, 1);
276}
277
278template <typename T>
279template <typename U>
280CONSTEXPR TMat33<T>::TMat33(U v) {
281    m_value[0] = col_type(v, 0, 0);
282    m_value[1] = col_type(0, v, 0);
283    m_value[2] = col_type(0, 0, v);
284}
285
286template<typename T>
287template<typename U>
288CONSTEXPR TMat33<T>::TMat33(const TVec3<U>& v) {
289    m_value[0] = col_type(v.x, 0, 0);
290    m_value[1] = col_type(0, v.y, 0);
291    m_value[2] = col_type(0, 0, v.z);
292}
293
294// construct from 9 scalars. Note that the arrangement
295// of values in the constructor is the transpose of the matrix
296// notation.
297template<typename T>
298template <
299    typename A, typename B, typename C,
300    typename D, typename E, typename F,
301    typename G, typename H, typename I>
302CONSTEXPR TMat33<T>::TMat33(
303        A m00, B m01, C m02,
304        D m10, E m11, F m12,
305        G m20, H m21, I m22) {
306    m_value[0] = col_type(m00, m01, m02);
307    m_value[1] = col_type(m10, m11, m12);
308    m_value[2] = col_type(m20, m21, m22);
309}
310
311template <typename T>
312template <typename U>
313CONSTEXPR TMat33<T>::TMat33(const TMat33<U>& rhs) {
314    for (size_t col = 0; col < NUM_COLS; ++col) {
315        m_value[col] = col_type(rhs[col]);
316    }
317}
318
319// Construct from 3 column vectors.
320template <typename T>
321template <typename A, typename B, typename C>
322CONSTEXPR TMat33<T>::TMat33(const TVec3<A>& v0, const TVec3<B>& v1, const TVec3<C>& v2) {
323    m_value[0] = v0;
324    m_value[1] = v1;
325    m_value[2] = v2;
326}
327
328// Construct from raw array, in column-major form.
329template <typename T>
330template <typename U>
331CONSTEXPR TMat33<T>::TMat33(U const* rawArray) {
332    for (size_t col = 0; col < NUM_COLS; ++col) {
333        for (size_t row = 0; row < NUM_ROWS; ++row) {
334            m_value[col][row] = *rawArray++;
335        }
336    }
337}
338
339template <typename T>
340template <typename U>
341CONSTEXPR TMat33<T>::TMat33(const TQuaternion<U>& q) {
342    const U n = q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w;
343    const U s = n > 0 ? 2/n : 0;
344    const U x = s*q.x;
345    const U y = s*q.y;
346    const U z = s*q.z;
347    const U xx = x*q.x;
348    const U xy = x*q.y;
349    const U xz = x*q.z;
350    const U xw = x*q.w;
351    const U yy = y*q.y;
352    const U yz = y*q.z;
353    const U yw = y*q.w;
354    const U zz = z*q.z;
355    const U zw = z*q.w;
356    m_value[0] = col_type(1-yy-zz,    xy+zw,    xz-yw);  // NOLINT
357    m_value[1] = col_type(  xy-zw,  1-xx-zz,    yz+xw);  // NOLINT
358    m_value[2] = col_type(  xz+yw,    yz-xw,  1-xx-yy);  // NOLINT
359}
360
361// ----------------------------------------------------------------------------------------
362// Arithmetic operators outside of class
363// ----------------------------------------------------------------------------------------
364
365/* We use non-friend functions here to prevent the compiler from using
366 * implicit conversions, for instance of a scalar to a vector. The result would
367 * not be what the caller expects.
368 *
369 * Also note that the order of the arguments in the inner loop is important since
370 * it determines the output type (only relevant when T != U).
371 */
372
373// matrix * column-vector, result is a vector of the same type than the input vector
374template <typename T, typename U>
375CONSTEXPR typename TMat33<U>::col_type PURE operator *(const TMat33<T>& lhs, const TVec3<U>& rhs) {
376    // Result is initialized to zero.
377    typename TMat33<U>::col_type result;
378    for (size_t col = 0; col < TMat33<T>::NUM_COLS; ++col) {
379        result += lhs[col] * rhs[col];
380    }
381    return result;
382}
383
384// row-vector * matrix, result is a vector of the same type than the input vector
385template <typename T, typename U>
386CONSTEXPR typename TMat33<U>::row_type PURE operator *(const TVec3<U>& lhs, const TMat33<T>& rhs) {
387    typename TMat33<U>::row_type result(TMat33<U>::row_type::NO_INIT);
388    for (size_t col = 0; col < TMat33<T>::NUM_COLS; ++col) {
389        result[col] = dot(lhs, rhs[col]);
390    }
391    return result;
392}
393
394// matrix * scalar, result is a matrix of the same type than the input matrix
395template<typename T, typename U>
396constexpr typename std::enable_if<std::is_arithmetic<U>::value, TMat33<T>>::type PURE
397operator*(TMat33<T> lhs, U rhs) {
398    return lhs *= rhs;
399}
400
401// scalar * matrix, result is a matrix of the same type than the input matrix
402template<typename T, typename U>
403constexpr typename std::enable_if<std::is_arithmetic<U>::value, TMat33<T>>::type PURE
404operator*(U lhs, const TMat33<T>& rhs) {
405    return rhs * lhs;
406}
407
408//------------------------------------------------------------------------------
409template <typename T>
410CONSTEXPR TMat33<T> orthogonalize(const TMat33<T>& m) {
411    TMat33<T> ret(TMat33<T>::NO_INIT);
412    ret[0] = normalize(m[0]);
413    ret[2] = normalize(cross(ret[0], m[1]));
414    ret[1] = normalize(cross(ret[2], ret[0]));
415    return ret;
416}
417
418// ----------------------------------------------------------------------------------------
419
420/* FIXME: this should go into TMatSquareFunctions<> but for some reason
421 * BASE<T>::col_type is not accessible from there (???)
422 */
423template<typename T>
424CONSTEXPR typename TMat33<T>::col_type PURE diag(const TMat33<T>& m) {
425    return matrix::diag(m);
426}
427
428}  // namespace details
429
430// ----------------------------------------------------------------------------------------
431
432typedef details::TMat33<double> mat3d;
433typedef details::TMat33<float> mat3;
434typedef details::TMat33<float> mat3f;
435
436// ----------------------------------------------------------------------------------------
437}  // namespace android
438
439#undef PURE
440#undef CONSTEXPR
441