1/*
2 * Copyright (C) 2011 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#ifndef ANDROID_MAT_H
18#define ANDROID_MAT_H
19
20#include "vec.h"
21#include "traits.h"
22
23// -----------------------------------------------------------------------
24
25namespace android {
26
27template <typename TYPE, size_t C, size_t R>
28class mat;
29
30namespace helpers {
31
32template <typename TYPE, size_t C, size_t R>
33mat<TYPE, C, R>& doAssign(
34        mat<TYPE, C, R>& lhs,
35        typename TypeTraits<TYPE>::ParameterType rhs) {
36    for (size_t i=0 ; i<C ; i++)
37        for (size_t j=0 ; j<R ; j++)
38            lhs[i][j] = (i==j) ? rhs : 0;
39    return lhs;
40}
41
42template <typename TYPE, size_t C, size_t R, size_t D>
43mat<TYPE, C, R> PURE doMul(
44        const mat<TYPE, D, R>& lhs,
45        const mat<TYPE, C, D>& rhs)
46{
47    mat<TYPE, C, R> res;
48    for (size_t c=0 ; c<C ; c++) {
49        for (size_t r=0 ; r<R ; r++) {
50            TYPE v(0);
51            for (size_t k=0 ; k<D ; k++) {
52                v += lhs[k][r] * rhs[c][k];
53            }
54            res[c][r] = v;
55        }
56    }
57    return res;
58}
59
60template <typename TYPE, size_t R, size_t D>
61vec<TYPE, R> PURE doMul(
62        const mat<TYPE, D, R>& lhs,
63        const vec<TYPE, D>& rhs)
64{
65    vec<TYPE, R> res;
66    for (size_t r=0 ; r<R ; r++) {
67        TYPE v(0);
68        for (size_t k=0 ; k<D ; k++) {
69            v += lhs[k][r] * rhs[k];
70        }
71        res[r] = v;
72    }
73    return res;
74}
75
76template <typename TYPE, size_t C, size_t R>
77mat<TYPE, C, R> PURE doMul(
78        const vec<TYPE, R>& lhs,
79        const mat<TYPE, C, 1>& rhs)
80{
81    mat<TYPE, C, R> res;
82    for (size_t c=0 ; c<C ; c++) {
83        for (size_t r=0 ; r<R ; r++) {
84            res[c][r] = lhs[r] * rhs[c][0];
85        }
86    }
87    return res;
88}
89
90template <typename TYPE, size_t C, size_t R>
91mat<TYPE, C, R> PURE doMul(
92        const mat<TYPE, C, R>& rhs,
93        typename TypeTraits<TYPE>::ParameterType v)
94{
95    mat<TYPE, C, R> res;
96    for (size_t c=0 ; c<C ; c++) {
97        for (size_t r=0 ; r<R ; r++) {
98            res[c][r] = rhs[c][r] * v;
99        }
100    }
101    return res;
102}
103
104template <typename TYPE, size_t C, size_t R>
105mat<TYPE, C, R> PURE doMul(
106        typename TypeTraits<TYPE>::ParameterType v,
107        const mat<TYPE, C, R>& rhs)
108{
109    mat<TYPE, C, R> res;
110    for (size_t c=0 ; c<C ; c++) {
111        for (size_t r=0 ; r<R ; r++) {
112            res[c][r] = v * rhs[c][r];
113        }
114    }
115    return res;
116}
117
118
119}; // namespace helpers
120
121// -----------------------------------------------------------------------
122
123template <typename TYPE, size_t C, size_t R>
124class mat : public vec< vec<TYPE, R>, C > {
125    typedef typename TypeTraits<TYPE>::ParameterType pTYPE;
126    typedef vec< vec<TYPE, R>, C > base;
127public:
128    // STL-like interface.
129    typedef TYPE value_type;
130    typedef TYPE& reference;
131    typedef TYPE const& const_reference;
132    typedef size_t size_type;
133    size_type size() const { return R*C; }
134    enum { ROWS = R, COLS = C };
135
136
137    // -----------------------------------------------------------------------
138    // default constructors
139
140    mat() { }
141    mat(const mat& rhs)  : base(rhs) { }
142    mat(const base& rhs) : base(rhs) { }
143
144    // -----------------------------------------------------------------------
145    // conversion constructors
146
147    // sets the diagonal to the value, off-diagonal to zero
148    mat(pTYPE rhs) {
149        helpers::doAssign(*this, rhs);
150    }
151
152    // -----------------------------------------------------------------------
153    // Assignment
154
155    mat& operator=(const mat& rhs) {
156        base::operator=(rhs);
157        return *this;
158    }
159
160    mat& operator=(const base& rhs) {
161        base::operator=(rhs);
162        return *this;
163    }
164
165    mat& operator=(pTYPE rhs) {
166        return helpers::doAssign(*this, rhs);
167    }
168
169    // -----------------------------------------------------------------------
170    // non-member function declaration and definition
171
172    friend inline mat PURE operator + (const mat& lhs, const mat& rhs) {
173        return helpers::doAdd(
174                static_cast<const base&>(lhs),
175                static_cast<const base&>(rhs));
176    }
177    friend inline mat PURE operator - (const mat& lhs, const mat& rhs) {
178        return helpers::doSub(
179                static_cast<const base&>(lhs),
180                static_cast<const base&>(rhs));
181    }
182
183    // matrix*matrix
184    template <size_t D>
185    friend mat PURE operator * (
186            const mat<TYPE, D, R>& lhs,
187            const mat<TYPE, C, D>& rhs) {
188        return helpers::doMul(lhs, rhs);
189    }
190
191    // matrix*vector
192    friend vec<TYPE, R> PURE operator * (
193            const mat& lhs, const vec<TYPE, C>& rhs) {
194        return helpers::doMul(lhs, rhs);
195    }
196
197    // vector*matrix
198    friend mat PURE operator * (
199            const vec<TYPE, R>& lhs, const mat<TYPE, C, 1>& rhs) {
200        return helpers::doMul(lhs, rhs);
201    }
202
203    // matrix*scalar
204    friend inline mat PURE operator * (const mat& lhs, pTYPE v) {
205        return helpers::doMul(lhs, v);
206    }
207
208    // scalar*matrix
209    friend inline mat PURE operator * (pTYPE v, const mat& rhs) {
210        return helpers::doMul(v, rhs);
211    }
212
213    // -----------------------------------------------------------------------
214    // streaming operator to set the columns of the matrix:
215    // example:
216    //    mat33_t m;
217    //    m << v0 << v1 << v2;
218
219    // column_builder<> stores the matrix and knows which column to set
220    template<size_t PREV_COLUMN>
221    struct column_builder {
222        mat& matrix;
223        column_builder(mat& matrix) : matrix(matrix) { }
224    };
225
226    // operator << is not a method of column_builder<> so we can
227    // overload it for unauthorized values (partial specialization
228    // not allowed in class-scope).
229    // we just set the column and return the next column_builder<>
230    template<size_t PREV_COLUMN>
231    friend column_builder<PREV_COLUMN+1> operator << (
232            const column_builder<PREV_COLUMN>& lhs,
233            const vec<TYPE, R>& rhs) {
234        lhs.matrix[PREV_COLUMN+1] = rhs;
235        return column_builder<PREV_COLUMN+1>(lhs.matrix);
236    }
237
238    // we return void here so we get a compile-time error if the
239    // user tries to set too many columns
240    friend void operator << (
241            const column_builder<C-2>& lhs,
242            const vec<TYPE, R>& rhs) {
243        lhs.matrix[C-1] = rhs;
244    }
245
246    // this is where the process starts. we set the first columns and
247    // return the next column_builder<>
248    column_builder<0> operator << (const vec<TYPE, R>& rhs) {
249        (*this)[0] = rhs;
250        return column_builder<0>(*this);
251    }
252};
253
254// Specialize column matrix so they're exactly equivalent to a vector
255template <typename TYPE, size_t R>
256class mat<TYPE, 1, R> : public vec<TYPE, R> {
257    typedef vec<TYPE, R> base;
258public:
259    // STL-like interface.
260    typedef TYPE value_type;
261    typedef TYPE& reference;
262    typedef TYPE const& const_reference;
263    typedef size_t size_type;
264    size_type size() const { return R; }
265    enum { ROWS = R, COLS = 1 };
266
267    mat() { }
268    mat(const base& rhs) : base(rhs) { }
269    mat(const mat& rhs) : base(rhs) { }
270    mat(const TYPE& rhs) { helpers::doAssign(*this, rhs); }
271    mat& operator=(const mat& rhs) { base::operator=(rhs); return *this; }
272    mat& operator=(const base& rhs) { base::operator=(rhs); return *this; }
273    mat& operator=(const TYPE& rhs) { return helpers::doAssign(*this, rhs); }
274    // we only have one column, so ignore the index
275    const base& operator[](size_t) const { return *this; }
276    base& operator[](size_t) { return *this; }
277    void operator << (const vec<TYPE, R>& rhs) { base::operator[](0) = rhs; }
278};
279
280// -----------------------------------------------------------------------
281// matrix functions
282
283// transpose. this handles matrices of matrices
284inline int     PURE transpose(int v)    { return v; }
285inline float   PURE transpose(float v)  { return v; }
286inline double  PURE transpose(double v) { return v; }
287
288// Transpose a matrix
289template <typename TYPE, size_t C, size_t R>
290mat<TYPE, R, C> PURE transpose(const mat<TYPE, C, R>& m) {
291    mat<TYPE, R, C> r;
292    for (size_t i=0 ; i<R ; i++)
293        for (size_t j=0 ; j<C ; j++)
294            r[i][j] = transpose(m[j][i]);
295    return r;
296}
297
298// Calculate the trace of a matrix
299template <typename TYPE, size_t C> static TYPE trace(const mat<TYPE, C, C>& m) {
300    TYPE t;
301    for (size_t i=0 ; i<C ; i++)
302        t += m[i][i];
303    return t;
304}
305
306// Test positive-semidefiniteness of a matrix
307template <typename TYPE, size_t C>
308static bool isPositiveSemidefinite(const mat<TYPE, C, C>& m, TYPE tolerance) {
309    for (size_t i=0 ; i<C ; i++)
310        if (m[i][i] < 0)
311            return false;
312
313    for (size_t i=0 ; i<C ; i++)
314      for (size_t j=i+1 ; j<C ; j++)
315          if (fabs(m[i][j] - m[j][i]) > tolerance)
316              return false;
317
318    return true;
319}
320
321// Transpose a vector
322template <
323    template<typename T, size_t S> class VEC,
324    typename TYPE,
325    size_t SIZE
326>
327mat<TYPE, SIZE, 1> PURE transpose(const VEC<TYPE, SIZE>& v) {
328    mat<TYPE, SIZE, 1> r;
329    for (size_t i=0 ; i<SIZE ; i++)
330        r[i][0] = transpose(v[i]);
331    return r;
332}
333
334// -----------------------------------------------------------------------
335// "dumb" matrix inversion
336template<typename T, size_t N>
337mat<T, N, N> PURE invert(const mat<T, N, N>& src) {
338    T t;
339    size_t swap;
340    mat<T, N, N> tmp(src);
341    mat<T, N, N> inverse(1);
342
343    for (size_t i=0 ; i<N ; i++) {
344        // look for largest element in column
345        swap = i;
346        for (size_t j=i+1 ; j<N ; j++) {
347            if (fabs(tmp[j][i]) > fabs(tmp[i][i])) {
348                swap = j;
349            }
350        }
351
352        if (swap != i) {
353            /* swap rows. */
354            for (size_t k=0 ; k<N ; k++) {
355                t = tmp[i][k];
356                tmp[i][k] = tmp[swap][k];
357                tmp[swap][k] = t;
358
359                t = inverse[i][k];
360                inverse[i][k] = inverse[swap][k];
361                inverse[swap][k] = t;
362            }
363        }
364
365        t = 1 / tmp[i][i];
366        for (size_t k=0 ; k<N ; k++) {
367            tmp[i][k] *= t;
368            inverse[i][k] *= t;
369        }
370        for (size_t j=0 ; j<N ; j++) {
371            if (j != i) {
372                t = tmp[j][i];
373                for (size_t k=0 ; k<N ; k++) {
374                    tmp[j][k] -= tmp[i][k] * t;
375                    inverse[j][k] -= inverse[i][k] * t;
376                }
377            }
378        }
379    }
380    return inverse;
381}
382
383// -----------------------------------------------------------------------
384
385typedef mat<float, 2, 2> mat22_t;
386typedef mat<float, 3, 3> mat33_t;
387typedef mat<float, 4, 4> mat44_t;
388
389// -----------------------------------------------------------------------
390
391}; // namespace android
392
393#endif /* ANDROID_MAT_H */
394