services/sensorservice/vec.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_VEC_H
  18 #define ANDROID_VEC_H
  19
  20 #include <math.h>
  21
  22 #include <stdint.h>
  23 #include <stddef.h>
  24
  25 #include "traits.h"
  26
  27 // -----------------------------------------------------------------------
  28
  29 #define PURE __attribute__((pure))
  30
  31 namespace android {
  32
  33 // -----------------------------------------------------------------------
  34 // non-inline helpers
  35
  36 template <typename TYPE, size_t SIZE>
  37 class vec;
  38
  39 template <typename TYPE, size_t SIZE>
  40 class vbase;
  41
  42 namespace helpers {
  43
  44 template <typename T> inline T min(T a, T b) { return a<b ? a : b; }
  45 template <typename T> inline T max(T a, T b) { return a>b ? a : b; }
  46
  47 template < template<typename T, size_t S> class VEC,
  48     typename TYPE, size_t SIZE, size_t S>
  49 vec<TYPE, SIZE>& doAssign(
  50         vec<TYPE, SIZE>& lhs, const VEC<TYPE, S>& rhs) {
  51     const size_t minSize = min(SIZE, S);
  52     const size_t maxSize = max(SIZE, S);
  53     for (size_t i=0 ; i<minSize ; i++)
  54         lhs[i] = rhs[i];
  55     for (size_t i=minSize ; i<maxSize ; i++)
  56         lhs[i] = 0;
  57     return lhs;
  58 }
  59
  60
  61 template <
  62     template<typename T, size_t S> class VLHS,
  63     template<typename T, size_t S> class VRHS,
  64     typename TYPE,
  65     size_t SIZE
  66 >
  67 VLHS<TYPE, SIZE> PURE doAdd(
  68         const VLHS<TYPE, SIZE>& lhs,
  69         const VRHS<TYPE, SIZE>& rhs) {
  70     VLHS<TYPE, SIZE> r;
  71     for (size_t i=0 ; i<SIZE ; i++)
  72         r[i] = lhs[i] + rhs[i];
  73     return r;
  74 }
  75
  76 template <
  77     template<typename T, size_t S> class VLHS,
  78     template<typename T, size_t S> class VRHS,
  79     typename TYPE,
  80     size_t SIZE
  81 >
  82 VLHS<TYPE, SIZE> PURE doSub(
  83         const VLHS<TYPE, SIZE>& lhs,
  84         const VRHS<TYPE, SIZE>& rhs) {
  85     VLHS<TYPE, SIZE> r;
  86     for (size_t i=0 ; i<SIZE ; i++)
  87         r[i] = lhs[i] - rhs[i];
  88     return r;
  89 }
  90
  91 template <
  92     template<typename T, size_t S> class VEC,
  93     typename TYPE,
  94     size_t SIZE
  95 >
  96 VEC<TYPE, SIZE> PURE doMulScalar(
  97         const VEC<TYPE, SIZE>& lhs,
  98         typename TypeTraits<TYPE>::ParameterType rhs) {
  99     VEC<TYPE, SIZE> r;
 100     for (size_t i=0 ; i<SIZE ; i++)
 101         r[i] = lhs[i] * rhs;
 102     return r;
 103 }
 104
 105 template <
 106     template<typename T, size_t S> class VEC,
 107     typename TYPE,
 108     size_t SIZE
 109 >
 110 VEC<TYPE, SIZE> PURE doScalarMul(
 111         typename TypeTraits<TYPE>::ParameterType lhs,
 112         const VEC<TYPE, SIZE>& rhs) {
 113     VEC<TYPE, SIZE> r;
 114     for (size_t i=0 ; i<SIZE ; i++)
 115         r[i] = lhs * rhs[i];
 116     return r;
 117 }
 118
 119 }; // namespace helpers
 120
 121 // -----------------------------------------------------------------------
 122 // Below we define the mathematical operators for vectors.
 123 // We use template template arguments so we can generically
 124 // handle the case where the right-hand-size and left-hand-side are
 125 // different vector types (but with same value_type and size).
 126 // This is needed for performance when using ".xy{z}" element access
 127 // on vec<>. Without this, an extra conversion to vec<> would be needed.
 128 //
 129 // example:
 130 //      vec4_t a;
 131 //      vec3_t b;
 132 //      vec3_t c = a.xyz + b;
 133 //
 134 //  "a.xyz + b" is a mixed-operation between a vbase<> and a vec<>, requiring
 135 //  a conversion of vbase<> to vec<>. The template gunk below avoids this,
 136 // by allowing the addition on these different vector types directly
 137 //
 138
 139 template <
 140     template<typename T, size_t S> class VLHS,
 141     template<typename T, size_t S> class VRHS,
 142     typename TYPE,
 143     size_t SIZE
 144 >
 145 inline VLHS<TYPE, SIZE> PURE operator + (
 146         const VLHS<TYPE, SIZE>& lhs,
 147         const VRHS<TYPE, SIZE>& rhs) {
 148     return helpers::doAdd(lhs, rhs);
 149 }
 150
 151 template <
 152     template<typename T, size_t S> class VLHS,
 153     template<typename T, size_t S> class VRHS,
 154     typename TYPE,
 155     size_t SIZE
 156 >
 157 inline VLHS<TYPE, SIZE> PURE operator - (
 158         const VLHS<TYPE, SIZE>& lhs,
 159         const VRHS<TYPE, SIZE>& rhs) {
 160     return helpers::doSub(lhs, rhs);
 161 }
 162
 163 template <
 164     template<typename T, size_t S> class VEC,
 165     typename TYPE,
 166     size_t SIZE
 167 >
 168 inline VEC<TYPE, SIZE> PURE operator * (
 169         const VEC<TYPE, SIZE>& lhs,
 170         typename TypeTraits<TYPE>::ParameterType rhs) {
 171     return helpers::doMulScalar(lhs, rhs);
 172 }
 173
 174 template <
 175     template<typename T, size_t S> class VEC,
 176     typename TYPE,
 177     size_t SIZE
 178 >
 179 inline VEC<TYPE, SIZE> PURE operator * (
 180         typename TypeTraits<TYPE>::ParameterType lhs,
 181         const VEC<TYPE, SIZE>& rhs) {
 182     return helpers::doScalarMul(lhs, rhs);
 183 }
 184
 185
 186 template <
 187     template<typename T, size_t S> class VLHS,
 188     template<typename T, size_t S> class VRHS,
 189     typename TYPE,
 190     size_t SIZE
 191 >
 192 TYPE PURE dot_product(
 193         const VLHS<TYPE, SIZE>& lhs,
 194         const VRHS<TYPE, SIZE>& rhs) {
 195     TYPE r(0);
 196     for (size_t i=0 ; i<SIZE ; i++)
 197         r += lhs[i] * rhs[i];
 198     return r;
 199 }
 200
 201 template <
 202     template<typename T, size_t S> class V,
 203     typename TYPE,
 204     size_t SIZE
 205 >
 206 TYPE PURE length(const V<TYPE, SIZE>& v) {
 207     return sqrt(dot_product(v, v));
 208 }
 209
 210 template <
 211     template<typename T, size_t S> class V,
 212     typename TYPE,
 213     size_t SIZE
 214 >
 215 TYPE PURE length_squared(const V<TYPE, SIZE>& v) {
 216     return dot_product(v, v);
 217 }
 218
 219 template <
 220     template<typename T, size_t S> class V,
 221     typename TYPE,
 222     size_t SIZE
 223 >
 224 V<TYPE, SIZE> PURE normalize(const V<TYPE, SIZE>& v) {
 225     return v * (1/length(v));
 226 }
 227
 228 template <
 229     template<typename T, size_t S> class VLHS,
 230     template<typename T, size_t S> class VRHS,
 231     typename TYPE
 232 >
 233 VLHS<TYPE, 3> PURE cross_product(
 234         const VLHS<TYPE, 3>& u,
 235         const VRHS<TYPE, 3>& v) {
 236     VLHS<TYPE, 3> r;
 237     r.x = u.y*v.z - u.z*v.y;
 238     r.y = u.z*v.x - u.x*v.z;
 239     r.z = u.x*v.y - u.y*v.x;
 240     return r;
 241 }
 242
 243
 244 template <typename TYPE, size_t SIZE>
 245 vec<TYPE, SIZE> PURE operator - (const vec<TYPE, SIZE>& lhs) {
 246     vec<TYPE, SIZE> r;
 247     for (size_t i=0 ; i<SIZE ; i++)
 248         r[i] = -lhs[i];
 249     return r;
 250 }
 251
 252 // -----------------------------------------------------------------------
 253
 254 // This our basic vector type, it just implements the data storage
 255 // and accessors.
 256
 257 template <typename TYPE, size_t SIZE>
 258 struct vbase {
 259     TYPE v[SIZE];
 260     inline const TYPE& operator[](size_t i) const { return v[i]; }
 261     inline       TYPE& operator[](size_t i)       { return v[i]; }
 262 };
 263 template<> struct vbase<float, 2> {
 264     union {
 265         float v[2];
 266         struct { float x, y; };
 267         struct { float s, t; };
 268     };
 269     inline const float& operator[](size_t i) const { return v[i]; }
 270     inline       float& operator[](size_t i)       { return v[i]; }
 271 };
 272 template<> struct vbase<float, 3> {
 273     union {
 274         float v[3];
 275         struct { float x, y, z; };
 276         struct { float s, t, r; };
 277         vbase<float, 2> xy;
 278         vbase<float, 2> st;
 279     };
 280     inline const float& operator[](size_t i) const { return v[i]; }
 281     inline       float& operator[](size_t i)       { return v[i]; }
 282 };
 283 template<> struct vbase<float, 4> {
 284     union {
 285         float v[4];
 286         struct { float x, y, z, w; };
 287         struct { float s, t, r, q; };
 288         vbase<float, 3> xyz;
 289         vbase<float, 3> str;
 290         vbase<float, 2> xy;
 291         vbase<float, 2> st;
 292     };
 293     inline const float& operator[](size_t i) const { return v[i]; }
 294     inline       float& operator[](size_t i)       { return v[i]; }
 295 };
 296
 297 // -----------------------------------------------------------------------
 298
 299 template <typename TYPE, size_t SIZE>
 300 class vec : public vbase<TYPE, SIZE>
 301 {
 302     typedef typename TypeTraits<TYPE>::ParameterType pTYPE;
 303     typedef vbase<TYPE, SIZE> base;
 304
 305 public:
 306     // STL-like interface.
 307     typedef TYPE value_type;
 308     typedef TYPE& reference;
 309     typedef TYPE const& const_reference;
 310     typedef size_t size_type;
 311
 312     typedef TYPE* iterator;
 313     typedef TYPE const* const_iterator;
 314     iterator begin() { return base::v; }
 315     iterator end() { return base::v + SIZE; }
 316     const_iterator begin() const { return base::v; }
 317     const_iterator end() const { return base::v + SIZE; }
 318     size_type size() const { return SIZE; }
 319
 320     // -----------------------------------------------------------------------
 321     // default constructors
 322
 323     vec() { }
 324     vec(const vec& rhs)  : base(rhs) { }
 325     vec(const base& rhs) : base(rhs) { }
 326
 327     // -----------------------------------------------------------------------
 328     // conversion constructors
 329
 330     vec(pTYPE rhs) {
 331         for (size_t i=0 ; i<SIZE ; i++)
 332             base::operator[](i) = rhs;
 333     }
 334
 335     template < template<typename T, size_t S> class VEC, size_t S>
 336     explicit vec(const VEC<TYPE, S>& rhs) {
 337         helpers::doAssign(*this, rhs);
 338     }
 339
 340     explicit vec(TYPE const* array) {
 341         for (size_t i=0 ; i<SIZE ; i++)
 342             base::operator[](i) = array[i];
 343     }
 344
 345     // -----------------------------------------------------------------------
 346     // Assignment
 347
 348     vec& operator = (const vec& rhs) {
 349         base::operator=(rhs);
 350         return *this;
 351     }
 352
 353     vec& operator = (const base& rhs) {
 354         base::operator=(rhs);
 355         return *this;
 356     }
 357
 358     vec& operator = (pTYPE rhs) {
 359         for (size_t i=0 ; i<SIZE ; i++)
 360             base::operator[](i) = rhs;
 361         return *this;
 362     }
 363
 364     template < template<typename T, size_t S> class VEC, size_t S>
 365     vec& operator = (const VEC<TYPE, S>& rhs) {
 366         return helpers::doAssign(*this, rhs);
 367     }
 368
 369     // -----------------------------------------------------------------------
 370     // operation-assignment
 371
 372     vec& operator += (const vec& rhs);
 373     vec& operator -= (const vec& rhs);
 374     vec& operator *= (pTYPE rhs);
 375
 376     // -----------------------------------------------------------------------
 377     // non-member function declaration and definition
 378     // NOTE: we declare the non-member function as friend inside the class
 379     // so that they are known to the compiler when the class is instantiated.
 380     // This helps the compiler doing template argument deduction when the
 381     // passed types are not identical. Essentially this helps with
 382     // type conversion so that you can multiply a vec<float> by an scalar int
 383     // (for instance).
 384
 385     friend inline vec PURE operator + (const vec& lhs, const vec& rhs) {
 386         return helpers::doAdd(lhs, rhs);
 387     }
 388     friend inline vec PURE operator - (const vec& lhs, const vec& rhs) {
 389         return helpers::doSub(lhs, rhs);
 390     }
 391     friend inline vec PURE operator * (const vec& lhs, pTYPE v) {
 392         return helpers::doMulScalar(lhs, v);
 393     }
 394     friend inline vec PURE operator * (pTYPE v, const vec& rhs) {
 395         return helpers::doScalarMul(v, rhs);
 396     }
 397     friend inline TYPE PURE dot_product(const vec& lhs, const vec& rhs) {
 398         return android::dot_product(lhs, rhs);
 399     }
 400 };
 401
 402 // -----------------------------------------------------------------------
 403
 404 template <typename TYPE, size_t SIZE>
 405 vec<TYPE, SIZE>& vec<TYPE, SIZE>::operator += (const vec<TYPE, SIZE>& rhs) {
 406     vec<TYPE, SIZE>& lhs(*this);
 407     for (size_t i=0 ; i<SIZE ; i++)
 408         lhs[i] += rhs[i];
 409     return lhs;
 410 }
 411
 412 template <typename TYPE, size_t SIZE>
 413 vec<TYPE, SIZE>& vec<TYPE, SIZE>::operator -= (const vec<TYPE, SIZE>& rhs) {
 414     vec<TYPE, SIZE>& lhs(*this);
 415     for (size_t i=0 ; i<SIZE ; i++)
 416         lhs[i] -= rhs[i];
 417     return lhs;
 418 }
 419
 420 template <typename TYPE, size_t SIZE>
 421 vec<TYPE, SIZE>& vec<TYPE, SIZE>::operator *= (vec<TYPE, SIZE>::pTYPE rhs) {
 422     vec<TYPE, SIZE>& lhs(*this);
 423     for (size_t i=0 ; i<SIZE ; i++)
 424         lhs[i] *= rhs;
 425     return lhs;
 426 }
 427
 428 // -----------------------------------------------------------------------
 429
 430 typedef vec<float, 2> vec2_t;
 431 typedef vec<float, 3> vec3_t;
 432 typedef vec<float, 4> vec4_t;
 433
 434 // -----------------------------------------------------------------------
 435
 436 }; // namespace android
 437
 438 #endif /* ANDROID_VEC_H */