diff --git a/CMakeLists.txt b/CMakeLists.txt index 63619ebcfc..fecbf88934 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -50,13 +50,6 @@ if (COVERAGE) ) include(Coveralls) coveralls_turn_on_coverage() - add_custom_target(coverage - COMMAND lcov --capture --directory . --output-file coverage.info - COMMAND genhtml coverage.info --output-directory out - COMMAND x-www-browser out/index.html - WORKING_DIRECTORY ${CMAKE_BUILD_DIR} - DEPENDS coveralls - ) endif() string(REPLACE ";" " " CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}") diff --git a/matrix/Matrix.hpp b/matrix/Matrix.hpp index d06cef3755..2f10bcf46a 100644 --- a/matrix/Matrix.hpp +++ b/matrix/Matrix.hpp @@ -94,6 +94,20 @@ public: return res; } + Matrix emult(const Matrix &other) const + { + Matrix res; + const Matrix &self = *this; + + for (size_t i = 0; i < M; i++) { + for (size_t j = 0; j < N; j++) { + res(i , j) = self(i, j)*other(i, j); + } + } + + return res; + } + Matrix operator+(const Matrix &other) const { Matrix res; diff --git a/matrix/Quaternion.hpp b/matrix/Quaternion.hpp index a6b25b281f..a0f0441db3 100644 --- a/matrix/Quaternion.hpp +++ b/matrix/Quaternion.hpp @@ -25,6 +25,9 @@ class Quaternion : public Vector public: virtual ~Quaternion() {}; + typedef Matrix Matrix41; + typedef Matrix Matrix31; + Quaternion() : Vector() { @@ -35,12 +38,7 @@ public: q(3) = 0; } - Quaternion(const Vector & other) : - Vector(other) - { - } - - Quaternion(const Matrix & other) : + Quaternion(const Matrix41 & other) : Vector(other) { } @@ -100,7 +98,7 @@ public: return r; } - Matrix derivative(const Vector & w) const { + Matrix41 derivative(const Matrix31 & w) const { const Quaternion &q = *this; Type dataQ[] = { q(0), -q(1), -q(2), -q(3), @@ -111,9 +109,9 @@ public: Matrix Q(dataQ); Vector v; v(0) = 0; - v(1) = w(0); - v(2) = w(1); - v(3) = w(2); + v(1) = w(0,0); + v(2) = w(1,0); + v(3) = w(2,0); return Q * v * Type(0.5); } }; diff --git a/matrix/SquareMatrix.hpp b/matrix/SquareMatrix.hpp index ffbe1045af..ee06ccdd29 100644 --- a/matrix/SquareMatrix.hpp +++ b/matrix/SquareMatrix.hpp @@ -79,7 +79,7 @@ SquareMatrix diag(Vector d) { } template -SquareMatrix expm(const SquareMatrix & A, size_t order=5) +SquareMatrix expm(const Matrix & A, size_t order=5) { SquareMatrix res; SquareMatrix A_pow = A; diff --git a/matrix/Vector.hpp b/matrix/Vector.hpp index c0184dac8c..fb771ecc18 100644 --- a/matrix/Vector.hpp +++ b/matrix/Vector.hpp @@ -21,42 +21,39 @@ class Vector : public Matrix public: virtual ~Vector() {}; - Vector() : Matrix() + typedef Matrix MatrixM1; + + Vector() : MatrixM1() { } - Vector(const Vector & other) : - Matrix(other) - { - } - - Vector(const Matrix & other) : - Matrix(other) + Vector(const MatrixM1 & other) : + MatrixM1(other) { } Vector(const Type *data_) : - Matrix(data_) + MatrixM1(data_) { } inline Type operator()(size_t i) const { - const Matrix &v = *this; + const MatrixM1 &v = *this; return v(i, 0); } inline Type &operator()(size_t i) { - Matrix &v = *this; + MatrixM1 &v = *this; return v(i, 0); } - Type dot(const Vector & b) const { + Type dot(const MatrixM1 & b) const { const Vector &a(*this); Type r = 0; for (int i = 0; i + */ + +#pragma once + +#include "math.hpp" + +namespace matrix +{ + +template +class Vector; + +template +class Vector2 : public Vector +{ +public: + + typedef Matrix Matrix21; + + virtual ~Vector2() {}; + + Vector2() : + Vector() + { + } + + Vector2(const Matrix21 & other) : + Vector(other) + { + } + + Vector2(const Type *data_) : + Vector(data_) + { + } + + Vector2(Type x, Type y) : Vector() + { + Vector2 &v(*this); + v(0) = x; + v(1) = y; + } + + Type cross(const Matrix21 & b) const { + const Vector2 &a(*this); + return a(0)*b(1, 0) - a(1)*b(0, 0); + } + + Type operator%(const Matrix21 & b) const { + return (*this).cross(b); + } + +}; + +typedef Vector2 Vector2f; + +}; // namespace matrix + +/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */ diff --git a/matrix/Vector3.hpp b/matrix/Vector3.hpp index 5a3d5c8376..c33ff77e58 100644 --- a/matrix/Vector3.hpp +++ b/matrix/Vector3.hpp @@ -20,6 +20,9 @@ template class Vector3 : public Vector { public: + + typedef Matrix Matrix31; + virtual ~Vector3() {}; Vector3() : @@ -27,12 +30,7 @@ public: { } - Vector3(const Vector & other) : - Vector(other) - { - } - - Vector3(const Matrix & other) : + Vector3(const Matrix31 & other) : Vector(other) { } @@ -50,14 +48,19 @@ public: v(2) = z; } - Vector3 cross(const Vector3 & b) const { + Vector3 cross(const Matrix31 & b) const { const Vector3 &a(*this); Vector3 c; - c(0) = a(1)*b(2) - a(2)*b(1); - c(1) = -a(0)*b(2) + a(2)*b(0); - c(2) = a(0)*b(1) - a(1)*b(0); + c(0) = a(1)*b(2,0) - a(2)*b(1,0); + c(1) = -a(0)*b(2,0) + a(2)*b(0,0); + c(2) = a(0)*b(1,0) - a(1)*b(0,0); return c; } + + Vector3 operator%(const Matrix31 & b) const { + return (*this).cross(b); + } + }; typedef Vector3 Vector3f; diff --git a/matrix/math.hpp b/matrix/math.hpp index 10f3973e1b..da1d73d992 100644 --- a/matrix/math.hpp +++ b/matrix/math.hpp @@ -3,6 +3,7 @@ #include "Matrix.hpp" #include "SquareMatrix.hpp" #include "Vector.hpp" +#include "Vector2.hpp" #include "Vector3.hpp" #include "Euler.hpp" #include "Dcm.hpp" diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 2f0570c7a6..c7ae87b675 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -7,14 +7,29 @@ set(tests matrixScalarMult transpose vector + vector2 vector3 attitude filter squareMatrix ) -foreach(test ${tests}) - add_executable(${test} - ${test}.cpp) - add_test(${test} ${test}) +add_custom_target(test_build) +foreach(test_name ${tests}) + add_executable(${test_name} + ${test_name}.cpp) + add_test(test_${test_name} ${test_name}) + add_dependencies(test_build ${test_name}) endforeach() + +if (COVERAGE) + add_custom_target(coverage + COMMAND lcov --capture --directory . --output-file coverage.info + COMMAND genhtml coverage.info --output-directory out + COMMAND x-www-browser out/index.html + WORKING_DIRECTORY ${CMAKE_BUILD_DIR} + DEPENDS coveralls test_build + ) +endif() + + diff --git a/test/matrixMult.cpp b/test/matrixMult.cpp index 5ba8b72254..613d61db7d 100644 --- a/test/matrixMult.cpp +++ b/test/matrixMult.cpp @@ -23,6 +23,12 @@ int main() R2 *= A_I; R2.print(); assert(R2 == I); + + + Matrix3f A2 = eye()*2; + Matrix3f B = A2.emult(A2); + Matrix3f B_check = eye()*4; + assert(B == B_check); return 0; } diff --git a/test/vector.cpp b/test/vector.cpp index 053e22dec8..c340997f11 100644 --- a/test/vector.cpp +++ b/test/vector.cpp @@ -15,6 +15,7 @@ int main() assert(fabs(v1.norm() - 7.416198487095663f) < 1e-5); Vector v2(data2); assert(fabs(v1.dot(v2) - 130.0f) < 1e-5); + assert(fabs(v1*v2 - 130.0f) < 1e-5); v2.normalize(); Vector v3(v2); assert(v2 == v3); diff --git a/test/vector2.cpp b/test/vector2.cpp new file mode 100644 index 0000000000..671d9f9b72 --- /dev/null +++ b/test/vector2.cpp @@ -0,0 +1,18 @@ +#include +#include + +#include + +using namespace matrix; + +template class Vector; + +int main() +{ + Vector2f a(1, 0); + Vector2f b(0, 1); + assert (fabs(a % b - 1.0f) < 1e-5); + return 0; +} + +/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */