INSTALL(FILES include/sensor.h include/sensor-internal.h DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/sensor)
INSTALL(TARGETS ${PROJECT_NAME} LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR})
-ADD_SUBDIRECTORY(tests/stress_test)
-ADD_SUBDIRECTORY(tests/orientation_test)
+ADD_SUBDIRECTORY(tests)
ADD_SUBDIRECTORY(tools)
%manifest packaging/capi-system-sensor.manifest
%defattr(-,root,root,-)
%{_bindir}/sensor-test
-%{_bindir}/sensor-orientation_test
%files tools
%{_bindir}/sensor-tool
+++ /dev/null
-CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
-PROJECT(sensor-orientation_test)
-
-SET(PKG_MODULES
- glib-2.0)
-
-INCLUDE(FindPkgConfig)
-PKG_CHECK_MODULES(pkgs REQUIRED ${PKG_MODULES})
-
-INCLUDE_DIRECTORIES(${pkgs_INCLUDE_DIRS} ${CMAKE_SOURCE_DIR}/include)
-
-FOREACH(flag ${REQUIRED_PKGS_CFLAGS})
- SET(EXTRA_CFLAGS "${EXTRA_CFLAGS} ${flag}")
-ENDFOREACH(flag)
-
-SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${EXTRA_CFLAGS} -fPIE")
-SET(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -pie")
-
-FILE(GLOB SRCS *.cpp *.h)
-ADD_EXECUTABLE(${PROJECT_NAME} ${SRCS})
-TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${PKGS_LDFLAGS} capi-system-sensor)
-INSTALL(TARGETS ${PROJECT_NAME} DESTINATION ${CMAKE_INSTALL_BINDIR})
+++ /dev/null
-/*
- * Copyright (C) 2007 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.
- */
-
-// released in android-11.0.0_r9
-
-#ifndef ANDROID_ERRORS_H
-#define ANDROID_ERRORS_H
-#include <sys/types.h>
-#include <errno.h>
-namespace android {
-// use this type to return error codes
-#ifdef HAVE_MS_C_RUNTIME
-typedef int status_t;
-#else
-typedef int32_t status_t;
-#endif
-/* the MS C runtime lacks a few error codes */
-/*
- * Error codes.
- * All error codes are negative values.
- */
-// Win32 #defines NO_ERROR as well. It has the same value, so there's no
-// real conflict, though it's a bit awkward.
-#ifdef _WIN32
-# undef NO_ERROR
-#endif
-
-enum {
- OK = 0, // Everything's swell.
- NO_ERROR = 0, // No errors.
-
- UNKNOWN_ERROR = 0x80000000,
- NO_MEMORY = -ENOMEM,
- INVALID_OPERATION = -ENOSYS,
- BAD_VALUE = -EINVAL,
- BAD_TYPE = 0x80000001,
- NAME_NOT_FOUND = -ENOENT,
- PERMISSION_DENIED = -EPERM,
- NO_INIT = -ENODEV,
- ALREADY_EXISTS = -EEXIST,
- DEAD_OBJECT = -EPIPE,
- FAILED_TRANSACTION = 0x80000002,
- JPARKS_BROKE_IT = -EPIPE,
-#if !defined(HAVE_MS_C_RUNTIME)
- BAD_INDEX = -EOVERFLOW,
- NOT_ENOUGH_DATA = -ENODATA,
- WOULD_BLOCK = -EWOULDBLOCK,
- TIMED_OUT = -ETIMEDOUT,
- UNKNOWN_TRANSACTION = -EBADMSG,
-#else
- BAD_INDEX = -E2BIG,
- NOT_ENOUGH_DATA = 0x80000003,
- WOULD_BLOCK = 0x80000004,
- TIMED_OUT = 0x80000005,
- UNKNOWN_TRANSACTION = 0x80000006,
-#endif
- FDS_NOT_ALLOWED = 0x80000007,
-};
-// Restore define; enumeration is in "android" namespace, so the value defined
-// there won't work for Win32 code in a different namespace.
-#ifdef _WIN32
-# define NO_ERROR 0L
-#endif
-}; // namespace android
-
-// ---------------------------------------------------------------------------
-
-#endif // ANDROID_ERRORS_H
\ No newline at end of file
+++ /dev/null
-/*
- * 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.
- */
-
-// released in android-11.0.0_r9
-
-#include <stdio.h>
-#include "fusion.h"
-
-namespace android {
-
-// -----------------------------------------------------------------------
-
-/*==================== BEGIN FUSION SENSOR PARAMETER =========================*/
-
-/* Note:
- * If a platform uses software fusion, it is necessary to tune the following
- * parameters to fit the hardware sensors prior to release.
- *
- * The DEFAULT_ parameters will be used in FUSION_9AXIS and FUSION_NOMAG mode.
- * The GEOMAG_ parameters will be used in FUSION_NOGYRO mode.
- */
-
-/*
- * GYRO_VAR gives the measured variance of the gyro's output per
- * Hz (or variance at 1 Hz). This is an "intrinsic" parameter of the gyro,
- * which is independent of the sampling frequency.
- *
- * The variance of gyro's output at a given sampling period can be
- * calculated as:
- * variance(T) = GYRO_VAR / T
- *
- * The variance of the INTEGRATED OUTPUT at a given sampling period can be
- * calculated as:
- * variance_integrate_output(T) = GYRO_VAR * T
- */
-static const float DEFAULT_GYRO_VAR = 1e-7; // (rad/s)^2 / Hz
-static const float DEFAULT_GYRO_BIAS_VAR = 1e-12; // (rad/s)^2 / s (guessed)
-static const float GEOMAG_GYRO_VAR = 1e-4; // (rad/s)^2 / Hz
-static const float GEOMAG_GYRO_BIAS_VAR = 1e-8; // (rad/s)^2 / s (guessed)
-
-/*
- * Standard deviations of accelerometer and magnetometer
- */
-static const float DEFAULT_ACC_STDEV = 0.015f; // m/s^2 (measured 0.08 / CDD 0.05)
-static const float DEFAULT_MAG_STDEV = 0.1f; // uT (measured 0.7 / CDD 0.5)
-static const float GEOMAG_ACC_STDEV = 0.05f; // m/s^2 (measured 0.08 / CDD 0.05)
-static const float GEOMAG_MAG_STDEV = 0.1f; // uT (measured 0.7 / CDD 0.5)
-
-
-/* ====================== END FUSION SENSOR PARAMETER ========================*/
-
-static const float SYMMETRY_TOLERANCE = 1e-10f;
-
-/*
- * Accelerometer updates will not be performed near free fall to avoid
- * ill-conditioning and div by zeros.
- * Threshhold: 10% of g, in m/s^2
- */
-static const float NOMINAL_GRAVITY = 9.81f;
-static const float FREE_FALL_THRESHOLD = 0.1f * (NOMINAL_GRAVITY);
-
-/*
- * The geomagnetic-field should be between 30uT and 60uT.
- * Fields strengths greater than this likely indicate a local magnetic
- * disturbance which we do not want to update into the fused frame.
- */
-static const float MAX_VALID_MAGNETIC_FIELD = 100; // uT
-static const float MAX_VALID_MAGNETIC_FIELD_SQ =
- MAX_VALID_MAGNETIC_FIELD*MAX_VALID_MAGNETIC_FIELD;
-
-/*
- * Values of the field smaller than this should be ignored in fusion to avoid
- * ill-conditioning. This state can happen with anomalous local magnetic
- * disturbances canceling the Earth field.
- */
-static const float MIN_VALID_MAGNETIC_FIELD = 10; // uT
-static const float MIN_VALID_MAGNETIC_FIELD_SQ =
- MIN_VALID_MAGNETIC_FIELD*MIN_VALID_MAGNETIC_FIELD;
-
-/*
- * If the cross product of two vectors has magnitude squared less than this,
- * we reject it as invalid due to alignment of the vectors.
- * This threshold is used to check for the case where the magnetic field sample
- * is parallel to the gravity field, which can happen in certain places due
- * to magnetic field disturbances.
- */
-static const float MIN_VALID_CROSS_PRODUCT_MAG = 1.0e-3;
-static const float MIN_VALID_CROSS_PRODUCT_MAG_SQ =
- MIN_VALID_CROSS_PRODUCT_MAG*MIN_VALID_CROSS_PRODUCT_MAG;
-
-static const float SQRT_3 = 1.732f;
-static const float WVEC_EPS = 1e-4f/SQRT_3;
-// -----------------------------------------------------------------------
-
-template <typename TYPE, size_t C, size_t R>
-static mat<TYPE, R, R> scaleCovariance(
- const mat<TYPE, C, R>& A,
- const mat<TYPE, C, C>& P) {
- // A*P*transpose(A);
- mat<TYPE, R, R> APAt;
- for (size_t r=0 ; r<R ; r++) {
- for (size_t j=r ; j<R ; j++) {
- double apat(0);
- for (size_t c=0 ; c<C ; c++) {
- double v(A[c][r]*P[c][c]*0.5);
- for (size_t k=c+1 ; k<C ; k++)
- v += A[k][r] * P[c][k];
- apat += 2 * v * A[c][j];
- }
- APAt[j][r] = apat;
- APAt[r][j] = apat;
- }
- }
- return APAt;
-}
-
-template <typename TYPE, typename OTHER_TYPE>
-static mat<TYPE, 3, 3> crossMatrix(const vec<TYPE, 3>& p, OTHER_TYPE diag) {
- mat<TYPE, 3, 3> r;
- r[0][0] = diag;
- r[1][1] = diag;
- r[2][2] = diag;
- r[0][1] = p.z;
- r[1][0] =-p.z;
- r[0][2] =-p.y;
- r[2][0] = p.y;
- r[1][2] = p.x;
- r[2][1] =-p.x;
- return r;
-}
-
-
-template<typename TYPE, size_t SIZE>
-class Covariance {
- mat<TYPE, SIZE, SIZE> mSumXX;
- vec<TYPE, SIZE> mSumX;
- size_t mN;
-public:
- Covariance() : mSumXX(0.0f), mSumX(0.0f), mN(0) { }
- void update(const vec<TYPE, SIZE>& x) {
- mSumXX += x*transpose(x);
- mSumX += x;
- mN++;
- }
- mat<TYPE, SIZE, SIZE> operator()() const {
- const float N = 1.0f / mN;
- return mSumXX*N - (mSumX*transpose(mSumX))*(N*N);
- }
- void reset() {
- mN = 0;
- mSumXX = 0;
- mSumX = 0;
- }
- size_t getCount() const {
- return mN;
- }
-};
-
-// -----------------------------------------------------------------------
-
-Fusion::Fusion() {
- Phi[0][1] = 0;
- Phi[1][1] = 1;
-
- Ba.x = 0;
- Ba.y = 0;
- Ba.z = 1;
-
- Bm.x = 0;
- Bm.y = 1;
- Bm.z = 0;
-
- x0 = 0;
- x1 = 0;
-
- init();
-}
-
-void Fusion::init(int mode) {
- mInitState = 0;
-
- mGyroRate = 0;
-
- mCount[0] = 0;
- mCount[1] = 0;
- mCount[2] = 0;
-
- mData = 0;
- mMode = mode;
-
- if (mMode != FUSION_NOGYRO) { //normal or game rotation
- mParam.gyroVar = DEFAULT_GYRO_VAR;
- mParam.gyroBiasVar = DEFAULT_GYRO_BIAS_VAR;
- mParam.accStdev = DEFAULT_ACC_STDEV;
- mParam.magStdev = DEFAULT_MAG_STDEV;
- } else {
- mParam.gyroVar = GEOMAG_GYRO_VAR;
- mParam.gyroBiasVar = GEOMAG_GYRO_BIAS_VAR;
- mParam.accStdev = GEOMAG_ACC_STDEV;
- mParam.magStdev = GEOMAG_MAG_STDEV;
- }
-}
-
-void Fusion::initFusion(const vec4_t& q, float dT)
-{
- // initial estimate: E{ x(t0) }
- x0 = q;
- x1 = 0;
-
- // process noise covariance matrix: G.Q.Gt, with
- //
- // G = | -1 0 | Q = | q00 q10 |
- // | 0 1 | | q01 q11 |
- //
- // q00 = sv^2.dt + 1/3.su^2.dt^3
- // q10 = q01 = 1/2.su^2.dt^2
- // q11 = su^2.dt
- //
-
- const float dT2 = dT*dT;
- const float dT3 = dT2*dT;
-
- // variance of integrated output at 1/dT Hz (random drift)
- const float q00 = mParam.gyroVar * dT + 0.33333f * mParam.gyroBiasVar * dT3;
-
- // variance of drift rate ramp
- const float q11 = mParam.gyroBiasVar * dT;
- const float q10 = 0.5f * mParam.gyroBiasVar * dT2;
- const float q01 = q10;
-
- GQGt[0][0] = q00; // rad^2
- GQGt[1][0] = -q10;
- GQGt[0][1] = -q01;
- GQGt[1][1] = q11; // (rad/s)^2
-
- // initial covariance: Var{ x(t0) }
- // TODO: initialize P correctly
- P = 0;
-}
-
-bool Fusion::hasEstimate() const {
- return ((mInitState & MAG) || (mMode == FUSION_NOMAG)) &&
- ((mInitState & GYRO) || (mMode == FUSION_NOGYRO)) &&
- (mInitState & ACC);
-}
-
-bool Fusion::checkInitComplete(int what, const vec3_t& d, float dT) {
- if (hasEstimate())
- return true;
-
- if (what == ACC) {
- mData[0] += d * (1/length(d));
- mCount[0]++;
- mInitState |= ACC;
- if (mMode == FUSION_NOGYRO ) {
- mGyroRate = dT;
- }
- } else if (what == MAG) {
- mData[1] += d * (1/length(d));
- mCount[1]++;
- mInitState |= MAG;
- } else if (what == GYRO) {
- mGyroRate = dT;
- mData[2] += d*dT;
- mCount[2]++;
- mInitState |= GYRO;
- }
-
- if (hasEstimate()) {
- // Average all the values we collected so far
- mData[0] *= 1.0f/mCount[0];
- if (mMode != FUSION_NOMAG) {
- mData[1] *= 1.0f/mCount[1];
- }
- mData[2] *= 1.0f/mCount[2];
-
- // calculate the MRPs from the data collection, this gives us
- // a rough estimate of our initial state
- mat33_t R;
- vec3_t up(mData[0]);
- vec3_t east;
-
- if (mMode != FUSION_NOMAG) {
- east = normalize(cross_product(mData[1], up));
- } else {
- east = getOrthogonal(up);
- }
-
- vec3_t north(cross_product(up, east));
- R << east << north << up;
- const vec4_t q = matrixToQuat(R);
-
- initFusion(q, mGyroRate);
- }
-
- return false;
-}
-
-void Fusion::handleGyro(const vec3_t& w, float dT) {
- if (!checkInitComplete(GYRO, w, dT))
- return;
-
- predict(w, dT);
-}
-
-status_t Fusion::handleAcc(const vec3_t& a, float dT) {
- if (!checkInitComplete(ACC, a, dT))
- return BAD_VALUE;
-
- // ignore acceleration data if we're close to free-fall
- const float l = length(a);
- if (l < FREE_FALL_THRESHOLD) {
- return BAD_VALUE;
- }
-
- const float l_inv = 1.0f/l;
-
- if ( mMode == FUSION_NOGYRO ) {
- //geo mag
- vec3_t w_dummy;
- w_dummy = x1; //bias
- predict(w_dummy, dT);
- }
-
- if ( mMode == FUSION_NOMAG) {
- vec3_t m;
- m = getRotationMatrix()*Bm;
- update(m, Bm, mParam.magStdev);
- }
-
- vec3_t unityA = a * l_inv;
- const float d = sqrtf(fabsf(l- NOMINAL_GRAVITY));
- const float p = l_inv * mParam.accStdev*expf(d);
-
- update(unityA, Ba, p);
- return NO_ERROR;
-}
-
-status_t Fusion::handleMag(const vec3_t& m) {
- if (!checkInitComplete(MAG, m))
- return BAD_VALUE;
-
- // the geomagnetic-field should be between 30uT and 60uT
- // reject if too large to avoid spurious magnetic sources
- const float magFieldSq = length_squared(m);
- if (magFieldSq > MAX_VALID_MAGNETIC_FIELD_SQ) {
- return BAD_VALUE;
- } else if (magFieldSq < MIN_VALID_MAGNETIC_FIELD_SQ) {
- // Also reject if too small since we will get ill-defined (zero mag)
- // cross-products below
- return BAD_VALUE;
- }
-
- // Orthogonalize the magnetic field to the gravity field, mapping it into
- // tangent to Earth.
- const vec3_t up( getRotationMatrix() * Ba );
- const vec3_t east( cross_product(m, up) );
-
- // If the m and up vectors align, the cross product magnitude will
- // approach 0.
- // Reject this case as well to avoid div by zero problems and
- // ill-conditioning below.
- if (length_squared(east) < MIN_VALID_CROSS_PRODUCT_MAG_SQ) {
- return BAD_VALUE;
- }
-
- // If we have created an orthogonal magnetic field successfully,
- // then pass it in as the update.
- vec3_t north( cross_product(up, east) );
-
- const float l_inv = 1 / length(north);
- north *= l_inv;
-
- update(north, Bm, mParam.magStdev*l_inv);
- return NO_ERROR;
-}
-
-void Fusion::checkState() {
- // P needs to stay positive semidefinite or the fusion diverges. When we
- // detect divergence, we reset the fusion.
- // TODO(braun): Instead, find the reason for the divergence and fix it.
-
- if (!isPositiveSemidefinite(P[0][0], SYMMETRY_TOLERANCE) ||
- !isPositiveSemidefinite(P[1][1], SYMMETRY_TOLERANCE)) {
- P = 0;
- }
-}
-
-vec4_t Fusion::getAttitude() const {
- return x0;
-}
-
-vec3_t Fusion::getBias() const {
- return x1;
-}
-
-mat33_t Fusion::getRotationMatrix() const {
- return quatToMatrix(x0);
-}
-
-mat34_t Fusion::getF(const vec4_t& q) {
- mat34_t F;
-
- // This is used to compute the derivative of q
- // F = | [q.xyz]x |
- // | -q.xyz |
-
- F[0].x = q.w; F[1].x =-q.z; F[2].x = q.y;
- F[0].y = q.z; F[1].y = q.w; F[2].y =-q.x;
- F[0].z =-q.y; F[1].z = q.x; F[2].z = q.w;
- F[0].w =-q.x; F[1].w =-q.y; F[2].w =-q.z;
- return F;
-}
-
-void Fusion::predict(const vec3_t& w, float dT) {
- const vec4_t q = x0;
- const vec3_t b = x1;
- vec3_t we = w - b;
-
- if (length(we) < WVEC_EPS) {
- we = (we[0]>0.f)?WVEC_EPS:-WVEC_EPS;
- }
- // q(k+1) = O(we)*q(k)
- // --------------------
- //
- // O(w) = | cos(0.5*||w||*dT)*I33 - [psi]x psi |
- // | -psi' cos(0.5*||w||*dT) |
- //
- // psi = sin(0.5*||w||*dT)*w / ||w||
- //
- //
- // P(k+1) = Phi(k)*P(k)*Phi(k)' + G*Q(k)*G'
- // ----------------------------------------
- //
- // G = | -I33 0 |
- // | 0 I33 |
- //
- // Phi = | Phi00 Phi10 |
- // | 0 1 |
- //
- // Phi00 = I33
- // - [w]x * sin(||w||*dt)/||w||
- // + [w]x^2 * (1-cos(||w||*dT))/||w||^2
- //
- // Phi10 = [w]x * (1 - cos(||w||*dt))/||w||^2
- // - [w]x^2 * (||w||*dT - sin(||w||*dt))/||w||^3
- // - I33*dT
-
- const mat33_t I33(1);
- const mat33_t I33dT(dT);
- const mat33_t wx(crossMatrix(we, 0));
- const mat33_t wx2(wx*wx);
- const float lwedT = length(we)*dT;
- const float hlwedT = 0.5f*lwedT;
- const float ilwe = 1.f/length(we);
- const float k0 = (1-cosf(lwedT))*(ilwe*ilwe);
- const float k1 = sinf(lwedT);
- const float k2 = cosf(hlwedT);
- const vec3_t psi(sinf(hlwedT)*ilwe*we);
- const mat33_t O33(crossMatrix(-psi, k2));
- mat44_t O;
- O[0].xyz = O33[0]; O[0].w = -psi.x;
- O[1].xyz = O33[1]; O[1].w = -psi.y;
- O[2].xyz = O33[2]; O[2].w = -psi.z;
- O[3].xyz = psi; O[3].w = k2;
-
- Phi[0][0] = I33 - wx*(k1*ilwe) + wx2*k0;
- Phi[1][0] = wx*k0 - I33dT - wx2*(ilwe*ilwe*ilwe)*(lwedT-k1);
-
- x0 = O*q;
-
- if (x0.w < 0)
- x0 = -x0;
-
- P = Phi*P*transpose(Phi) + GQGt;
-
- checkState();
-}
-
-void Fusion::update(const vec3_t& z, const vec3_t& Bi, float sigma) {
- vec4_t q(x0);
- // measured vector in body space: h(p) = A(p)*Bi
- const mat33_t A(quatToMatrix(q));
- const vec3_t Bb(A*Bi);
-
- // Sensitivity matrix H = dh(p)/dp
- // H = [ L 0 ]
- const mat33_t L(crossMatrix(Bb, 0));
-
- // gain...
- // K = P*Ht / [H*P*Ht + R]
- vec<mat33_t, 2> K;
- const mat33_t R(sigma*sigma);
- const mat33_t S(scaleCovariance(L, P[0][0]) + R);
- const mat33_t Si(invert(S));
- const mat33_t LtSi(transpose(L)*Si);
- K[0] = P[0][0] * LtSi;
- K[1] = transpose(P[1][0])*LtSi;
-
- // update...
- // P = (I-K*H) * P
- // P -= K*H*P
- // | K0 | * | L 0 | * P = | K0*L 0 | * | P00 P10 | = | K0*L*P00 K0*L*P10 |
- // | K1 | | K1*L 0 | | P01 P11 | | K1*L*P00 K1*L*P10 |
- // Note: the Joseph form is numerically more stable and given by:
- // P = (I-KH) * P * (I-KH)' + K*R*R'
- const mat33_t K0L(K[0] * L);
- const mat33_t K1L(K[1] * L);
- P[0][0] -= K0L*P[0][0];
- P[1][1] -= K1L*P[1][0];
- P[1][0] -= K0L*P[1][0];
- P[0][1] = transpose(P[1][0]);
-
- const vec3_t e(z - Bb);
- const vec3_t dq(K[0]*e);
-
- q += getF(q)*(0.5f*dq);
- x0 = normalize_quat(q);
-
- if (mMode != FUSION_NOMAG) {
- const vec3_t db(K[1]*e);
- x1 += db;
- }
-
- checkState();
-}
-
-vec3_t Fusion::getOrthogonal(const vec3_t &v) {
- vec3_t w;
- if (fabsf(v[0])<= fabsf(v[1]) && fabsf(v[0]) <= fabsf(v[2])) {
- w[0]=0.f;
- w[1] = v[2];
- w[2] = -v[1];
- } else if (fabsf(v[1]) <= fabsf(v[2])) {
- w[0] = v[2];
- w[1] = 0.f;
- w[2] = -v[0];
- }else {
- w[0] = v[1];
- w[1] = -v[0];
- w[2] = 0.f;
- }
- return normalize(w);
-}
-
-
-// -----------------------------------------------------------------------
-
-}; // namespace android
-
+++ /dev/null
-/*
- * 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.
- */
-
-// released in android-11.0.0_r9
-
-#ifndef ANDROID_FUSION_H
-#define ANDROID_FUSION_H
-
-#include "quat.h"
-#include "mat.h"
-#include "vec.h"
-#include "errors.h"
-
-namespace android {
-
-typedef mat<float, 3, 4> mat34_t;
-
-enum FUSION_MODE{
- FUSION_9AXIS, // use accel gyro mag
- FUSION_NOMAG, // use accel gyro (game rotation, gravity)
- FUSION_NOGYRO, // use accel mag (geomag rotation)
- NUM_FUSION_MODE
-};
-
-class Fusion {
- /*
- * the state vector is made of two sub-vector containing respectively:
- * - modified Rodrigues parameters
- * - the estimated gyro bias
- */
-// quat_t x0;
- vec3_t x1;
-
- /*
- * the predicated covariance matrix is made of 4 3x3 sub-matrices and it is
- * semi-definite positive.
- *
- * P = | P00 P10 | = | P00 P10 |
- * | P01 P11 | | P10t P11 |
- *
- * Since P01 = transpose(P10), the code below never calculates or
- * stores P01.
- */
- mat<mat33_t, 2, 2> P;
-
- /*
- * the process noise covariance matrix
- */
- mat<mat33_t, 2, 2> GQGt;
-
-public:
- quat_t x0;
- Fusion();
- void init(int mode = FUSION_9AXIS);
- void handleGyro(const vec3_t& w, float dT);
- status_t handleAcc(const vec3_t& a, float dT);
- status_t handleMag(const vec3_t& m);
- vec4_t getAttitude() const;
- vec3_t getBias() const;
- mat33_t getRotationMatrix() const;
- bool hasEstimate() const;
-
-private:
- struct Parameter {
- float gyroVar;
- float gyroBiasVar;
- float accStdev;
- float magStdev;
- } mParam;
-
- mat<mat33_t, 2, 2> Phi;
- vec3_t Ba, Bm;
- uint32_t mInitState;
- float mGyroRate;
- vec<vec3_t, 3> mData;
- size_t mCount[3];
- int mMode;
-
- enum { ACC=0x1, MAG=0x2, GYRO=0x4 };
- bool checkInitComplete(int, const vec3_t& w, float d = 0);
- void initFusion(const vec4_t& q0, float dT);
- void checkState();
- void predict(const vec3_t& w, float dT);
- void update(const vec3_t& z, const vec3_t& Bi, float sigma);
- static mat34_t getF(const vec4_t& p);
- static vec3_t getOrthogonal(const vec3_t &v);
-};
-
-}; // namespace android
-
-#endif // ANDROID_FUSION_H
+++ /dev/null
-#include <stdio.h>
-#include <sensor.h>
-#include <glib.h>
-#include <math.h>
-
-#include <vector>
-#include <string>
-
-#include "tizen_orientation.h"
-#include "fusion.h"
-
-const float TIMEUNIT = 1000000.0f;
-
-std::vector<sensor_type_e> ev;
-std::vector<std::string> sv;
-
-android::Fusion Android_Fusion; // the class that calculates Android rotation vector to Android orientation
-android::Fusion Android_Fusion2; // the class that calculates TIZEN rotation vector to Android orientation
-
-static unsigned long long timestamp[3]; // timestamps of accelerometer, gyroscope and magnetometer
-
-static float TIZEN_rv[4]; // TIZEN rotation vector
-static float Android_rv[4]; // Android rotation vector
-
-static float TIZEN_ori[3]; // TIZEN rotation vector to TIZEN orientation
-static float Android_ori[3]; // Android rotation vector to Android orientation
-
-static float TA_ori[3]; // TIZEN rotation vector to Android orientation
-static float AT_ori[3]; // Android rotation vector to TIZEN orientation
-
-static void sensor_callback(sensor_h sensor, sensor_event_s events[], int events_count, void *user_data)
-{
- sensor_type_e type;
- sensor_get_type(sensor, &type);
-
- if (type == SENSOR_ROTATION_VECTOR) {
- TIZEN_rv[0] = events[0].values[0];
- TIZEN_rv[1] = events[0].values[1];
- TIZEN_rv[2] = events[0].values[2];
- TIZEN_rv[3] = events[0].values[3];
-
- Android_Fusion2.x0 = android::vec3_t(TIZEN_rv);
- android::mat33_t m(Android_Fusion2.getRotationMatrix());
-
- android::vec3_t g;
- g[0] = atan2f(-m[1][0], m[0][0]) * RAD2DEG;
- g[1] = atan2f(-m[2][1], m[2][2]) * RAD2DEG;
- g[2] = asinf ( m[2][0]) * RAD2DEG;
- if (g[0] < 0)
- g[0] += 360;
-
- TA_ori[0] = g.x;
- TA_ori[1] = g.y;
- TA_ori[2] = g.z;
- }
-
- /* 6-axis rotation vector
- else if (type == SENSOR_GYROSCOPE_ROTATION_VECTOR) {
- T6_rv_f = true;
- T6_rv[0] = events[0].values[0];
- T6_rv[1] = events[0].values[1];
- T6_rv[2] = events[0].values[2];
- T6_rv[3] = events[0].values[3];
- }
- */
-
- else if (type == SENSOR_ORIENTATION) {
- TIZEN_ori[0] = events[0].values[0];
- TIZEN_ori[1] = events[0].values[1];
- TIZEN_ori[2] = events[0].values[2];
- }
-
- else {
- if (type == SENSOR_ACCELEROMETER) {
- android::vec3_t v(events[0].values);
- float dT = (events[0].timestamp - timestamp[0]) / TIMEUNIT;
- timestamp[0] = events[0].timestamp;
- Android_Fusion.handleAcc(v, dT);
- }
-
- else if (type == SENSOR_GYROSCOPE) {
- events[0].values[0] /= RAD2DEG;
- events[0].values[1] /= RAD2DEG;
- events[0].values[2] /= RAD2DEG;
-
- android::vec3_t v(events[0].values);
- float dT = (events[0].timestamp - timestamp[1]) / TIMEUNIT;
- timestamp[1] = events[0].timestamp;
-
- Android_Fusion.handleGyro(v, dT);
- }
-
- else if (type == SENSOR_MAGNETIC) {
- android::vec3_t v(events[0].values);
- Android_Fusion.handleMag(v);
- }
-
- android::quat_t v(Android_Fusion.getAttitude());
-
- Android_rv[0] = v[0];
- Android_rv[1] = v[1];
- Android_rv[2] = v[2];
- Android_rv[3] = v[3];
-
- quat_to_orientation(Android_rv, AT_ori[0], AT_ori[1], AT_ori[2]);
-
- android::mat33_t m(Android_Fusion.getRotationMatrix());
- android::vec3_t g;
- g[0] = atan2f(-m[1][0], m[0][0]) * RAD2DEG;
- g[1] = atan2f(-m[2][1], m[2][2]) * RAD2DEG;
- g[2] = asinf ( m[2][0]) * RAD2DEG;
- if (g[0] < 0)
- g[0] += 360;
-
- Android_ori[0] = g.x;
- Android_ori[1] = g.y;
- Android_ori[2] = g.z;
-
- /* 6-axis rotation vector
- A6_rv_f = true;
- android::quat_t v6(F6.getAttitude());
-
- A6_rv[0] = v6[0];
- A6_rv[1] = v6[1];
- A6_rv[2] = v6[2];
- A6_rv[3] = v6[3];
- */
- }
-
- printf("\n\n");
- printf("(Tizen / Android) Rotation vector : \n");
- printf("T : %f %f %f %f\n", TIZEN_rv[0], TIZEN_rv[1], TIZEN_rv[2], TIZEN_rv[3]);
- printf("A : %f %f %f %f\n\n", Android_rv[0], Android_rv[1], Android_rv[2], Android_rv[3]);
-
- printf("(Tizen / Android) Rotation vector -> (Tizen / Android) Orientation : \n");
- printf("T -> T : %f %f %f\n", TIZEN_ori[0], TIZEN_ori[1], TIZEN_ori[2]);
- printf("A -> A : %f %f %f\n", Android_ori[0], Android_ori[1], Android_ori[2]);
- printf("T -> A : %f %f %f\n", TA_ori[0], TA_ori[1], TA_ori[2]);
- printf("A -> T : %f %f %f\n", AT_ori[0], AT_ori[1], AT_ori[2]);
-}
-
-int main()
-{
- ev.push_back(SENSOR_ACCELEROMETER);
- sv.push_back("SENSOR_ACCELEROMETER");
-
- ev.push_back(SENSOR_GYROSCOPE);
- sv.push_back("SENSOR_GYROSCOPE");
-
- ev.push_back(SENSOR_MAGNETIC);
- sv.push_back("SENSOR_MAGNETIC");
-
- ev.push_back(SENSOR_ORIENTATION);
- sv.push_back("SENSOR_ORIENTATION");
-
- ev.push_back(SENSOR_ROTATION_VECTOR);
- sv.push_back("SENSOR_ROTATION_VECTOR");
-
- /* 6-axis rotation vector
- ev.push_back(SENSOR_GYROSCOPE_ROTATION_VECTOR);
- sv.push_back("SENSOR_GYROSCOPE_ROTATION_VECTOR");
- */
-
- sensor_h *sh;
- sensor_listener_h lh[10];
-
- int count;
- int num = ev.size();
- bool sup;
-
- for (int i = 0; i < num; ++i) {
- sensor_is_supported(ev[i], &sup);
-
- if (sup) {
- sensor_get_sensor_list(ev[i], &sh, &count);
- sensor_create_listener(sh[0], &lh[i]);
-
- sensor_listener_set_events_cb(lh[i], sensor_callback, NULL);
- sensor_listener_set_option(lh[i], SENSOR_OPTION_ALWAYS_ON);
- sensor_listener_set_interval(lh[i], 100);
- sensor_listener_start(lh[i]);
- }
-
- else {
- printf("%s sensor is not supported in this device.\n", sv[i].c_str());
- }
- }
-
- GMainLoop *loop;
- loop = g_main_loop_new(NULL, FALSE);
- g_main_loop_run(loop);
-
- for (int i = 0; i < num; ++i) {
- sensor_is_supported(ev[i], &sup);
- if (sup)
- sensor_destroy_listener(lh[i]);
- }
-
- return 0;
-}
\ No newline at end of file
+++ /dev/null
-/*
- * 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.
- */
-
-// released in android-11.0.0_r9
-
-#ifndef ANDROID_MAT_H
-#define ANDROID_MAT_H
-
-#include "vec.h"
-#include "traits.h"
-
-// -----------------------------------------------------------------------
-
-namespace android {
-
-template <typename TYPE, size_t C, size_t R>
-class mat;
-
-namespace helpers {
-
-template <typename TYPE, size_t C, size_t R>
-mat<TYPE, C, R>& doAssign(
- mat<TYPE, C, R>& lhs,
- typename TypeTraits<TYPE>::ParameterType rhs) {
- for (size_t i=0 ; i<C ; i++)
- for (size_t j=0 ; j<R ; j++)
- lhs[i][j] = (i==j) ? rhs : 0;
- return lhs;
-}
-
-template <typename TYPE, size_t C, size_t R, size_t D>
-mat<TYPE, C, R> PURE doMul(
- const mat<TYPE, D, R>& lhs,
- const mat<TYPE, C, D>& rhs)
-{
- mat<TYPE, C, R> res;
- for (size_t c=0 ; c<C ; c++) {
- for (size_t r=0 ; r<R ; r++) {
- TYPE v(0);
- for (size_t k=0 ; k<D ; k++) {
- v += lhs[k][r] * rhs[c][k];
- }
- res[c][r] = v;
- }
- }
- return res;
-}
-
-template <typename TYPE, size_t R, size_t D>
-vec<TYPE, R> PURE doMul(
- const mat<TYPE, D, R>& lhs,
- const vec<TYPE, D>& rhs)
-{
- vec<TYPE, R> res;
- for (size_t r=0 ; r<R ; r++) {
- TYPE v(0);
- for (size_t k=0 ; k<D ; k++) {
- v += lhs[k][r] * rhs[k];
- }
- res[r] = v;
- }
- return res;
-}
-
-template <typename TYPE, size_t C, size_t R>
-mat<TYPE, C, R> PURE doMul(
- const vec<TYPE, R>& lhs,
- const mat<TYPE, C, 1>& rhs)
-{
- mat<TYPE, C, R> res;
- for (size_t c=0 ; c<C ; c++) {
- for (size_t r=0 ; r<R ; r++) {
- res[c][r] = lhs[r] * rhs[c][0];
- }
- }
- return res;
-}
-
-template <typename TYPE, size_t C, size_t R>
-mat<TYPE, C, R> PURE doMul(
- const mat<TYPE, C, R>& rhs,
- typename TypeTraits<TYPE>::ParameterType v)
-{
- mat<TYPE, C, R> res;
- for (size_t c=0 ; c<C ; c++) {
- for (size_t r=0 ; r<R ; r++) {
- res[c][r] = rhs[c][r] * v;
- }
- }
- return res;
-}
-
-template <typename TYPE, size_t C, size_t R>
-mat<TYPE, C, R> PURE doMul(
- typename TypeTraits<TYPE>::ParameterType v,
- const mat<TYPE, C, R>& rhs)
-{
- mat<TYPE, C, R> res;
- for (size_t c=0 ; c<C ; c++) {
- for (size_t r=0 ; r<R ; r++) {
- res[c][r] = v * rhs[c][r];
- }
- }
- return res;
-}
-
-
-}; // namespace helpers
-
-// -----------------------------------------------------------------------
-
-template <typename TYPE, size_t C, size_t R>
-class mat : public vec< vec<TYPE, R>, C > {
- typedef typename TypeTraits<TYPE>::ParameterType pTYPE;
- typedef vec< vec<TYPE, R>, C > base;
-public:
- // STL-like interface.
- typedef TYPE value_type;
- typedef TYPE& reference;
- typedef TYPE const& const_reference;
- typedef size_t size_type;
- size_type size() const { return R*C; }
- enum { ROWS = R, COLS = C };
-
-
- // -----------------------------------------------------------------------
- // default constructors
-
- mat() { }
- mat(const mat& rhs) : base(rhs) { }
- mat(const base& rhs) : base(rhs) { } // NOLINT(google-explicit-constructor)
-
- // -----------------------------------------------------------------------
- // conversion constructors
-
- // sets the diagonal to the value, off-diagonal to zero
- mat(pTYPE rhs) { // NOLINT(google-explicit-constructor)
- helpers::doAssign(*this, rhs);
- }
-
- // -----------------------------------------------------------------------
- // Assignment
-
- mat& operator=(const mat& rhs) {
- base::operator=(rhs);
- return *this;
- }
-
- mat& operator=(const base& rhs) {
- base::operator=(rhs);
- return *this;
- }
-
- mat& operator=(pTYPE rhs) {
- return helpers::doAssign(*this, rhs);
- }
-
- // -----------------------------------------------------------------------
- // non-member function declaration and definition
-
- friend inline mat PURE operator + (const mat& lhs, const mat& rhs) {
- return helpers::doAdd(
- static_cast<const base&>(lhs),
- static_cast<const base&>(rhs));
- }
- friend inline mat PURE operator - (const mat& lhs, const mat& rhs) {
- return helpers::doSub(
- static_cast<const base&>(lhs),
- static_cast<const base&>(rhs));
- }
-
- // matrix*matrix
- template <size_t D>
- friend mat PURE operator * (
- const mat<TYPE, D, R>& lhs,
- const mat<TYPE, C, D>& rhs) {
- return helpers::doMul(lhs, rhs);
- }
-
- // matrix*vector
- friend vec<TYPE, R> PURE operator * (
- const mat& lhs, const vec<TYPE, C>& rhs) {
- return helpers::doMul(lhs, rhs);
- }
-
- // vector*matrix
- friend mat PURE operator * (
- const vec<TYPE, R>& lhs, const mat<TYPE, C, 1>& rhs) {
- return helpers::doMul(lhs, rhs);
- }
-
- // matrix*scalar
- friend inline mat PURE operator * (const mat& lhs, pTYPE v) {
- return helpers::doMul(lhs, v);
- }
-
- // scalar*matrix
- friend inline mat PURE operator * (pTYPE v, const mat& rhs) {
- return helpers::doMul(v, rhs);
- }
-
- // -----------------------------------------------------------------------
- // streaming operator to set the columns of the matrix:
- // example:
- // mat33_t m;
- // m << v0 << v1 << v2;
-
- // column_builder<> stores the matrix and knows which column to set
- template<size_t PREV_COLUMN>
- struct column_builder {
- mat& matrix;
- explicit column_builder(mat& matrix) : matrix(matrix) { }
- };
-
- // operator << is not a method of column_builder<> so we can
- // overload it for unauthorized values (partial specialization
- // not allowed in class-scope).
- // we just set the column and return the next column_builder<>
- template<size_t PREV_COLUMN>
- friend column_builder<PREV_COLUMN+1> operator << (
- const column_builder<PREV_COLUMN>& lhs,
- const vec<TYPE, R>& rhs) {
- lhs.matrix[PREV_COLUMN+1] = rhs;
- return column_builder<PREV_COLUMN+1>(lhs.matrix);
- }
-
- // we return void here so we get a compile-time error if the
- // user tries to set too many columns
- friend void operator << (
- const column_builder<C-2>& lhs,
- const vec<TYPE, R>& rhs) {
- lhs.matrix[C-1] = rhs;
- }
-
- // this is where the process starts. we set the first columns and
- // return the next column_builder<>
- column_builder<0> operator << (const vec<TYPE, R>& rhs) {
- (*this)[0] = rhs;
- return column_builder<0>(*this);
- }
-};
-
-// Specialize column matrix so they're exactly equivalent to a vector
-template <typename TYPE, size_t R>
-class mat<TYPE, 1, R> : public vec<TYPE, R> {
- typedef vec<TYPE, R> base;
-public:
- // STL-like interface.
- typedef TYPE value_type;
- typedef TYPE& reference;
- typedef TYPE const& const_reference;
- typedef size_t size_type;
- size_type size() const { return R; }
- enum { ROWS = R, COLS = 1 };
-
- mat() { }
- explicit mat(const base& rhs) : base(rhs) { }
- mat(const mat& rhs) : base(rhs) { }
- explicit mat(const TYPE& rhs) { helpers::doAssign(*this, rhs); }
- mat& operator=(const mat& rhs) { base::operator=(rhs); return *this; }
- mat& operator=(const base& rhs) { base::operator=(rhs); return *this; }
- mat& operator=(const TYPE& rhs) { return helpers::doAssign(*this, rhs); }
- // we only have one column, so ignore the index
- const base& operator[](size_t) const { return *this; }
- base& operator[](size_t) { return *this; }
- void operator << (const vec<TYPE, R>& rhs) { base::operator[](0) = rhs; }
-};
-
-// -----------------------------------------------------------------------
-// matrix functions
-
-// transpose. this handles matrices of matrices
-inline int PURE transpose(int v) { return v; }
-inline float PURE transpose(float v) { return v; }
-inline double PURE transpose(double v) { return v; }
-
-// Transpose a matrix
-template <typename TYPE, size_t C, size_t R>
-mat<TYPE, R, C> PURE transpose(const mat<TYPE, C, R>& m) {
- mat<TYPE, R, C> r;
- for (size_t i=0 ; i<R ; i++)
- for (size_t j=0 ; j<C ; j++)
- r[i][j] = transpose(m[j][i]);
- return r;
-}
-
-// Calculate the trace of a matrix
-template <typename TYPE, size_t C> static TYPE trace(const mat<TYPE, C, C>& m) {
- TYPE t;
- for (size_t i=0 ; i<C ; i++)
- t += m[i][i];
- return t;
-}
-
-// Test positive-semidefiniteness of a matrix
-template <typename TYPE, size_t C>
-static bool isPositiveSemidefinite(const mat<TYPE, C, C>& m, TYPE tolerance) {
- for (size_t i=0 ; i<C ; i++)
- if (m[i][i] < 0)
- return false;
-
- for (size_t i=0 ; i<C ; i++)
- for (size_t j=i+1 ; j<C ; j++)
- if (fabs(m[i][j] - m[j][i]) > tolerance)
- return false;
-
- return true;
-}
-
-// Transpose a vector
-template <
- template<typename T, size_t S> class VEC,
- typename TYPE,
- size_t SIZE
->
-mat<TYPE, SIZE, 1> PURE transpose(const VEC<TYPE, SIZE>& v) {
- mat<TYPE, SIZE, 1> r;
- for (size_t i=0 ; i<SIZE ; i++)
- r[i][0] = transpose(v[i]);
- return r;
-}
-
-// -----------------------------------------------------------------------
-// "dumb" matrix inversion
-template<typename T, size_t N>
-mat<T, N, N> PURE invert(const mat<T, N, N>& src) {
- T t;
- size_t swap;
- mat<T, N, N> tmp(src);
- mat<T, N, N> inverse(1);
-
- for (size_t i=0 ; i<N ; i++) {
- // look for largest element in column
- swap = i;
- for (size_t j=i+1 ; j<N ; j++) {
- if (fabs(tmp[j][i]) > fabs(tmp[i][i])) {
- swap = j;
- }
- }
-
- if (swap != i) {
- /* swap rows. */
- for (size_t k=0 ; k<N ; k++) {
- t = tmp[i][k];
- tmp[i][k] = tmp[swap][k];
- tmp[swap][k] = t;
-
- t = inverse[i][k];
- inverse[i][k] = inverse[swap][k];
- inverse[swap][k] = t;
- }
- }
-
- t = 1 / tmp[i][i];
- for (size_t k=0 ; k<N ; k++) {
- tmp[i][k] *= t;
- inverse[i][k] *= t;
- }
- for (size_t j=0 ; j<N ; j++) {
- if (j != i) {
- t = tmp[j][i];
- for (size_t k=0 ; k<N ; k++) {
- tmp[j][k] -= tmp[i][k] * t;
- inverse[j][k] -= inverse[i][k] * t;
- }
- }
- }
- }
- return inverse;
-}
-
-// -----------------------------------------------------------------------
-
-typedef mat<float, 2, 2> mat22_t;
-typedef mat<float, 3, 3> mat33_t;
-typedef mat<float, 4, 4> mat44_t;
-
-// -----------------------------------------------------------------------
-
-}; // namespace android
-
-#endif /* ANDROID_MAT_H */
+++ /dev/null
-/*
- * 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.
- */
-
-// released in android-11.0.0_r9
-
-#ifndef ANDROID_QUAT_H
-#define ANDROID_QUAT_H
-
-#include <math.h>
-
-#include "vec.h"
-#include "mat.h"
-
-// -----------------------------------------------------------------------
-namespace android {
-// -----------------------------------------------------------------------
-
-template <typename TYPE>
-mat<TYPE, 3, 3> quatToMatrix(const vec<TYPE, 4>& q) {
- mat<TYPE, 3, 3> R;
- TYPE q0(q.w);
- TYPE q1(q.x);
- TYPE q2(q.y);
- TYPE q3(q.z);
- TYPE sq_q1 = 2 * q1 * q1;
- TYPE sq_q2 = 2 * q2 * q2;
- TYPE sq_q3 = 2 * q3 * q3;
- TYPE q1_q2 = 2 * q1 * q2;
- TYPE q3_q0 = 2 * q3 * q0;
- TYPE q1_q3 = 2 * q1 * q3;
- TYPE q2_q0 = 2 * q2 * q0;
- TYPE q2_q3 = 2 * q2 * q3;
- TYPE q1_q0 = 2 * q1 * q0;
- R[0][0] = 1 - sq_q2 - sq_q3;
- R[0][1] = q1_q2 - q3_q0;
- R[0][2] = q1_q3 + q2_q0;
- R[1][0] = q1_q2 + q3_q0;
- R[1][1] = 1 - sq_q1 - sq_q3;
- R[1][2] = q2_q3 - q1_q0;
- R[2][0] = q1_q3 - q2_q0;
- R[2][1] = q2_q3 + q1_q0;
- R[2][2] = 1 - sq_q1 - sq_q2;
- return R;
-}
-
-template <typename TYPE>
-vec<TYPE, 4> matrixToQuat(const mat<TYPE, 3, 3>& R) {
- // matrix to quaternion
-
- struct {
- inline TYPE operator()(TYPE v) {
- return v < 0 ? 0 : v;
- }
- } clamp;
-
- vec<TYPE, 4> q;
- const float Hx = R[0].x;
- const float My = R[1].y;
- const float Az = R[2].z;
- q.x = sqrtf( clamp( Hx - My - Az + 1) * 0.25f );
- q.y = sqrtf( clamp(-Hx + My - Az + 1) * 0.25f );
- q.z = sqrtf( clamp(-Hx - My + Az + 1) * 0.25f );
- q.w = sqrtf( clamp( Hx + My + Az + 1) * 0.25f );
- q.x = copysignf(q.x, R[2].y - R[1].z);
- q.y = copysignf(q.y, R[0].z - R[2].x);
- q.z = copysignf(q.z, R[1].x - R[0].y);
- // guaranteed to be unit-quaternion
- return q;
-}
-
-template <typename TYPE>
-vec<TYPE, 4> normalize_quat(const vec<TYPE, 4>& q) {
- vec<TYPE, 4> r(q);
- if (r.w < 0) {
- r = -r;
- }
- return normalize(r);
-}
-
-// -----------------------------------------------------------------------
-
-typedef vec4_t quat_t;
-
-// -----------------------------------------------------------------------
-}; // namespace android
-
-#endif /* ANDROID_QUAT_H */
+++ /dev/null
-#include <math.h>
-
-#define QUAT (M_PI/4)
-#define HALF (M_PI/2)
-#define ARCTAN(x, y) ((x) == 0 ? 0 : (y) != 0 ? atan2((x), (y)) : (x) > 0 ? M_PI/2.0 : -M_PI/2.0)
-
-const float RAD2DEG = 57.29577951;
-
-int quat_to_matrix(const float *quat, float *R)
-{
- if (quat == NULL || R == NULL)
- return -EINVAL;
-
- float q0 = quat[3];
- float q1 = quat[0];
- float q2 = quat[1];
- float q3 = quat[2];
-
- float sq_q1 = 2 * q1 * q1;
- float sq_q2 = 2 * q2 * q2;
- float sq_q3 = 2 * q3 * q3;
- float q1_q2 = 2 * q1 * q2;
- float q3_q0 = 2 * q3 * q0;
- float q1_q3 = 2 * q1 * q3;
- float q2_q0 = 2 * q2 * q0;
- float q2_q3 = 2 * q2 * q3;
- float q1_q0 = 2 * q1 * q0;
-
- R[0] = 1 - sq_q2 - sq_q3;
- R[1] = q1_q2 - q3_q0;
- R[2] = q1_q3 + q2_q0;
- R[3] = q1_q2 + q3_q0;
- R[4] = 1 - sq_q1 - sq_q3;
- R[5] = q2_q3 - q1_q0;
- R[6] = q1_q3 - q2_q0;
- R[7] = q2_q3 + q1_q0;
- R[8] = 1 - sq_q1 - sq_q2;
-
- return 0;
-}
-
-int quat_to_orientation(const float *quat, float &azimuth, float &pitch, float &roll)
-{
- int error;
- float g[3];
- float R[9];
-
- error = quat_to_matrix(quat, R);
-
- if (error < 0)
- return error;
-
- float xyz_z = ARCTAN(R[3], R[0]);
- float yxz_x = asinf(R[7]);
- float yxz_y = ARCTAN(-R[6], R[8]);
- float yxz_z = ARCTAN(-R[1], R[4]);
-
- float a = fabs(yxz_x / HALF);
- a = a * a;
-
- float p = (fabs(yxz_y) / HALF - 1.0);
-
- if (p < 0)
- p = 0;
-
- float v = 1 + (1 - a) / a * p;
-
- if (v > 20)
- v = 20;
-
- if (yxz_x * yxz_y > 0) {
- if (yxz_z > 0 && xyz_z < 0)
- xyz_z += M_PI * 2;
- } else {
- if (yxz_z < 0 && xyz_z > 0)
- xyz_z -= M_PI * 2;
- }
-
- g[0] = (1 - a * v) * yxz_z + (a * v) * xyz_z;
- g[0] *= -1;
-
- float tmp = R[7];
-
- if (tmp > 1.0f)
- tmp = 1.0f;
- else if (tmp < -1.0f)
- tmp = -1.0f;
-
- g[1] = -asinf(tmp);
- if (R[8] < 0)
- g[1] = M_PI - g[1];
-
- if (g[1] > M_PI)
- g[1] -= M_PI * 2;
-
- if ((fabs(R[7]) > QUAT))
- g[2] = (float) atan2f(R[6], R[7]);
- else
- g[2] = (float) atan2f(R[6], R[8]);
-
- if (g[2] > HALF)
- g[2] = M_PI - g[2];
- else if (g[2] < -HALF)
- g[2] = -M_PI - g[2];
-
- g[0] *= RAD2DEG;
- g[1] *= RAD2DEG;
- g[2] *= RAD2DEG;
-
- if (g[0] < 0)
- g[0] += 360;
-
- azimuth = g[0];
- pitch = g[1];
- roll = g[2];
-
- return 0;
-}
\ No newline at end of file
+++ /dev/null
-/*
- * 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.
- */
-
-// released in android-11.0.0_r9
-
-#ifndef ANDROID_TRAITS_H
-#define ANDROID_TRAITS_H
-
-// -----------------------------------------------------------------------
-// Typelists
-
-namespace android {
-
-// end-of-list marker
-class NullType {};
-
-// type-list node
-template <typename T, typename U>
-struct TypeList {
- typedef T Head;
- typedef U Tail;
-};
-
-// helpers to build typelists
-#define TYPELIST_1(T1) TypeList<T1, NullType>
-#define TYPELIST_2(T1, T2) TypeList<T1, TYPELIST_1(T2)>
-#define TYPELIST_3(T1, T2, T3) TypeList<T1, TYPELIST_2(T2, T3)>
-#define TYPELIST_4(T1, T2, T3, T4) TypeList<T1, TYPELIST_3(T2, T3, T4)>
-
-// typelists algorithms
-namespace TL {
-template <typename TList, typename T> struct IndexOf;
-
-template <typename T>
-struct IndexOf<NullType, T> {
- enum { value = -1 };
-};
-
-template <typename T, typename Tail>
-struct IndexOf<TypeList<T, Tail>, T> {
- enum { value = 0 };
-};
-
-template <typename Head, typename Tail, typename T>
-struct IndexOf<TypeList<Head, Tail>, T> {
-private:
- enum { temp = IndexOf<Tail, T>::value };
-public:
- enum { value = temp == -1 ? -1 : 1 + temp };
-};
-
-}; // namespace TL
-
-// type selection based on a boolean
-template <bool flag, typename T, typename U>
-struct Select {
- typedef T Result;
-};
-template <typename T, typename U>
-struct Select<false, T, U> {
- typedef U Result;
-};
-
-// -----------------------------------------------------------------------
-// Type traits
-
-template <typename T>
-class TypeTraits {
- typedef TYPELIST_4(
- unsigned char, unsigned short,
- unsigned int, unsigned long int) UnsignedInts;
-
- typedef TYPELIST_4(
- signed char, signed short,
- signed int, signed long int) SignedInts;
-
- typedef TYPELIST_1(
- bool) OtherInts;
-
- typedef TYPELIST_3(
- float, double, long double) Floats;
-
- template<typename U> struct PointerTraits {
- enum { result = false };
- typedef NullType PointeeType;
- };
- template<typename U> struct PointerTraits<U*> {
- enum { result = true };
- typedef U PointeeType;
- };
-
-public:
- enum { isStdUnsignedInt = TL::IndexOf<UnsignedInts, T>::value >= 0 };
- enum { isStdSignedInt = TL::IndexOf<SignedInts, T>::value >= 0 };
- enum { isStdIntegral = TL::IndexOf<OtherInts, T>::value >= 0 || isStdUnsignedInt || isStdSignedInt };
- enum { isStdFloat = TL::IndexOf<Floats, T>::value >= 0 };
- enum { isPointer = PointerTraits<T>::result };
- enum { isStdArith = isStdIntegral || isStdFloat };
-
- // best parameter type for given type
- typedef typename Select<isStdArith || isPointer, T, const T&>::Result ParameterType;
-};
-
-// -----------------------------------------------------------------------
-}; // namespace android
-
-#endif /* ANDROID_TRAITS_H */
+++ /dev/null
-/*
- * 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.
- */
-
-// released in android-11.0.0_r9
-
-#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>
-struct 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 V,
- typename TYPE,
- size_t SIZE
->
-TYPE PURE length_squared(const V<TYPE, SIZE>& v) {
- return dot_product(v, v);
-}
-
-template <
- template<typename T, size_t S> class V,
- typename TYPE,
- size_t SIZE
->
-V<TYPE, SIZE> PURE normalize(const V<TYPE, SIZE>& v) {
- return v * (1/length(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) { } // NOLINT(google-explicit-constructor)
-
- // -----------------------------------------------------------------------
- // conversion constructors
-
- vec(pTYPE rhs) { // NOLINT(google-explicit-constructor)
- 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 */
projects / platform / core / api / sensor.git / commitdiff