upm  1.7.1
Sensor/Actuator repository for libmraa (v2.0.0)
mat.h
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>
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>
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>
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>
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>
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>
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;
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
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;
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
284 inline int PURE transpose(int v) { return v; }
285 inline float PURE transpose(float v) { return 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>
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 template <typename TYPE, size_t M, size_t N, size_t P>
299 mat<TYPE, M, P> PURE multiply(const mat<TYPE, M, N>& m1, const mat<TYPE, N, P>& m2)
300 {
301  mat<TYPE, M, P> r;
302  for (size_t i = 0; i < M; i++)
303  for (size_t k = 0; k < P; k++) {
304  r [i][k] = 0;
305  for (size_t j = 0; j < N; j++)
306  r [i][k] += m1[i][j] * m2 [j][k];
307  }
308  return r;
309 }
310 
311 // Calculate the trace of a matrix
312 template <typename TYPE, size_t C> static TYPE trace(const mat<TYPE, C, C>& m) {
313  TYPE t;
314  for (size_t i=0 ; i<C ; i++)
315  t += m[i][i];
316  return t;
317 }
318 
319 // Test positive-semidefiniteness of a matrix
320 template <typename TYPE, size_t C>
321 static bool isPositiveSemidefinite(const mat<TYPE, C, C>& m, TYPE tolerance) {
322  for (size_t i=0 ; i<C ; i++)
323  if (m[i][i] < 0)
324  return false;
325 
326  for (size_t i=0 ; i<C ; i++)
327  for (size_t j=i+1 ; j<C ; j++)
328  if (fabs(m[i][j] - m[j][i]) > tolerance)
329  return false;
330 
331  return true;
332 }
333 
334 // Transpose a vector
335 template <
336  template<typename T, size_t S> class VEC,
337  typename TYPE,
338  size_t SIZE
339 >
340 mat<TYPE, SIZE, 1> PURE transpose(const VEC<TYPE, SIZE>& v) {
342  for (size_t i=0 ; i<SIZE ; i++)
343  r[i][0] = transpose(v[i]);
344  return r;
345 }
346 
347 // -----------------------------------------------------------------------
348 // "dumb" matrix inversion
349 template<typename T, size_t N>
350 mat<T, N, N> PURE invert(const mat<T, N, N>& src) {
351  T t;
352  size_t swap;
353  mat<T, N, N> tmp(src);
354  mat<T, N, N> inverse(1);
355 
356  for (size_t i=0 ; i<N ; i++) {
357  // look for largest element in column
358  swap = i;
359  for (size_t j=i+1 ; j<N ; j++) {
360  if (fabs(tmp[j][i]) > fabs(tmp[i][i])) {
361  swap = j;
362  }
363  }
364 
365  if (swap != i) {
366  /* swap rows. */
367  for (size_t k=0 ; k<N ; k++) {
368  t = tmp[i][k];
369  tmp[i][k] = tmp[swap][k];
370  tmp[swap][k] = t;
371 
372  t = inverse[i][k];
373  inverse[i][k] = inverse[swap][k];
374  inverse[swap][k] = t;
375  }
376  }
377 
378  t = 1 / tmp[i][i];
379  for (size_t k=0 ; k<N ; k++) {
380  tmp[i][k] *= t;
381  inverse[i][k] *= t;
382  }
383  for (size_t j=0 ; j<N ; j++) {
384  if (j != i) {
385  t = tmp[j][i];
386  for (size_t k=0 ; k<N ; k++) {
387  tmp[j][k] -= tmp[i][k] * t;
388  inverse[j][k] -= inverse[i][k] * t;
389  }
390  }
391  }
392  }
393  return inverse;
394 }
395 
396 // -----------------------------------------------------------------------
397 
398 typedef mat<float, 2, 2> mat22_t;
399 typedef mat<float, 3, 3> mat33_t;
400 typedef mat<float, 4, 4> mat44_t;
401 
402 // -----------------------------------------------------------------------
403 
404 }; // namespace android
405 
406 #endif /* ANDROID_MAT_H */
Definition: mat.h:221
Definition: mat.h:25
Definition: mat.h:28