Skip to content
This repository

HTTPS clone URL

Subversion checkout URL

You can clone with HTTPS or Subversion.

Download ZIP
Browse code

Sensors: backport of virtual RotationVector from ICS

- attempt to fix the magnetometer issues in some apps

Change-Id: I2999cba68367283613d4b357e533306a8b931a92
  • Loading branch information...
commit 5951c645df4aaea7a56f0f7fcf14c309bd635805 1 parent 5975567
C3C0 authored May 24, 2012
1  services/sensorservice/Android.mk
@@ -5,6 +5,7 @@ LOCAL_SRC_FILES:= \
5 5
 	GravitySensor.cpp \
6 6
 	LinearAccelerationSensor.cpp \
7 7
 	RotationVectorSensor.cpp \
  8
+    RotationVectorSensor2.cpp \
8 9
     SensorService.cpp \
9 10
     SensorInterface.cpp \
10 11
     SensorDevice.cpp \
126  services/sensorservice/RotationVectorSensor2.cpp
... ...
@@ -0,0 +1,126 @@
  1
+/*
  2
+ * Copyright (C) 2010 The Android Open Source Project
  3
+ * Copyright (C) 2012 The CyanogenMod Project
  4
+ *
  5
+ * Licensed under the Apache License, Version 2.0 (the "License");
  6
+ * you may not use this file except in compliance with the License.
  7
+ * You may obtain a copy of the License at
  8
+ *
  9
+ *      http://www.apache.org/licenses/LICENSE-2.0
  10
+ *
  11
+ * Unless required by applicable law or agreed to in writing, software
  12
+ * distributed under the License is distributed on an "AS IS" BASIS,
  13
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  14
+ * See the License for the specific language governing permissions and
  15
+ * limitations under the License.
  16
+ */
  17
+
  18
+#include <stdint.h>
  19
+#include <math.h>
  20
+#include <sys/types.h>
  21
+
  22
+#include <utils/Errors.h>
  23
+
  24
+#include <hardware/sensors.h>
  25
+
  26
+#include "RotationVectorSensor2.h"
  27
+#include "vec.h"
  28
+
  29
