diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2013-12-24 23:56:28 +0400 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2013-12-24 23:56:28 +0400 |
commit | 9dfe366e908ce0100875996c3ea0d4cfdfcc24bf (patch) | |
tree | 8d465ad2afbd51406284c7d5a43dbc9c8753c196 | |
parent | a26e46bede186c36b475e0b5a9cf3ef758cc1c9b (diff) | |
download | px4-firmware-9dfe366e908ce0100875996c3ea0d4cfdfcc24bf.tar.gz px4-firmware-9dfe366e908ce0100875996c3ea0d4cfdfcc24bf.tar.bz2 px4-firmware-9dfe366e908ce0100875996c3ea0d4cfdfcc24bf.zip |
mathlib: Vector class major cleanup
-rw-r--r-- | src/lib/mathlib/math/Matrix.hpp | 27 | ||||
-rw-r--r-- | src/lib/mathlib/math/Quaternion.hpp | 48 | ||||
-rw-r--r-- | src/lib/mathlib/math/Vector.hpp | 195 | ||||
-rw-r--r-- | src/modules/att_pos_estimator_ekf/KalmanNav.cpp | 8 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 3 | ||||
-rw-r--r-- | src/modules/sensors/sensors.cpp | 6 | ||||
-rw-r--r-- | src/systemcmds/tests/test_mathlib.cpp | 114 |
7 files changed, 209 insertions, 192 deletions
diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index 7ed8879a7..9896a16d0 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -94,11 +94,11 @@ public: return data[row][col]; } - unsigned int getRows() { + unsigned int get_rows() { return M; } - unsigned int getCols() { + unsigned int get_cols() { return N; } @@ -295,11 +295,7 @@ public: return res; } }; -} - -#include "Quaternion.hpp" -namespace math { template <> class __EXPORT Matrix<3, 3> : public MatrixBase<3, 3> { public: @@ -330,25 +326,6 @@ public: } /** - * create a rotation matrix from given quaternion - */ - void from_quaternion(const Quaternion &q) { - float aSq = q.data[0] * q.data[0]; - float bSq = q.data[1] * q.data[1]; - float cSq = q.data[2] * q.data[2]; - float dSq = q.data[3] * q.data[3]; - data[0][0] = aSq + bSq - cSq - dSq; - data[0][1] = 2.0f * (q.data[1] * q.data[2] - q.data[0] * q.data[3]); - data[0][2] = 2.0f * (q.data[0] * q.data[2] + q.data[1] * q.data[3]); - data[1][0] = 2.0f * (q.data[1] * q.data[2] + q.data[0] * q.data[3]); - data[1][1] = aSq - bSq + cSq - dSq; - data[1][2] = 2.0f * (q.data[2] * q.data[3] - q.data[0] * q.data[1]); - data[2][0] = 2.0f * (q.data[1] * q.data[3] - q.data[0] * q.data[2]); - data[2][1] = 2.0f * (q.data[0] * q.data[1] + q.data[2] * q.data[3]); - data[2][2] = aSq - bSq - cSq + dSq; - } - - /** * create a rotation matrix from given euler angles * based on http://gentlenav.googlecode.com/files/EulerAngles.pdf */ diff --git a/src/lib/mathlib/math/Quaternion.hpp b/src/lib/mathlib/math/Quaternion.hpp index 3735fb3d3..c19dbd29c 100644 --- a/src/lib/mathlib/math/Quaternion.hpp +++ b/src/lib/mathlib/math/Quaternion.hpp @@ -58,7 +58,7 @@ public: /** * trivial ctor */ - Quaternion() { + Quaternion() : Vector() { } /** @@ -70,10 +70,29 @@ public: Quaternion(const Vector<4> &v) : Vector(v) { } - Quaternion(const float *v) : Vector(v) { + Quaternion(const Quaternion &q) : Vector(q) { } - Quaternion derivative(const Vector<3> &w) { + Quaternion(const float v[4]) : Vector(v) { + } + + using Vector<4>::operator *; + + /** + * multiplication + */ + const Quaternion operator *(const Quaternion &q) const { + return Quaternion( + data[0] * q.data[0] - data[1] * q.data[1] - data[2] * q.data[2] - data[3] * q.data[3], + data[0] * q.data[1] + data[1] * q.data[0] + data[2] * q.data[3] - data[3] * q.data[2], + data[0] * q.data[2] - data[1] * q.data[3] + data[2] * q.data[0] + data[3] * q.data[1], + data[0] * q.data[3] + data[1] * q.data[2] - data[2] * q.data[1] + data[3] * q.data[0]); + } + + /** + * derivative + */ + const Quaternion derivative(const Vector<3> &w) { float dataQ[] = { data[0], -data[1], -data[2], -data[3], data[1], data[0], -data[3], data[2], @@ -85,6 +104,9 @@ public: return Q * v * 0.5f; } + /** + * set quaternion to rotation defined by euler angles + */ void from_euler(float roll, float pitch, float yaw) { double cosPhi_2 = cos(double(roll) / 2.0); double sinPhi_2 = sin(double(roll) / 2.0); @@ -97,6 +119,26 @@ public: data[2] = cosPhi_2 * sinTheta_2 * cosPsi_2 + sinPhi_2 * cosTheta_2 * sinPsi_2; data[3] = cosPhi_2 * cosTheta_2 * sinPsi_2 - sinPhi_2 * sinTheta_2 * cosPsi_2; } + + /** + * create rotation matrix for the quaternion + */ + Matrix<3, 3> to_dcm(void) const { + Matrix<3, 3> R; + float aSq = data[0] * data[0]; + float bSq = data[1] * data[1]; + float cSq = data[2] * data[2]; + float dSq = data[3] * data[3]; + R.data[0][0] = aSq + bSq - cSq - dSq; + R.data[0][1] = 2.0f * (data[1] * data[2] - data[0] * data[3]); + R.data[0][2] = 2.0f * (data[0] * data[2] + data[1] * data[3]); + R.data[1][0] = 2.0f * (data[1] * data[2] + data[0] * data[3]); + R.data[1][1] = aSq - bSq + cSq - dSq; + R.data[1][2] = 2.0f * (data[2] * data[3] - data[0] * data[1]); + R.data[2][0] = 2.0f * (data[1] * data[3] - data[0] * data[2]); + R.data[2][1] = 2.0f * (data[0] * data[1] + data[2] * data[3]); + R.data[2][2] = aSq - bSq - cSq + dSq; + } }; } diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp index 744402e21..d579ecf73 100644 --- a/src/lib/mathlib/math/Vector.hpp +++ b/src/lib/mathlib/math/Vector.hpp @@ -1,8 +1,9 @@ /**************************************************************************** * * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Will Perone <will.perone@gmail.com> - * Anton Babushkin <anton.babushkin@me.com> + * Author: Anton Babushkin <anton.babushkin@me.com> + * Pavel Kirienko <pavel.kirienko@gmail.com> + * Lorenz Meier <lm@inf.ethz.ch> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -36,13 +37,12 @@ /** * @file Vector.hpp * - * Generic Vector + * Vector class */ #ifndef VECTOR_HPP #define VECTOR_HPP -#include <visibility.h> #include <stdio.h> #include <math.h> #include "../CMSIS/Include/arm_math.h" @@ -84,7 +84,7 @@ public: /** * setting ctor */ - VectorBase(const float *d) { + VectorBase(const float d[N]) { arm_col = {N, 1, &data[0]}; memcpy(data, d, sizeof(data)); } @@ -92,31 +92,30 @@ public: /** * access to elements by index */ - inline float &operator ()(unsigned int i) { + float &operator ()(const unsigned int i) { return data[i]; } /** * access to elements by index */ - inline const float &operator ()(unsigned int i) const { + float operator ()(const unsigned int i) const { return data[i]; } - unsigned int getRows() { + /** + * get rows number + */ + unsigned int get_size() const { return N; } - unsigned int getCols() { - return 1; - } - /** * test for equality */ bool operator ==(const Vector<N> &v) { for (unsigned int i = 0; i < N; i++) - if (data[i] != v(i)) + if (data[i] != v.data[i]) return false; return true; } @@ -126,7 +125,7 @@ public: */ bool operator !=(const Vector<N> &v) { for (unsigned int i = 0; i < N; i++) - if (data[i] != v(i)) + if (data[i] != v.data[i]) return true; return false; } @@ -155,7 +154,7 @@ public: const Vector<N> operator +(const Vector<N> &v) const { Vector<N> res; for (unsigned int i = 0; i < N; i++) - res.data[i] = data[i] + v(i); + res.data[i] = data[i] + v.data[i]; return res; } @@ -165,7 +164,7 @@ public: const Vector<N> operator -(const Vector<N> &v) const { Vector<N> res; for (unsigned int i = 0; i < N; i++) - res.data[i] = data[i] - v(i); + res.data[i] = data[i] - v.data[i]; return res; } @@ -173,16 +172,20 @@ public: * uniform scaling */ const Vector<N> operator *(const float num) const { - Vector<N> temp(*this); - return temp *= num; + Vector<N> res; + for (unsigned int i = 0; i < N; i++) + res.data[i] = data[i] * num; + return res; } /** * uniform scaling */ const Vector<N> operator /(const float num) const { - Vector<N> temp(*static_cast<const Vector<N>*>(this)); - return temp /= num; + Vector<N> res; + for (unsigned int i = 0; i < N; i++) + res.data[i] = data[i] / num; + return res; } /** @@ -190,7 +193,7 @@ public: */ const Vector<N> &operator +=(const Vector<N> &v) { for (unsigned int i = 0; i < N; i++) - data[i] += v(i); + data[i] += v.data[i]; return *static_cast<const Vector<N>*>(this); } @@ -199,7 +202,7 @@ public: */ const Vector<N> &operator -=(const Vector<N> &v) { for (unsigned int i = 0; i < N; i++) - data[i] -= v(i); + data[i] -= v.data[i]; return *static_cast<const Vector<N>*>(this); } @@ -227,7 +230,7 @@ public: float operator *(const Vector<N> &v) const { float res = 0.0f; for (unsigned int i = 0; i < N; i++) - res += data[i] * v(i); + res += data[i] * v.data[i]; return res; } @@ -235,14 +238,20 @@ public: * gets the length of this vector squared */ float length_squared() const { - return (*this * *this); + float res = 0.0f; + for (unsigned int i = 0; i < N; i++) + res += data[i] * data[i]; + return res; } /** * gets the length of this vector */ float length() const { - return sqrtf(*this * *static_cast<const Vector<N>*>(this)); + float res = 0.0f; + for (unsigned int i = 0; i < N; i++) + res += data[i] * data[i]; + return sqrtf(res); } /** @@ -277,25 +286,17 @@ public: template <unsigned int N> class __EXPORT Vector : public VectorBase<N> { public: - using VectorBase<N>::operator *; + //using VectorBase<N>::operator *; + Vector() : VectorBase<N>() {} - Vector() : VectorBase<N>() { - } + Vector(const Vector &v) : VectorBase<N>(v) {} - Vector(const float d[]) : VectorBase<N>(d) { - } - - Vector(const Vector<N> &v) : VectorBase<N>(v) { - } - - Vector(const VectorBase<N> &v) : VectorBase<N>(v) { - } + Vector(const float d[N]) : VectorBase<N>(d) {} /** * set to value */ const Vector<N> &operator =(const Vector<N> &v) { - this->arm_col = {N, 1, &this->data[0]}; memcpy(this->data, v.data, sizeof(this->data)); return *this; } @@ -304,22 +305,22 @@ public: template <> class __EXPORT Vector<2> : public VectorBase<2> { public: - Vector() : VectorBase<2>() { - } + Vector() : VectorBase<2>() {} - Vector(const float x, const float y) : VectorBase() { - data[0] = x; - data[1] = y; - } - - Vector(const Vector<2> &v) : VectorBase() { + /* simple copy is 1.6 times faster than memcpy */ + Vector(const Vector &v) : VectorBase<2>() { data[0] = v.data[0]; data[1] = v.data[1]; } - Vector(const VectorBase<2> &v) : VectorBase() { - data[0] = v.data[0]; - data[1] = v.data[1]; + Vector(const float d[2]) : VectorBase<2>() { + data[0] = d[0]; + data[1] = d[1]; + } + + Vector(const float x, const float y) : VectorBase<2>() { + data[0] = x; + data[1] = y; } /** @@ -331,55 +332,49 @@ public: return *this; } - float cross(const Vector<2> &b) const { - return data[0]*b.data[1] - data[1]*b.data[0]; - } - float operator %(const Vector<2> &v) const { - return cross(v); + return data[0] * v.data[1] - data[1] * v.data[0]; } - }; template <> class __EXPORT Vector<3> : public VectorBase<3> { public: - Vector() { - arm_col = {3, 1, &this->data[0]}; - } + Vector() : VectorBase<3>() {} - Vector(const float x, const float y, const float z) { - arm_col = {3, 1, &this->data[0]}; - data[0] = x; - data[1] = y; - data[2] = z; + /* simple copy is 1.6 times faster than memcpy */ + Vector(const Vector &v) : VectorBase<3>() { + for (unsigned int i = 0; i < 3; i++) + data[i] = v.data[i]; } - Vector(const Vector<3> &v) : VectorBase<3>() { - data[0] = v.data[0]; - data[1] = v.data[1]; - data[2] = v.data[2]; + Vector(const float d[3]) : VectorBase<3>() { + for (unsigned int i = 0; i < 3; i++) + data[i] = d[i]; } - /** - * setting ctor - */ - Vector(const float d[]) { - arm_col = {3, 1, &this->data[0]}; - data[0] = d[0]; - data[1] = d[1]; - data[2] = d[2]; + Vector(const float x, const float y, const float z) : VectorBase<3>() { + data[0] = x; + data[1] = y; + data[2] = z; } /** * set to value */ const Vector<3> &operator =(const Vector<3> &v) { - data[0] = v.data[0]; - data[1] = v.data[1]; - data[2] = v.data[2]; + for (unsigned int i = 0; i < 3; i++) + data[i] = v.data[i]; return *this; } + + Vector<3> operator %(const Vector<3> &v) const { + return Vector<3>( + data[1] * v.data[2] - data[2] * v.data[1], + data[2] * v.data[0] - data[0] * v.data[2], + data[0] * v.data[1] - data[1] * v.data[0] + ); + } }; template <> @@ -387,49 +382,31 @@ class __EXPORT Vector<4> : public VectorBase<4> { public: Vector() : VectorBase() {} - Vector(const float x, const float y, const float z, const float t) : VectorBase() { - data[0] = x; - data[1] = y; - data[2] = z; - data[3] = t; + Vector(const Vector &v) : VectorBase<4>() { + for (unsigned int i = 0; i < 4; i++) + data[i] = v.data[i]; } - Vector(const Vector<4> &v) : VectorBase() { - data[0] = v.data[0]; - data[1] = v.data[1]; - data[2] = v.data[2]; - data[3] = v.data[3]; + Vector(const float d[4]) : VectorBase<4>() { + for (unsigned int i = 0; i < 4; i++) + data[i] = d[i]; } - Vector(const VectorBase<4> &v) : VectorBase() { - data[0] = v.data[0]; - data[1] = v.data[1]; - data[2] = v.data[2]; - data[3] = v.data[3]; + Vector(const float x0, const float x1, const float x2, const float x3) : VectorBase() { + data[0] = x0; + data[1] = x1; + data[2] = x2; + data[3] = x3; } /** - * setting ctor - */ - /* - Vector(const float d[]) { - arm_col = {3, 1, &this->data[0]}; - data[0] = d[0]; - data[1] = d[1]; - data[2] = d[2]; - } -*/ - /** * set to value */ - /* - const Vector<3> &operator =(const Vector<3> &v) { - data[0] = v.data[0]; - data[1] = v.data[1]; - data[2] = v.data[2]; + const Vector<4> &operator =(const Vector<4> &v) { + for (unsigned int i = 0; i < 4; i++) + data[i] = v.data[i]; return *this; } - */ }; } diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp index 9d3ef07f2..aca3fe7b6 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp @@ -132,7 +132,7 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : _sensors.magnetometer_ga[2]); // initialize dcm - C_nb.from_quaternion(q); + C_nb = q.to_dcm(); // HPos is constant HPos(0, 3) = 1.0f; @@ -409,7 +409,7 @@ int KalmanNav::predictState(float dt) } // C_nb update - C_nb.from_quaternion(q); + C_nb = q.to_dcm(); // euler update Vector<3> euler = C_nb.to_euler(); @@ -628,7 +628,7 @@ int KalmanNav::correctAtt() Vector<9> xCorrect = K * y; // check correciton is sane - for (size_t i = 0; i < xCorrect.getRows(); i++) { + for (size_t i = 0; i < xCorrect.get_size(); i++) { float val = xCorrect(i); if (isnan(val) || isinf(val)) { @@ -694,7 +694,7 @@ int KalmanNav::correctPos() Vector<9> xCorrect = K * y; // check correction is sane - for (size_t i = 0; i < xCorrect.getRows(); i++) { + for (size_t i = 0; i < xCorrect.get_size(); i++) { float val = xCorrect(i); if (!isfinite(val)) { diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 653d4b6b3..b4f7f2dfe 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -683,8 +683,7 @@ handle_message(mavlink_message_t *msg) /* Calculate Rotation Matrix */ math::Quaternion q(hil_state.attitude_quaternion); - math::Matrix<3,3> C_nb; - C_nb.from_quaternion(q); + math::Matrix<3,3> C_nb = q.to_dcm(); math::Vector<3> euler = C_nb.to_euler(); /* set rotation matrix */ diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index eea9438f7..9e2eeafa4 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -465,8 +465,6 @@ Sensors::Sensors() : /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")), - _board_rotation(), - _external_mag_rotation(), _mag_is_external(false) { @@ -920,7 +918,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report); - math::Vector<3> vect = {accel_report.x, accel_report.y, accel_report.z}; + math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z); vect = _board_rotation * vect; raw.accelerometer_m_s2[0] = vect(0); @@ -946,7 +944,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report); - math::Vector<3> vect = {gyro_report.x, gyro_report.y, gyro_report.z}; + math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z); vect = _board_rotation * vect; raw.gyro_rad_s[0] = vect(0); diff --git a/src/systemcmds/tests/test_mathlib.cpp b/src/systemcmds/tests/test_mathlib.cpp index d2e1a93e3..e654e0f81 100644 --- a/src/systemcmds/tests/test_mathlib.cpp +++ b/src/systemcmds/tests/test_mathlib.cpp @@ -48,6 +48,8 @@ #include "tests.h" +#define TEST_OP(_title, _op) { unsigned int n = 60000; hrt_abstime t0, t1; t0 = hrt_absolute_time(); for (unsigned int j = 0; j < n; j++) { _op; }; t1 = hrt_absolute_time(); warnx(_title ": %.6fus", (double)(t1 - t0) / n); } + using namespace math; const char* formatResult(bool res) { @@ -58,60 +60,82 @@ int test_mathlib(int argc, char *argv[]) { warnx("testing mathlib"); - Matrix<3,3> m3; - m3.identity(); - Matrix<4,4> m4; - m4.identity(); - Vector<3> v3; - v3(0) = 1.0f; - v3(1) = 2.0f; - v3(2) = 3.0f; - Vector<4> v4; - v4(0) = 1.0f; - v4(1) = 2.0f; - v4(2) = 3.0f; - v4(3) = 4.0f; - Vector<3> vres3; - Matrix<3,3> mres3; - Matrix<4,4> mres4; - - unsigned int n = 60000; + Vector<2> v2(1.0f, 2.0f); + Vector<3> v3(1.0f, 2.0f, 3.0f); + Vector<4> v4(1.0f, 2.0f, 3.0f, 4.0f); + Vector<10> v10; + v10.zero(); - hrt_abstime t0, t1; + float data2[2] = {1.0f, 2.0f}; + float data3[3] = {1.0f, 2.0f, 3.0f}; + float data4[4] = {1.0f, 2.0f, 3.0f, 4.0f}; + float data10[10]; - t0 = hrt_absolute_time(); - for (unsigned int j = 0; j < n; j++) { - vres3 = m3 * v3; + { + Vector<2> v; + Vector<2> v1(1.0f, 2.0f); + Vector<2> v2(1.0f, -1.0f); + TEST_OP("Constructor Vector<2>()", Vector<2> v); + TEST_OP("Constructor Vector<2>(Vector<2>)", Vector<2> v(v2)); + TEST_OP("Constructor Vector<2>(float[])", Vector<2> v(data2)); + TEST_OP("Constructor Vector<2>(float, float)", Vector<2> v(1.0f, 2.0f)); + TEST_OP("Vector<2> = Vector<2>", v = v1); + TEST_OP("Vector<2> + Vector<2>", v + v1); + TEST_OP("Vector<2> - Vector<2>", v - v1); + TEST_OP("Vector<2> += Vector<2>", v += v1); + TEST_OP("Vector<2> -= Vector<2>", v -= v1); + TEST_OP("Vector<2> * Vector<2>", v * v1); + TEST_OP("Vector<2> %% Vector<2>", v1 % v2); } - t1 = hrt_absolute_time(); - warnx("Matrix3 * Vector3: %s %.6fus", formatResult(vres3 == v3), (double)(t1 - t0) / n); - t0 = hrt_absolute_time(); - for (unsigned int j = 0; j < n; j++) { - mres3 = m3 * m3; + { + Vector<3> v; + Vector<3> v1(1.0f, 2.0f, 0.0f); + Vector<3> v2(1.0f, -1.0f, 2.0f); + TEST_OP("Constructor Vector<3>()", Vector<3> v); + TEST_OP("Constructor Vector<3>(Vector<3>)", Vector<3> v(v3)); + TEST_OP("Constructor Vector<3>(float[])", Vector<3> v(data3)); + TEST_OP("Constructor Vector<3>(float, float, float)", Vector<3> v(1.0f, 2.0f, 3.0f)); + TEST_OP("Vector<3> = Vector<3>", v = v1); + TEST_OP("Vector<3> + Vector<3>", v + v1); + TEST_OP("Vector<3> - Vector<3>", v - v1); + TEST_OP("Vector<3> += Vector<3>", v += v1); + TEST_OP("Vector<3> -= Vector<3>", v -= v1); + TEST_OP("Vector<3> * float", v1 * 2.0f); + TEST_OP("Vector<3> / float", v1 / 2.0f); + TEST_OP("Vector<3> *= float", v1 *= 2.0f); + TEST_OP("Vector<3> /= float", v1 /= 2.0f); + TEST_OP("Vector<3> * Vector<3>", v * v1); + TEST_OP("Vector<3> %% Vector<3>", v1 % v2); + TEST_OP("Vector<3> length", v1.length()); + TEST_OP("Vector<3> length squared", v1.length_squared()); + TEST_OP("Vector<3> element read", volatile float a = v1(0)); + TEST_OP("Vector<3> element read direct", volatile float a = v1.data[0]); + TEST_OP("Vector<3> element write", v1(0) = 1.0f); + TEST_OP("Vector<3> element write direct", v1.data[0] = 1.0f); } - t1 = hrt_absolute_time(); - warnx("Matrix3 * Matrix3: %s %.6fus", formatResult(mres3 == m3), (double)(t1 - t0) / n); - t0 = hrt_absolute_time(); - for (unsigned int j = 0; j < n; j++) { - mres4 = m4 * m4; + { + Vector<4> v; + Vector<4> v1(1.0f, 2.0f, 0.0f, -1.0f); + Vector<4> v2(1.0f, -1.0f, 2.0f, 0.0f); + TEST_OP("Constructor Vector<4>()", Vector<4> v); + TEST_OP("Constructor Vector<4>(Vector<4>)", Vector<4> v(v4)); + TEST_OP("Constructor Vector<4>(float[])", Vector<4> v(data4)); + TEST_OP("Constructor Vector<4>(float, float, float, float)", Vector<4> v(1.0f, 2.0f, 3.0f, 4.0f)); + TEST_OP("Vector<4> = Vector<4>", v = v1); + TEST_OP("Vector<4> + Vector<4>", v + v1); + TEST_OP("Vector<4> - Vector<4>", v - v1); + TEST_OP("Vector<4> += Vector<4>", v += v1); + TEST_OP("Vector<4> -= Vector<4>", v -= v1); + TEST_OP("Vector<4> * Vector<4>", v * v1); } - t1 = hrt_absolute_time(); - warnx("Matrix4 * Matrix4: %s %.6fus", formatResult(mres4 == m4), (double)(t1 - t0) / n); - t0 = hrt_absolute_time(); - for (unsigned int j = 0; j < n; j++) { - mres3 = m3.transposed(); + { + TEST_OP("Constructor Vector<10>()", Vector<10> v); + TEST_OP("Constructor Vector<10>(Vector<10>)", Vector<10> v(v10)); + TEST_OP("Constructor Vector<10>(float[])", Vector<10> v(data10)); } - t1 = hrt_absolute_time(); - warnx("Matrix3 Transpose: %s %.6fus", formatResult(mres3 == m3), (double)(t1 - t0) / n); - t0 = hrt_absolute_time(); - for (unsigned int j = 0; j < n; j++) { - mres3 = m3.inversed(); - } - t1 = hrt_absolute_time(); - warnx("Matrix3 Invert: %s %.6fus", formatResult(mres3 == m3), (double)(t1 - t0) / n); return 0; } |