9-axis sensor fusion with Kalman filter
Add support for 9-axis gravity and linear-acceleration sensors
virtual orientation sensor using 9-axis fusion
Change-Id: I6717539373fce781c10e97b6fa59f68a831a592f
diff --git a/services/sensorservice/vec.h b/services/sensorservice/vec.h
new file mode 100644
index 0000000..736ff37
--- /dev/null
+++ b/services/sensorservice/vec.h
@@ -0,0 +1,420 @@
+/*
+ * Copyright (C) 2011 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef ANDROID_VEC_H
+#define ANDROID_VEC_H
+
+#include <math.h>
+
+#include <stdint.h>
+#include <stddef.h>
+
+#include "traits.h"
+
+// -----------------------------------------------------------------------
+
+#define PURE __attribute__((pure))
+
+namespace android {
+
+// -----------------------------------------------------------------------
+// non-inline helpers
+
+template <typename TYPE, size_t SIZE>
+class vec;
+
+template <typename TYPE, size_t SIZE>
+class vbase;
+
+namespace helpers {
+
+template <typename T> inline T min(T a, T b) { return a<b ? a : b; }
+template <typename T> inline T max(T a, T b) { return a>b ? a : b; }
+
+template < template<typename T, size_t S> class VEC,
+ typename TYPE, size_t SIZE, size_t S>
+vec<TYPE, SIZE>& doAssign(
+ vec<TYPE, SIZE>& lhs, const VEC<TYPE, S>& rhs) {
+ const size_t minSize = min(SIZE, S);
+ const size_t maxSize = max(SIZE, S);
+ for (size_t i=0 ; i<minSize ; i++)
+ lhs[i] = rhs[i];
+ for (size_t i=minSize ; i<maxSize ; i++)
+ lhs[i] = 0;
+ return lhs;
+}
+
+
+template <
+ template<typename T, size_t S> class VLHS,
+ template<typename T, size_t S> class VRHS,
+ typename TYPE,
+ size_t SIZE
+>
+VLHS<TYPE, SIZE> PURE doAdd(
+ const VLHS<TYPE, SIZE>& lhs,
+ const VRHS<TYPE, SIZE>& rhs) {
+ VLHS<TYPE, SIZE> r;
+ for (size_t i=0 ; i<SIZE ; i++)
+ r[i] = lhs[i] + rhs[i];
+ return r;
+}
+
+template <
+ template<typename T, size_t S> class VLHS,
+ template<typename T, size_t S> class VRHS,
+ typename TYPE,
+ size_t SIZE
+>
+VLHS<TYPE, SIZE> PURE doSub(
+ const VLHS<TYPE, SIZE>& lhs,
+ const VRHS<TYPE, SIZE>& rhs) {
+ VLHS<TYPE, SIZE> r;
+ for (size_t i=0 ; i<SIZE ; i++)
+ r[i] = lhs[i] - rhs[i];
+ return r;
+}
+
+template <
+ template<typename T, size_t S> class VEC,
+ typename TYPE,
+ size_t SIZE
+>
+VEC<TYPE, SIZE> PURE doMulScalar(
+ const VEC<TYPE, SIZE>& lhs,
+ typename TypeTraits<TYPE>::ParameterType rhs) {
+ VEC<TYPE, SIZE> r;
+ for (size_t i=0 ; i<SIZE ; i++)
+ r[i] = lhs[i] * rhs;
+ return r;
+}
+
+template <
+ template<typename T, size_t S> class VEC,
+ typename TYPE,
+ size_t SIZE
+>
+VEC<TYPE, SIZE> PURE doScalarMul(
+ typename TypeTraits<TYPE>::ParameterType lhs,
+ const VEC<TYPE, SIZE>& rhs) {
+ VEC<TYPE, SIZE> r;
+ for (size_t i=0 ; i<SIZE ; i++)
+ r[i] = lhs * rhs[i];
+ return r;
+}
+
+}; // namespace helpers
+
+// -----------------------------------------------------------------------
+// Below we define the mathematical operators for vectors.
+// We use template template arguments so we can generically
+// handle the case where the right-hand-size and left-hand-side are
+// different vector types (but with same value_type and size).
+// This is needed for performance when using ".xy{z}" element access
+// on vec<>. Without this, an extra conversion to vec<> would be needed.
+//
+// example:
+// vec4_t a;
+// vec3_t b;
+// vec3_t c = a.xyz + b;
+//
+// "a.xyz + b" is a mixed-operation between a vbase<> and a vec<>, requiring
+// a conversion of vbase<> to vec<>. The template gunk below avoids this,
+// by allowing the addition on these different vector types directly
+//
+
+template <
+ template<typename T, size_t S> class VLHS,
+ template<typename T, size_t S> class VRHS,
+ typename TYPE,
+ size_t SIZE
+>
+inline VLHS<TYPE, SIZE> PURE operator + (
+ const VLHS<TYPE, SIZE>& lhs,
+ const VRHS<TYPE, SIZE>& rhs) {
+ return helpers::doAdd(lhs, rhs);
+}
+
+template <
+ template<typename T, size_t S> class VLHS,
+ template<typename T, size_t S> class VRHS,
+ typename TYPE,
+ size_t SIZE
+>
+inline VLHS<TYPE, SIZE> PURE operator - (
+ const VLHS<TYPE, SIZE>& lhs,
+ const VRHS<TYPE, SIZE>& rhs) {
+ return helpers::doSub(lhs, rhs);
+}
+
+template <
+ template<typename T, size_t S> class VEC,
+ typename TYPE,
+ size_t SIZE
+>
+inline VEC<TYPE, SIZE> PURE operator * (
+ const VEC<TYPE, SIZE>& lhs,
+ typename TypeTraits<TYPE>::ParameterType rhs) {
+ return helpers::doMulScalar(lhs, rhs);
+}
+
+template <
+ template<typename T, size_t S> class VEC,
+ typename TYPE,
+ size_t SIZE
+>
+inline VEC<TYPE, SIZE> PURE operator * (
+ typename TypeTraits<TYPE>::ParameterType lhs,
+ const VEC<TYPE, SIZE>& rhs) {
+ return helpers::doScalarMul(lhs, rhs);
+}
+
+
+template <
+ template<typename T, size_t S> class VLHS,
+ template<typename T, size_t S> class VRHS,
+ typename TYPE,
+ size_t SIZE
+>
+TYPE PURE dot_product(
+ const VLHS<TYPE, SIZE>& lhs,
+ const VRHS<TYPE, SIZE>& rhs) {
+ TYPE r(0);
+ for (size_t i=0 ; i<SIZE ; i++)
+ r += lhs[i] * rhs[i];
+ return r;
+}
+
+template <
+ template<typename T, size_t S> class V,
+ typename TYPE,
+ size_t SIZE
+>
+TYPE PURE length(const V<TYPE, SIZE>& v) {
+ return sqrt(dot_product(v, v));
+}
+
+template <
+ template<typename T, size_t S> class VLHS,
+ template<typename T, size_t S> class VRHS,
+ typename TYPE
+>
+VLHS<TYPE, 3> PURE cross_product(
+ const VLHS<TYPE, 3>& u,
+ const VRHS<TYPE, 3>& v) {
+ VLHS<TYPE, 3> r;
+ r.x = u.y*v.z - u.z*v.y;
+ r.y = u.z*v.x - u.x*v.z;
+ r.z = u.x*v.y - u.y*v.x;
+ return r;
+}
+
+
+template <typename TYPE, size_t SIZE>
+vec<TYPE, SIZE> PURE operator - (const vec<TYPE, SIZE>& lhs) {
+ vec<TYPE, SIZE> r;
+ for (size_t i=0 ; i<SIZE ; i++)
+ r[i] = -lhs[i];
+ return r;
+}
+
+// -----------------------------------------------------------------------
+
+// This our basic vector type, it just implements the data storage
+// and accessors.
+
+template <typename TYPE, size_t SIZE>
+struct vbase {
+ TYPE v[SIZE];
+ inline const TYPE& operator[](size_t i) const { return v[i]; }
+ inline TYPE& operator[](size_t i) { return v[i]; }
+};
+template<> struct vbase<float, 2> {
+ union {
+ float v[2];
+ struct { float x, y; };
+ struct { float s, t; };
+ };
+ inline const float& operator[](size_t i) const { return v[i]; }
+ inline float& operator[](size_t i) { return v[i]; }
+};
+template<> struct vbase<float, 3> {
+ union {
+ float v[3];
+ struct { float x, y, z; };
+ struct { float s, t, r; };
+ vbase<float, 2> xy;
+ vbase<float, 2> st;
+ };
+ inline const float& operator[](size_t i) const { return v[i]; }
+ inline float& operator[](size_t i) { return v[i]; }
+};
+template<> struct vbase<float, 4> {
+ union {
+ float v[4];
+ struct { float x, y, z, w; };
+ struct { float s, t, r, q; };
+ vbase<float, 3> xyz;
+ vbase<float, 3> str;
+ vbase<float, 2> xy;
+ vbase<float, 2> st;
+ };
+ inline const float& operator[](size_t i) const { return v[i]; }
+ inline float& operator[](size_t i) { return v[i]; }
+};
+
+// -----------------------------------------------------------------------
+
+template <typename TYPE, size_t SIZE>
+class vec : public vbase<TYPE, SIZE>
+{
+ typedef typename TypeTraits<TYPE>::ParameterType pTYPE;
+ typedef vbase<TYPE, SIZE> base;
+
+public:
+ // STL-like interface.
+ typedef TYPE value_type;
+ typedef TYPE& reference;
+ typedef TYPE const& const_reference;
+ typedef size_t size_type;
+
+ typedef TYPE* iterator;
+ typedef TYPE const* const_iterator;
+ iterator begin() { return base::v; }
+ iterator end() { return base::v + SIZE; }
+ const_iterator begin() const { return base::v; }
+ const_iterator end() const { return base::v + SIZE; }
+ size_type size() const { return SIZE; }
+
+ // -----------------------------------------------------------------------
+ // default constructors
+
+ vec() { }
+ vec(const vec& rhs) : base(rhs) { }
+ vec(const base& rhs) : base(rhs) { }
+
+ // -----------------------------------------------------------------------
+ // conversion constructors
+
+ vec(pTYPE rhs) {
+ for (size_t i=0 ; i<SIZE ; i++)
+ base::operator[](i) = rhs;
+ }
+
+ template < template<typename T, size_t S> class VEC, size_t S>
+ explicit vec(const VEC<TYPE, S>& rhs) {
+ helpers::doAssign(*this, rhs);
+ }
+
+ explicit vec(TYPE const* array) {
+ for (size_t i=0 ; i<SIZE ; i++)
+ base::operator[](i) = array[i];
+ }
+
+ // -----------------------------------------------------------------------
+ // Assignment
+
+ vec& operator = (const vec& rhs) {
+ base::operator=(rhs);
+ return *this;
+ }
+
+ vec& operator = (const base& rhs) {
+ base::operator=(rhs);
+ return *this;
+ }
+
+ vec& operator = (pTYPE rhs) {
+ for (size_t i=0 ; i<SIZE ; i++)
+ base::operator[](i) = rhs;
+ return *this;
+ }
+
+ template < template<typename T, size_t S> class VEC, size_t S>
+ vec& operator = (const VEC<TYPE, S>& rhs) {
+ return helpers::doAssign(*this, rhs);
+ }
+
+ // -----------------------------------------------------------------------
+ // operation-assignment
+
+ vec& operator += (const vec& rhs);
+ vec& operator -= (const vec& rhs);
+ vec& operator *= (pTYPE rhs);
+
+ // -----------------------------------------------------------------------
+ // non-member function declaration and definition
+ // NOTE: we declare the non-member function as friend inside the class
+ // so that they are known to the compiler when the class is instantiated.
+ // This helps the compiler doing template argument deduction when the
+ // passed types are not identical. Essentially this helps with
+ // type conversion so that you can multiply a vec<float> by an scalar int
+ // (for instance).
+
+ friend inline vec PURE operator + (const vec& lhs, const vec& rhs) {
+ return helpers::doAdd(lhs, rhs);
+ }
+ friend inline vec PURE operator - (const vec& lhs, const vec& rhs) {
+ return helpers::doSub(lhs, rhs);
+ }
+ friend inline vec PURE operator * (const vec& lhs, pTYPE v) {
+ return helpers::doMulScalar(lhs, v);
+ }
+ friend inline vec PURE operator * (pTYPE v, const vec& rhs) {
+ return helpers::doScalarMul(v, rhs);
+ }
+ friend inline TYPE PURE dot_product(const vec& lhs, const vec& rhs) {
+ return android::dot_product(lhs, rhs);
+ }
+};
+
+// -----------------------------------------------------------------------
+
+template <typename TYPE, size_t SIZE>
+vec<TYPE, SIZE>& vec<TYPE, SIZE>::operator += (const vec<TYPE, SIZE>& rhs) {
+ vec<TYPE, SIZE>& lhs(*this);
+ for (size_t i=0 ; i<SIZE ; i++)
+ lhs[i] += rhs[i];
+ return lhs;
+}
+
+template <typename TYPE, size_t SIZE>
+vec<TYPE, SIZE>& vec<TYPE, SIZE>::operator -= (const vec<TYPE, SIZE>& rhs) {
+ vec<TYPE, SIZE>& lhs(*this);
+ for (size_t i=0 ; i<SIZE ; i++)
+ lhs[i] -= rhs[i];
+ return lhs;
+}
+
+template <typename TYPE, size_t SIZE>
+vec<TYPE, SIZE>& vec<TYPE, SIZE>::operator *= (vec<TYPE, SIZE>::pTYPE rhs) {
+ vec<TYPE, SIZE>& lhs(*this);
+ for (size_t i=0 ; i<SIZE ; i++)
+ lhs[i] *= rhs;
+ return lhs;
+}
+
+// -----------------------------------------------------------------------
+
+typedef vec<float, 2> vec2_t;
+typedef vec<float, 3> vec3_t;
+typedef vec<float, 4> vec4_t;
+
+// -----------------------------------------------------------------------
+
+}; // namespace android
+
+#endif /* ANDROID_VEC_H */