+namespace android {
  30
+// ---------------------------------------------------------------------------
  31
+
  32
+ANDROID_SINGLETON_STATIC_INSTANCE(RotationVectorSensor2)
  33
+
  34
+RotationVectorSensor2::RotationVectorSensor2()
  35
+    : mSensorDevice(SensorDevice::getInstance()),
  36
+      mEnabled(false), mHasData(false)
  37
+{
  38
+    sensor_t const* list;
  39
+    ssize_t count = mSensorDevice.getSensorList(&list);
  40
+    if (count > 0) {
  41
+        for (size_t i=0 ; i<size_t(count) ; i++) {
  42
+            if (list[i].type == SENSOR_TYPE_ORIENTATION) {
  43
+                mOrientation = Sensor(list + i);
  44
+            }
  45
+        }
  46
+    }
  47
+}
  48
+
  49
+bool RotationVectorSensor2::process(sensors_event_t* outEvent,
  50
+        const sensors_event_t& event)
  51
+{
  52
+    if (mHasData && event.type == SENSOR_TYPE_ACCELEROMETER) {
  53
+        *outEvent = event;
  54
+        outEvent->data[0] = mData[1];
  55
+        outEvent->data[1] = mData[2];
  56
+        outEvent->data[2] = mData[3];
  57
+        outEvent->data[3] = mData[0];
  58
+        outEvent->sensor = '_rv2';
  59
+        outEvent->type = SENSOR_TYPE_ROTATION_VECTOR;
  60
+
  61
+        mHasData = false;
  62
+        return true;
  63
+    }
  64
+    return false;
  65
+}
  66
+
  67
+status_t RotationVectorSensor2::activate(void* ident, bool enabled) {
  68
+    mEnabled = enabled;
  69
+    return mSensorDevice.activate(this, mOrientation.getHandle(), enabled);
  70
+}
  71
+
  72
+status_t RotationVectorSensor2::setDelay(void* ident, int handle, int64_t ns) {
  73
+    return mSensorDevice.setDelay(this, mOrientation.getHandle(), ns);
  74
+}
  75
+
  76
+Sensor RotationVectorSensor2::getSensor() const {
  77
+    sensor_t hwSensor;
  78
+    hwSensor.name       = "Rotation Vector Sensor 2";
  79
+    hwSensor.vendor     = "CyanogenMod Project";
  80
+    hwSensor.version    = 1;
  81
+    hwSensor.handle     = '_rv2';
  82
+    hwSensor.type       = SENSOR_TYPE_ROTATION_VECTOR;
  83
+    hwSensor.maxRange   = 1;
  84
+    hwSensor.resolution = 1.0f / (1<<24);
  85
+    hwSensor.power      = mOrientation.getPowerUsage();
  86
+    hwSensor.minDelay   = mOrientation.getMinDelay();
  87
+    Sensor sensor(&hwSensor);
  88
+    return sensor;
  89
+}
  90
+
  91
+void RotationVectorSensor2::process(const sensors_event_t& event) {
  92
+    if (event.type == SENSOR_TYPE_ORIENTATION) {
  93
+        const vec3_t v(event.data);
  94
+
  95
+        // Convert euler angle to quarternion
  96
+        const float deg2rad = M_PI / 180;
  97
+        float halfAzi = (v[0] / 2) * deg2rad;
  98
+        float halfPitch = (v[1] / 2) * deg2rad;
  99
+        float halfRoll = (-v[2] / 2) * deg2rad; // roll is reverse
  100
+
  101
+        float c1 = cosf(halfAzi);
  102
+        float s1 = sinf(halfAzi);
  103
+        float c2 = cosf(halfPitch);
  104
+        float s2 = sinf(halfPitch);
  105
+        float c3 = cosf(halfRoll);
  106
+        float s3 = sinf(halfRoll);
  107
+        mData[0] = c1*c2*c3 - s1*s2*s3;
  108
+        mData[1] = c1*s2*c3 - s1*c2*s3;
  109
+        mData[2] = c1*c2*s3 + s1*s2*c3;
  110
+        mData[3] = s1*c2*c3 + c1*s2*s3;
  111
+
  112
+        // Misc fixes (a.k.a. "magic")
  113
+        if (v[0] < 180) {
  114
+            mData[1] = -mData[1];
  115
+            mData[3] = -mData[3];
  116
+        } else {
  117
+            mData[2] = -mData[2];
  118
+        }
  119
+
  120
+        mHasData = true;
  121
+    }
  122
+}
  123
+
  124
+// ---------------------------------------------------------------------------
  125
+}; // namespace android
  126
+
63  services/sensorservice/RotationVectorSensor2.h
... ...
@@ -0,0 +1,63 @@
  1
+/*
  2
+ * Copyright (C) 2010 The Android Open Source Project
  3
+ * Copyright (C) 2012 The CyanogenMod Project
  4
+ *
  5
+ * Licensed under the Apache License, Version 2.0 (the "License");
  6
+ * you may not use this file except in compliance with the License.
  7
+ * You may obtain a copy of the License at
  8
+ *
  9
+ *      http://www.apache.org/licenses/LICENSE-2.0
  10
+ *
  11
+ * Unless required by applicable law or agreed to in writing, software
  12
+ * distributed under the License is distributed on an "AS IS" BASIS,
  13
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  14
+ * See the License for the specific language governing permissions and
  15
+ * limitations under the License.
  16
+ */
  17
+
  18
+#ifndef ANDROID_ROTATION_VECTOR_SENSOR2_H
  19
+#define ANDROID_ROTATION_VECTOR_SENSOR2_H
  20
+
  21
+#include <stdint.h>
  22
+#include <sys/types.h>
  23
+
  24
+#include <gui/Sensor.h>
  25
+
  26
+#include "SensorDevice.h"
  27
