services/sensorservice/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 // Calculate the trace of a matrix
 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>
 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 >
 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>
 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 */