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 
25 namespace android {
26 
27 template <typename TYPE, size_t C, size_t R>
28 class mat;
29 
30 namespace helpers {
31 
32 template <typename TYPE, size_t C, size_t R>
doAssign(mat<TYPE,C,R> & lhs,typename TypeTraits<TYPE>::ParameterType rhs)33 mat<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 
42 template <typename TYPE, size_t C, size_t R, size_t D>
doMul(const mat<TYPE,D,R> & lhs,const mat<TYPE,C,D> & rhs)43 mat<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 
60 template <typename TYPE, size_t R, size_t D>
doMul(const mat<TYPE,D,R> & lhs,const vec<TYPE,D> & rhs)61 vec<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 
76 template <typename TYPE, size_t C, size_t R>
doMul(const vec<TYPE,R> & lhs,const mat<TYPE,C,1> & rhs)77 mat<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 
90 template <typename TYPE, size_t C, size_t R>
doMul(const mat<TYPE,C,R> & rhs,typename TypeTraits<TYPE>::ParameterType v)91 mat<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 
104 template <typename TYPE, size_t C, size_t R>
doMul(typename TypeTraits<TYPE>::ParameterType v,const mat<TYPE,C,R> & rhs)105 mat<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 
123 template <typename TYPE, size_t C, size_t R>
124 class mat : public vec< vec<TYPE, R>, C > {
125     typedef typename TypeTraits<TYPE>::ParameterType pTYPE;
126     typedef vec< vec<TYPE, R>, C > base;
127 public:
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;
size()133     size_type size() const { return R*C; }
134     enum { ROWS = R, COLS = C };
135 
136 
137     // -----------------------------------------------------------------------
138     // default constructors
139 
mat()140     mat() { }
mat(const mat & rhs)141     mat(const mat& rhs)  : base(rhs) { }
mat(const 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
mat(pTYPE rhs)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;
column_buildercolumn_builder223         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
255 template <typename TYPE, size_t R>
256 class mat<TYPE, 1, R> : public vec<TYPE, R> {
257     typedef vec<TYPE, R> base;
258 public:
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;
size()264     size_type size() const { return R; }
265     enum { ROWS = R, COLS = 1 };
266 
mat()267     mat() { }
mat(const base & rhs)268     mat(const base& rhs) : base(rhs) { }
mat(const mat & rhs)269     mat(const mat& rhs) : base(rhs) { }
mat(const TYPE & 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
transpose(int v)284 inline int     PURE transpose(int v)    { return v; }
transpose(float v)285 inline float   PURE transpose(float v)  { return v; }
transpose(double v)286 inline double  PURE transpose(double v) { return v; }
287 
288 // Transpose a matrix
289 template <typename TYPE, size_t C, size_t R>
transpose(const mat<TYPE,C,R> & m)290 mat<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
trace(const mat<TYPE,C,C> & m)299 template <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
307 template <typename TYPE, size_t C>
isPositiveSemidefinite(const mat<TYPE,C,C> & m,TYPE tolerance)308 static 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
322 template <
323     template<typename T, size_t S> class VEC,
324     typename TYPE,
325     size_t SIZE
326 >
transpose(const VEC<TYPE,SIZE> & v)327 mat<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
336 template<typename T, size_t N>
invert(const mat<T,N,N> & src)337 mat<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 
385 typedef mat<float, 2, 2> mat22_t;
386 typedef mat<float, 3, 3> mat33_t;
387 typedef mat<float, 4, 4> mat44_t;
388 
389 // -----------------------------------------------------------------------
390 
391 }; // namespace android
392 
393 #endif /* ANDROID_MAT_H */
394