+#include "SensorInterface.h"
  28
+
  29
+#include "quat.h"
  30
+
  31
+// ---------------------------------------------------------------------------
  32
+namespace android {
  33
+// ---------------------------------------------------------------------------
  34
+
  35
+class RotationVectorSensor2 : public SensorInterface,
  36
+                              public Singleton<RotationVectorSensor2> {
  37
+    friend class Singleton<RotationVectorSensor2>;
  38
+
  39
+    SensorDevice& mSensorDevice;
  40
+
  41
+    Sensor mOrientation;
  42
+    bool mEnabled;
  43
+    bool mHasData;
  44
+    quat_t mData;
  45
+
  46
+public:
  47
+    RotationVectorSensor2();
  48
+    virtual bool process(sensors_event_t* outEvent,
  49
+            const sensors_event_t& event);
  50
+    virtual status_t activate(void* ident, bool enabled);
  51
+    virtual status_t setDelay(void* ident, int handle, int64_t ns);
  52
+    virtual Sensor getSensor() const;
  53
+    virtual bool isVirtual() const { return true; }
  54
+    bool isEnabled() const { return mEnabled; }
  55
+
  56
+    // Incoming data
  57
+    void process(const sensors_event_t& event);
  58
+};
  59
+
  60
+// ---------------------------------------------------------------------------
  61
+}; // namespace android
  62
+
  63
+#endif // ANDROID_ROTATION_VECTOR2_SENSOR_H
79  services/sensorservice/SensorService.cpp
@@ -39,6 +39,7 @@
39 39
 #include "GravitySensor.h"
40 40
 #include "LinearAccelerationSensor.h"
41 41
 #include "RotationVectorSensor.h"
  42
+#include "RotationVectorSensor2.h"
42 43
 
