Home | History | Annotate | Download | only in sensorservice 
      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 */
    394