diff --git a/Adafruit_BNO055.cpp b/Adafruit_BNO055.cpp index 8c0f8be..9c6cb65 100644 --- a/Adafruit_BNO055.cpp +++ b/Adafruit_BNO055.cpp @@ -378,8 +378,8 @@ int8_t Adafruit_BNO055::getTemp() { * VECTOR_GRAVITY] * @return vector from specified source */ -imu::Vector<3> Adafruit_BNO055::getVector(adafruit_vector_type_t vector_type) { - imu::Vector<3> xyz; +imu::Vector Adafruit_BNO055::getVector(adafruit_vector_type_t vector_type) { + imu::Vector xyz; uint8_t buffer[6]; memset(buffer, 0, 6); @@ -400,39 +400,39 @@ imu::Vector<3> Adafruit_BNO055::getVector(adafruit_vector_type_t vector_type) { switch (vector_type) { case VECTOR_MAGNETOMETER: /* 1uT = 16 LSB */ - xyz[0] = ((double)x) / 16.0; - xyz[1] = ((double)y) / 16.0; - xyz[2] = ((double)z) / 16.0; + xyz.x = ((double)x) / 16.0; + xyz.y = ((double)y) / 16.0; + xyz.z = ((double)z) / 16.0; break; case VECTOR_GYROSCOPE: /* 1dps = 16 LSB */ - xyz[0] = ((double)x) / 16.0; - xyz[1] = ((double)y) / 16.0; - xyz[2] = ((double)z) / 16.0; + xyz.x = ((double)x) / 16.0; + xyz.y = ((double)y) / 16.0; + xyz.z = ((double)z) / 16.0; break; case VECTOR_EULER: /* 1 degree = 16 LSB */ - xyz[0] = ((double)x) / 16.0; - xyz[1] = ((double)y) / 16.0; - xyz[2] = ((double)z) / 16.0; + xyz.x = ((double)x) / 16.0; + xyz.y = ((double)y) / 16.0; + xyz.z = ((double)z) / 16.0; break; case VECTOR_ACCELEROMETER: /* 1m/s^2 = 100 LSB */ - xyz[0] = ((double)x) / 100.0; - xyz[1] = ((double)y) / 100.0; - xyz[2] = ((double)z) / 100.0; + xyz.x = ((double)x) / 100.0; + xyz.y = ((double)y) / 100.0; + xyz.z = ((double)z) / 100.0; break; case VECTOR_LINEARACCEL: /* 1m/s^2 = 100 LSB */ - xyz[0] = ((double)x) / 100.0; - xyz[1] = ((double)y) / 100.0; - xyz[2] = ((double)z) / 100.0; + xyz.x = ((double)x) / 100.0; + xyz.y = ((double)y) / 100.0; + xyz.z = ((double)z) / 100.0; break; case VECTOR_GRAVITY: /* 1m/s^2 = 100 LSB */ - xyz[0] = ((double)x) / 100.0; - xyz[1] = ((double)y) / 100.0; - xyz[2] = ((double)z) / 100.0; + xyz.x = ((double)x) / 100.0; + xyz.y = ((double)y) / 100.0; + xyz.z = ((double)z) / 100.0; break; } @@ -505,10 +505,10 @@ bool Adafruit_BNO055::getEvent(sensors_event_t *event) { event->timestamp = millis(); /* Get a Euler angle sample for orientation */ - imu::Vector<3> euler = getVector(Adafruit_BNO055::VECTOR_EULER); - event->orientation.x = euler.x(); - event->orientation.y = euler.y(); - event->orientation.z = euler.z(); + imu::Vector euler = getVector(Adafruit_BNO055::VECTOR_EULER); + event->orientation.x = euler.x; + event->orientation.y = euler.y; + event->orientation.z = euler.z; return true; } @@ -531,49 +531,49 @@ bool Adafruit_BNO055::getEvent(sensors_event_t *event, event->timestamp = millis(); // read the data according to vec_type - imu::Vector<3> vec; + imu::Vector vec; if (vec_type == Adafruit_BNO055::VECTOR_LINEARACCEL) { event->type = SENSOR_TYPE_LINEAR_ACCELERATION; vec = getVector(Adafruit_BNO055::VECTOR_LINEARACCEL); - event->acceleration.x = vec.x(); - event->acceleration.y = vec.y(); - event->acceleration.z = vec.z(); + event->acceleration.x = vec.x; + event->acceleration.y = vec.y; + event->acceleration.z = vec.z; } else if (vec_type == Adafruit_BNO055::VECTOR_ACCELEROMETER) { event->type = SENSOR_TYPE_ACCELEROMETER; vec = getVector(Adafruit_BNO055::VECTOR_ACCELEROMETER); - event->acceleration.x = vec.x(); - event->acceleration.y = vec.y(); - event->acceleration.z = vec.z(); + event->acceleration.x = vec.x; + event->acceleration.y = vec.y; + event->acceleration.z = vec.z; } else if (vec_type == Adafruit_BNO055::VECTOR_GRAVITY) { event->type = SENSOR_TYPE_ACCELEROMETER; vec = getVector(Adafruit_BNO055::VECTOR_GRAVITY); - event->acceleration.x = vec.x(); - event->acceleration.y = vec.y(); - event->acceleration.z = vec.z(); + event->acceleration.x = vec.x; + event->acceleration.y = vec.y; + event->acceleration.z = vec.z; } else if (vec_type == Adafruit_BNO055::VECTOR_EULER) { event->type = SENSOR_TYPE_ORIENTATION; vec = getVector(Adafruit_BNO055::VECTOR_EULER); - event->orientation.x = vec.x(); - event->orientation.y = vec.y(); - event->orientation.z = vec.z(); + event->orientation.x = vec.x; + event->orientation.y = vec.y; + event->orientation.z = vec.z; } else if (vec_type == Adafruit_BNO055::VECTOR_GYROSCOPE) { event->type = SENSOR_TYPE_ROTATION_VECTOR; vec = getVector(Adafruit_BNO055::VECTOR_GYROSCOPE); - event->gyro.x = vec.x(); - event->gyro.y = vec.y(); - event->gyro.z = vec.z(); + event->gyro.x = vec.x; + event->gyro.y = vec.y; + event->gyro.z = vec.z; } else if (vec_type == Adafruit_BNO055::VECTOR_MAGNETOMETER) { event->type = SENSOR_TYPE_MAGNETIC_FIELD; vec = getVector(Adafruit_BNO055::VECTOR_MAGNETOMETER); - event->magnetic.x = vec.x(); - event->magnetic.y = vec.y(); - event->magnetic.z = vec.z(); + event->magnetic.x = vec.x; + event->magnetic.y = vec.y; + event->magnetic.z = vec.z; } return true; diff --git a/Adafruit_BNO055.h b/Adafruit_BNO055.h index 10ce113..8f58d71 100644 --- a/Adafruit_BNO055.h +++ b/Adafruit_BNO055.h @@ -25,7 +25,7 @@ #include "Arduino.h" #include -#include "utility/imumaths.h" +#include "imu.h" #include /** BNO055 Address A **/ @@ -294,7 +294,7 @@ class Adafruit_BNO055 : public Adafruit_Sensor { void getCalibration(uint8_t *system, uint8_t *gyro, uint8_t *accel, uint8_t *mag); - imu::Vector<3> getVector(adafruit_vector_type_t vector_type); + imu::Vector getVector(adafruit_vector_type_t vector_type); imu::Quaternion getQuat(); int8_t getTemp(); diff --git a/examples/bunny/bunny.ino b/examples/bunny/bunny.ino index 4d46a53..4ca1bf2 100644 --- a/examples/bunny/bunny.ino +++ b/examples/bunny/bunny.ino @@ -1,7 +1,6 @@ #include #include #include -#include /* This driver uses the Adafruit unified sensor library (Adafruit_Sensor), which provides a common 'type' for sensor data and some helper functions. @@ -73,12 +72,12 @@ void setup(void) Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!"); while(1); } - + delay(1000); /* Use external crystal for better accuracy */ bno.setExtCrystalUse(true); - + /* Display some basic information on this sensor */ displaySensorDetails(); } diff --git a/examples/rawdata/rawdata.ino b/examples/rawdata/rawdata.ino index bdccafb..565afc8 100644 --- a/examples/rawdata/rawdata.ino +++ b/examples/rawdata/rawdata.ino @@ -1,7 +1,6 @@ #include #include #include -#include /* This driver reads raw data from the BNO055 @@ -15,6 +14,7 @@ History ======= 2015/MAR/03 - First release (KTOWN) + 2021/Jan/18 - Modified Vector code (Walchko) */ /* Set the delay between fresh samples */ @@ -71,28 +71,28 @@ void loop(void) // - VECTOR_EULER - degrees // - VECTOR_LINEARACCEL - m/s^2 // - VECTOR_GRAVITY - m/s^2 - imu::Vector<3> euler = bno.getVector(Adafruit_BNO055::VECTOR_EULER); + imu::Vector euler = bno.getVector(Adafruit_BNO055::VECTOR_EULER); /* Display the floating point data */ Serial.print("X: "); - Serial.print(euler.x()); + Serial.print(euler.x); Serial.print(" Y: "); - Serial.print(euler.y()); + Serial.print(euler.y); Serial.print(" Z: "); - Serial.print(euler.z()); + Serial.print(euler.z); Serial.print("\t\t"); /* // Quaternion data imu::Quaternion quat = bno.getQuat(); Serial.print("qW: "); - Serial.print(quat.w(), 4); + Serial.print(quat.w, 4); Serial.print(" qX: "); - Serial.print(quat.x(), 4); + Serial.print(quat.x, 4); Serial.print(" qY: "); - Serial.print(quat.y(), 4); + Serial.print(quat.y, 4); Serial.print(" qZ: "); - Serial.print(quat.z(), 4); + Serial.print(quat.z, 4); Serial.print("\t\t"); */ diff --git a/examples/read_all_data/read_all_data.ino b/examples/read_all_data/read_all_data.ino index bc65935..cd5ae77 100644 --- a/examples/read_all_data/read_all_data.ino +++ b/examples/read_all_data/read_all_data.ino @@ -1,7 +1,6 @@ #include #include #include -#include /* This driver uses the Adafruit unified sensor library (Adafruit_Sensor), which provides a common 'type' for sensor data and some helper functions. @@ -138,5 +137,3 @@ void printEvent(sensors_event_t* event) { Serial.print(" |\tz= "); Serial.println(z); } - - diff --git a/examples/restore_offsets/restore_offsets.ino b/examples/restore_offsets/restore_offsets.ino index eb978f4..ff44c64 100644 --- a/examples/restore_offsets/restore_offsets.ino +++ b/examples/restore_offsets/restore_offsets.ino @@ -1,7 +1,6 @@ #include #include #include -#include #include /* This driver uses the Adafruit unified sensor library (Adafruit_Sensor), diff --git a/examples/sensorapi/sensorapi.ino b/examples/sensorapi/sensorapi.ino index 1428b22..71a17b6 100644 --- a/examples/sensorapi/sensorapi.ino +++ b/examples/sensorapi/sensorapi.ino @@ -1,7 +1,6 @@ #include #include #include -#include /* This driver uses the Adafruit unified sensor library (Adafruit_Sensor), which provides a common 'type' for sensor data and some helper functions. diff --git a/examples/webserial_3d/webserial_3d.ino b/examples/webserial_3d/webserial_3d.ino index e870393..f718167 100644 --- a/examples/webserial_3d/webserial_3d.ino +++ b/examples/webserial_3d/webserial_3d.ino @@ -1,11 +1,10 @@ #include #include #include -#include /* Returns the IMU data as both a euler angles and quaternions as the WebSerial 3D Model viewer at https://adafruit-3dmodel-viewer.glitch.me/ expects. - + This driver uses the Adafruit unified sensor library (Adafruit_Sensor), which provides a common 'type' for sensor data and some helper functions. @@ -76,12 +75,12 @@ void setup(void) Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!"); while(1); } - + delay(1000); /* Use external crystal for better accuracy */ bno.setExtCrystalUse(true); - + /* Display some basic information on this sensor */ displaySensorDetails(); } @@ -120,15 +119,15 @@ void loop(void) /* The WebSerial 3D Model Viewer also expects data as roll, pitch, heading */ imu::Quaternion quat = bno.getQuat(); - + Serial.print(F("Quaternion: ")); - Serial.print((float)quat.w()); + Serial.print((float)quat.w); Serial.print(F(", ")); - Serial.print((float)quat.x()); + Serial.print((float)quat.x); Serial.print(F(", ")); - Serial.print((float)quat.y()); + Serial.print((float)quat.y); Serial.print(F(", ")); - Serial.print((float)quat.z()); + Serial.print((float)quat.z); Serial.println(F("")); /* Also send calibration data for each sensor. */ diff --git a/imu.h b/imu.h new file mode 100644 index 0000000..b1b5155 --- /dev/null +++ b/imu.h @@ -0,0 +1,103 @@ +/*! + * @file imu.h + * + * This is a library for the BNO055 orientation sensor + * + * Designed specifically to work with the Adafruit BNO055 Breakout. + * + * Pick one up today in the adafruit shop! + * ------> https://www.adafruit.com/product/2472 + * + * These sensors use I2C to communicate, 2 pins are required to interface. + * + * Adafruit invests time and resources providing this open source code, + * please support Adafruit andopen-source hardware by purchasing products + * from Adafruit! + * + * Kevin Walchko (for Adafruit Industries) + * + * MIT license, all text above must be included in any redistribution + */ + +#pragma once + +#include + +namespace imu { + +/*! + * @brief Class that stores orientation information in it + */ +class Quaternion { +public: + /*! + * @brief Instantiates a new Quaternion class to the identity + */ + Quaternion() : w(1.0), x(0.0), y(0.0), z(0.0) {} + + /*! + * @brief Instantiates a new Quaternion class + * @param ww + * real component + * @param xx + * imaginary x + * @param yy + * imaginary y + * @param zz + * imaginary z + */ + Quaternion(double ww, double xx, double yy, double zz) + : w(ww), x(xx), y(yy), z(zz) {} + + /*! + * @brief Calculates magnitude + * @return magnitude of quaternion + */ + double magnitude() const { return sqrt(w * w + x * x + y * y + z * z); } + /*! + * @brief Normalizes the quaternion + */ + void normalize() { + double mag = magnitude(); + if (mag < 1e-10) + return; + double inv = 1.0 / mag; + w *= inv; + x *= inv; + y *= inv; + z *= inv; + } + + double w; /*!< real */ + double x; /*!< x */ + double y; /*!< y */ + double z; /*!< z */ +}; + +/*! + * @brief Class that stores vector information in it + */ +class Vector { +public: + /*! + * @brief Instantiates a new Vector class to (0,0,0) + */ + Vector() : x(0.0), y(0.0), z(0.0) {} + + /*! + * @brief Instantiates a new Vector class to (0,0,0) + * @param xx + * x + * @param yy + * y + * @param zz + * z + */ + Vector(double xx, double yy, double zz) : x(xx), y(yy), z(zz) {} + + double x; /*!< x */ + double y; /*!< y */ + double z; /*!< z */ +}; + +}; // namespace imu diff --git a/utility/imumaths.h b/utility/imumaths.h deleted file mode 100644 index 8538550..0000000 --- a/utility/imumaths.h +++ /dev/null @@ -1,27 +0,0 @@ -/* - Inertial Measurement Unit Maths Library - Copyright (C) 2013-2014 Samuel Cowen - www.camelsoftware.com - - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program. If not, see . -*/ - -#ifndef IMUMATH_H -#define IMUMATH_H - -#include "matrix.h" -#include "quaternion.h" -#include "vector.h" - -#endif diff --git a/utility/matrix.h b/utility/matrix.h deleted file mode 100644 index 1fb6b60..0000000 --- a/utility/matrix.h +++ /dev/null @@ -1,183 +0,0 @@ -/* - Inertial Measurement Unit Maths Library - Copyright (C) 2013-2014 Samuel Cowen - www.camelsoftware.com - - Bug fixes and cleanups by Gé Vissers (gvissers@gmail.com) - - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program. If not, see . -*/ - -#ifndef IMUMATH_MATRIX_HPP -#define IMUMATH_MATRIX_HPP - -#include -#include - -#include "vector.h" - -namespace imu { - -template class Matrix { -public: - Matrix() { memset(_cell_data, 0, N * N * sizeof(double)); } - - Matrix(const Matrix &m) { - for (int ij = 0; ij < N * N; ++ij) { - _cell_data[ij] = m._cell_data[ij]; - } - } - - ~Matrix() {} - - Matrix &operator=(const Matrix &m) { - for (int ij = 0; ij < N * N; ++ij) { - _cell_data[ij] = m._cell_data[ij]; - } - return *this; - } - - Vector row_to_vector(int i) const { - Vector ret; - for (int j = 0; j < N; j++) { - ret[j] = cell(i, j); - } - return ret; - } - - Vector col_to_vector(int j) const { - Vector ret; - for (int i = 0; i < N; i++) { - ret[i] = cell(i, j); - } - return ret; - } - - void vector_to_row(const Vector &v, int i) { - for (int j = 0; j < N; j++) { - cell(i, j) = v[j]; - } - } - - void vector_to_col(const Vector &v, int j) { - for (int i = 0; i < N; i++) { - cell(i, j) = v[i]; - } - } - - double operator()(int i, int j) const { return cell(i, j); } - double &operator()(int i, int j) { return cell(i, j); } - - double cell(int i, int j) const { return _cell_data[i * N + j]; } - double &cell(int i, int j) { return _cell_data[i * N + j]; } - - Matrix operator+(const Matrix &m) const { - Matrix ret; - for (int ij = 0; ij < N * N; ++ij) { - ret._cell_data[ij] = _cell_data[ij] + m._cell_data[ij]; - } - return ret; - } - - Matrix operator-(const Matrix &m) const { - Matrix ret; - for (int ij = 0; ij < N * N; ++ij) { - ret._cell_data[ij] = _cell_data[ij] - m._cell_data[ij]; - } - return ret; - } - - Matrix operator*(double scalar) const { - Matrix ret; - for (int ij = 0; ij < N * N; ++ij) { - ret._cell_data[ij] = _cell_data[ij] * scalar; - } - return ret; - } - - Matrix operator*(const Matrix &m) const { - Matrix ret; - for (int i = 0; i < N; i++) { - Vector row = row_to_vector(i); - for (int j = 0; j < N; j++) { - ret(i, j) = row.dot(m.col_to_vector(j)); - } - } - return ret; - } - - Matrix transpose() const { - Matrix ret; - for (int i = 0; i < N; i++) { - for (int j = 0; j < N; j++) { - ret(j, i) = cell(i, j); - } - } - return ret; - } - - Matrix minor_matrix(int row, int col) const { - Matrix ret; - for (int i = 0, im = 0; i < N; i++) { - if (i == row) - continue; - - for (int j = 0, jm = 0; j < N; j++) { - if (j != col) { - ret(im, jm++) = cell(i, j); - } - } - im++; - } - return ret; - } - - double determinant() const { - // specialization for N == 1 given below this class - double det = 0.0, sign = 1.0; - for (int i = 0; i < N; ++i, sign = -sign) - det += sign * cell(0, i) * minor_matrix(0, i).determinant(); - return det; - } - - Matrix invert() const { - Matrix ret; - double det = determinant(); - - for (int i = 0; i < N; i++) { - for (int j = 0; j < N; j++) { - ret(i, j) = minor_matrix(j, i).determinant() / det; - if ((i + j) % 2 == 1) - ret(i, j) = -ret(i, j); - } - } - return ret; - } - - double trace() const { - double tr = 0.0; - for (int i = 0; i < N; ++i) - tr += cell(i, i); - return tr; - } - -private: - double _cell_data[N * N]; -}; - -template <> inline double Matrix<1>::determinant() const { return cell(0, 0); } - -}; // namespace imu - -#endif diff --git a/utility/quaternion.h b/utility/quaternion.h deleted file mode 100644 index 6acb988..0000000 --- a/utility/quaternion.h +++ /dev/null @@ -1,212 +0,0 @@ -/* - Inertial Measurement Unit Maths Library - Copyright (C) 2013-2014 Samuel Cowen - www.camelsoftware.com - - Bug fixes and cleanups by Gé Vissers (gvissers@gmail.com) - - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program. If not, see . -*/ - -#ifndef IMUMATH_QUATERNION_HPP -#define IMUMATH_QUATERNION_HPP - -#include -#include -#include -#include - -#include "matrix.h" - -namespace imu { - -class Quaternion { -public: - Quaternion() : _w(1.0), _x(0.0), _y(0.0), _z(0.0) {} - - Quaternion(double w, double x, double y, double z) - : _w(w), _x(x), _y(y), _z(z) {} - - Quaternion(double w, Vector<3> vec) - : _w(w), _x(vec.x()), _y(vec.y()), _z(vec.z()) {} - - double &w() { return _w; } - double &x() { return _x; } - double &y() { return _y; } - double &z() { return _z; } - - double w() const { return _w; } - double x() const { return _x; } - double y() const { return _y; } - double z() const { return _z; } - - double magnitude() const { - return sqrt(_w * _w + _x * _x + _y * _y + _z * _z); - } - - void normalize() { - double mag = magnitude(); - *this = this->scale(1 / mag); - } - - Quaternion conjugate() const { return Quaternion(_w, -_x, -_y, -_z); } - - void fromAxisAngle(const Vector<3> &axis, double theta) { - _w = cos(theta / 2); - // only need to calculate sine of half theta once - double sht = sin(theta / 2); - _x = axis.x() * sht; - _y = axis.y() * sht; - _z = axis.z() * sht; - } - - void fromMatrix(const Matrix<3> &m) { - double tr = m.trace(); - - double S; - if (tr > 0) { - S = sqrt(tr + 1.0) * 2; - _w = 0.25 * S; - _x = (m(2, 1) - m(1, 2)) / S; - _y = (m(0, 2) - m(2, 0)) / S; - _z = (m(1, 0) - m(0, 1)) / S; - } else if (m(0, 0) > m(1, 1) && m(0, 0) > m(2, 2)) { - S = sqrt(1.0 + m(0, 0) - m(1, 1) - m(2, 2)) * 2; - _w = (m(2, 1) - m(1, 2)) / S; - _x = 0.25 * S; - _y = (m(0, 1) + m(1, 0)) / S; - _z = (m(0, 2) + m(2, 0)) / S; - } else if (m(1, 1) > m(2, 2)) { - S = sqrt(1.0 + m(1, 1) - m(0, 0) - m(2, 2)) * 2; - _w = (m(0, 2) - m(2, 0)) / S; - _x = (m(0, 1) + m(1, 0)) / S; - _y = 0.25 * S; - _z = (m(1, 2) + m(2, 1)) / S; - } else { - S = sqrt(1.0 + m(2, 2) - m(0, 0) - m(1, 1)) * 2; - _w = (m(1, 0) - m(0, 1)) / S; - _x = (m(0, 2) + m(2, 0)) / S; - _y = (m(1, 2) + m(2, 1)) / S; - _z = 0.25 * S; - } - } - - void toAxisAngle(Vector<3> &axis, double &angle) const { - double sqw = sqrt(1 - _w * _w); - if (sqw == 0) // it's a singularity and divide by zero, avoid - return; - - angle = 2 * acos(_w); - axis.x() = _x / sqw; - axis.y() = _y / sqw; - axis.z() = _z / sqw; - } - - Matrix<3> toMatrix() const { - Matrix<3> ret; - ret.cell(0, 0) = 1 - 2 * _y * _y - 2 * _z * _z; - ret.cell(0, 1) = 2 * _x * _y - 2 * _w * _z; - ret.cell(0, 2) = 2 * _x * _z + 2 * _w * _y; - - ret.cell(1, 0) = 2 * _x * _y + 2 * _w * _z; - ret.cell(1, 1) = 1 - 2 * _x * _x - 2 * _z * _z; - ret.cell(1, 2) = 2 * _y * _z - 2 * _w * _x; - - ret.cell(2, 0) = 2 * _x * _z - 2 * _w * _y; - ret.cell(2, 1) = 2 * _y * _z + 2 * _w * _x; - ret.cell(2, 2) = 1 - 2 * _x * _x - 2 * _y * _y; - return ret; - } - - // Returns euler angles that represent the quaternion. Angles are - // returned in rotation order and right-handed about the specified - // axes: - // - // v[0] is applied 1st about z (ie, roll) - // v[1] is applied 2nd about y (ie, pitch) - // v[2] is applied 3rd about x (ie, yaw) - // - // Note that this means result.x() is not a rotation about x; - // similarly for result.z(). - // - Vector<3> toEuler() const { - Vector<3> ret; - double sqw = _w * _w; - double sqx = _x * _x; - double sqy = _y * _y; - double sqz = _z * _z; - - ret.x() = atan2(2.0 * (_x * _y + _z * _w), (sqx - sqy - sqz + sqw)); - ret.y() = asin(-2.0 * (_x * _z - _y * _w) / (sqx + sqy + sqz + sqw)); - ret.z() = atan2(2.0 * (_y * _z + _x * _w), (-sqx - sqy + sqz + sqw)); - - return ret; - } - - Vector<3> toAngularVelocity(double dt) const { - Vector<3> ret; - Quaternion one(1.0, 0.0, 0.0, 0.0); - Quaternion delta = one - *this; - Quaternion r = (delta / dt); - r = r * 2; - r = r * one; - - ret.x() = r.x(); - ret.y() = r.y(); - ret.z() = r.z(); - return ret; - } - - Vector<3> rotateVector(const Vector<2> &v) const { - return rotateVector(Vector<3>(v.x(), v.y())); - } - - Vector<3> rotateVector(const Vector<3> &v) const { - Vector<3> qv(_x, _y, _z); - Vector<3> t = qv.cross(v) * 2.0; - return v + t * _w + qv.cross(t); - } - - Quaternion operator*(const Quaternion &q) const { - return Quaternion(_w * q._w - _x * q._x - _y * q._y - _z * q._z, - _w * q._x + _x * q._w + _y * q._z - _z * q._y, - _w * q._y - _x * q._z + _y * q._w + _z * q._x, - _w * q._z + _x * q._y - _y * q._x + _z * q._w); - } - - Quaternion operator+(const Quaternion &q) const { - return Quaternion(_w + q._w, _x + q._x, _y + q._y, _z + q._z); - } - - Quaternion operator-(const Quaternion &q) const { - return Quaternion(_w - q._w, _x - q._x, _y - q._y, _z - q._z); - } - - Quaternion operator/(double scalar) const { - return Quaternion(_w / scalar, _x / scalar, _y / scalar, _z / scalar); - } - - Quaternion operator*(double scalar) const { return scale(scalar); } - - Quaternion scale(double scalar) const { - return Quaternion(_w * scalar, _x * scalar, _y * scalar, _z * scalar); - } - -private: - double _w, _x, _y, _z; -}; - -} // namespace imu - -#endif diff --git a/utility/vector.h b/utility/vector.h deleted file mode 100644 index 68ece98..0000000 --- a/utility/vector.h +++ /dev/null @@ -1,182 +0,0 @@ -/* - Inertial Measurement Unit Maths Library - Copyright (C) 2013-2014 Samuel Cowen - www.camelsoftware.com - - Bug fixes and cleanups by Gé Vissers (gvissers@gmail.com) - - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program. If not, see . -*/ - -#ifndef IMUMATH_VECTOR_HPP -#define IMUMATH_VECTOR_HPP - -#include -#include -#include - -namespace imu { - -template class Vector { -public: - Vector() { memset(p_vec, 0, sizeof(double) * N); } - - Vector(double a) { - memset(p_vec, 0, sizeof(double) * N); - p_vec[0] = a; - } - - Vector(double a, double b) { - memset(p_vec, 0, sizeof(double) * N); - p_vec[0] = a; - p_vec[1] = b; - } - - Vector(double a, double b, double c) { - memset(p_vec, 0, sizeof(double) * N); - p_vec[0] = a; - p_vec[1] = b; - p_vec[2] = c; - } - - Vector(double a, double b, double c, double d) { - memset(p_vec, 0, sizeof(double) * N); - p_vec[0] = a; - p_vec[1] = b; - p_vec[2] = c; - p_vec[3] = d; - } - - Vector(const Vector &v) { - for (int x = 0; x < N; x++) - p_vec[x] = v.p_vec[x]; - } - - ~Vector() {} - - uint8_t n() { return N; } - - double magnitude() const { - double res = 0; - for (int i = 0; i < N; i++) - res += p_vec[i] * p_vec[i]; - - return sqrt(res); - } - - void normalize() { - double mag = magnitude(); - if (isnan(mag) || mag == 0.0) - return; - - for (int i = 0; i < N; i++) - p_vec[i] /= mag; - } - - double dot(const Vector &v) const { - double ret = 0; - for (int i = 0; i < N; i++) - ret += p_vec[i] * v.p_vec[i]; - - return ret; - } - - // The cross product is only valid for vectors with 3 dimensions, - // with the exception of higher dimensional stuff that is beyond - // the intended scope of this library. - // Only a definition for N==3 is given below this class, using - // cross() with another value for N will result in a link error. - Vector cross(const Vector &v) const; - - Vector scale(double scalar) const { - Vector ret; - for (int i = 0; i < N; i++) - ret.p_vec[i] = p_vec[i] * scalar; - return ret; - } - - Vector invert() const { - Vector ret; - for (int i = 0; i < N; i++) - ret.p_vec[i] = -p_vec[i]; - return ret; - } - - Vector &operator=(const Vector &v) { - for (int x = 0; x < N; x++) - p_vec[x] = v.p_vec[x]; - return *this; - } - - double &operator[](int n) { return p_vec[n]; } - - double operator[](int n) const { return p_vec[n]; } - - double &operator()(int n) { return p_vec[n]; } - - double operator()(int n) const { return p_vec[n]; } - - Vector operator+(const Vector &v) const { - Vector ret; - for (int i = 0; i < N; i++) - ret.p_vec[i] = p_vec[i] + v.p_vec[i]; - return ret; - } - - Vector operator-(const Vector &v) const { - Vector ret; - for (int i = 0; i < N; i++) - ret.p_vec[i] = p_vec[i] - v.p_vec[i]; - return ret; - } - - Vector operator*(double scalar) const { return scale(scalar); } - - Vector operator/(double scalar) const { - Vector ret; - for (int i = 0; i < N; i++) - ret.p_vec[i] = p_vec[i] / scalar; - return ret; - } - - void toDegrees() { - for (int i = 0; i < N; i++) - p_vec[i] *= 57.2957795131; // 180/pi - } - - void toRadians() { - for (int i = 0; i < N; i++) - p_vec[i] *= 0.01745329251; // pi/180 - } - - double &x() { return p_vec[0]; } - double &y() { return p_vec[1]; } - double &z() { return p_vec[2]; } - double x() const { return p_vec[0]; } - double y() const { return p_vec[1]; } - double z() const { return p_vec[2]; } - -private: - double p_vec[N]; -}; - -template <> inline Vector<3> Vector<3>::cross(const Vector &v) const { - return Vector(p_vec[1] * v.p_vec[2] - p_vec[2] * v.p_vec[1], - p_vec[2] * v.p_vec[0] - p_vec[0] * v.p_vec[2], - p_vec[0] * v.p_vec[1] - p_vec[1] * v.p_vec[0]); -} - -} // namespace imu - -#endif