43 44
 namespace android {
44 45
 // ---------------------------------------------------------------------------
@@ -57,36 +58,54 @@ void SensorService::onFirstRef()
57 58
     SensorDevice& dev(SensorDevice::getInstance());
58 59
 
59 60
     if (dev.initCheck() == NO_ERROR) {
60  
-        uint32_t virtualSensorsNeeds =
61  
-                (1<<SENSOR_TYPE_GRAVITY) |
62  
-                (1<<SENSOR_TYPE_LINEAR_ACCELERATION) |
63  
-                (1<<SENSOR_TYPE_ROTATION_VECTOR);
64 61
         sensor_t const* list;
65  
-        int count = dev.getSensorList(&list);
66  
-        mLastEventSeen.setCapacity(count);
67  
-        for (int i=0 ; i<count ; i++) {
68  
-            registerSensor( new HardwareSensor(list[i]) );
69  
-            switch (list[i].type) {
70  
-                case SENSOR_TYPE_GRAVITY:
71  
-                case SENSOR_TYPE_LINEAR_ACCELERATION:
72  
-                case SENSOR_TYPE_ROTATION_VECTOR:
73  
-                    virtualSensorsNeeds &= ~(1<<list[i].type);
74  
-                    break;
  62
+        ssize_t count = dev.getSensorList(&list);
  63
+        if (count > 0) {
  64
+            ssize_t orientationIndex = -1;
  65
+            bool hasGyro = false;
  66
+            uint32_t virtualSensorsNeeds =
  67
+                    (1<<SENSOR_TYPE_GRAVITY) |
  68
+                    (1<<SENSOR_TYPE_LINEAR_ACCELERATION) |
  69
+                    (1<<SENSOR_TYPE_ROTATION_VECTOR);
  70
+
  71
+            mLastEventSeen.setCapacity(count);
  72
+            for (ssize_t i=0 ; i<count ; i++) {
  73
+                registerSensor( new HardwareSensor(list[i]) );
  74
+                switch (list[i].type) {
  75
+                    case SENSOR_TYPE_ORIENTATION:
  76
+                        orientationIndex = i;
  77
+                        break;
  78
+                    case SENSOR_TYPE_GYROSCOPE:
  79
+                        hasGyro = true;
  80
+                        break;
  81
+                    case SENSOR_TYPE_GRAVITY:
  82
+                    case SENSOR_TYPE_LINEAR_ACCELERATION:
  83
+                    case SENSOR_TYPE_ROTATION_VECTOR:
  84
+                        virtualSensorsNeeds &= ~(1<<list[i].type);
  85
+                        break;
  86
+                }
75 87
             }
76  
-        }
77 88
 
78  
-        if (virtualSensorsNeeds & (1<<SENSOR_TYPE_GRAVITY)) {
79  
-            registerVirtualSensor( new GravitySensor(list, count) );
80  
-        }
81  
-        if (virtualSensorsNeeds & (1<<SENSOR_TYPE_LINEAR_ACCELERATION)) {
82  
-            registerVirtualSensor( new LinearAccelerationSensor(list, count) );
83  
-        }
84  
-        if (virtualSensorsNeeds & (1<<SENSOR_TYPE_ROTATION_VECTOR)) {
85  
-            registerVirtualSensor( new RotationVectorSensor(list, count) );
86  
-        }
  89
+            if (hasGyro) {
  90
+                // Always instantiate Android's virtual sensors. Since they are
  91
+                // instantiated behind sensors from the HAL, they won't
  92
+                // interfere with applications, unless they looks specifically
  93
+                // for them (by name).
  94
+
  95
+                registerVirtualSensor( new RotationVectorSensor(list, count) );
  96
+                registerVirtualSensor( new GravitySensor(list, count) );
  97
+                registerVirtualSensor( new LinearAccelerationSensor(list, count) );
  98
+            } else if (orientationIndex != -1) {
  99
+                // If we don't have a gyro but have a orientation sensor from
  100
+                // elsewhere, we can compute rotation vector from that.
  101
+                // (Google Maps expects rotation vector sensor to exist.)
  102
+
  103
+                registerVirtualSensor( &RotationVectorSensor2::getInstance() );
  104
+            }
87 105
 
88  
-        run("SensorService", PRIORITY_URGENT_DISPLAY);
89  
-        mInitCheck = NO_ERROR;
  106
+            run("SensorService", PRIORITY_URGENT_DISPLAY);
  107
+            mInitCheck = NO_ERROR;
  108
+        }
90 109
     }
91 110
 }
92 111
 
@@ -184,13 +203,19 @@ bool SensorService::threadLoop()
184 203
 
185 204
         // handle virtual sensors
186 205
         if (count && vcount) {
  206
+            sensors_event_t const * const event = buffer;
187 207
             const DefaultKeyedVector<int, SensorInterface*> virtualSensors(
188 208
                     getActiveVirtualSensors());
189 209
             const size_t activeVirtualSensorCount = virtualSensors.size();
190 210
             if (activeVirtualSensorCount) {
191 211
                 size_t k = 0;
  212
+                RotationVectorSensor2& rv2(RotationVectorSensor2::getInstance());
  213
+                if (rv2.isEnabled()) {
  214
+                    for (size_t i=0 ; i<size_t(count) ; i++) {
  215
+                        rv2.process(event[i]);
  216
+                    }
  217
+                }
192 218
                 for (size_t i=0 ; i<size_t(count) ; i++) {
193  
-                    sensors_event_t const * const event = buffer;
194 219
                     for (size_t j=0 ; j<activeVirtualSensorCount ; j++) {
195 220
                         sensors_event_t out;
196 221
                         if (virtualSensors.valueAt(j)->process(&out, event[i])) {
393  services/sensorservice/mat.h
... ...
@@ -0,0 +1,393 @@
  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 */
98  services/sensorservice/quat.h
... ...
@@ -0,0 +1,98 @@
  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_QUAT_H
  18
+#define ANDROID_QUAT_H
  19
+
  20
+#include <math.h>
  21
+
  22
+#include "vec.h"
  23
+#include "mat.h"
  24
+
  25
+// -----------------------------------------------------------------------
  26
+namespace android {
  27
+// -----------------------------------------------------------------------
  28
+
  29
+template <typename TYPE>
  30
+mat<TYPE, 3, 3> quatToMatrix(const vec<TYPE, 4>& q) {
  31
+    mat<TYPE, 3, 3> R;
  32
+    TYPE q0(q.w);
  33
+    TYPE q1(q.x);
  34
+    TYPE q2(q.y);
  35
+    TYPE q3(q.z);
  36
+    TYPE sq_q1 = 2 * q1 * q1;
  37
+    TYPE sq_q2 = 2 * q2 * q2;
  38
+    TYPE sq_q3 = 2 * q3 * q3;
  39
+    TYPE q1_q2 = 2 * q1 * q2;
  40
+    TYPE q3_q0 = 2 * q3 * q0;
  41
+    TYPE q1_q3 = 2 * q1 * q3;
  42
+    TYPE q2_q0 = 2 * q2 * q0;
  43
+    TYPE q2_q3 = 2 * q2 * q3;
  44
+    TYPE q1_q0 = 2 * q1 * q0;
  45
+    R[0][0] = 1 - sq_q2 - sq_q3;
  46
+    R[0][1] = q1_q2 - q3_q0;
  47
+    R[0][2] = q1_q3 + q2_q0;
  48
+    R[1][0] = q1_q2 + q3_q0;
  49
+    R[1][1] = 1 - sq_q1 - sq_q3;
  50
+    R[1][2] = q2_q3 - q1_q0;
  51
+    R[2][0] = q1_q3 - q2_q0;
  52
+    R[2][1] = q2_q3 + q1_q0;
  53
+    R[2][2] = 1 - sq_q1 - sq_q2;
  54
+    return R;
  55
+}
  56
+
  57
+template <typename TYPE>
  58
+vec<TYPE, 4> matrixToQuat(const mat<TYPE, 3, 3>& R) {
  59
+    // matrix to quaternion
  60
+
  61
+    struct {
  62
+        inline TYPE operator()(TYPE v) {
  63
+            return v < 0 ? 0 : v;
  64
+        }
  65
+    } clamp;
  66
+
  67
+    vec<TYPE, 4> q;
  68
+    const float Hx = R[0].x;
  69
+    const float My = R[1].y;
  70
+    const float Az = R[2].z;
  71
+    q.x = sqrtf( clamp( Hx - My - Az + 1) * 0.25f );
  72
+    q.y = sqrtf( clamp(-Hx + My - Az + 1) * 0.25f );
  73
+    q.z = sqrtf( clamp(-Hx - My + Az + 1) * 0.25f );
  74
+    q.w = sqrtf( clamp( Hx + My + Az + 1) * 0.25f );
  75
+    q.x = copysignf(q.x, R[2].y - R[1].z);
  76
+    q.y = copysignf(q.y, R[0].z - R[2].x);
  77
+    q.z = copysignf(q.z, R[1].x - R[0].y);
  78
+    // guaranteed to be unit-quaternion
  79
+    return q;
  80
+}
  81
+
  82
+template <typename TYPE>
  83
+vec<TYPE, 4> normalize_quat(const vec<TYPE, 4>& q) {
  84
+    vec<TYPE, 4> r(q);
  85
+    if (r.w < 0) {
  86
+        r = -r;
  87
+    }
  88
+    return normalize(r);
  89
+}
  90
+
  91
+// -----------------------------------------------------------------------
  92
+
  93
+typedef vec4_t quat_t;
  94
+
  95
+// -----------------------------------------------------------------------
  96
+}; // namespace android
  97
+
  98
+#endif /* ANDROID_QUAT_H */
118  services/sensorservice/traits.h
... ...
@@ -0,0 +1,118 @@
  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_TRAITS_H
  18
+#define ANDROID_TRAITS_H
  19
+
  20
+// -----------------------------------------------------------------------
  21
+// Typelists
  22
+
  23
+namespace android {
  24
+
  25
+// end-of-list marker
  26
+class NullType {};
  27
+
  28
+// type-list node
  29
+template <typename T, typename U>
  30
+struct TypeList {
  31
+    typedef T Head;
  32
+    typedef U Tail;
  33
+};
  34
+
  35
+// helpers to build typelists
  36
+#define TYPELIST_1(T1) TypeList<T1, NullType>
  37
+#define TYPELIST_2(T1, T2) TypeList<T1, TYPELIST_1(T2)>
  38
+#define TYPELIST_3(T1, T2, T3) TypeList<T1, TYPELIST_2(T2, T3)>
  39
+#define TYPELIST_4(T1, T2, T3, T4) TypeList<T1, TYPELIST_3(T2, T3, T4)>
  40
+
  41
+// typelists algorithms
  42
+namespace TL {
  43
+template <typename TList, typename T> struct IndexOf;
  44
+
  45
+template <typename T>
  46
+struct IndexOf<NullType, T> {
  47
+    enum { value = -1 };
  48
+};
  49
+
  50
+template <typename T, typename Tail>
  51
+struct IndexOf<TypeList<T, Tail>, T> {
  52
+    enum { value = 0 };
  53
+};
  54
+
  55
+template <typename Head, typename Tail, typename T>
  56
+struct IndexOf<TypeList<Head, Tail>, T> {
  57
+private:
  58
+    enum { temp = IndexOf<Tail, T>::value };
  59
+public:
  60
+    enum { value = temp == -1 ? -1 : 1 + temp };
  61
+};
  62
+
  63
+}; // namespace TL
  64
+
  65
+// type selection based on a boolean
  66
+template <bool flag, typename T, typename U>
  67
+struct Select {
  68
+    typedef T Result;
  69
+};
  70
+template <typename T, typename U>
  71
+struct Select<false, T, U> {
  72
+    typedef U Result;
  73
+};
  74
+
  75
+// -----------------------------------------------------------------------
  76
+// Type traits
  77
+
  78
+template <typename T>
  79
+class TypeTraits {
  80
+    typedef TYPELIST_4(
  81
+            unsigned char, unsigned short,
  82
+            unsigned int, unsigned long int) UnsignedInts;
  83
+
  84
+    typedef TYPELIST_4(
  85
+            signed char, signed short,
  86
+            signed int, signed long int) SignedInts;
  87
+
  88
+    typedef TYPELIST_1(
  89
+            bool) OtherInts;
  90
+
  91
+    typedef TYPELIST_3(
  92
+            float, double, long double) Floats;
  93
+
  94
+    template<typename U> struct PointerTraits {
  95
+        enum { result = false };
  96
+        typedef NullType PointeeType;
  97
+    };
  98
+    template<typename U> struct PointerTraits<U*> {
  99
+        enum { result = true };
  100
+        typedef U PointeeType;
  101
+    };
  102
+
  103
+public:
  104
+    enum { isStdUnsignedInt = TL::IndexOf<UnsignedInts, T>::value >= 0 };
  105
+    enum { isStdSignedInt   = TL::IndexOf<SignedInts,   T>::value >= 0 };
  106
+    enum { isStdIntegral    = TL::IndexOf<OtherInts,    T>::value >= 0 || isStdUnsignedInt || isStdSignedInt };
  107
+    enum { isStdFloat       = TL::IndexOf<Floats,       T>::value >= 0 };
  108
+    enum { isPointer        = PointerTraits<T>::result };
  109
+    enum { isStdArith       = isStdIntegral || isStdFloat };
  110
+
  111
+    // best parameter type for given type
  112
+    typedef typename Select<isStdArith || isPointer, T, const T&>::Result ParameterType;
  113
+};
  114
+
  115
+// -----------------------------------------------------------------------
  116
+}; // namespace android
  117
+
  118
+#endif /* ANDROID_TRAITS_H */
438  services/sensorservice/vec.h
... ...
@@ -0,0 +1,438 @@
  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