diff --git a/src/lib/matrix/.clang-tidy b/src/lib/matrix/.clang-tidy new file mode 100644 index 0000000000..2878339187 --- /dev/null +++ b/src/lib/matrix/.clang-tidy @@ -0,0 +1,9 @@ +Checks: 'clang-diagnostic-*,clang-analyzer-*,*, + ,-cppcoreguidelines-pro-type-vararg + ,-cppcoreguidelines-pro-bounds-array-to-pointer-decay + ,-cppcoreguidelines-pro-bounds-constant-array-index + ,-cppcoreguidelines-pro-bounds-pointer-arithmetic + ' +WarningsAsErrors: '*' +HeaderFilterRegex: '*.h, *.hpp, *.hh, *.hxx' + diff --git a/src/lib/matrix/.github/workflows/test.yml b/src/lib/matrix/.github/workflows/test.yml new file mode 100644 index 0000000000..c368930777 --- /dev/null +++ b/src/lib/matrix/.github/workflows/test.yml @@ -0,0 +1,53 @@ +name: Tests + +on: + push: + branches: + - 'master' + pull_request: + branches: + - '*' + +jobs: + build: + runs-on: ubuntu-latest + strategy: + fail-fast: false # don't cancel if a job from the matrix fails + matrix: + compiler: + - { compiler: GNU, CC: gcc, CXX: g++, FORMAT: ON } + - { compiler: LLVM, CC: clang, CXX: clang++, FORMAT: OFF } + #flavor: [ Release, Coverage ] # TODO + flavor: [ Release ] + exclude: + - compiler: { compiler: LLVM, CC: clang, CXX: clang++ } + flavor: Coverage + + steps: + - uses: actions/checkout@v2 + with: + submodules: 'recursive' + - name: Install Dependencies + run: | + sudo apt install -y clang cmake g++ gcc lcov + - name: Fetch coveralls + if: matrix.flavor == 'Coverage' + run: | + pip install --user cpp-coveralls + + - name: Build & Run + run: | + cmake -DCMAKE_BUILD_TYPE=${{ matrix.flavor }} -DSUPPORT_STDIOSTREAM=ON -DTESTING=ON -DFORMAT=${{ matrix.compiler.FORMAT }} . + make check + + # TODO: enable +# - name: Coveralls build +# if: matrix.flavor == 'Coverage' +# run: | +# cpp-coveralls -i matrix +# - name: Coveralls +# if: matrix.flavor == 'Coverage' +# uses: coverallsapp/github-action@master +# with: +# github-token: ${{ secrets.GITHUB_TOKEN }} + diff --git a/src/lib/matrix/.gitignore b/src/lib/matrix/.gitignore new file mode 100644 index 0000000000..1edf5f525d --- /dev/null +++ b/src/lib/matrix/.gitignore @@ -0,0 +1,39 @@ +*.orig +*.swp +*~ +astyle/ +build*/ +cmake_install.cmake +CMakeCache.txt +CMakeFiles/ +compile_commands.json +CPackConfig.cmake +CPackSourceConfig.cmake +CTestTestfile.cmake +Makefile +test/attitude +test/cmake_install.cmake +test/CMakeFiles/ +test/copyto +test/coverage.info +test/CTestTestfile.cmake +test/filter +test/hatvee +test/helper +test/integration +test/inverse +test/Makefile +test/matrixAssignment +test/matrixMult +test/matrixScalarMult +test/out/ +test/pseudoinverse +test/setIdentity +test/slice +test/squareMatrix +test/transpose +test/vector +test/vector2 +test/vector3 +test/vectorAssignment +Testing/ diff --git a/src/lib/matrix/.gitmodules b/src/lib/matrix/.gitmodules new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/lib/matrix/CMakeLists.txt b/src/lib/matrix/CMakeLists.txt new file mode 100644 index 0000000000..a899d67989 --- /dev/null +++ b/src/lib/matrix/CMakeLists.txt @@ -0,0 +1,180 @@ +cmake_minimum_required(VERSION 2.8) + +set(VERSION_MAJOR "1") +set(VERSION_MINOR "0") +set(VERSION_PATCH "2") + +project(matrix CXX) + +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +if (NOT CMAKE_BUILD_TYPE) + if(TESTING) + set(CMAKE_BUILD_TYPE "Debug" CACHE STRING "Build type" FORCE) + else() + set(CMAKE_BUILD_TYPE "RelWithDebInfo" CACHE STRING "Build type" FORCE) + endif() + message(STATUS "set build type to ${CMAKE_BUILD_TYPE}") +endif() + +set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS "Debug;Release;RelWithDebInfo;MinSizeRel;Coverage") + +option(SUPPORT_STDIOSTREAM "If enabled provides support for << operator (as used with std::cout)" OFF) +option(TESTING "Enable testing" OFF) +option(FORMAT "Enable formatting" OFF) +option(COV_HTML "Display html for coverage" OFF) +option(ASAN "Enable address sanitizer" OFF) +option(UBSAN "Enable undefined behaviour sanitizer" OFF) + +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) + +include_directories(${CMAKE_SOURCE_DIR}) + +if(SUPPORT_STDIOSTREAM) + add_definitions(-DSUPPORT_STDIOSTREAM) +endif() + +if (("${CMAKE_CXX_COMPILER_ID}" MATCHES "Clang") OR ("${CMAKE_CXX_COMPILER_ID}" MATCHES "AppleClang")) + set(CMAKE_CXX_FLAGS_COVERAGE + "--coverage -ftest-coverage -fdiagnostics-absolute-paths -O0 -fprofile-arcs -fno-inline-functions -fno-elide-constructors" + CACHE STRING "Flags used by the C++ compiler during coverage builds" FORCE) + set(CMAKE_EXE_LINKER_FLAGS_COVERAGE + "-ftest-coverage -fdiagnostics-absolute-paths" + CACHE STRING "Flags used for linking binaries during coverage builds" FORCE) +else() + set(CMAKE_CXX_FLAGS_COVERAGE + # Add when GCC 9 or later is available as part of the default toolchain: -fprofile-abs-path + "--coverage -fprofile-arcs -ftest-coverage -O0 -fno-default-inline -fno-inline -fno-inline-small-functions -fno-elide-constructors" + CACHE STRING "Flags used by the C++ compiler during coverage builds" FORCE) + set(CMAKE_EXE_LINKER_FLAGS_COVERAGE + "--coverage -ftest-coverage -lgcov" + CACHE STRING "Flags used for linking binaries during coverage builds" FORCE) +endif() +mark_as_advanced(CMAKE_CXX_FLAGS_COVERAGE CMAKE_C_FLAGS_COVERAGE CMAKE_EXE_LINKER_FLAGS_COVERAGE) + +add_compile_options( + -pedantic + -Wall + -Warray-bounds + -Wcast-align + -Wcast-qual + -Wconversion + -Wctor-dtor-privacy + -Wdisabled-optimization + -Werror + -Wextra + -Wfloat-equal + -Wformat-security + -Wformat=2 + -Winit-self + -Wlogical-op + -Wmissing-declarations + -Wmissing-include-dirs + -Wno-sign-compare + -Wno-unused + -Wno-unused-parameter + -Wnoexcept + -Wold-style-cast + -Woverloaded-virtual + -Wpointer-arith + -Wredundant-decls + -Wreorder + -Wshadow + -Wsign-conversion + -Wsign-promo + -Wstrict-null-sentinel + -Wswitch-default + -Wundef + -Wuninitialized + -Wunused-variable + ) + +# clang tolerate unknown gcc options +if("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang") + add_compile_options( + -Wno-error=unused-command-line-argument-hard-error-in-future + -Wno-unknown-warning-option + ) +else() + add_compile_options( + -Wstrict-overflow=5 + ) +endif() + +# santiziers (ASAN, UBSAN) +if(ASAN) + message(STATUS "address sanitizer enabled") + + # environment variables + # ASAN_OPTIONS=detect_stack_use_after_return=1 + # ASAN_OPTIONS=check_initialization_order=1 + + add_compile_options( + -fsanitize=address + -g3 + -O1 + -fno-omit-frame-pointer + ) + + set(CMAKE_EXE_LINKER_FLAGS ${CMAKE_EXE_LINKER_FLAGS} -fsanitize=address) + +elseif(UBSAN) + message(STATUS "undefined behaviour sanitizer enabled") + + add_compile_options( + -fsanitize=undefined + -fsanitize=integer + -g3 + -O1 + -fno-omit-frame-pointer + ) + + set(CMAKE_EXE_LINKER_FLAGS ${CMAKE_EXE_LINKER_FLAGS} -fsanitize=undefined) +endif() + + +add_custom_target(check COMMAND ${CMAKE_CTEST_COMMAND} --output-on-failure) + +if(TESTING) + enable_testing() + add_subdirectory(test) + add_dependencies(check test_build) + + add_custom_target(clang-tidy COMMAND clang-tidy -p . ${CMAKE_SOURCE_DIR}/test/*.cpp) + add_dependencies(clang-tidy test_build) +endif() + +if(FORMAT) + set(astyle_exe ${CMAKE_BINARY_DIR}/astyle/src/bin/astyle) + add_custom_command(OUTPUT ${astyle_exe} + COMMAND wget http://sourceforge.net/projects/astyle/files/astyle/astyle%202.06/astyle_2.06_linux.tar.gz -O /tmp/astyle.tar.gz + COMMAND tar -xvf /tmp/astyle.tar.gz + COMMAND cd astyle/src && make -f ../build/gcc/Makefile + WORKING_DIRECTORY ${CMAKE_BINARY_DIR} + ) + + add_custom_target(check_format + COMMAND scripts/format.sh ${astyle_exe} 0 + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} + DEPENDS ${astyle_exe} + VERBATIM + ) + + add_custom_target(format + COMMAND scripts/format.sh ${astyle_exe} 1 + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} + VERBATIM + DEPENDS ${astyle_exe} + ) + + add_dependencies(check check_format) +endif() + +set(CPACK_PACKAGE_VERSION_MAJOR ${VERSION_MAJOR}) +set(CPACK_PACKAGE_VERSION_MINOR ${VERSION_MINOR}) +set(CPACK_PACKAGE_VERSION_PATCH ${VERSION_PATCH}) +set(CPACK_PACKAGE_CONTACT "james.goppert@gmail.com") +include(CPack) + +# vim: set et fenc=utf-8 ft=cmake ff=unix sts=0 sw=4 ts=4 : diff --git a/src/lib/matrix/LICENSE b/src/lib/matrix/LICENSE new file mode 100644 index 0000000000..cc64b5a484 --- /dev/null +++ b/src/lib/matrix/LICENSE @@ -0,0 +1,29 @@ +BSD 3-Clause License + +Copyright (c) 2015, PX4 Pro Drone Autopilot +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/src/lib/matrix/README.md b/src/lib/matrix/README.md new file mode 100644 index 0000000000..1cc31a5e2f --- /dev/null +++ b/src/lib/matrix/README.md @@ -0,0 +1,146 @@ +# matrix [![Build Status](https://travis-ci.org/PX4/Matrix.svg?branch=master)](https://travis-ci.org/PX4/Matrix) [![Coverage Status](https://coveralls.io/repos/PX4/Matrix/badge.svg?branch=master&service=github)](https://coveralls.io/github/PX4/Matrix?branch=master) + +A simple and efficient template based matrix library. + +## License +* (BSD) The Matrix library is licensed under a permissive 3-clause BSD license. Contributions must be made under the same license. + +## Features +* Compile time size checking. +* Euler angle (321), DCM, Quaternion conversion through constructors. +* High testing coverage. + +## Limitations +* No dynamically sized matrices. + +## Library Overview + +* matrix/math.hpp : Provides matrix math routines. + * Matrix (MxN) + * Square Matrix (MxM, has inverse) + * Vector (Mx1) + * Scalar (1x1) + * Quaternion + * Dcm + * Euler (Body 321) + * Axis Angle + +* matrix/filter.hpp : Provides filtering routines. + * kalman_correct + +* matrix/integrate.hpp : Provides integration routines. + * integrate_rk4 (Runge-Kutta 4th order) + +## Testing +The tests can be executed as following: +``` +mkdir build +cd build +cmake -DTESTING=ON .. +make check +``` + +## Formatting +The format can be checked as following: +``` +mkdir build +cd build +cmake -DFORMAT=ON .. +make check_format +``` + + +## Example + +See the test directory for detailed examples. Some simple examples are included below: + +```c++ + // define an euler angle (Body 3(yaw)-2(pitch)-1(roll) rotation) + float roll = 0.1f; + float pitch = 0.2f; + float yaw = 0.3f; + Eulerf euler(roll, pitch, yaw); + + // convert to quaternion from euler + Quatf q_nb(euler); + + // convert to DCM from quaternion + Dcmf dcm(q_nb); + + // you can assign a rotation instance that already exist to another rotation instance, e.g. + dcm = euler; + + // you can also directly create a DCM instance from euler angles like this + dcm = Eulerf(roll, pitch, yaw); + + // create an axis angle representation from euler angles + AxisAngle axis_angle = euler; + + // use axis angle to initialize a DCM + Dcmf dcm2(AxisAngle(1, 2, 3)); + + // use axis angle with axis/angle separated to init DCM + Dcmf dcm3(AxisAngle(Vector3f(1, 0, 0), 0.2)); + + // do some kalman filtering + const size_t n_x = 5; + const size_t n_y = 3; + + // define matrix sizes + SquareMatrix P; + Vector x; + Vector y; + Matrix C; + SquareMatrix R; + SquareMatrix S; + Matrix K; + + // define measurement matrix + C = zero(); // or C.setZero() + C(0,0) = 1; + C(1,1) = 2; + C(2,2) = 3; + + // set x to zero + x = zero(); // or x.setZero() + + // set P to identity * 0.01 + P = eye()*0.01; + + // set R to identity * 0.1 + R = eye()*0.1; + + // measurement + y(0) = 1; + y(1) = 2; + y(2) = 3; + + // innovation + r = y - C*x; + + // innovations variance + S = C*P*C.T() + R; + + // Kalman gain matrix + K = P*C.T()*S.I(); + // S.I() is the inverse, defined for SquareMatrix + + // correction + x += K*r; + + // slicing + float data[9] = {0, 2, 3, + 4, 5, 6, + 7, 8, 10 + }; + SquareMatrix A(data); + + // Slice a 3,3 matrix starting at row 1, col 0, + // with size 2 x 3, warning, no size checking + Matrix B(A.slice<2, 3>(1, 0)); + + // this results in: + // 4, 5, 6 + // 7, 8, 10 +``` + diff --git a/src/lib/matrix/doc/.gitignore b/src/lib/matrix/doc/.gitignore new file mode 100644 index 0000000000..87620ac7e7 --- /dev/null +++ b/src/lib/matrix/doc/.gitignore @@ -0,0 +1 @@ +.ipynb_checkpoints/ diff --git a/src/lib/matrix/doc/euler_gimbal_lock.ipynb b/src/lib/matrix/doc/euler_gimbal_lock.ipynb new file mode 100644 index 0000000000..9131c84848 --- /dev/null +++ b/src/lib/matrix/doc/euler_gimbal_lock.ipynb @@ -0,0 +1,240 @@ +{ + "cells": [ + { + "cell_type": "code", + "execution_count": 4, + "metadata": { + "collapsed": true + }, + "outputs": [], + "source": [ + "from sympy import *\n", + "init_printing()" + ] + }, + { + "cell_type": "code", + "execution_count": 16, + "metadata": { + "collapsed": false + }, + "outputs": [ + { + "data": { + "image/png": "iVBORw0KGgoAAAANSUhEUgAAA6cAAABMBAMAAABjWdyfAAAAMFBMVEX///8AAAAAAAAAAAAAAAAA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\nKMBQ2em6pwAAAABJRU5ErkJggg==\n", + "text/latex": [ + "$$\\left[\\begin{matrix}\\cos{\\left (\\theta_{1} \\right )} \\cos{\\left (\\theta_{2} \\right )} & - \\sin{\\left (\\theta_{1} \\right )} \\cos{\\left (\\theta_{3} \\right )} + \\sin{\\left (\\theta_{2} \\right )} \\sin{\\left (\\theta_{3} \\right )} \\cos{\\left (\\theta_{1} \\right )} & \\sin{\\left (\\theta_{1} \\right )} \\sin{\\left (\\theta_{3} \\right )} + \\sin{\\left (\\theta_{2} \\right )} \\cos{\\left (\\theta_{1} \\right )} \\cos{\\left (\\theta_{3} \\right )}\\\\\\sin{\\left (\\theta_{1} \\right )} \\cos{\\left (\\theta_{2} \\right )} & \\sin{\\left (\\theta_{1} \\right )} \\sin{\\left (\\theta_{2} \\right )} \\sin{\\left (\\theta_{3} \\right )} + \\cos{\\left (\\theta_{1} \\right )} \\cos{\\left (\\theta_{3} \\right )} & \\sin{\\left (\\theta_{1} \\right )} \\sin{\\left (\\theta_{2} \\right )} \\cos{\\left (\\theta_{3} \\right )} - \\sin{\\left (\\theta_{3} \\right )} \\cos{\\left (\\theta_{1} \\right )}\\\\- \\sin{\\left (\\theta_{2} \\right )} & \\sin{\\left (\\theta_{3} \\right )} \\cos{\\left (\\theta_{2} \\right )} & \\cos{\\left (\\theta_{2} \\right )} \\cos{\\left (\\theta_{3} \\right )}\\end{matrix}\\right]$$" + ], + "text/plain": [ + "⎡cos(θ₁)⋅cos(θ₂) -sin(θ₁)⋅cos(θ₃) + sin(θ₂)⋅sin(θ₃)⋅cos(θ₁) sin(θ₁)⋅sin(θ₃) \n", + "⎢ \n", + "⎢sin(θ₁)⋅cos(θ₂) sin(θ₁)⋅sin(θ₂)⋅sin(θ₃) + cos(θ₁)⋅cos(θ₃) sin(θ₁)⋅sin(θ₂)⋅\n", + "⎢ \n", + "⎣ -sin(θ₂) sin(θ₃)⋅cos(θ₂) cos\n", + "\n", + "+ sin(θ₂)⋅cos(θ₁)⋅cos(θ₃)⎤\n", + " ⎥\n", + "cos(θ₃) - sin(θ₃)⋅cos(θ₁)⎥\n", + " ⎥\n", + "(θ₂)⋅cos(θ₃) ⎦" + ] + }, + "execution_count": 16, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "th1, th2, th3 = symbols('theta_1, theta_2, theta_3')\n", + "dcm_321 = Matrix([[cos(th1)*cos(th2),\n", + " cos(th1)*sin(th2)*sin(th3) - sin(th1)*cos(th3),\n", + " cos(th1)*sin(th2)*cos(th3) + sin(th1)*sin(th3)],\n", + " [sin(th1)*cos(th2),\n", + " sin(th1)*sin(th2)*sin(th3) + cos(th1)*cos(th3),\n", + " sin(th1)*sin(th2)*cos(th3) - cos(th1)*sin(th3)],\n", + " [-sin(th2),\n", + " cos(th2)*sin(th3),\n", + " cos(th2)*cos(th3)]\n", + " ])\n", + "dcm_321" + ] + }, + { + "cell_type": "code", + "execution_count": 18, + "metadata": { + "collapsed": false + }, + "outputs": [ + { + "data": { + "image/png": "iVBORw0KGgoAAAANSUhEUgAAA1oAAABMBAMAAACG69u2AAAAMFBMVEX///8AAAAAAAAAAAAAAAAA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\nlOIAAAAASUVORK5CYII=\n", + "text/latex": [ + "$$\\left[\\begin{matrix}\\cos{\\left (\\psi \\right )} \\cos{\\left (\\theta \\right )} & \\sin{\\left (\\phi \\right )} \\sin{\\left (\\theta \\right )} \\cos{\\left (\\psi \\right )} - \\sin{\\left (\\psi \\right )} \\cos{\\left (\\phi \\right )} & \\sin{\\left (\\phi \\right )} \\sin{\\left (\\psi \\right )} + \\sin{\\left (\\theta \\right )} \\cos{\\left (\\phi \\right )} \\cos{\\left (\\psi \\right )}\\\\\\sin{\\left (\\psi \\right )} \\cos{\\left (\\theta \\right )} & \\sin{\\left (\\phi \\right )} \\sin{\\left (\\psi \\right )} \\sin{\\left (\\theta \\right )} + \\cos{\\left (\\phi \\right )} \\cos{\\left (\\psi \\right )} & - \\sin{\\left (\\phi \\right )} \\cos{\\left (\\psi \\right )} + \\sin{\\left (\\psi \\right )} \\sin{\\left (\\theta \\right )} \\cos{\\left (\\phi \\right )}\\\\- \\sin{\\left (\\theta \\right )} & \\sin{\\left (\\phi \\right )} \\cos{\\left (\\theta \\right )} & \\cos{\\left (\\phi \\right )} \\cos{\\left (\\theta \\right )}\\end{matrix}\\right]$$" + ], + "text/plain": [ + "⎡cos(ψ)⋅cos(θ) sin(φ)⋅sin(θ)⋅cos(ψ) - sin(ψ)⋅cos(φ) sin(φ)⋅sin(ψ) + sin(θ)⋅c\n", + "⎢ \n", + "⎢sin(ψ)⋅cos(θ) sin(φ)⋅sin(ψ)⋅sin(θ) + cos(φ)⋅cos(ψ) -sin(φ)⋅cos(ψ) + sin(ψ)⋅\n", + "⎢ \n", + "⎣ -sin(θ) sin(φ)⋅cos(θ) cos(φ)⋅cos(θ\n", + "\n", + "os(φ)⋅cos(ψ) ⎤\n", + " ⎥\n", + "sin(θ)⋅cos(φ)⎥\n", + " ⎥\n", + ") ⎦" + ] + }, + "execution_count": 18, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "phi, theta, psi = symbols('phi, theta, psi')\n", + "sub_eulers = {\n", + " th1: psi, th2: theta, th3: phi\n", + "}\n", + "dcm_321_euler = dcm_321.subs(sub_eulers)\n", + "dcm_321_euler" + ] + }, + { + "cell_type": "code", + "execution_count": 38, + "metadata": { + "collapsed": false + }, + "outputs": [ + { + "data": { + "image/png": "iVBORw0KGgoAAAANSUhEUgAAAOAAAABLCAMAAABeKHv+AAAAPFBMVEX///8AAAAAAAAAAAAAAAAA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\nRK5CYII=\n", + "text/latex": [ + "$$\\left[\\begin{matrix}0 & - \\sin{\\left (\\psi \\right )} & \\cos{\\left (\\psi \\right )}\\\\0 & \\cos{\\left (\\psi \\right )} & \\sin{\\left (\\psi \\right )}\\\\-1 & 0 & 0\\end{matrix}\\right]$$" + ], + "text/plain": [ + "⎡0 -sin(ψ) cos(ψ)⎤\n", + "⎢ ⎥\n", + "⎢0 cos(ψ) sin(ψ)⎥\n", + "⎢ ⎥\n", + "⎣-1 0 0 ⎦" + ] + }, + "execution_count": 38, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "dcm_pitch_up = dcm_321_euler.subs({theta:pi/2, phi:0})\n", + "dcm_pitch_up" + ] + }, + { + "cell_type": "code", + "execution_count": 39, + "metadata": { + "collapsed": false + }, + "outputs": [ + { + "data": { + "image/png": "iVBORw0KGgoAAAANSUhEUgAAAMUAAAAVBAMAAAAEKDfsAAAAMFBMVEX///8AAAAAAAAAAAAAAAAA\nAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAv3aB7AAAAD3RSTlMAZnaJVN0imavvMkQQ\nzbsZbzSbAAAACXBIWXMAAA7EAAAOxAGVKw4bAAADfUlEQVRIDcVVS4gUVxQ91VVdM139mWYWGvLB\n0iEhi4ilJo6QQJqQ4CLgNCi69AlukoUZxi9Z6JAsEtzYQYkhcdGKgQwmTEMSQTdT6MIPiE0yiwTF\ntLNQAjFOxC8K47n31fQ4XW22Xrivzjv3njrvUz0DPO84klpAUBYqAJx6qjZLWBXrL5KR/L/wW1od\nC+eatip0Oe6bI+ejRBUBvXWbtp6f35bMirGCcbbPxhUF4lGcpTqfiUpEzSS15Wxno84HUmzG2m1m\nIYGpFliVX2ZFYPslC9OtZL5OsTmj1AKOhVqqagmrcgxn55OUgndfxs7IVy3zcb1d+dUi3c1km50H\nEpUrZLFis38gxAt3v6xAwE+Hjr4eSlXCHQaOvrfUvzUa/Pnpks+EGuT6B0NEGdoTa5xd+SYKKwYa\n0ktCVGdG4OJVizn3NmA3sJF7UXA19O9RfiNmd6mMQgtZlEZRnMInhtQ16rxpv5J5DLzCOcM/iDHz\nvcGU9qoK0RC2YFkMp8mPvAnvW0zE6qHgb+A/uKaXRWQryB/mguhRqqGvTGo9sKWn6sSoAR9wzig2\nMVRfB4wfl15V+ZUdeBk5rrtqMxjYa8QDCm4Ct5E1ATeDXAPYNGXUo4m+kNR3QJyL+KphvpRzxiXC\n4A7QF0mvqgo4gAg95Kc1C1fNeF08LPhXPHIN/5Htxpm/7qtHq+2BCeOiELY9Jgg9tve1pNeuLH+P\nHqfbHsUqxn+MNyK0QD0o4nnLWfkGixpyVokHzwpv8azdGEi+sfY+dkqvquBW/QreAeQbY16KMHS8\n/gciCxKPbMiX8c69CL3lpzx459jDs/6Qz+TOeR+oHAImfpFeVaGn5cQe/ZI7z7Xw7pF4ByILEo9B\nvgPuKLzDKBluYnYfq0mfROhQj128C77f/wquGavjd+1VFfK1DF5iS8+wpv/FktO/ofdzo+CHmX1r\nH47CYU13mV+++O1g/4ML+x8cu/UNKTkfb/u116R8meciR3puZBX8basb0sup/gbXrO03xEWuRbJb\n/IxTQm9I1bKGVFAR3q9xOCGoI1R1TMmPOEp2iWDy/X+EXpiq6QYzsfBuyKEsqCNU9YaSKzhKdonc\nzMy00CWTKl4no3+KcJGo0M1DVZEoC02bgp8VfitV2Uomo6y4Oak6CVWFUhG3LuuU0lxsnoMJ0n8L\ngh2TMOlHW7WANeYTnXDuNRzecvUAAAAASUVORK5CYII=\n", + "text/latex": [ + "$$\\operatorname{atan_{2}}{\\left (\\sin{\\left (\\psi \\right )},\\cos{\\left (\\psi \\right )} \\right )}$$" + ], + "text/plain": [ + "atan2(sin(ψ), cos(ψ))" + ] + }, + "execution_count": 39, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "atan2(dcm_pitch_up[1,2], dcm_pitch_up[0,2])" + ] + }, + { + "cell_type": "code", + "execution_count": 40, + "metadata": { + "collapsed": false + }, + "outputs": [ + { + "data": { + "image/png": "iVBORw0KGgoAAAANSUhEUgAAAOAAAABLCAMAAABeKHv+AAAAPFBMVEX///8AAAAAAAAAAAAAAAAA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\nRK5CYII=\n", + "text/latex": [ + "$$\\left[\\begin{matrix}0 & - \\sin{\\left (\\psi \\right )} & \\cos{\\left (\\psi \\right )}\\\\0 & \\cos{\\left (\\psi \\right )} & \\sin{\\left (\\psi \\right )}\\\\-1 & 0 & 0\\end{matrix}\\right]$$" + ], + "text/plain": [ + "⎡0 -sin(ψ) cos(ψ)⎤\n", + "⎢ ⎥\n", + "⎢0 cos(ψ) sin(ψ)⎥\n", + "⎢ ⎥\n", + "⎣-1 0 0 ⎦" + ] + }, + "execution_count": 40, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "dcm_pitch_down = dcm_321_euler.subs({theta:pi/2, phi:0})\n", + "dcm_pitch_down" + ] + }, + { + "cell_type": "code", + "execution_count": 41, + "metadata": { + "collapsed": false + }, + "outputs": [ + { + "data": { + "image/png": "iVBORw0KGgoAAAANSUhEUgAAAMUAAAAVBAMAAAAEKDfsAAAAMFBMVEX///8AAAAAAAAAAAAAAAAA\nAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAv3aB7AAAAD3RSTlMAZnaJVN0imavvMkQQ\nzbsZbzSbAAAACXBIWXMAAA7EAAAOxAGVKw4bAAADfUlEQVRIDcVVS4gUVxQ91VVdM139mWYWGvLB\n0iEhi4ilJo6QQJqQ4CLgNCi69AlukoUZxi9Z6JAsEtzYQYkhcdGKgQwmTEMSQTdT6MIPiE0yiwTF\ntLNQAjFOxC8K47n31fQ4XW22Xrivzjv3njrvUz0DPO84klpAUBYqAJx6qjZLWBXrL5KR/L/wW1od\nC+eatip0Oe6bI+ejRBUBvXWbtp6f35bMirGCcbbPxhUF4lGcpTqfiUpEzSS15Wxno84HUmzG2m1m\nIYGpFliVX2ZFYPslC9OtZL5OsTmj1AKOhVqqagmrcgxn55OUgndfxs7IVy3zcb1d+dUi3c1km50H\nEpUrZLFis38gxAt3v6xAwE+Hjr4eSlXCHQaOvrfUvzUa/Pnpks+EGuT6B0NEGdoTa5xd+SYKKwYa\n0ktCVGdG4OJVizn3NmA3sJF7UXA19O9RfiNmd6mMQgtZlEZRnMInhtQ16rxpv5J5DLzCOcM/iDHz\nvcGU9qoK0RC2YFkMp8mPvAnvW0zE6qHgb+A/uKaXRWQryB/mguhRqqGvTGo9sKWn6sSoAR9wzig2\nMVRfB4wfl15V+ZUdeBk5rrtqMxjYa8QDCm4Ct5E1ATeDXAPYNGXUo4m+kNR3QJyL+KphvpRzxiXC\n4A7QF0mvqgo4gAg95Kc1C1fNeF08LPhXPHIN/5Htxpm/7qtHq+2BCeOiELY9Jgg9tve1pNeuLH+P\nHqfbHsUqxn+MNyK0QD0o4nnLWfkGixpyVokHzwpv8azdGEi+sfY+dkqvquBW/QreAeQbY16KMHS8\n/gciCxKPbMiX8c69CL3lpzx459jDs/6Qz+TOeR+oHAImfpFeVaGn5cQe/ZI7z7Xw7pF4ByILEo9B\nvgPuKLzDKBluYnYfq0mfROhQj128C77f/wquGavjd+1VFfK1DF5iS8+wpv/FktO/ofdzo+CHmX1r\nH47CYU13mV+++O1g/4ML+x8cu/UNKTkfb/u116R8meciR3puZBX8basb0sup/gbXrO03xEWuRbJb\n/IxTQm9I1bKGVFAR3q9xOCGoI1R1TMmPOEp2iWDy/X+EXpiq6QYzsfBuyKEsqCNU9YaSKzhKdonc\nzMy00CWTKl4no3+KcJGo0M1DVZEoC02bgp8VfitV2Uomo6y4Oak6CVWFUhG3LuuU0lxsnoMJ0n8L\ngh2TMOlHW7WANeYTnXDuNRzecvUAAAAASUVORK5CYII=\n", + "text/latex": [ + "$$\\operatorname{atan_{2}}{\\left (\\sin{\\left (\\psi \\right )},\\cos{\\left (\\psi \\right )} \\right )}$$" + ], + "text/plain": [ + "atan2(sin(ψ), cos(ψ))" + ] + }, + "execution_count": 41, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "dcm_pitch_down = dcm_321_euler.subs({theta:-pi/2, phi:0})\n", + "atan2(-dcm_pitch_down[1,2], -dcm_pitch_down[0,2])" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "Python 2", + "language": "python", + "name": "python2" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 2 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython2", + "version": "2.7.6" + } + }, + "nbformat": 4, + "nbformat_minor": 0 +} diff --git a/src/lib/matrix/doc/nasa_rotation_def.pdf b/src/lib/matrix/doc/nasa_rotation_def.pdf new file mode 100644 index 0000000000..eb67a4bfcf Binary files /dev/null and b/src/lib/matrix/doc/nasa_rotation_def.pdf differ diff --git a/src/lib/matrix/matrix/AxisAngle.hpp b/src/lib/matrix/matrix/AxisAngle.hpp new file mode 100644 index 0000000000..41a9711541 --- /dev/null +++ b/src/lib/matrix/matrix/AxisAngle.hpp @@ -0,0 +1,160 @@ +/** + * @file AxisAngle.hpp + * + * @author James Goppert + */ + +#pragma once + +#include "math.hpp" + +namespace matrix +{ + +template +class Dcm; + +template +class Euler; + +template +class AxisAngle; + +/** + * AxisAngle class + * + * The rotation between two coordinate frames is + * described by this class. + */ +template +class AxisAngle : public Vector +{ +public: + using Matrix31 = Matrix; + + /** + * Constructor from array + * + * @param data_ array + */ + explicit AxisAngle(const Type data_[3]) : + Vector(data_) + { + } + + /** + * Standard constructor + */ + AxisAngle() = default; + + /** + * Constructor from Matrix31 + * + * @param other Matrix31 to copy + */ + AxisAngle(const Matrix31 &other) : + Vector(other) + { + } + + /** + * Constructor from quaternion + * + * This sets the instance from a quaternion representing coordinate transformation from + * frame 2 to frame 1 where the rotation from frame 1 to frame 2 is described + * by a 3-2-1 intrinsic Tait-Bryan rotation sequence. + * + * @param q quaternion + */ + AxisAngle(const Quaternion &q) + { + AxisAngle &v = *this; + Type mag = q.imag().norm(); + if (fabs(mag) >= Type(1e-10)) { + v = q.imag() * Type(Type(2) * atan2(mag, q(0)) / mag); + } else { + v = q.imag() * Type(Type(2) * Type(sign(q(0)))); + } + } + + /** + * Constructor from dcm + * + * Instance is initialized from a dcm representing coordinate transformation + * from frame 2 to frame 1. + * + * @param dcm dcm to set quaternion to + */ + AxisAngle(const Dcm &dcm) + { + AxisAngle &v = *this; + v = AxisAngle(Quaternion(dcm)); + } + + /** + * Constructor from euler angles + * + * This sets the instance to a quaternion representing coordinate transformation from + * frame 2 to frame 1 where the rotation from frame 1 to frame 2 is described + * by a 3-2-1 intrinsic Tait-Bryan rotation sequence. + * + * @param euler euler angle instance + */ + AxisAngle(const Euler &euler) + { + AxisAngle &v = *this; + v = AxisAngle(Quaternion(euler)); + } + + /** + * Constructor from 3 axis angle values (unit vector * angle) + * + * @param x r_x*angle + * @param y r_y*angle + * @param z r_z*angle + */ + AxisAngle(Type x, Type y, Type z) + { + AxisAngle &v = *this; + v(0) = x; + v(1) = y; + v(2) = z; + } + + /** + * Constructor from axis and angle + * + * @param axis An axis of rotation, normalized if not unit length + * @param angle The amount to rotate + */ + AxisAngle(const Matrix31 & axis_, Type angle_) + { + AxisAngle &v = *this; + // make sure axis is a unit vector + Vector a = axis_; + a = a.unit(); + v(0) = a(0)*angle_; + v(1) = a(1)*angle_; + v(2) = a(2)*angle_; + } + + + Vector axis() { + if (Vector::norm() > 0) { + return Vector::unit(); + } else { + return Vector3(1, 0, 0); + } + } + + Type angle() { + return Vector::norm(); + } +}; + +using AxisAnglef = AxisAngle; +using AxisAngled = AxisAngle; + +} // namespace matrix + +/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */ diff --git a/src/lib/matrix/matrix/Dcm.hpp b/src/lib/matrix/matrix/Dcm.hpp new file mode 100644 index 0000000000..0e1cf49960 --- /dev/null +++ b/src/lib/matrix/matrix/Dcm.hpp @@ -0,0 +1,187 @@ +/** + * @file Dcm.hpp + * + * A direction cosine matrix class. + * All rotations and axis systems follow the right-hand rule. + * + * This library uses the convention that premultiplying a three dimensional + * vector represented in coordinate system 1 will apply a rotation from coordinate system + * 1 to coordinate system 2 to the vector. + * Likewise, a matrix instance of this class also represents a coordinate transformation + * from frame 2 to frame 1. + * + * @author James Goppert + */ + +#pragma once + +#include "math.hpp" + +namespace matrix +{ + +template +class Quaternion; + +template +class Euler; + +template +class AxisAngle; + +/** + * Direction cosine matrix class + * + * The rotation between two coordinate frames is + * described by this class. + */ +template +class Dcm : public SquareMatrix +{ +public: + using Vector3 = Matrix; + + /** + * Standard constructor + * + * Initializes to identity + */ + Dcm() : SquareMatrix(eye()) {} + + /** + * Constructor from array + * + * @param _data pointer to array + */ + explicit Dcm(const Type data_[3][3]) : SquareMatrix(data_) + { + } + + /** + * Constructor from array + * + * @param _data pointer to array + */ + explicit Dcm(const Type data_[9]) : SquareMatrix(data_) + { + } + + /** + * Copy constructor + * + * @param other Matrix33 to set dcm to + */ + Dcm(const Matrix &other) : SquareMatrix(other) + { + } + + /** + * Constructor from quaternion + * + * Instance is initialized from quaternion representing + * coordinate transformation from frame 2 to frame 1. + * + * @param q quaternion to set dcm to + */ + Dcm(const Quaternion &q) + { + Dcm &dcm = *this; + const Type a = q(0); + const Type b = q(1); + const Type c = q(2); + const Type d = q(3); + const Type aa = a * a; + const Type ab = a * b; + const Type ac = a * c; + const Type ad = a * d; + const Type bb = b * b; + const Type bc = b * c; + const Type bd = b * d; + const Type cc = c * c; + const Type cd = c * d; + const Type dd = d * d; + dcm(0, 0) = aa + bb - cc - dd; + dcm(0, 1) = Type(2) * (bc - ad); + dcm(0, 2) = Type(2) * (ac + bd); + dcm(1, 0) = Type(2) * (bc + ad); + dcm(1, 1) = aa - bb + cc - dd; + dcm(1, 2) = Type(2) * (cd - ab); + dcm(2, 0) = Type(2) * (bd - ac); + dcm(2, 1) = Type(2) * (ab + cd); + dcm(2, 2) = aa - bb - cc + dd; + } + + /** + * Constructor from euler angles + * + * This sets the transformation matrix from frame 2 to frame 1 where the rotation + * from frame 1 to frame 2 is described by a 3-2-1 intrinsic Tait-Bryan rotation sequence. + * + * + * @param euler euler angle instance + */ + Dcm(const Euler &euler) + { + Dcm &dcm = *this; + Type cosPhi = Type(cos(euler.phi())); + Type sinPhi = Type(sin(euler.phi())); + Type cosThe = Type(cos(euler.theta())); + Type sinThe = Type(sin(euler.theta())); + Type cosPsi = Type(cos(euler.psi())); + Type sinPsi = Type(sin(euler.psi())); + + dcm(0, 0) = cosThe * cosPsi; + dcm(0, 1) = -cosPhi * sinPsi + sinPhi * sinThe * cosPsi; + dcm(0, 2) = sinPhi * sinPsi + cosPhi * sinThe * cosPsi; + + dcm(1, 0) = cosThe * sinPsi; + dcm(1, 1) = cosPhi * cosPsi + sinPhi * sinThe * sinPsi; + dcm(1, 2) = -sinPhi * cosPsi + cosPhi * sinThe * sinPsi; + + dcm(2, 0) = -sinThe; + dcm(2, 1) = sinPhi * cosThe; + dcm(2, 2) = cosPhi * cosThe; + } + + + /** + * Constructor from axis angle + * + * This sets the transformation matrix from frame 2 to frame 1 where the rotation + * from frame 1 to frame 2 is described by a 3-2-1 intrinsic Tait-Bryan rotation sequence. + * + * + * @param euler euler angle instance + */ + Dcm(const AxisAngle &aa) + { + Dcm &dcm = *this; + dcm = Quaternion(aa); + } + + Vector vee() const // inverse to Vector.hat() operation + { + const Dcm &A(*this); + Vector v; + v(0) = -A(1, 2); + v(1) = A(0, 2); + v(2) = -A(0, 1); + return v; + } + + void renormalize() + { + /* renormalize rows */ + for (size_t r = 0; r < 3; r++) { + matrix::Vector3 rvec(Matrix(this->Matrix::row(r)).transpose()); + this->Matrix::row(r) = rvec.normalized(); + } + } +}; + +using Dcmf = Dcm; +using Dcmd = Dcm; + +} // namespace matrix + +/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */ diff --git a/src/lib/matrix/matrix/Dual.hpp b/src/lib/matrix/matrix/Dual.hpp new file mode 100644 index 0000000000..da8eabf9e8 --- /dev/null +++ b/src/lib/matrix/matrix/Dual.hpp @@ -0,0 +1,373 @@ +/** + * @file Dual.hpp + * + * This is a dual number type for calculating automatic derivatives. + * + * Based roughly on the methods described in: + * Automatic Differentiation, C++ Templates and Photogrammetry, by Dan Piponi + * and + * Ceres Solver's excellent Jet.h + * + * @author Julian Kent + */ + +#pragma once + +#include "math.hpp" + +namespace matrix +{ + +template +class Vector; + +template +struct Dual +{ + static constexpr size_t WIDTH = N; + + Scalar value {}; + Vector derivative; + + Dual() = default; + + explicit Dual(Scalar v, size_t inputDimension = 65535) + { + value = v; + if (inputDimension < N) { + derivative(inputDimension) = Scalar(1); + } + } + + explicit Dual(Scalar v, const Vector& d) : + value(v), derivative(d) + {} + + Dual& operator=(const Scalar& a) + { + derivative.setZero(); + value = a; + return *this; + } + + Dual& operator +=(const Dual& a) + { + return (*this = *this + a); + } + + Dual& operator *=(const Dual& a) + { + return (*this = *this * a); + } + + Dual& operator -=(const Dual& a) + { + return (*this = *this - a); + } + + Dual& operator /=(const Dual& a) + { + return (*this = *this / a); + } + + Dual& operator +=(Scalar a) + { + return (*this = *this + a); + } + + Dual& operator -=(Scalar a) + { + return (*this = *this - a); + } + + Dual& operator *=(Scalar a) + { + return (*this = *this * a); + } + + Dual& operator /=(Scalar a) + { + return (*this = *this / a); + } + +}; + +// operators + +template +Dual operator+(const Dual& a) +{ + return a; +} + +template +Dual operator-(const Dual& a) +{ + return Dual(-a.value, -a.derivative); +} + +template +Dual operator+(const Dual& a, const Dual& b) +{ + return Dual(a.value + b.value, a.derivative + b.derivative); +} + +template +Dual operator-(const Dual& a, const Dual& b) +{ + return a + (-b); +} + +template +Dual operator+(const Dual& a, Scalar b) +{ + return Dual(a.value + b, a.derivative); +} + +template +Dual operator-(const Dual& a, Scalar b) +{ + return a + (-b); +} + +template +Dual operator+(Scalar a, const Dual& b) +{ + return Dual(a + b.value, b.derivative); +} + +template +Dual operator-(Scalar a, const Dual& b) +{ + return a + (-b); +} + +template +Dual operator*(const Dual& a, const Dual& b) +{ + return Dual(a.value * b.value, a.value * b.derivative + b.value * a.derivative); +} + +template +Dual operator*(const Dual& a, Scalar b) +{ + return Dual(a.value * b, a.derivative * b); +} + +template +Dual operator*(Scalar a, const Dual& b) +{ + return b * a; +} + +template +Dual operator/(const Dual& a, const Dual& b) +{ + const Scalar inv_b_real = Scalar(1) / b.value; + return Dual(a.value * inv_b_real, a.derivative * inv_b_real - + a.value * b.derivative * inv_b_real * inv_b_real); +} + +template +Dual operator/(const Dual& a, Scalar b) +{ + return a * (Scalar(1) / b); +} + +template +Dual operator/(Scalar a, const Dual& b) +{ + const Scalar inv_b_real = Scalar(1) / b.value; + return Dual(a * inv_b_real, (-inv_b_real * a * inv_b_real) * b.derivative); +} + +// basic math + +// sqrt +template +Dual sqrt(const Dual& a) +{ + Scalar real = sqrt(a.value); + return Dual(real, a.derivative * (Scalar(1) / (Scalar(2) * real))); +} + +// abs +template +Dual abs(const Dual& a) +{ + return a.value >= Scalar(0) ? a : -a; +} + +// ceil +template +Dual ceil(const Dual& a) +{ + return Dual(ceil(a.value)); +} + +// floor +template +Dual floor(const Dual& a) +{ + return Dual(floor(a.value)); +} + +// fmod +template +Dual fmod(const Dual& a, Scalar mod) +{ + return Dual(a.value - Scalar(size_t(a.value / mod)) * mod, a.derivative); +} + +// max +template +Dual max(const Dual& a, const Dual& b) +{ + return a.value >= b.value ? a : b; +} + +// min +template +Dual min(const Dual& a, const Dual& b) +{ + return a.value < b.value ? a : b; +} + +// isnan +template +bool IsNan(Scalar a) +{ + return isnan(a); +} + +template +bool IsNan(const Dual& a) +{ + return IsNan(a.value); +} + +// isfinite +template +bool IsFinite(Scalar a) +{ + return isfinite(a); +} + +template +bool IsFinite(const Dual& a) +{ + return IsFinite(a.value); +} + +// isinf +template +bool IsInf(Scalar a) +{ + return isinf(a); +} + +template +bool IsInf(const Dual& a) +{ + return IsInf(a.value); +} + +// trig + +// sin +template +Dual sin(const Dual& a) +{ + return Dual(sin(a.value), cos(a.value) * a.derivative); +} + +// cos +template +Dual cos(const Dual& a) +{ + return Dual(cos(a.value), -sin(a.value) * a.derivative); +} + +// tan +template +Dual tan(const Dual& a) +{ + Scalar real = tan(a.value); + return Dual(real, (Scalar(1) + real * real) * a.derivative); +} + +// asin +template +Dual asin(const Dual& a) +{ + Scalar asin_d = Scalar(1) / sqrt(Scalar(1) - a.value * a.value); + return Dual(asin(a.value), asin_d * a.derivative); +} + +// acos +template +Dual acos(const Dual& a) +{ + Scalar acos_d = -Scalar(1) / sqrt(Scalar(1) - a.value * a.value); + return Dual(acos(a.value), acos_d * a.derivative); +} + +// atan +template +Dual atan(const Dual& a) +{ + Scalar atan_d = Scalar(1) / (Scalar(1) + a.value * a.value); + return Dual(atan(a.value), atan_d * a.derivative); +} + +// atan2 +template +Dual atan2(const Dual& a, const Dual& b) +{ + // derivative is equal to that of atan(a/b), so substitute a/b into atan and simplify + Scalar atan_d = Scalar(1) / (a.value * a.value + b.value * b.value); + return Dual(atan2(a.value, b.value), (a.derivative * b.value - a.value * b.derivative) * atan_d); +} + +// retrieve the derivative elements of a vector of Duals into a matrix +template +Matrix collectDerivatives(const Matrix, M, 1>& input) +{ + Matrix jac; + for (size_t i = 0; i < M; i++) { + jac.row(i) = input(i, 0).derivative; + } + return jac; +} + +// retrieve the real (non-derivative) elements of a matrix of Duals into an equally sized matrix +template +Matrix collectReals(const Matrix, M, N>& input) +{ + Matrix r; + for (size_t i = 0; i < M; i++) { + for (size_t j = 0; j < N; j++) { + r(i,j) = input(i,j).value; + } + } + return r; +} + +#if defined(SUPPORT_STDIOSTREAM) +template +std::ostream& operator<<(std::ostream& os, + const matrix::Dual& dual) +{ + os << "["; + os << std::setw(10) << dual.value << ";"; + for (size_t j = 0; j < N; ++j) { + os << "\t"; + os << std::setw(10) << static_cast(dual.derivative(j)); + } + os << "]"; + return os; +} +#endif // defined(SUPPORT_STDIOSTREAM) + +} + diff --git a/src/lib/matrix/matrix/Euler.hpp b/src/lib/matrix/matrix/Euler.hpp new file mode 100644 index 0000000000..6145cffd38 --- /dev/null +++ b/src/lib/matrix/matrix/Euler.hpp @@ -0,0 +1,157 @@ +/** + * @file Euler.hpp + * + * All rotations and axis systems follow the right-hand rule + * + * An instance of this class defines a rotation from coordinate frame 1 to coordinate frame 2. + * It follows the convention of a 3-2-1 intrinsic Tait-Bryan rotation sequence. + * In order to go from frame 1 to frame 2 we apply the following rotations consecutively. + * 1) We rotate about our initial Z axis by an angle of _psi. + * 2) We rotate about the newly created Y' axis by an angle of _theta. + * 3) We rotate about the newly created X'' axis by an angle of _phi. + * + * @author James Goppert + */ + +#pragma once + +#include "math.hpp" + +namespace matrix +{ + +template +class Dcm; + +template +class Quaternion; + +/** + * Euler angles class + * + * This class describes the rotation from frame 1 + * to frame 2 via 3-2-1 intrinsic Tait-Bryan rotation sequence. + */ +template +class Euler : public Vector +{ +public: + /** + * Standard constructor + */ + Euler() = default; + + /** + * Copy constructor + * + * @param other vector to copy + */ + Euler(const Vector &other) : + Vector(other) + { + } + + /** + * Constructor from Matrix31 + * + * @param other Matrix31 to copy + */ + Euler(const Matrix &other) : + Vector(other) + { + } + + /** + * Constructor from euler angles + * + * Instance is initialized from an 3-2-1 intrinsic Tait-Bryan + * rotation sequence representing transformation from frame 1 + * to frame 2. + * + * @param phi_ rotation angle about X axis + * @param theta_ rotation angle about Y axis + * @param psi_ rotation angle about Z axis + */ + Euler(Type phi_, Type theta_, Type psi_) : Vector() + { + phi() = phi_; + theta() = theta_; + psi() = psi_; + } + + /** + * Constructor from DCM matrix + * + * Instance is set from Dcm representing transformation from + * frame 2 to frame 1. + * This instance will hold the angles defining the 3-2-1 intrinsic + * Tait-Bryan rotation sequence from frame 1 to frame 2. + * + * @param dcm Direction cosine matrix + */ + Euler(const Dcm &dcm) + { + theta() = asin(-dcm(2, 0)); + + if ((fabs(theta() - Type(M_PI / 2))) < Type(1.0e-3)) { + phi() = 0; + psi() = atan2(dcm(1, 2), dcm(0, 2)); + + } else if ((fabs(theta() + Type(M_PI / 2))) < Type(1.0e-3)) { + phi() = 0; + psi() = atan2(-dcm(1, 2), -dcm(0, 2)); + + } else { + phi() = atan2(dcm(2, 1), dcm(2, 2)); + psi() = atan2(dcm(1, 0), dcm(0, 0)); + } + } + + /** + * Constructor from quaternion instance. + * + * Instance is set from a quaternion representing transformation + * from frame 2 to frame 1. + * This instance will hold the angles defining the 3-2-1 intrinsic + * Tait-Bryan rotation sequence from frame 1 to frame 2. + * + * @param q quaternion + */ + Euler(const Quaternion &q) : Vector(Euler(Dcm(q))) + { + } + + inline Type phi() const + { + return (*this)(0); + } + inline Type theta() const + { + return (*this)(1); + } + inline Type psi() const + { + return (*this)(2); + } + + inline Type &phi() + { + return (*this)(0); + } + inline Type &theta() + { + return (*this)(1); + } + inline Type &psi() + { + return (*this)(2); + } + +}; + +using Eulerf = Euler; +using Eulerd = Euler; + +} // namespace matrix + +/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */ diff --git a/src/lib/matrix/matrix/LeastSquaresSolver.hpp b/src/lib/matrix/matrix/LeastSquaresSolver.hpp new file mode 100644 index 0000000000..1f518b4d69 --- /dev/null +++ b/src/lib/matrix/matrix/LeastSquaresSolver.hpp @@ -0,0 +1,146 @@ +/** + * @file LeastSquaresSolver.hpp + * + * Least Squares Solver using QR householder decomposition. + * It calculates x for Ax = b. + * A = Q*R + * where R is an upper triangular matrix. + * + * R*x = Q^T*b + * This is efficiently solved for x because of the upper triangular property of R. + * + * @author Bart Slinger + */ + +#pragma once + +#include "math.hpp" + +namespace matrix { + +template +class LeastSquaresSolver +{ +public: + + /** + * @brief Class calculates QR decomposition which can be used for linear + * least squares + * @param A Matrix of size MxN + * + * Initialize the class with a MxN matrix. The constructor starts the + * QR decomposition. This class does not check the rank of the matrix. + * The user needs to make sure that rank(A) = N and M >= N. + */ + LeastSquaresSolver(const Matrix &A) + { + static_assert(M >= N, "Matrix dimension should be M >= N"); + + // Copy contentents of matrix A + _A = A; + + for (size_t j = 0; j < N; j++) { + Type normx = Type(0); + for (size_t i = j; i < M; i++) { + normx += _A(i,j) * _A(i,j); + } + normx = sqrt(normx); + Type s = _A(j,j) > 0 ? Type(-1) : Type(1); + Type u1 = _A(j,j) - s*normx; + // prevent divide by zero + // also covers u1. normx is never negative + if (normx < Type(1e-8)) { + break; + } + Type w[M] = {}; + w[0] = Type(1); + for (size_t i = j+1; i < M; i++) { + w[i-j] = _A(i,j) / u1; + _A(i,j) = w[i-j]; + } + _A(j,j) = s*normx; + _tau(j) = -s*u1/normx; + + for (size_t k = j+1; k < N; k++) { + Type tmp = Type(0); + for (size_t i = j; i < M; i++) { + tmp += w[i-j] * _A(i,k); + } + for (size_t i = j; i < M; i++) { + _A(i,k) -= _tau(j) * w[i-j] * tmp; + } + } + + } + } + + /** + * @brief qtb Calculate Q^T * b + * @param b + * @return Q^T*b + * + * This function calculates Q^T * b. This is useful for the solver + * because R*x = Q^T*b. + */ + Vector qtb(const Vector &b) { + Vector qtbv = b; + + for (size_t j = 0; j < N; j++) { + Type w[M]; + w[0] = Type(1); + // fill vector w + for (size_t i = j+1; i < M; i++) { + w[i-j] = _A(i,j); + } + Type tmp = Type(0); + for (size_t i = j; i < M; i++) { + tmp += w[i-j] * qtbv(i); + } + + for (size_t i = j; i < M; i++) { + qtbv(i) -= _tau(j) * w[i-j] * tmp; + } + } + return qtbv; + } + + /** + * @brief Solve Ax=b for x + * @param b + * @return Vector x + * + * Find x in the equation Ax = b. + * A is provided in the initializer of the class. + */ + Vector solve(const Vector &b) { + Vector qtbv = qtb(b); + Vector x; + + // size_t is unsigned and wraps i = 0 - 1 to i > N + for (size_t i = N - 1; i < N; i--) { + printf("i %d\n", static_cast(i)); + x(i) = qtbv(i); + for (size_t r = i+1; r < N; r++) { + x(i) -= _A(i,r) * x(r); + } + // divide by zero, return vector of zeros + if (isEqualF(_A(i,i), Type(0), Type(1e-8))) { + for (size_t z = 0; z < N; z++) { + x(z) = Type(0); + } + break; + } + x(i) /= _A(i,i); + } + return x; + } + +private: + Matrix _A; + Vector _tau; + +}; + +} // namespace matrix + +/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */ diff --git a/src/lib/matrix/matrix/Matrix.hpp b/src/lib/matrix/matrix/Matrix.hpp new file mode 100644 index 0000000000..2b865f4ab3 --- /dev/null +++ b/src/lib/matrix/matrix/Matrix.hpp @@ -0,0 +1,764 @@ +/** + * @file Matrix.hpp + * + * A simple matrix template library. + * + * @author James Goppert + */ + +#pragma once + +#include +#include + +#if defined(SUPPORT_STDIOSTREAM) +#include +#include +#endif // defined(SUPPORT_STDIOSTREAM) + +#include "math.hpp" + +namespace matrix +{ + +template +class Vector; + +template +class Matrix; + +template +class Slice; + +template +class Matrix +{ + Type _data[M][N] {}; + +public: + + // Constructors + Matrix() = default; + + explicit Matrix(const Type data_[M*N]) + { + for (size_t i = 0; i < M; i++) { + for (size_t j = 0; j < N; j++) { + _data[i][j] = data_[N*i + j]; + } + } + } + + explicit Matrix(const Type data_[M][N]) + { + for (size_t i = 0; i < M; i++) { + for (size_t j = 0; j < N; j++) { + _data[i][j] = data_[i][j]; + } + } + } + + Matrix(const Matrix &other) + { + for (size_t i = 0; i < M; i++) { + for (size_t j = 0; j < N; j++) { + _data[i][j] = other(i, j); + } + } + } + + template + Matrix(const Slice& in_slice) + { + Matrix& self = *this; + for (size_t i = 0; i < M; i++) { + for (size_t j = 0; j < N; j++) { + self(i, j) = in_slice(i, j); + } + } + } + + /** + * Accessors/ Assignment etc. + */ + + + inline const Type &operator()(size_t i, size_t j) const + { + assert(i < M); + assert(j < N); + + return _data[i][j]; + } + + inline Type &operator()(size_t i, size_t j) + { + assert(i < M); + assert(j < N); + + return _data[i][j]; + } + + Matrix & operator=(const Matrix &other) + { + if (this != &other) { + Matrix &self = *this; + for (size_t i = 0; i < M; i++) { + for (size_t j = 0; j < N; j++) { + self(i, j) = other(i, j); + } + } + } + return (*this); + } + + void copyTo(Type dst[M*N]) const + { + const Matrix &self = *this; + for (size_t i = 0; i < M; i++) { + for (size_t j = 0; j < N; j++) { + dst[N*i + j] = self(i, j); + } + } + } + + void copyToColumnMajor(Type dst[M*N]) const + { + const Matrix &self = *this; + + for (size_t i = 0; i < M; i++) { + for (size_t j = 0; j < N; j++) { + dst[i+(j*M)] = self(i, j); + } + } + } + + /** + * Matrix Operations + */ + + // this might use a lot of programming memory + // since it instantiates a class for every + // required mult pair, but it provides + // compile time size_t checking + template + Matrix operator*(const Matrix &other) const + { + const Matrix &self = *this; + Matrix res{}; + + for (size_t i = 0; i < M; i++) { + for (size_t k = 0; k < P; k++) { + for (size_t j = 0; j < N; j++) { + res(i, k) += self(i, j) * other(j, k); + } + } + } + + 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 edivide(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; + 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; + 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; + } + + // unary minus + Matrix operator-() 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); + } + } + + return res; + } + + void operator+=(const Matrix &other) + { + Matrix &self = *this; + for (size_t i = 0; i < M; i++) { + for (size_t j = 0; j < N; j++) { + self(i, j) += other(i, j); + } + } + } + + void operator-=(const Matrix &other) + { + Matrix &self = *this; + for (size_t i = 0; i < M; i++) { + for (size_t j = 0; j < N; j++) { + self(i, j) -= other(i, j); + } + } + } + + template + void operator*=(const Matrix &other) + { + Matrix &self = *this; + self = self * other; + } + + /** + * Scalar Operations + */ + + Matrix operator*(Type scalar) 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) * scalar; + } + } + + return res; + } + + inline Matrix operator/(Type scalar) const + { + return (*this)*(1/scalar); + } + + Matrix operator+(Type scalar) 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) + scalar; + } + } + + return res; + } + + inline Matrix operator-(Type scalar) const + { + return (*this) + (-1*scalar); + } + + void operator*=(Type scalar) + { + Matrix &self = *this; + + for (size_t i = 0; i < M; i++) { + for (size_t j = 0; j < N; j++) { + self(i, j) *= scalar; + } + } + } + + void operator/=(Type scalar) + { + Matrix &self = *this; + self *= (Type(1) / scalar); + } + + inline void operator+=(Type scalar) + { + Matrix &self = *this; + for (size_t i = 0; i < M; i++) { + for (size_t j = 0; j < N; j++) { + self(i, j) += scalar; + } + } + } + + inline void operator-=(Type scalar) + { + Matrix &self = *this; + self += (-scalar); + } + + bool operator==(const Matrix &other) const + { + return isEqual(*this, other); + } + + bool operator!=(const Matrix &other) const + { + const Matrix &self = *this; + return !(self == other); + } + + /** + * Misc. Functions + */ + + void write_string(char * buf, size_t n) const + { + buf[0] = '\0'; // make an empty string to begin with (we need the '\0' for strlen to work) + const Matrix &self = *this; + for (size_t i = 0; i < M; i++) { + for (size_t j = 0; j < N; j++) { + snprintf(buf + strlen(buf), n - strlen(buf), "\t%8.8g", double(self(i, j))); // directly append to the string buffer + } + snprintf(buf + strlen(buf), n - strlen(buf), "\n"); + } + } + + void print() const + { + // element: tab, point, 8 digits, 4 scientific notation chars; row: newline; string: \0 end + static const size_t n = 15*N*M + M + 1; + char * buf = new char[n]; + write_string(buf, n); + printf("%s\n", buf); + delete[] buf; + } + + Matrix transpose() const + { + Matrix res; + const Matrix &self = *this; + + for (size_t i = 0; i < M; i++) { + for (size_t j = 0; j < N; j++) { + res(j, i) = self(i, j); + } + } + + return res; + } + + // tranpose alias + inline Matrix T() const + { + return transpose(); + } + + template + const Slice slice(size_t x0, size_t y0) const + { + return Slice(x0, y0, this); + } + + template + Slice slice(size_t x0, size_t y0) + { + return Slice(x0, y0, this); + } + + const Slice row(size_t i) const + { + return slice<1, N>(i,0); + } + + Slice row(size_t i) + { + return slice<1, N>(i,0); + } + + const Slice col(size_t j) const + { + return slice(0,j); + } + + Slice col(size_t j) + { + return slice(0,j); + } + + void setRow(size_t i, const Matrix &row_in) + { + slice<1,N>(i,0) = row_in.transpose(); + } + + void setRow(size_t i, Type val) + { + slice<1,N>(i,0) = val; + } + + void setCol(size_t j, const Matrix &column) + { + slice(0,j) = column; + } + + void setCol(size_t j, Type val) + { + slice(0,j) = val; + } + + void setZero() + { + memset(_data, 0, sizeof(_data)); + } + + inline void zero() + { + setZero(); + } + + void setAll(Type val) + { + Matrix &self = *this; + + for (size_t i = 0; i < M; i++) { + for (size_t j = 0; j < N; j++) { + self(i, j) = val; + } + } + } + + inline void setOne() + { + setAll(1); + } + + inline void setNaN() + { + setAll(NAN); + } + + void setIdentity() + { + setZero(); + Matrix &self = *this; + + const size_t min_i = M > N ? N : M; + for (size_t i = 0; i < min_i; i++) { + self(i, i) = 1; + } + } + + inline void identity() + { + setIdentity(); + } + + inline void swapRows(size_t a, size_t b) + { + assert(a < M); + assert(b < M); + + if (a == b) { + return; + } + + Matrix &self = *this; + + for (size_t j = 0; j < N; j++) { + Type tmp = self(a, j); + self(a, j) = self(b, j); + self(b, j) = tmp; + } + } + + inline void swapCols(size_t a, size_t b) + { + assert(a < N); + assert(b < N); + + if (a == b) { + return; + } + + Matrix &self = *this; + + for (size_t i = 0; i < M; i++) { + Type tmp = self(i, a); + self(i, a) = self(i, b); + self(i, b) = tmp; + } + } + + Matrix abs() const + { + Matrix r; + for (size_t i=0; i max_val) { + max_val = val; + } + } + } + return max_val; + } + + Type min() const + { + Type min_val = (*this)(0,0); + for (size_t i=0; i &self = *this; + bool result = true; + for (size_t i = 0; i < M; i++) { + for (size_t j = 0; j < N; j++) { + result = result && isnan(self(i, j)); + } + } + return result; + } +}; + +template +Matrix zeros() { + Matrix m; + m.setZero(); + return m; +} + +template +Matrix ones() { + Matrix m; + m.setOne(); + return m; +} + +template +Matrix nans() { + Matrix m; + m.setNaN(); + return m; +} + +template +Matrix operator*(Type scalar, const Matrix &other) +{ + return other * scalar; +} + +template +bool isEqual(const Matrix &x, + const Matrix &y, const Type eps=Type(1e-4f)) { + for (size_t i = 0; i < M; i++) { + for (size_t j = 0; j < N; j++) { + if (!isEqualF(x(i,j), y(i,j), eps)) { + return false; + } + } + } + return true; +} + +namespace typeFunction +{ +template +Type min(const Type x, const Type y) { + bool x_is_nan = isnan(x); + bool y_is_nan = isnan(y); + // take the non-nan value if there is one + if (x_is_nan || y_is_nan) { + if (x_is_nan && !y_is_nan) { + return y; + } + // either !x_is_nan && y_is_nan or both are NAN anyways + return x; + } + return (x < y) ? x : y; +} + +template +Type max(const Type x, const Type y) { + bool x_is_nan = isnan(x); + bool y_is_nan = isnan(y); + // take the non-nan value if there is one + if (x_is_nan || y_is_nan) { + if (x_is_nan && !y_is_nan) { + return y; + } + // either !x_is_nan && y_is_nan or both are NAN anyways + return x; + } + return (x > y) ? x : y; +} + +template +Type constrain(const Type x, const Type lower_bound, const Type upper_bound) { + if (lower_bound > upper_bound) { + return NAN; + } else if(isnan(x)) { + return NAN; + } else { + return typeFunction::max(lower_bound, typeFunction::min(upper_bound, x)); + } +} +} + +template +Matrix min(const Matrix &x, const Type scalar_upper_bound) { + Matrix m; + for (size_t i = 0; i < M; i++) { + for (size_t j = 0; j < N; j++) { + m(i,j) = typeFunction::min(x(i,j),scalar_upper_bound); + } + } + return m; +} + +template +Matrix min(const Type scalar_upper_bound, const Matrix &x) { + return min(x, scalar_upper_bound); +} + +template +Matrix min(const Matrix &x1, const Matrix &x2) { + Matrix m; + for (size_t i = 0; i < M; i++) { + for (size_t j = 0; j < N; j++) { + m(i,j) = typeFunction::min(x1(i,j),x2(i,j)); + } + } + return m; +} + +template +Matrix max(const Matrix &x, const Type scalar_lower_bound) { + Matrix m; + for (size_t i = 0; i < M; i++) { + for (size_t j = 0; j < N; j++) { + m(i,j) = typeFunction::max(x(i,j),scalar_lower_bound); + } + } + return m; +} + +template +Matrix max(const Type scalar_lower_bound, const Matrix &x) { + return max(x, scalar_lower_bound); +} + +template +Matrix max(const Matrix &x1, const Matrix &x2) { + Matrix m; + for (size_t i = 0; i < M; i++) { + for (size_t j = 0; j < N; j++) { + m(i,j) = typeFunction::max(x1(i,j),x2(i,j)); + } + } + return m; +} + +template +Matrix constrain(const Matrix &x, + const Type scalar_lower_bound, + const Type scalar_upper_bound) { + Matrix m; + if (scalar_lower_bound > scalar_upper_bound) { + m.setNaN(); + } else { + for (size_t i = 0; i < M; i++) { + for (size_t j = 0; j < N; j++) { + m(i,j) = typeFunction::constrain(x(i,j), scalar_lower_bound, scalar_upper_bound); + } + } + } + return m; +} + +template +Matrix constrain(const Matrix &x, + const Matrix &x_lower_bound, + const Matrix &x_upper_bound) { + Matrix m; + for (size_t i = 0; i < M; i++) { + for (size_t j = 0; j < N; j++) { + m(i,j) = typeFunction::constrain(x(i,j), x_lower_bound(i,j), x_upper_bound(i,j)); + } + } + return m; +} + +#if defined(SUPPORT_STDIOSTREAM) +template +std::ostream& operator<<(std::ostream& os, + const matrix::Matrix& matrix) +{ + for (size_t i = 0; i < M; ++i) { + os << "["; + for (size_t j = 0; j < N; ++j) { + os << std::setw(10) << matrix(i, j); + os << "\t"; + } + os << "]" << std::endl; + } + return os; +} +#endif // defined(SUPPORT_STDIOSTREAM) + +} // namespace matrix + +/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */ diff --git a/src/lib/matrix/matrix/PseudoInverse.hpp b/src/lib/matrix/matrix/PseudoInverse.hpp new file mode 100644 index 0000000000..1590efecfd --- /dev/null +++ b/src/lib/matrix/matrix/PseudoInverse.hpp @@ -0,0 +1,119 @@ +/** + * @file PseudoInverse.hpp + * + * Implementation of matrix pseudo inverse + * + * @author Julien Lecoeur + * @author Julian Kent + */ + +#pragma once + +#include "math.hpp" + +namespace matrix +{ + +/** + * Geninv + * Fast pseudoinverse based on full rank cholesky factorisation + * + * Courrieu, P. (2008). Fast Computation of Moore-Penrose Inverse Matrices, 8(2), 25–29. http://arxiv.org/abs/0804.4809 + */ +template +bool geninv(const Matrix & G, Matrix& res) +{ + size_t rank; + if (M <= N) { + SquareMatrix A = G * G.transpose(); + SquareMatrix L = fullRankCholesky(A, rank); + + A = L.transpose() * L; + SquareMatrix X; + if (!inv(A, X, rank)) { + res = Matrix(); + return false; // LCOV_EXCL_LINE -- this can only be hit from numerical issues + } + // doing an intermediate assignment reduces stack usage + A = X * X * L.transpose(); + res = G.transpose() * (L * A); + + } else { + SquareMatrix A = G.transpose() * G; + SquareMatrix L = fullRankCholesky(A, rank); + + A = L.transpose() * L; + SquareMatrix X; + if(!inv(A, X, rank)) { + res = Matrix(); + return false; // LCOV_EXCL_LINE -- this can only be hit from numerical issues + } + // doing an intermediate assignment reduces stack usage + A = X * X * L.transpose(); + res = (L * A) * G.transpose(); + } + return true; +} + + +template +Type typeEpsilon(); + +template<> inline +float typeEpsilon() +{ + return FLT_EPSILON; +} + +/** + * Full rank Cholesky factorization of A + */ +template +SquareMatrix fullRankCholesky(const SquareMatrix & A, + size_t& rank) +{ + // Loses one ulp accuracy per row of diag, relative to largest magnitude + const Type tol = N * typeEpsilon() * A.diag().max(); + + Matrix L; + + size_t r = 0; + for (size_t k = 0; k < N; k++) { + + if (r == 0) { + for (size_t i = k; i < N; i++) { + L(i, r) = A(i, k); + } + + } else { + for (size_t i = k; i < N; i++) { + // Compute LL = L[k:n, :r] * L[k, :r].T + Type LL = Type(); + for (size_t j = 0; j < r; j++) { + LL += L(i, j) * L(k, j); + } + L(i, r) = A(i, k) - LL; + } + } + if (L(k, r) > tol) { + L(k, r) = sqrt(L(k, r)); + + if (k < N - 1) { + for (size_t i = k + 1; i < N; i++) { + L(i, r) = L(i, r) / L(k, r); + } + } + + r = r + 1; + } + } + + // Return rank + rank = r; + + return L; +} + +} // namespace matrix + +/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */ diff --git a/src/lib/matrix/matrix/Quaternion.hpp b/src/lib/matrix/matrix/Quaternion.hpp new file mode 100644 index 0000000000..ddcbdf9ec0 --- /dev/null +++ b/src/lib/matrix/matrix/Quaternion.hpp @@ -0,0 +1,532 @@ +/** + * @file Quaternion.hpp + * + * All rotations and axis systems follow the right-hand rule. + * The Hamilton quaternion convention including its product definition is used. + * + * In order to rotate a vector in frame b (v_b) to frame n by a righthand + * rotation defined by the quaternion q_nb (from frame b to n) + * one can use the following operation: + * v_n = q_nb * [0;v_b] * q_nb^(-1) + * + * Just like DCM's: v_n = C_nb * v_b (vector rotation) + * M_n = C_nb * M_b * C_nb^(-1) (matrix rotation) + * + * or similarly the reverse operation + * v_b = q_nb^(-1) * [0;v_n] * q_nb + * + * where q_nb^(-1) represents the inverse of the quaternion q_nb^(-1) = q_bn + * + * The product z of two quaternions z = q2 * q1 represents an intrinsic rotation + * in the order of first q1 followed by q2. + * The first element of the quaternion + * represents the real part, thus, a quaternion representing a zero-rotation + * is defined as (1,0,0,0). + * + * @author James Goppert + */ + +#pragma once + +#include "math.hpp" + +namespace matrix +{ + +template +class Dcm; + +template +class Euler; + +template +class AxisAngle; + + +/** + * Quaternion class + * + * The rotation between two coordinate frames is + * described by this class. + */ +template +class Quaternion : public Vector +{ +public: + using Matrix41 = Matrix; + using Matrix31 = Matrix; + + /** + * Constructor from array + * + * @param data_ array + */ + explicit Quaternion(const Type data_[4]) : + Vector(data_) + { + } + + /** + * Standard constructor + */ + Quaternion() + { + Quaternion &q = *this; + q(0) = 1; + q(1) = 0; + q(2) = 0; + q(3) = 0; + } + + /** + * Constructor from Matrix41 + * + * @param other Matrix41 to copy + */ + Quaternion(const Matrix41 &other) : + Vector(other) + { + } + + /** + * Constructor from dcm + * + * Instance is initialized from a dcm representing coordinate transformation + * from frame 2 to frame 1. + * + * @param dcm dcm to set quaternion to + */ + Quaternion(const Dcm &R) + { + Quaternion &q = *this; + Type t = R.trace(); + if (t > Type(0)) { + t = sqrt(Type(1) + t); + q(0) = Type(0.5) * t; + t = Type(0.5) / t; + q(1) = (R(2,1) - R(1,2)) * t; + q(2) = (R(0,2) - R(2,0)) * t; + q(3) = (R(1,0) - R(0,1)) * t; + } else if (R(0,0) > R(1,1) && R(0,0) > R(2,2)) { + t = sqrt(Type(1) + R(0,0) - R(1,1) - R(2,2)); + q(1) = Type(0.5) * t; + t = Type(0.5) / t; + q(0) = (R(2,1) - R(1,2)) * t; + q(2) = (R(1,0) + R(0,1)) * t; + q(3) = (R(0,2) + R(2,0)) * t; + } else if (R(1,1) > R(2,2)) { + t = sqrt(Type(1) - R(0,0) + R(1,1) - R(2,2)); + q(2) = Type(0.5) * t; + t = Type(0.5) / t; + q(0) = (R(0,2) - R(2,0)) * t; + q(1) = (R(1,0) + R(0,1)) * t; + q(3) = (R(2,1) + R(1,2)) * t; + } else { + t = sqrt(Type(1) - R(0,0) - R(1,1) + R(2,2)); + q(3) = Type(0.5) * t; + t = Type(0.5) / t; + q(0) = (R(1,0) - R(0,1)) * t; + q(1) = (R(0,2) + R(2,0)) * t; + q(2) = (R(2,1) + R(1,2)) * t; + } + } + + /** + * Constructor from euler angles + * + * This sets the instance to a quaternion representing coordinate transformation from + * frame 2 to frame 1 where the rotation from frame 1 to frame 2 is described + * by a 3-2-1 intrinsic Tait-Bryan rotation sequence. + * + * @param euler euler angle instance + */ + Quaternion(const Euler &euler) + { + Quaternion &q = *this; + Type cosPhi_2 = Type(cos(euler.phi() / Type(2))); + Type cosTheta_2 = Type(cos(euler.theta() / Type(2))); + Type cosPsi_2 = Type(cos(euler.psi() / Type(2))); + Type sinPhi_2 = Type(sin(euler.phi() / Type(2))); + Type sinTheta_2 = Type(sin(euler.theta() / Type(2))); + Type sinPsi_2 = Type(sin(euler.psi() / Type(2))); + q(0) = cosPhi_2 * cosTheta_2 * cosPsi_2 + + sinPhi_2 * sinTheta_2 * sinPsi_2; + q(1) = sinPhi_2 * cosTheta_2 * cosPsi_2 - + cosPhi_2 * sinTheta_2 * sinPsi_2; + q(2) = cosPhi_2 * sinTheta_2 * cosPsi_2 + + sinPhi_2 * cosTheta_2 * sinPsi_2; + q(3) = cosPhi_2 * cosTheta_2 * sinPsi_2 - + sinPhi_2 * sinTheta_2 * cosPsi_2; + } + + /** + * Quaternion from AxisAngle + * + * @param aa axis-angle vector + */ + Quaternion(const AxisAngle &aa) + { + Quaternion &q = *this; + Type angle = aa.norm(); + Vector axis = aa.unit(); + if (angle < Type(1e-10)) { + q(0) = Type(1); + q(1) = q(2) = q(3) = 0; + } else { + Type magnitude = sin(angle / Type(2)); + q(0) = cos(angle / Type(2)); + q(1) = axis(0) * magnitude; + q(2) = axis(1) * magnitude; + q(3) = axis(2) * magnitude; + } + } + + /** + * Quaternion from two vectors + * Generates shortest rotation from source to destination vector + * + * @param dst destination vector (no need to normalize) + * @param src source vector (no need to normalize) + * @param eps epsilon threshold which decides if a value is considered zero + */ + Quaternion(const Vector3 &src, const Vector3 &dst, const Type eps = Type(1e-5)) + { + Quaternion &q = *this; + Vector3 cr = src.cross(dst); + const float dt = src.dot(dst); + if (cr.norm() < eps && dt < 0) { + // handle corner cases with 180 degree rotations + // if the two vectors are parallel, cross product is zero + // if they point opposite, the dot product is negative + cr = src.abs(); + if (cr(0) < cr(1)) { + if (cr(0) < cr(2)) { + cr = Vector3(1, 0, 0); + } else { + cr = Vector3(0, 0, 1); + } + } else { + if (cr(1) < cr(2)) { + cr = Vector3(0, 1, 0); + } else { + cr = Vector3(0, 0, 1); + } + } + q(0) = Type(0); + cr = src.cross(cr); + } else { + // normal case, do half-way quaternion solution + q(0) = dt + sqrt(src.norm_squared() * dst.norm_squared()); + } + q(1) = cr(0); + q(2) = cr(1); + q(3) = cr(2); + q.normalize(); + } + + /** + * Constructor from quaternion values + * + * Instance is initialized from quaternion values representing coordinate + * transformation from frame 2 to frame 1. + * A zero-rotation quaternion is represented by (1,0,0,0). + * + * @param a set quaternion value 0 + * @param b set quaternion value 1 + * @param c set quaternion value 2 + * @param d set quaternion value 3 + */ + Quaternion(Type a, Type b, Type c, Type d) + { + Quaternion &q = *this; + q(0) = a; + q(1) = b; + q(2) = c; + q(3) = d; + } + + /** + * Quaternion multiplication operator + * + * @param q quaternion to multiply with + * @return product + */ + Quaternion operator*(const Quaternion &p) const + { + const Quaternion &q = *this; + return { + q(0) * p(0) - q(1) * p(1) - q(2) * p(2) - q(3) * p(3), + q(1) * p(0) + q(0) * p(1) - q(3) * p(2) + q(2) * p(3), + q(2) * p(0) + q(3) * p(1) + q(0) * p(2) - q(1) * p(3), + q(3) * p(0) - q(2) * p(1) + q(1) * p(2) + q(0) * p(3) }; + } + + /** + * Self-multiplication operator + * + * @param other quaternion to multiply with + */ + void operator*=(const Quaternion &other) + { + Quaternion &self = *this; + self = self * other; + } + + /** + * Scalar multiplication operator + * + * @param scalar scalar to multiply with + * @return product + */ + Quaternion operator*(Type scalar) const + { + const Quaternion &q = *this; + return scalar * q; + } + + /** + * Scalar self-multiplication operator + * + * @param scalar scalar to multiply with + */ + void operator*=(Type scalar) + { + Quaternion &q = *this; + q = q * scalar; + } + + /** + * Computes the derivative of q_21 when + * rotated with angular velocity expressed in frame 1 + * v_2 = q_21 * v_1 * q_21^-1 + * d/dt q_21 = 0.5 * q_21 * omega_2 + * + * @param w angular rate in frame 1 (typically body frame) + */ + Matrix41 derivative1(const Matrix31 &w) const + { + const Quaternion &q = *this; + Quaternion v(0, w(0, 0), w(1, 0), w(2, 0)); + return q * v * Type(0.5); + } + + /** + * Computes the derivative of q_21 when + * rotated with angular velocity expressed in frame 2 + * v_2 = q_21 * v_1 * q_21^-1 + * d/dt q_21 = 0.5 * omega_1 * q_21 + * + * @param w angular rate in frame 2 (typically reference frame) + */ + Matrix41 derivative2(const Matrix31 &w) const + { + const Quaternion &q = *this; + Quaternion v(0, w(0, 0), w(1, 0), w(2, 0)); + return v * q * Type(0.5); + } + + /** + * Computes the quaternion exponential of the 3D vector u + * as proposed in + * [1] Sveier A, Sjøberg AM, Egeland O. "Applied Runge–Kutta–Munthe-Kaas Integration + * for the Quaternion Kinematics".Journal of Guidance, Control, and Dynamics. 2019 + * + * return a quaternion computed as + * expq(u)=[cos||u||, sinc||u||*u] + * sinc(x)=sin(x)/x in the sin cardinal function + * + * This can be used to update a quaternion from the body rates + * rather than using + * qk+1=qk+qk.derivative1(wb)*dt + * we can use + * qk+1=qk*expq(dt*wb/2) + * which is a more robust update. + * A re-normalization step might necessary with both methods. + * + * @param u 3D vector u + */ + static Quaternion expq(const Vector3 &u) + { + const Type tol = Type(0.2); // ensures an error < 10^-10 + const Type c2 = Type(1.0 / 2.0); // 1 / 2! + const Type c3 = Type(1.0 / 6.0); // 1 / 3! + const Type c4 = Type(1.0 / 24.0); // 1 / 4! + const Type c5 = Type(1.0 / 120.0); // 1 / 5! + const Type c6 = Type(1.0 / 720.0); // 1 / 6! + const Type c7 = Type(1.0 / 5040.0); // 1 / 7! + + Type u_norm = u.norm(); + Type sinc_u, cos_u; + + if (u_norm < tol) { + Type u2 = u_norm * u_norm; + Type u4 = u2 * u2; + Type u6 = u4 * u2; + + // compute the first 4 terms of the Taylor serie + sinc_u = Type(1.0) - u2 * c3 + u4 * c5 - u6 * c7; + cos_u = Type(1.0) - u2 * c2 + u4 * c4 - u6 * c6; + } else { + sinc_u = Type(sin(u_norm) / u_norm); + cos_u = Type(cos(u_norm)); + } + Vector v = sinc_u * u; + return Quaternion (cos_u, v(0), v(1), v(2)); + } + + /** inverse right Jacobian of the quaternion logarithm u + * equation (20) in reference + * [1] Sveier A, Sjøberg AM, Egeland O. "Applied Runge–Kutta–Munthe-Kaas Integration + * for the Quaternion Kinematics".Journal of Guidance, Control, and Dynamics. 2019 + * + * This can be used to update a quaternion kinematic cleanly + * with higher order integration methods (like RK4) on the quaternion logarithm u. + * + * @param u 3D vector u + */ + static Dcm inv_r_jacobian (const Vector3 &u) + { + const Type tol = Type(1.0e-4); + Type u_norm = u.norm(); + Dcm u_hat = u.hat(); + + if (u_norm < tol) { // result smaller than O(||.||^3) + return Type(0.5) * (Dcm() + u_hat + (Type(1.0 / 3.0) + u_norm * u_norm / Type(45.0)) * u_hat * u_hat); + } else { + return Type(0.5) * (Dcm() + u_hat + (Type(1.0) - u_norm * Type(cos(u_norm) / sin(u_norm))) / (u_norm * u_norm) * u_hat * u_hat); + } + } + + /** + * Invert quaternion in place + */ + void invert() + { + *this = this->inversed(); + } + + /** + * Invert quaternion + * + * @return inverted quaternion + */ + Quaternion inversed() const + { + const Quaternion &q = *this; + Type normSq = q.dot(q); + return Quaternion( + q(0)/normSq, + -q(1)/normSq, + -q(2)/normSq, + -q(3)/normSq); + } + + /** + * Bring quaternion to canonical form + */ + void canonicalize() + { + *this = this->canonical(); + } + + /** + * Return canonical form of the quaternion + * + * @return quaternion in canonical from + */ + Quaternion canonical() const + { + const Quaternion &q = *this; + + for (size_t i = 0; i < 4; i++) { + if (fabs(q(i)) > FLT_EPSILON) { + return q * Type(matrix::sign(q(i))); + } + } + return q; + } + + /** + * Rotate quaternion from rotation vector + * + * @param vec rotation vector + */ + void rotate(const AxisAngle &vec) + { + Quaternion res(vec); + (*this) = res * (*this); + } + + /** + * Rotates vector v_1 in frame 1 to vector v_2 in frame 2 + * using the rotation quaternion q_21 + * describing the rotation from frame 1 to 2 + * v_2 = q_21 * v_1 * q_21^-1 + * + * @param vec vector to rotate in frame 1 (typically body frame) + * @return rotated vector in frame 2 (typically reference frame) + */ + Vector3 conjugate(const Vector3 &vec) const { + const Quaternion& q = *this; + Quaternion v(Type(0), vec(0), vec(1), vec(2)); + Quaternion res = q*v*q.inversed(); + return Vector3(res(1), res(2), res(3)); + } + + /** + * Rotates vector v_2 in frame 2 to vector v_1 in frame 1 + * using the rotation quaternion q_21 + * describing the rotation from frame 1 to 2 + * v_1 = q_21^-1 * v_2 * q_21 + * + * @param vec vector to rotate in frame 2 (typically reference frame) + * @return rotated vector in frame 1 (typically body frame) + */ + Vector3 conjugate_inversed(const Vector3 &vec) const + { + const Quaternion& q = *this; + Quaternion v(Type(0), vec(0), vec(1), vec(2)); + Quaternion res = q.inversed()*v*q; + return Vector3(res(1), res(2), res(3)); + } + + /** + * Imaginary components of quaternion + */ + Vector3 imag() const + { + const Quaternion &q = *this; + return Vector3(q(1), q(2), q(3)); + } + + /** + * Corresponding body z-axis to an attitude quaternion / + * last orthogonal unit basis vector + * + * == last column of the equivalent rotation matrix + * but calculated more efficiently than a full conversion + */ + Vector3 dcm_z() const + { + const Quaternion &q = *this; + Vector3 R_z; + const Type a = q(0); + const Type b = q(1); + const Type c = q(2); + const Type d = q(3); + R_z(0) = 2 * (a * c + b * d); + R_z(1) = 2 * (c * d - a * b); + R_z(2) = a * a - b * b - c * c + d * d; + return R_z; + } +}; + +using Quatf = Quaternion; +using Quaternionf = Quaternion; + +using Quatd = Quaternion; +using Quaterniond = Quaternion; + +} // namespace matrix + +/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */ diff --git a/src/lib/matrix/matrix/Scalar.hpp b/src/lib/matrix/matrix/Scalar.hpp new file mode 100644 index 0000000000..182e0f7786 --- /dev/null +++ b/src/lib/matrix/matrix/Scalar.hpp @@ -0,0 +1,58 @@ +/** + * @file Scalar.hpp + * + * Defines conversion of matrix to scalar. + * + * @author James Goppert + */ + +#pragma once + +#include "math.hpp" + +namespace matrix +{ + +template +class Scalar +{ +public: + Scalar() = delete; + + Scalar(const Matrix & other) : + _value{other(0,0)} + { + } + + Scalar(Type other) : _value(other) + { + } + + operator const Type &() + { + return _value; + } + + operator Matrix() const { + Matrix m; + m(0, 0) = _value; + return m; + } + + operator Vector() const { + Vector m; + m(0) = _value; + return m; + } + +private: + const Type _value; + +}; + +using Scalarf = Scalar; +using Scalard = Scalar; + +} // namespace matrix + +/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */ diff --git a/src/lib/matrix/matrix/Slice.hpp b/src/lib/matrix/matrix/Slice.hpp new file mode 100644 index 0000000000..f98ac483c0 --- /dev/null +++ b/src/lib/matrix/matrix/Slice.hpp @@ -0,0 +1,304 @@ +/** + * @file Slice.hpp + * + * A simple matrix template library. + * + * @author Julian Kent < julian@auterion.com > + */ + +#pragma once + +#include "math.hpp" + + +namespace matrix { + +template +class Matrix; + +template +class Vector; + +template +class Slice { +public: + Slice(size_t x0, size_t y0, const Matrix* data) : + _x0(x0), + _y0(y0), + _data(const_cast*>(data)) { + static_assert(P <= M, "Slice rows bigger than backing matrix"); + static_assert(Q <= N, "Slice cols bigger than backing matrix"); + assert(x0 + P <= M); + assert(y0 + Q <= N); + } + + const Type &operator()(size_t i, size_t j) const + { + assert(i < P); + assert(j < Q); + + return (*_data)(_x0 + i, _y0 + j); + } + + Type &operator()(size_t i, size_t j) + + { + assert(i < P); + assert(j < Q); + + return (*_data)(_x0 + i, _y0 + j); + } + + template + Slice& operator=(const Slice& other) + { + Slice& self = *this; + for (size_t i = 0; i < P; i++) { + for (size_t j = 0; j < Q; j++) { + self(i, j) = other(i, j); + } + } + return self; + } + + Slice& operator=(const Matrix& other) + { + Slice& self = *this; + for (size_t i = 0; i < P; i++) { + for (size_t j = 0; j < Q; j++) { + self(i, j) = other(i, j); + } + } + return self; + } + + Slice& operator=(const Type& other) + { + Slice& self = *this; + for (size_t i = 0; i < P; i++) { + for (size_t j = 0; j < Q; j++) { + self(i, j) = other; + } + } + return self; + } + + // allow assigning vectors to a slice that are in the axis + template // make this a template function since it only exists for some instantiations + Slice& operator=(const Vector& other) + { + Slice& self = *this; + for (size_t j = 0; j < Q; j++) { + self(0, j) = other(j); + } + return self; + } + + template + Slice& operator+=(const Slice& other) + { + Slice& self = *this; + for (size_t i = 0; i < P; i++) { + for (size_t j = 0; j < Q; j++) { + self(i, j) += other(i, j); + } + } + return self; + } + + Slice& operator+=(const Matrix& other) + { + Slice& self = *this; + for (size_t i = 0; i < P; i++) { + for (size_t j = 0; j < Q; j++) { + self(i, j) += other(i, j); + } + } + return self; + } + + Slice& operator+=(const Type& other) + { + Slice& self = *this; + for (size_t i = 0; i < P; i++) { + for (size_t j = 0; j < Q; j++) { + self(i, j) += other; + } + } + return self; + } + + template + Slice& operator-=(const Slice& other) + { + Slice& self = *this; + for (size_t i = 0; i < P; i++) { + for (size_t j = 0; j < Q; j++) { + self(i, j) -= other(i, j); + } + } + return self; + } + + Slice& operator-=(const Matrix& other) + { + Slice& self = *this; + for (size_t i = 0; i < P; i++) { + for (size_t j = 0; j < Q; j++) { + self(i, j) -= other(i, j); + } + } + return self; + } + + Slice& operator-=(const Type& other) + { + Slice& self = *this; + for (size_t i = 0; i < P; i++) { + for (size_t j = 0; j < Q; j++) { + self(i, j) -= other; + } + } + return self; + } + + Slice& operator*=(const Type& other) + { + Slice& self = *this; + for (size_t i = 0; i < P; i++) { + for (size_t j = 0; j < Q; j++) { + self(i, j) *= other; + } + } + return self; + } + + Slice& operator/=(const Type& other) + { + return operator*=(Type(1) / other); + } + + Matrix operator*(const Type& other) const + { + const Slice& self = *this; + Matrix res; + for (size_t i = 0; i < P; i++) { + for (size_t j = 0; j < Q; j++) { + res(i, j) = self(i, j) * other; + } + } + return res; + } + + Matrix operator/(const Type& other) const + { + const Slice& self = *this; + return self * (Type(1) / other); + } + + template + const Slice slice(size_t x0, size_t y0) const + { + return Slice(x0 + _x0, y0 + _y0, _data); + } + + template + Slice slice(size_t x0, size_t y0) + { + return Slice(x0 + _x0, y0 + _y0, _data); + } + + void copyTo(Type dst[P*Q]) const + { + const Slice &self = *this; + + for (size_t i = 0; i < P; i++) { + for (size_t j = 0; j < Q; j++) { + dst[i*N+j] = self(i, j); + } + } + } + + void copyToColumnMajor(Type dst[P*Q]) const + { + const Slice &self = *this; + + for (size_t i = 0; i < P; i++) { + for (size_t j = 0; j < Q; j++) { + dst[i+(j*M)] = self(i, j); + } + } + } + + Vector diag() const + { + const Slice& self = *this; + Vector res; + for (size_t j = 0; j < (P& self = *this; + Type accum(0); + for (size_t i = 0; i < P; i++) { + for (size_t j = 0; j < Q; j++) { + accum += self(i, j)*self(i, j); + } + } + return accum; + } + + Type norm() const + { + return matrix::sqrt(norm_squared()); + } + + bool longerThan(Type testVal) const + { + return norm_squared() > testVal*testVal; + } + + Type max() const + { + Type max_val = (*this)(0,0); + + for (size_t i = 0; i < P; i++) { + for (size_t j = 0; j < Q; j++) { + Type val = (*this)(i,j); + + if (val > max_val) { + max_val = val; + } + } + } + + return max_val; + } + + Type min() const + { + Type min_val = (*this)(0,0); + + for (size_t i = 0; i < P; i++) { + for (size_t j = 0; j < Q; j++) { + Type val = (*this)(i,j); + + if (val < min_val) { + min_val = val; + } + } + } + + return min_val; + } + +private: + size_t _x0, _y0; + Matrix* _data; +}; + +} diff --git a/src/lib/matrix/matrix/SparseVector.hpp b/src/lib/matrix/matrix/SparseVector.hpp new file mode 100644 index 0000000000..ca4659a7a3 --- /dev/null +++ b/src/lib/matrix/matrix/SparseVector.hpp @@ -0,0 +1,189 @@ +/** + * @file SparseVector.hpp + * + * SparseVector class. + * + * @author Kamil Ritz + * @author Julian Kent + * + */ + +#pragma once + +#include "math.hpp" + +namespace matrix { +template struct force_constexpr_eval { + static const int value = N; +}; + +// Vector that only store nonzero elements, +// which indices are specified as parameter pack +template +class SparseVector { +private: + static constexpr size_t N = sizeof...(Idxs); + static constexpr size_t _indices[N] {Idxs...}; + + static constexpr bool duplicateIndices() { + for (size_t i = 0; i < N; i++) { + for (size_t j = 0; j < i; j++) { + if (_indices[i] == _indices[j]) { + return true; + } + } + } + return false; + } + static constexpr size_t findMaxIndex() { + size_t maxIndex = 0; + for (size_t i = 0; i < N; i++) { + if (maxIndex < _indices[i]) { + maxIndex = _indices[i]; + } + } + return maxIndex; + } + + static_assert(!duplicateIndices(), "Duplicate indices"); + static_assert(N < M, "More entries than elements, use a dense vector"); + static_assert(N > 0, "A sparse vector needs at least one element"); + static_assert(findMaxIndex() < M, "Largest entry doesn't fit in sparse vector"); + + Type _data[N] {}; + + static constexpr int findCompressedIndex(size_t index) { + int compressedIndex = -1; + for (size_t i = 0; i < N; i++) { + if (index == _indices[i]) { + compressedIndex = static_cast(i); + } + } + return compressedIndex; + } + +public: + constexpr size_t non_zeros() const { + return N; + } + + constexpr size_t index(size_t i) const { + return SparseVector::_indices[i]; + } + + SparseVector() = default; + + SparseVector(const matrix::Vector& data) { + for (size_t i = 0; i < N; i++) { + _data[i] = data(_indices[i]); + } + } + + explicit SparseVector(const Type data[N]) { + memcpy(_data, data, sizeof(_data)); + } + + template + inline Type at() const { + static constexpr int compressed_index = force_constexpr_eval::value; + static_assert(compressed_index >= 0, "cannot access unpopulated indices"); + return _data[compressed_index]; + } + + template + inline Type& at() { + static constexpr int compressed_index = force_constexpr_eval::value; + static_assert(compressed_index >= 0, "cannot access unpopulated indices"); + return _data[compressed_index]; + } + + inline Type atCompressedIndex(size_t i) const { + assert(i < N); + return _data[i]; + } + + inline Type& atCompressedIndex(size_t i) { + assert(i < N); + return _data[i]; + } + + void setZero() { + for (size_t i = 0; i < N; i++) { + _data[i] = Type(0); + } + } + + Type dot(const matrix::Vector& other) const { + Type accum (0); + for (size_t i = 0; i < N; i++) { + accum += _data[i] * other(_indices[i]); + } + return accum; + } + + matrix::Vector operator+(const matrix::Vector& other) const { + matrix::Vector vec = other; + for (size_t i = 0; i < N; i++) { + vec(_indices[i]) += _data[i]; + } + return vec; + } + + SparseVector& operator+=(Type t) { + for (size_t i = 0; i < N; i++) { + _data[i] += t; + } + return *this; + } + + Type norm_squared() const + { + Type accum(0); + for (size_t i = 0; i < N; i++) { + accum += _data[i] * _data[i]; + } + return accum; + } + + Type norm() const + { + return matrix::sqrt(norm_squared()); + } + + bool longerThan(Type testVal) const + { + return norm_squared() > testVal*testVal; + } +}; + +template +matrix::Vector operator*(const matrix::Matrix& mat, const matrix::SparseVector& vec) { + matrix::Vector res; + for (size_t i = 0; i < Q; i++) { + const Vector row = mat.row(i); + res(i) = vec.dot(row); + } + return res; +} + +// returns x.T * A * x +template +Type quadraticForm(const matrix::SquareMatrix& A, const matrix::SparseVector& x) { + Type res = Type(0); + for (size_t i = 0; i < x.non_zeros(); i++) { + Type tmp = Type(0); + for (size_t j = 0; j < x.non_zeros(); j++) { + tmp += A(x.index(i), x.index(j)) * x.atCompressedIndex(j); + } + res += x.atCompressedIndex(i) * tmp; + } + return res; +} + +template +constexpr size_t SparseVector::_indices[SparseVector::N]; + +template +using SparseVectorf = SparseVector; + +} diff --git a/src/lib/matrix/matrix/SquareMatrix.hpp b/src/lib/matrix/matrix/SquareMatrix.hpp new file mode 100644 index 0000000000..127e89b2f1 --- /dev/null +++ b/src/lib/matrix/matrix/SquareMatrix.hpp @@ -0,0 +1,544 @@ +/** + * @file SquareMatrix.hpp + * + * A square matrix + * + * @author James Goppert + */ + +#pragma once + +#include "math.hpp" + +namespace matrix +{ + +template +class Matrix; + +template +class Vector; + +template +class Slice; + +template +class SquareMatrix : public Matrix +{ +public: + SquareMatrix() = default; + + explicit SquareMatrix(const Type data_[M][M]) : + Matrix(data_) + { + } + + explicit SquareMatrix(const Type data_[M*M]) : + Matrix(data_) + { + } + + SquareMatrix(const Matrix &other) : + Matrix(other) + { + } + + template + SquareMatrix(const Slice& in_slice) : Matrix(in_slice) + { + } + + SquareMatrix& operator=(const Matrix& other) + { + Matrix::operator=(other); + return *this; + } + + template + SquareMatrix & operator=(const Slice& in_slice) + { + Matrix::operator=(in_slice); + return *this; + } + + template + const Slice slice(size_t x0, size_t y0) const + { + return Slice(x0, y0, this); + } + + template + Slice slice(size_t x0, size_t y0) + { + return Slice(x0, y0, this); + } + + // inverse alias + inline SquareMatrix I() const + { + SquareMatrix i; + if (inv(*this, i)) { + return i; + } else { + i.setZero(); + return i; + } + } + + // inverse alias + inline bool I(SquareMatrix &i) const + { + return inv(*this, i); + } + + + Vector diag() const + { + Vector res; + const SquareMatrix &self = *this; + + for (size_t i = 0; i < M; i++) { + res(i) = self(i, i); + } + return res; + } + + // get matrix upper right triangle in a row-major vector format + Vector upper_right_triangle() const + { + Vector res; + const SquareMatrix &self = *this; + + unsigned idx = 0; + for (size_t x = 0; x < M; x++) { + for (size_t y = x; y < M; y++) { + res(idx) = self(x, y); + ++idx; + } + } + + return res; + } + + Type trace() const + { + Type res = 0; + const SquareMatrix &self = *this; + + for (size_t i = 0; i < M; i++) { + res += self(i, i); + } + return res; + } + + // zero all offdiagonal elements and keep corresponding diagonal elements + template + void uncorrelateCovariance(size_t first) + { + static_assert(Width <= M, "Width bigger than matrix"); + assert(first + Width <= M); + + SquareMatrix &self = *this; + Vector diag_elements = self.slice(first, first).diag(); + self.uncorrelateCovarianceSetVariance(first, diag_elements); + } + + template + void uncorrelateCovarianceSetVariance(size_t first, const Vector &vec) + { + static_assert(Width <= M, "Width bigger than matrix"); + assert(first + Width <= M); + + SquareMatrix &self = *this; + // zero rows and columns + self.slice(first, 0) = Type(0); + self.slice(0, first) = Type(0); + + // set diagonals + unsigned vec_idx = 0; + for (size_t idx = first; idx < first+Width; idx++) { + self(idx,idx) = vec(vec_idx); + vec_idx ++; + } + } + + template + void uncorrelateCovarianceSetVariance(size_t first, Type val) + { + static_assert(Width <= M, "Width bigger than matrix"); + assert(first + Width <= M); + + SquareMatrix &self = *this; + // zero rows and columns + self.slice(first, 0) = Type(0); + self.slice(0, first) = Type(0); + + // set diagonals + for (size_t idx = first; idx < first+Width; idx++) { + self(idx,idx) = val; + } + } + + // make block diagonal symmetric by taking the average of the two corresponding off diagonal values + template + void makeBlockSymmetric(size_t first) + { + static_assert(Width <= M, "Width bigger than matrix"); + assert(first + Width <= M); + + SquareMatrix &self = *this; + if(Width>1) { + for (size_t row_idx = first+1; row_idx < first+Width; row_idx++) { + for (size_t col_idx = first; col_idx < row_idx; col_idx++) { + Type tmp = (self(row_idx,col_idx) + self(col_idx,row_idx)) / Type(2); + self(row_idx,col_idx) = tmp; + self(col_idx,row_idx) = tmp; + } + } + } + } + + // make rows and columns symmetric by taking the average of the two corresponding off diagonal values + template + void makeRowColSymmetric(size_t first) + { + static_assert(Width <= M, "Width bigger than matrix"); + assert(first + Width <= M); + + SquareMatrix &self = *this; + self.makeBlockSymmetric(first); + for (size_t row_idx = first; row_idx < first+Width; row_idx++) { + for (size_t col_idx = 0; col_idx < first; col_idx++) { + Type tmp = (self(row_idx,col_idx) + self(col_idx,row_idx)) / Type(2); + self(row_idx,col_idx) = tmp; + self(col_idx,row_idx) = tmp; + } + for (size_t col_idx = first+Width; col_idx < M; col_idx++) { + Type tmp = (self(row_idx,col_idx) + self(col_idx,row_idx)) / Type(2); + self(row_idx,col_idx) = tmp; + self(col_idx,row_idx) = tmp; + } + } + } + + // checks if block diagonal is symmetric + template + bool isBlockSymmetric(size_t first, const Type eps = Type(1e-8f)) + { + static_assert(Width <= M, "Width bigger than matrix"); + assert(first + Width <= M); + + SquareMatrix &self = *this; + if(Width>1) { + for (size_t row_idx = first+1; row_idx < first+Width; row_idx++) { + for (size_t col_idx = first; col_idx < row_idx; col_idx++) { + if(!isEqualF(self(row_idx,col_idx), self(col_idx,row_idx), eps)) { + return false; + } + } + } + } + return true; + } + + // checks if rows and columns are symmetric + template + bool isRowColSymmetric(size_t first, const Type eps = Type(1e-8f)) + { + static_assert(Width <= M, "Width bigger than matrix"); + assert(first + Width <= M); + + SquareMatrix &self = *this; + for (size_t row_idx = first; row_idx < first+Width; row_idx++) { + for (size_t col_idx = 0; col_idx < first; col_idx++) { + if(!isEqualF(self(row_idx,col_idx), self(col_idx,row_idx), eps)) { + return false; + } + } + for (size_t col_idx = first+Width; col_idx < M; col_idx++) { + if(!isEqualF(self(row_idx,col_idx), self(col_idx,row_idx), eps)) { + return false; + } + } + } + return self.isBlockSymmetric(first, eps); + } + +}; + +using SquareMatrix3f = SquareMatrix; +using SquareMatrix3d = SquareMatrix; + +template +SquareMatrix eye() { + SquareMatrix m; + m.setIdentity(); + return m; +} + +template +SquareMatrix diag(Vector d) { + SquareMatrix m; + for (size_t i=0; i +SquareMatrix expm(const Matrix & A, size_t order=5) +{ + SquareMatrix res; + SquareMatrix A_pow = A; + res.setIdentity(); + size_t i_factorial = 1; + for (size_t i=1; i<=order; i++) { + i_factorial *= i; + res += A_pow / Type(i_factorial); + A_pow *= A_pow; + } + + return res; +} + + +/** + * inverse based on LU factorization with partial pivotting + */ +template +bool inv(const SquareMatrix & A, SquareMatrix & inv, size_t rank = M) +{ + SquareMatrix L; + L.setIdentity(); + SquareMatrix U = A; + SquareMatrix P; + P.setIdentity(); + + //printf("A:\n"); A.print(); + + // for all diagonal elements + for (size_t n = 0; n < rank; n++) { + + // if diagonal is zero, swap with row below + if (fabs(U(n, n)) < Type(FLT_EPSILON)) { + //printf("trying pivot for row %d\n",n); + for (size_t i = n + 1; i < rank; i++) { + + //printf("\ttrying row %d\n",i); + if (fabs(U(i, n)) > Type(FLT_EPSILON)) { + //printf("swapped %d\n",i); + U.swapRows(i, n); + P.swapRows(i, n); + L.swapRows(i, n); + L.swapCols(i, n); + break; + } + } + } + +#ifdef MATRIX_ASSERT + //printf("A:\n"); A.print(); + //printf("U:\n"); U.print(); + //printf("P:\n"); P.print(); + //fflush(stdout); + //ASSERT(fabs(U(n, n)) > 1e-8f); +#endif + + // failsafe, return zero matrix + if (fabs(static_cast(U(n, n))) < FLT_EPSILON) { + return false; + } + + // for all rows below diagonal + for (size_t i = (n + 1); i < rank; i++) { + L(i, n) = U(i, n) / U(n, n); + + // add i-th row and n-th row + // multiplied by: -a(i,n)/a(n,n) + for (size_t k = n; k < rank; k++) { + U(i, k) -= L(i, n) * U(n, k); + } + } + } + + //printf("L:\n"); L.print(); + //printf("U:\n"); U.print(); + + // solve LY=P*I for Y by forward subst + //SquareMatrix Y = P; + + // for all columns of Y + for (size_t c = 0; c < rank; c++) { + // for all rows of L + for (size_t i = 0; i < rank; i++) { + // for all columns of L + for (size_t j = 0; j < i; j++) { + // for all existing y + // subtract the component they + // contribute to the solution + P(i, c) -= L(i, j) * P(j, c); + } + + // divide by the factor + // on current + // term to be solved + // Y(i,c) /= L(i,i); + // but L(i,i) = 1.0 + } + } + + //printf("Y:\n"); Y.print(); + + // solve Ux=y for x by back subst + //SquareMatrix X = Y; + + // for all columns of X + for (size_t c = 0; c < rank; c++) { + // for all rows of U + for (size_t k = 0; k < rank; k++) { + // have to go in reverse order + size_t i = rank - 1 - k; + + // for all columns of U + for (size_t j = i + 1; j < rank; j++) { + // for all existing x + // subtract the component they + // contribute to the solution + P(i, c) -= U(i, j) * P(j, c); + } + + // divide by the factor + // on current + // term to be solved + // + // we know that U(i, i) != 0 from above + P(i, c) /= U(i, i); + } + } + + //check sanity of results + for (size_t i = 0; i < rank; i++) { + for (size_t j = 0; j < rank; j++) { + if (!is_finite(P(i,j))) { + return false; + } + } + } + //printf("X:\n"); X.print(); + inv = P; + return true; +} + +template +bool inv(const SquareMatrix & A, SquareMatrix & inv) +{ + Type det = A(0, 0) * A(1, 1) - A(1, 0) * A(0, 1); + + if(fabs(static_cast(det)) < FLT_EPSILON || !is_finite(det)) { + return false; + } + + inv(0, 0) = A(1, 1); + inv(1, 0) = -A(1, 0); + inv(0, 1) = -A(0, 1); + inv(1, 1) = A(0, 0); + inv /= det; + return true; +} + +template +bool inv(const SquareMatrix & A, SquareMatrix & inv) +{ + Type det = A(0, 0) * (A(1, 1) * A(2, 2) - A(2, 1) * A(1, 2)) - + A(0, 1) * (A(1, 0) * A(2, 2) - A(1, 2) * A(2, 0)) + + A(0, 2) * (A(1, 0) * A(2, 1) - A(1, 1) * A(2, 0)); + + if(fabs(static_cast(det)) < FLT_EPSILON || !is_finite(det)) { + return false; + } + + inv(0, 0) = A(1, 1) * A(2, 2) - A(2, 1) * A(1, 2); + inv(0, 1) = A(0, 2) * A(2, 1) - A(0, 1) * A(2, 2); + inv(0, 2) = A(0, 1) * A(1, 2) - A(0, 2) * A(1, 1); + inv(1, 0) = A(1, 2) * A(2, 0) - A(1, 0) * A(2, 2); + inv(1, 1) = A(0, 0) * A(2, 2) - A(0, 2) * A(2, 0); + inv(1, 2) = A(1, 0) * A(0, 2) - A(0, 0) * A(1, 2); + inv(2, 0) = A(1, 0) * A(2, 1) - A(2, 0) * A(1, 1); + inv(2, 1) = A(2, 0) * A(0, 1) - A(0, 0) * A(2, 1); + inv(2, 2) = A(0, 0) * A(1, 1) - A(1, 0) * A(0, 1); + inv /= det; + return true; +} + +/** + * inverse based on LU factorization with partial pivotting + */ +template +SquareMatrix inv(const SquareMatrix & A) +{ + SquareMatrix i; + if (inv(A, i)) { + return i; + } else { + i.setZero(); + return i; + } +} + +/** + * cholesky decomposition + * + * Note: A must be positive definite + */ +template +SquareMatrix cholesky(const SquareMatrix & A) +{ + SquareMatrix L; + for (size_t j = 0; j < M; j++) { + for (size_t i = j; i < M; i++) { + if (i==j) { + float sum = 0; + for (size_t k = 0; k < j; k++) { + sum += L(j, k)*L(j, k); + } + Type res = A(j, j) - sum; + if (res <= 0) { + L(j, j) = 0; + } else { + L(j, j) = sqrt(res); + } + } else { + float sum = 0; + for (size_t k = 0; k < j; k++) { + sum += L(i, k)*L(j, k); + } + if (L(j, j) <= 0) { + L(i, j) = 0; + } else { + L(i, j) = (A(i, j) - sum)/L(j, j); + } + } + } + } + return L; +} + +/** + * cholesky inverse + * + * TODO: Check if gaussian elimination jumps straight to back-substitution + * for L or we need to do it manually. Will impact speed otherwise. + */ +template +SquareMatrix choleskyInv(const SquareMatrix & A) +{ + SquareMatrix L_inv = inv(cholesky(A)); + return L_inv.T()*L_inv; +} + +using Matrix3f = SquareMatrix; +using Matrix3d = SquareMatrix; + +} // namespace matrix + +/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */ diff --git a/src/lib/matrix/matrix/Vector.hpp b/src/lib/matrix/matrix/Vector.hpp new file mode 100644 index 0000000000..09e79d1bbb --- /dev/null +++ b/src/lib/matrix/matrix/Vector.hpp @@ -0,0 +1,136 @@ +/** + * @file Vector.hpp + * + * Vector class. + * + * @author James Goppert + */ + +#pragma once + +#include "math.hpp" + +namespace matrix +{ + +template +class Matrix; + +template +class Vector : public Matrix +{ +public: + using MatrixM1 = Matrix; + + Vector() = default; + + Vector(const MatrixM1 & other) : + MatrixM1(other) + { + } + + explicit Vector(const Type data_[M]) : + MatrixM1(data_) + { + } + + template + Vector(const Slice& slice_in) : + Matrix(slice_in) + { + } + + template + Vector(const Slice& slice_in) + { + Vector &self(*this); + for (size_t i = 0; i eps) { + return (*this) / n; + } + return Vector(); + } + + inline Vector normalized() const { + return unit(); + } + + bool longerThan(Type testVal) const { + return norm_squared() > testVal*testVal; + } + + Vector sqrt() const { + const Vector &a(*this); + Vector r; + for (size_t i = 0; i + */ + +#pragma once + +#include "math.hpp" + +namespace matrix +{ + +template +class Vector; + +template +class Vector2 : public Vector +{ +public: + + using Matrix21 = Matrix; + using Vector3 = Vector; + + Vector2() = default; + + Vector2(const Matrix21 & other) : + Vector(other) + { + } + + explicit Vector2(const Type data_[2]) : + Vector(data_) + { + } + + Vector2(Type x, Type y) + { + Vector2 &v(*this); + v(0) = x; + v(1) = y; + } + + template + Vector2(const Slice& slice_in) : Vector(slice_in) + { + } + + template + Vector2(const Slice& slice_in) : Vector(slice_in) + { + } + + explicit Vector2(const Vector3 & other) + { + Vector2 &v(*this); + v(0) = other(0); + v(1) = other(1); + } + + 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); + } + +}; + + +using Vector2f = Vector2; +using Vector2d = Vector2; + +} // namespace matrix + +/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */ diff --git a/src/lib/matrix/matrix/Vector3.hpp b/src/lib/matrix/matrix/Vector3.hpp new file mode 100644 index 0000000000..f67ade4618 --- /dev/null +++ b/src/lib/matrix/matrix/Vector3.hpp @@ -0,0 +1,156 @@ +/** + * @file Vector3.hpp + * + * 3D vector class. + * + * @author James Goppert + */ + +#pragma once + +#include "math.hpp" + +namespace matrix +{ + +template +class Matrix; + +template +class Vector; + +template +class Dcm; + +template +class Vector2; + +template +class Vector3 : public Vector +{ +public: + + using Matrix31 = Matrix; + + Vector3() = default; + + Vector3(const Matrix31 & other) : + Vector(other) + { + } + + explicit Vector3(const Type data_[3]) : + Vector(data_) + { + } + + Vector3(Type x, Type y, Type z) { + Vector3 &v(*this); + v(0) = x; + v(1) = y; + v(2) = z; + } + + template + Vector3(const Slice& slice_in) : Vector(slice_in) + { + } + + template + Vector3(const Slice& slice_in) : Vector(slice_in) + { + } + + Vector3 cross(const Matrix31 & b) const { + const Vector3 &a(*this); + return {a(1)*b(2,0) - a(2)*b(1,0), -a(0)*b(2,0) + a(2)*b(0,0), a(0)*b(1,0) - a(1)*b(0,0)}; + } + + /** + * Override matrix ops so Vector3 type is returned + */ + + inline Vector3 operator+(Vector3 other) const + { + return Matrix31::operator+(other); + } + + inline Vector3 operator+(Type scalar) const + { + return Matrix31::operator+(scalar); + } + + inline Vector3 operator-(Vector3 other) const + { + return Matrix31::operator-(other); + } + + inline Vector3 operator-(Type scalar) const + { + return Matrix31::operator-(scalar); + } + + inline Vector3 operator-() const + { + return Matrix31::operator-(); + } + + inline Vector3 operator*(Type scalar) const + { + return Matrix31::operator*(scalar); + } + + inline Type operator*(Vector3 b) const + { + return Vector::operator*(b); + } + + inline Vector3 operator%(const Matrix31 & b) const { + return (*this).cross(b); + } + + /** + * Override vector ops so Vector3 type is returned + */ + inline Vector3 unit() const { + return Vector3(Vector::unit()); + } + + inline Vector3 normalized() const { + return unit(); + } + + const Slice xy() const + { + return Slice(0, 0, this); + } + + Slice xy() + { + return Slice(0, 0, this); + } + + + Dcm hat() const { // inverse to Dcm.vee() operation + const Vector3 &v(*this); + Dcm A; + A(0,0) = 0; + A(0,1) = -v(2); + A(0,2) = v(1); + A(1,0) = v(2); + A(1,1) = 0; + A(1,2) = -v(0); + A(2,0) = -v(1); + A(2,1) = v(0); + A(2,2) = 0; + return A; + } + +}; + +using Vector3f = Vector3; +using Vector3d = Vector3; + +} // namespace matrix + +/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */ diff --git a/src/lib/matrix/matrix/filter.hpp b/src/lib/matrix/matrix/filter.hpp new file mode 100644 index 0000000000..7951cb3cb0 --- /dev/null +++ b/src/lib/matrix/matrix/filter.hpp @@ -0,0 +1,26 @@ +#pragma once + +#include "math.hpp" + +namespace matrix { + +template +int kalman_correct( + const Matrix & P, + const Matrix & C, + const Matrix & R, + const Matrix &r, + Matrix & dx, + Matrix & dP, + Type & beta +) +{ + SquareMatrix S_I = SquareMatrix(C*P*C.T() + R).I(); + Matrix K = P*C.T()*S_I; + dx = K*r; + beta = Scalar(r.T()*S_I*r); + dP = K*C*P*(-1); + return 0; +} + +} // namespace matrix diff --git a/src/lib/matrix/matrix/helper_functions.hpp b/src/lib/matrix/matrix/helper_functions.hpp new file mode 100644 index 0000000000..191ff12228 --- /dev/null +++ b/src/lib/matrix/matrix/helper_functions.hpp @@ -0,0 +1,127 @@ +#pragma once + +#include "math.hpp" + +#if defined (__PX4_NUTTX) || defined (__PX4_QURT) +#include +#endif + +namespace matrix +{ + +template +bool is_finite(Type x) { +#if defined (__PX4_NUTTX) + return PX4_ISFINITE(x); +#elif defined (__PX4_QURT) + return __builtin_isfinite(x); +#else + return std::isfinite(x); +#endif +} + +/** + * Compare if two floating point numbers are equal + * + * NAN is considered equal to NAN and -NAN + * INFINITY is considered equal INFINITY but not -INFINITY + * + * @param x right side of equality check + * @param y left side of equality check + * @param eps numerical tolerance of the check + * @return true if the two values are considered equal, false otherwise + */ +template +bool isEqualF(const Type x, const Type y, const Type eps = Type(1e-4f)) +{ + return (matrix::fabs(x - y) <= eps) + || (isnan(x) && isnan(y)) + || (isinf(x) && isinf(y) && isnan(x - y)); +} + +namespace detail +{ + +template +Floating wrap_floating(Floating x, Floating low, Floating high) { + // already in range + if (low <= x && x < high) { + return x; + } + + const auto range = high - low; + const auto inv_range = Floating(1) / range; // should evaluate at compile time, multiplies below at runtime + const auto num_wraps = floor((x - low) * inv_range); + return x - range * num_wraps; +} + +} // namespace detail + +/** + * Wrap single precision floating point value to stay in range [low, high) + * + * @param x input possibly outside of the range + * @param low lower limit of the allowed range + * @param high upper limit of the allowed range + * @return wrapped value inside the range + */ +inline float wrap(float x, float low, float high) { + return matrix::detail::wrap_floating(x, low, high); +} + +/** + * Wrap double precision floating point value to stay in range [low, high) + * + * @param x input possibly outside of the range + * @param low lower limit of the allowed range + * @param high upper limit of the allowed range + * @return wrapped value inside the range + */ +inline double wrap(double x, double low, double high) { + return matrix::detail::wrap_floating(x, low, high); +} + +/** + * Wrap integer value to stay in range [low, high) + * + * @param x input possibly outside of the range + * @param low lower limit of the allowed range + * @param high upper limit of the allowed range + * @return wrapped value inside the range + */ +template +Integer wrap(Integer x, Integer low, Integer high) { + const auto range = high - low; + + if (x < low) { + x += range * ((low - x) / range + 1); + } + + return low + (x - low) % range; +} + +/** + * Wrap value in range [-π, π) + */ +template +Type wrap_pi(Type x) +{ + return wrap(x, Type(-M_PI), Type(M_PI)); +} + +/** + * Wrap value in range [0, 2π) + */ +template +Type wrap_2pi(Type x) +{ + return wrap(x, Type(0), Type(M_TWOPI)); +} + +template +int sign(T val) +{ + return (T(FLT_EPSILON) < val) - (val < T(FLT_EPSILON)); +} + +} // namespace matrix diff --git a/src/lib/matrix/matrix/integration.hpp b/src/lib/matrix/matrix/integration.hpp new file mode 100644 index 0000000000..8909a17c60 --- /dev/null +++ b/src/lib/matrix/matrix/integration.hpp @@ -0,0 +1,42 @@ +#pragma once + +#include "math.hpp" + +namespace matrix { + +template +int integrate_rk4( + Vector (*f)(Type, const Matrix &x, const Matrix & u), + const Matrix & y0, + const Matrix & u, + Type t0, + Type tf, + Type h0, + Matrix & y1 +) +{ + // https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta_methods + Type t1 = t0; + y1 = y0; + Type h = h0; + Vector k1, k2, k3, k4; + if (tf < t0) return -1; // make sure t1 > t0 + while (t1 < tf) { + if (t1 + h0 < tf) { + h = h0; + } else { + h = tf - t1; + } + k1 = f(t1, y1, u); + k2 = f(t1 + h/2, y1 + k1*h/2, u); + k3 = f(t1 + h/2, y1 + k2*h/2, u); + k4 = f(t1 + h, y1 + k3*h, u); + y1 += (k1 + k2*2 + k3*2 + k4)*(h/6); + t1 += h; + } + return 0; +} + +} // namespace matrix + +// vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : diff --git a/src/lib/matrix/matrix/math.hpp b/src/lib/matrix/matrix/math.hpp new file mode 100644 index 0000000000..cc4e5e81d7 --- /dev/null +++ b/src/lib/matrix/matrix/math.hpp @@ -0,0 +1,23 @@ +#pragma once + +#include +#include "stdlib_imports.hpp" +#ifdef __PX4_QURT +#include "dspal_math.h" +#endif +#include "helper_functions.hpp" +#include "Matrix.hpp" +#include "SquareMatrix.hpp" +#include "Slice.hpp" +#include "Vector.hpp" +#include "Vector2.hpp" +#include "Vector3.hpp" +#include "Euler.hpp" +#include "Dcm.hpp" +#include "Scalar.hpp" +#include "Quaternion.hpp" +#include "AxisAngle.hpp" +#include "LeastSquaresSolver.hpp" +#include "Dual.hpp" +#include "PseudoInverse.hpp" +#include "SparseVector.hpp" diff --git a/src/lib/matrix/matrix/stdlib_imports.hpp b/src/lib/matrix/matrix/stdlib_imports.hpp new file mode 100644 index 0000000000..547b5fed75 --- /dev/null +++ b/src/lib/matrix/matrix/stdlib_imports.hpp @@ -0,0 +1,139 @@ +/** + * @file stdlib_imports.hpp + * + * This file is needed to shadow the C standard library math functions with ones provided by the C++ standard library. + * This way we can guarantee that unwanted functions from the C library will never creep back in unexpectedly. + * + * @author Pavel Kirienko + */ + +#pragma once + +#include +#include +#include + +#ifndef M_PI +#define M_PI 3.14159265358979323846 +#endif +#ifndef M_TWOPI +#define M_TWOPI (M_PI * 2.0) +#endif + +namespace matrix { + +#if !defined(FLT_EPSILON) +#define FLT_EPSILON __FLT_EPSILON__ +#endif + +#if defined(__PX4_NUTTX) +/* + * NuttX has no usable C++ math library, so we need to provide the needed definitions here manually. + */ +#define MATRIX_NUTTX_WRAP_MATH_FUN_UNARY(name) \ + inline float name(float x) { return ::name##f(x); } \ + inline double name(double x) { return ::name(x); } \ + inline long double name(long double x) { return ::name##l(x); } + +#define MATRIX_NUTTX_WRAP_MATH_FUN_BINARY(name) \ + inline float name(float x, float y) { return ::name##f(x, y); } \ + inline double name(double x, double y) { return ::name(x, y); } \ + inline long double name(long double x, long double y) { return ::name##l(x, y); } + +MATRIX_NUTTX_WRAP_MATH_FUN_UNARY(fabs) +MATRIX_NUTTX_WRAP_MATH_FUN_UNARY(log) +MATRIX_NUTTX_WRAP_MATH_FUN_UNARY(log10) +MATRIX_NUTTX_WRAP_MATH_FUN_UNARY(exp) +MATRIX_NUTTX_WRAP_MATH_FUN_UNARY(sqrt) +MATRIX_NUTTX_WRAP_MATH_FUN_UNARY(sin) +MATRIX_NUTTX_WRAP_MATH_FUN_UNARY(cos) +MATRIX_NUTTX_WRAP_MATH_FUN_UNARY(tan) +MATRIX_NUTTX_WRAP_MATH_FUN_UNARY(asin) +MATRIX_NUTTX_WRAP_MATH_FUN_UNARY(acos) +MATRIX_NUTTX_WRAP_MATH_FUN_UNARY(atan) +MATRIX_NUTTX_WRAP_MATH_FUN_UNARY(sinh) +MATRIX_NUTTX_WRAP_MATH_FUN_UNARY(cosh) +MATRIX_NUTTX_WRAP_MATH_FUN_UNARY(tanh) +MATRIX_NUTTX_WRAP_MATH_FUN_UNARY(ceil) +MATRIX_NUTTX_WRAP_MATH_FUN_UNARY(floor) + +MATRIX_NUTTX_WRAP_MATH_FUN_BINARY(pow) +MATRIX_NUTTX_WRAP_MATH_FUN_BINARY(atan2) + +#else // Not NuttX, using the C++ standard library + +using std::abs; +using std::div; +using std::fabs; +using std::fmod; +using std::exp; +using std::log; +using std::log10; +using std::pow; +using std::sqrt; +using std::sin; +using std::cos; +using std::tan; +using std::asin; +using std::acos; +using std::atan; +using std::atan2; +using std::sinh; +using std::cosh; +using std::tanh; +using std::ceil; +using std::floor; +using std::frexp; +using std::ldexp; +using std::modf; + +# if (__cplusplus >= 201103L) + +using std::remainder; +using std::remquo; +using std::fma; +using std::fmax; +using std::fmin; +using std::fdim; +using std::nan; +using std::nanf; +using std::nanl; +using std::exp2; +using std::expm1; +using std::log2; +using std::log1p; +using std::cbrt; +using std::hypot; +using std::asinh; +using std::acosh; +using std::atanh; +using std::erf; +using std::erfc; +using std::tgamma; +using std::lgamma; +using std::trunc; +using std::round; +using std::nearbyint; +using std::rint; +using std::scalbn; +using std::ilogb; +using std::logb; +using std::nextafter; +using std::copysign; +using std::fpclassify; +using std::isfinite; +using std::isinf; +using std::isnan; +using std::isnormal; +using std::signbit; +using std::isgreater; +using std::isgreaterequal; +using std::isless; +using std::islessequal; +using std::islessgreater; +using std::isunordered; + +# endif +#endif + +} diff --git a/src/lib/matrix/scripts/format.sh b/src/lib/matrix/scripts/format.sh new file mode 100755 index 0000000000..feee98a76c --- /dev/null +++ b/src/lib/matrix/scripts/format.sh @@ -0,0 +1,30 @@ +#!/bin/bash +echo pwd:$PWD +astyle=$1 +format=$2 +format_wildcards=""" +./matrix/*.*pp +./test/*.*pp +""" + +#echo astyle: $astyle +#echo format: $format + +if [[ $format -eq 1 ]] +then + echo formatting + $astyle ${format_wildcards} +else + echo checking format... + $astyle --dry-run ${format_wildcards} | grep Formatted &>/dev/null + if [[ $? -eq 0 ]] + then + echo Error: need to format + echo "From cmake build directory run: 'make format'" + exit 1 + fi + echo no formatting needed + exit 0 +fi + +# vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : diff --git a/src/lib/matrix/test/CMakeLists.txt b/src/lib/matrix/test/CMakeLists.txt new file mode 100644 index 0000000000..944bfc32a3 --- /dev/null +++ b/src/lib/matrix/test/CMakeLists.txt @@ -0,0 +1,67 @@ +include(gtest.cmake) + +set(tests + setIdentity + inverse + slice + matrixMult + vectorAssignment + matrixAssignment + matrixScalarMult + transpose + vector + vector2 + vector3 + attitude + filter + integration + sparseVector + squareMatrix + helper + hatvee + copyto + least_squares + upperRightTriangle + dual + pseudoInverse + ) + +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() +target_link_libraries(sparseVector gtest_main) + +if (${CMAKE_BUILD_TYPE} STREQUAL "Coverage") + + add_custom_target(coverage_build + COMMAND ${CMAKE_CTEST_COMMAND} + COMMAND lcov --capture --directory . --output-file coverage.info --rc lcov_branch_coverage=1 + COMMAND lcov --rc lcov_branch_coverage=1 --summary coverage.info + WORKING_DIRECTORY ${CMAKE_BUILD_DIR} + DEPENDS test_build + ) + + add_custom_target(coverage_html + COMMAND genhtml coverage.info --output-directory out --branch-coverage + COMMAND x-www-browser out/index.html + WORKING_DIRECTORY ${CMAKE_BUILD_DIR} + DEPENDS coverage_build + ) + + set(coverage_deps + coverage_build) + + if (COV_HTML) + list(APPEND coverage_deps coverage_html) + endif() + + add_custom_target(coverage + DEPENDS ${coverage_deps} + ) +endif() + +# vim: set et fenc=utf-8 ft=cmake ff=unix sts=0 sw=4 ts=4 : diff --git a/src/lib/matrix/test/CMakeLists.txt.in b/src/lib/matrix/test/CMakeLists.txt.in new file mode 100644 index 0000000000..a341cac5c5 --- /dev/null +++ b/src/lib/matrix/test/CMakeLists.txt.in @@ -0,0 +1,18 @@ +cmake_minimum_required(VERSION 2.8.4) + +project(googletest-download NONE) + +include(ExternalProject) +ExternalProject_Add(googletest + URL https://github.com/google/googletest/archive/e2239ee6043f73722e7aa812a459f54a28552929.zip + SOURCE_DIR "${CMAKE_CURRENT_BINARY_DIR}/googletest-src" + BINARY_DIR "${CMAKE_CURRENT_BINARY_DIR}/googletest-build" + CONFIGURE_COMMAND "" + BUILD_COMMAND "" + INSTALL_COMMAND "" + TEST_COMMAND "" + # Wrap download, configure and build steps in a script to log output + LOG_DOWNLOAD ON + LOG_CONFIGURE ON + LOG_BUILD ON +) diff --git a/src/lib/matrix/test/attitude.cpp b/src/lib/matrix/test/attitude.cpp new file mode 100644 index 0000000000..d8954965d3 --- /dev/null +++ b/src/lib/matrix/test/attitude.cpp @@ -0,0 +1,490 @@ +#include "test_macros.hpp" +#include +#include + +using namespace matrix; + +// manually instantiated all files we intend to test +// so that coverage works correctly +// doesn't matter what test this is in +namespace matrix { +template class Matrix; +template class Vector3; +template class Vector2; +template class Vector; +template class Quaternion; +template class AxisAngle; +template class Scalar; +template class SquareMatrix; +} + +int main() +{ + // check data + Eulerf euler_check(0.1f, 0.2f, 0.3f); + Quatf q_check(0.98334744f, 0.0342708f, 0.10602051f, .14357218f); + float dcm_data[] = { + 0.93629336f, -0.27509585f, 0.21835066f, + 0.28962948f, 0.95642509f, -0.03695701f, + -0.19866933f, 0.0978434f, 0.97517033f + }; + Dcmf dcm_check(dcm_data); + + // euler ctor + TEST(isEqual(euler_check, Vector3f(0.1f, 0.2f, 0.3f))); + + // euler default ctor + Eulerf e; + Eulerf e_zero = zeros(); + TEST(isEqual(e, e_zero)); + TEST(isEqual(e, e)); + + // euler vector ctor + Vector3f v(0.1f, 0.2f, 0.3f); + Eulerf euler_copy(v); + TEST(isEqual(euler_copy, euler_check)); + + // quaternion ctor + Quatf q0(1, 2, 3, 4); + Quatf q(q0); + double eps = 1e-6; + TEST(fabs(q(0) - 1) < eps); + TEST(fabs(q(1) - 2) < eps); + TEST(fabs(q(2) - 3) < eps); + TEST(fabs(q(3) - 4) < eps); + + // quaternion ctor: vector to vector + // identity test + Quatf quat_v(v,v); + TEST(isEqual(quat_v.conjugate(v), v)); + // random test (vector norm can not be preserved with a pure rotation) + Vector3f v1(-80.1f, 1.5f, -6.89f); + quat_v = Quatf(v1, v); + TEST(isEqual(quat_v.conjugate(v1).normalized() * v.norm(), v)); + // special 180 degree case 1 + v1 = Vector3f(0.f, 1.f, 1.f); + quat_v = Quatf(v1, -v1); + TEST(isEqual(quat_v.conjugate(v1), -v1)); + // special 180 degree case 2 + v1 = Vector3f(1.f, 2.f, 0.f); + quat_v = Quatf(v1, -v1); + TEST(isEqual(quat_v.conjugate(v1), -v1)); + // special 180 degree case 3 + v1 = Vector3f(0.f, 0.f, 1.f); + quat_v = Quatf(v1, -v1); + TEST(isEqual(quat_v.conjugate(v1), -v1)); + // special 180 degree case 4 + v1 = Vector3f(1.f, 1.f, 1.f); + quat_v = Quatf(v1, -v1); + TEST(isEqual(quat_v.conjugate(v1), -v1)); + + // quat normalization + q.normalize(); + TEST(isEqual(q, Quatf(0.18257419f, 0.36514837f, + 0.54772256f, 0.73029674f))); + TEST(isEqual(q0.unit(), q)); + TEST(isEqual(q0.unit(), q0.normalized())); + + // quat default ctor + q = Quatf(); + TEST(isEqual(q, Quatf(1, 0, 0, 0))); + + // quaternion exponential with v=0 + v = Vector3f(); + q = Quatf(1.0f, 0.0f, 0.0f, 0.0f); + Dcmf M = Dcmf()*0.5f; + TEST(isEqual(q, Quatf::expq(v))); + TEST(isEqual(M, Quatf::inv_r_jacobian(v))); + + // quaternion exponential with small v + v = Vector3f(0.001f,0.002f,-0.003f); + q = Quatf(0.999993000008167f, 0.000999997666668f, + 0.001999995333337f, -0.002999993000005f); + { + float M_data[] = { + 0.499997833331311f, 0.001500333333644f, 0.000999499999533f, + -0.001499666666356f, 0.499998333331778f, -0.000501000000933f, + -0.001000500000467f, 0.000498999999067f, 0.499999166665889f + }; + M = Dcmf(M_data); + } + TEST(isEqual(q, Quatf::expq(v))); + TEST(isEqual(M, Quatf::inv_r_jacobian(v))); + + // quaternion exponential with v + v = Vector3f(1.0f, -2.0f, 3.0f); + q = Quatf(-0.825299062075259f, -0.150921327219964f, + 0.301842654439929f, -0.452763981659893f); + { + float M_data[] = { + 2.574616981530584f, -1.180828156687602f, -1.478757764968596f, + 1.819171843312398f, 2.095859216561988f, 0.457515529937193f, + 0.521242235031404f, 1.457515529937193f, 1.297929608280994f + }; + M = Dcmf(M_data); + } + TEST(isEqual(q, Quatf::expq(v))); + TEST(isEqual(M, Quatf::inv_r_jacobian(v))); + + // quaternion kinematic update + q = Quatf(); + float h=0.001f; // sampling time [s] + Vector3f w_B=Vector3f(0.1f,0.2f,0.3f); // body rate in body frame + Quatf qa=q+0.5f*h*q.derivative1(w_B); + qa.normalize(); + Quatf qb=q*Quatf::expq(0.5f*h*w_B); + TEST(isEqual(qa, qb)); + + // euler to quaternion + q = Quatf(euler_check); + TEST(isEqual(q, q_check)); + + // euler to dcm + Dcmf dcm(euler_check); + TEST(isEqual(dcm, dcm_check)); + + // quaternion to euler + Eulerf e1(q_check); + TEST(isEqual(e1, euler_check)); + + // quaternion to dcm + Dcmf dcm1(q_check); + TEST(isEqual(dcm1, dcm_check)); + // quaternion z-axis unit base vector + Vector3f q_z = q_check.dcm_z(); + Vector3f R_z(dcm_check(0, 2), dcm_check(1, 2), dcm_check(2, 2)); + TEST(isEqual(q_z, R_z)); + + // dcm default ctor + Dcmf dcm2; + SquareMatrix I = eye(); + TEST(isEqual(dcm2, I)); + + // dcm to euler + Eulerf e2(dcm_check); + TEST(isEqual(e2, euler_check)); + + // dcm to quaterion + Quatf q2(dcm_check); + TEST(isEqual(q2, q_check)); + + // dcm renormalize + Dcmf A = eye(); + Dcmf R(euler_check); + for (size_t i = 0; i < 1000; i++) { + A = R * A; + } + + A.renormalize(); + float err = 0.0f; + + for (size_t r = 0; r < 3; r++) { + Vector3f rvec(matrix::Matrix(A.row(r)).transpose()); + err += fabs(1.0f - rvec.length()); + } + TEST(err < eps); + + // constants + double deg2rad = M_PI / 180.0; + double rad2deg = 180.0 / M_PI; + + // euler dcm round trip check + for (double roll = -90; roll <= 90; roll += 90) { + for (double pitch = -90; pitch <= 90; pitch += 90) { + for (double yaw = -179; yaw <= 180; yaw += 90) { + // note if theta = pi/2, then roll is set to zero + double roll_expected = roll; + double yaw_expected = yaw; + + if (fabs(pitch -90) < eps) { + roll_expected = 0; + yaw_expected = yaw - roll; + + } else if (fabs(pitch + 90) < eps) { + roll_expected = 0; + yaw_expected = yaw + roll; + } + + if (yaw_expected < -180) { + yaw_expected += 360; + } + + if (yaw_expected > 180) { + yaw_expected -= 360; + } + + //printf("roll:%d pitch:%d yaw:%d\n", roll, pitch, yaw); + Euler euler_expected( + deg2rad * roll_expected, + deg2rad * pitch, + deg2rad * yaw_expected); + Euler euler( + deg2rad * roll, + deg2rad * pitch, + deg2rad * yaw); + Dcm dcm_from_euler(euler); + //dcm_from_euler.print(); + Euler euler_out(dcm_from_euler); + TEST(isEqual(rad2deg * euler_expected, rad2deg * euler_out)); + + Eulerf eulerf_expected( + float(deg2rad)*float(roll_expected), + float(deg2rad)*float(pitch), + float(deg2rad)*float(yaw_expected)); + Eulerf eulerf(float(deg2rad)*float(roll), + float(deg2rad)*float(pitch), + float(deg2rad)*float(yaw)); + Dcm dcm_from_eulerf; + dcm_from_eulerf = eulerf; + Euler euler_outf(dcm_from_eulerf); + TEST(isEqual(float(rad2deg)*eulerf_expected, + float(rad2deg)*euler_outf)); + } + } + } + + // quaterion copy ctors + float data_v4[] = {1, 2, 3, 4}; + Vector v4(data_v4); + Quatf q_from_v(v4); + TEST(isEqual(q_from_v, v4)); + + Matrix m4(data_v4); + Quatf q_from_m(m4); + TEST(isEqual(q_from_m, m4)); + + // quaternion derivative in frame 1 + Quatf q1(0, 1, 0, 0); + Vector q1_dot1 = q1.derivative1(Vector3f(1, 2, 3)); + float data_q_dot1_check[] = { -0.5f, 0.0f, -1.5f, 1.0f}; + Vector q1_dot1_check(data_q_dot1_check); + TEST(isEqual(q1_dot1, q1_dot1_check)); + + // quaternion derivative in frame 2 + Vector q1_dot2 = q1.derivative2(Vector3f(1, 2, 3)); + float data_q_dot2_check[] = { -0.5f, 0.0f, 1.5f, -1.0f}; + Vector q1_dot2_check(data_q_dot2_check); + TEST(isEqual(q1_dot2, q1_dot2_check)); + + // quaternion product + Quatf q_prod_check( + 0.93394439f, 0.0674002f, 0.20851f, 0.28236266f); + TEST(isEqual(q_prod_check, q_check * q_check)); + q_check *= q_check; + TEST(isEqual(q_prod_check, q_check)); + + // Quaternion scalar multiplication + float scalar = 0.5; + Quatf q_scalar_mul(1.0f, 2.0f, 3.0f, 4.0f); + Quatf q_scalar_mul_check(1.0f * scalar, 2.0f * scalar, + 3.0f * scalar, 4.0f * scalar); + Quatf q_scalar_mul_res = scalar * q_scalar_mul; + TEST(isEqual(q_scalar_mul_check, q_scalar_mul_res)); + Quatf q_scalar_mul_res2 = q_scalar_mul * scalar; + TEST(isEqual(q_scalar_mul_check, q_scalar_mul_res2)); + Quatf q_scalar_mul_res3(q_scalar_mul); + q_scalar_mul_res3 *= scalar; + TEST(isEqual(q_scalar_mul_check, q_scalar_mul_res3)); + + // quaternion inverse + q = q_check.inversed(); + TEST(fabs(q_check(0) - q(0)) < eps); + TEST(fabs(q_check(1) + q(1)) < eps); + TEST(fabs(q_check(2) + q(2)) < eps); + TEST(fabs(q_check(3) + q(3)) < eps); + + q = q_check; + q.invert(); + TEST(fabs(q_check(0) - q(0)) < eps); + TEST(fabs(q_check(1) + q(1)) < eps); + TEST(fabs(q_check(2) + q(2)) < eps); + TEST(fabs(q_check(3) + q(3)) < eps); + + // quaternion canonical + Quatf q_non_canonical_1(-0.7f,0.4f, 0.3f, -0.3f); + Quatf q_canonical_1(0.7f,-0.4f, -0.3f, 0.3f); + Quatf q_canonical_ref_1(0.7f,-0.4f, -0.3f, 0.3f); + TEST(isEqual(q_non_canonical_1.canonical(),q_canonical_ref_1)); + TEST(isEqual(q_canonical_1.canonical(),q_canonical_ref_1)); + q_non_canonical_1.canonicalize(); + q_canonical_1.canonicalize(); + TEST(isEqual(q_non_canonical_1,q_canonical_ref_1)); + TEST(isEqual(q_canonical_1,q_canonical_ref_1)); + + Quatf q_non_canonical_2(0.0f, -1.0f, 0.0f, 0.0f); + Quatf q_canonical_2(0.0f, 1.0f, 0.0f, 0.0f); + Quatf q_canonical_ref_2(0.0f, 1.0f, 0.0f, 0.0f); + TEST(isEqual(q_non_canonical_2.canonical(),q_canonical_ref_2)); + TEST(isEqual(q_canonical_2.canonical(),q_canonical_ref_2)); + q_non_canonical_2.canonicalize(); + q_canonical_2.canonicalize(); + TEST(isEqual(q_non_canonical_2,q_canonical_ref_2)); + TEST(isEqual(q_canonical_2,q_canonical_ref_2)); + + Quatf q_non_canonical_3(0.0f, 0.0f, -1.0f, 0.0f); + Quatf q_canonical_3(0.0f, 0.0f, 1.0f, 0.0f); + Quatf q_canonical_ref_3(0.0f, 0.0f, 1.0f, 0.0f); + TEST(isEqual(q_non_canonical_3.canonical(),q_canonical_ref_3)); + TEST(isEqual(q_canonical_3.canonical(),q_canonical_ref_3)); + q_non_canonical_3.canonicalize(); + q_canonical_3.canonicalize(); + TEST(isEqual(q_non_canonical_3,q_canonical_ref_3)); + TEST(isEqual(q_canonical_3,q_canonical_ref_3)); + + Quatf q_non_canonical_4(0.0f, 0.0f, 0.0f, -1.0f); + Quatf q_canonical_4(0.0f, 0.0f, 0.0f, 1.0f); + Quatf q_canonical_ref_4(0.0f, 0.0f, 0.0f, 1.0f); + TEST(isEqual(q_non_canonical_4.canonical(),q_canonical_ref_4)); + TEST(isEqual(q_canonical_4.canonical(),q_canonical_ref_4)); + q_non_canonical_4.canonicalize(); + q_canonical_4.canonicalize(); + TEST(isEqual(q_non_canonical_4,q_canonical_ref_4)); + TEST(isEqual(q_canonical_4,q_canonical_ref_4)); + + Quatf q_non_canonical_5(0.0f, 0.0f, 0.0f, 0.0f); + Quatf q_canonical_5(0.0f, 0.0f, 0.0f, 0.0f); + Quatf q_canonical_ref_5(0.0f, 0.0f, 0.0f, 0.0f); + TEST(isEqual(q_non_canonical_5.canonical(),q_canonical_ref_5)); + TEST(isEqual(q_canonical_5.canonical(),q_canonical_ref_5)); + q_non_canonical_5.canonicalize(); + q_canonical_5.canonicalize(); + TEST(isEqual(q_non_canonical_5,q_canonical_ref_5)); + TEST(isEqual(q_canonical_5,q_canonical_ref_5)); + + // quaternion setIdentity + Quatf q_nonIdentity(-0.7f, 0.4f, 0.5f, -0.3f); + q_nonIdentity.setIdentity(); + TEST(isEqual(q_nonIdentity, Quatf())); + + // non-unit quaternion invese + Quatf q_nonunit(0.1f, 0.2f, 0.3f, 0.4f); + TEST(isEqual(q_nonunit*q_nonunit.inversed(), Quatf())); + + // rotate quaternion (nonzero rotation) + Vector3f rot(1.f, 0.f, 0.f); + Quatf q_test; + q_test.rotate(rot); + Quatf q_true(cos(1.0f / 2), sin(1.0f / 2), 0.0f, 0.0f); + TEST(isEqual(q_test, q_true)); + + // rotate quaternion (zero rotation) + rot(0) = rot(1) = rot(2) = 0.0f; + q_test = Quatf(); + q_test.rotate(rot); + q_true = Quatf(cos(0.0f), sin(0.0f), 0.0f, 0.0f); + TEST(isEqual(q_test, q_true)); + + // rotate quaternion (random non-commutating rotation) + q = Quatf(AxisAnglef(5.1f, 3.2f, 8.4f)); + rot = Vector3f(1.1f, 2.5f, 3.8f); + q.rotate(rot); + q_true = Quatf(0.3019f, 0.2645f, 0.2268f, 0.8874f); + TEST(isEqual(q, q_true)); + + // get rotation axis from quaternion (nonzero rotation) + q = Quatf(cos(1.0f / 2), 0.0f, sin(1.0f / 2), 0.0f); + rot = AxisAnglef(q); + TEST(fabs(rot(0)) < eps); + TEST(fabs(rot(1) - 1.0f) < eps); + TEST(fabs(rot(2)) < eps); + + // get rotation axis from quaternion (zero rotation) + q = Quatf(1.0f, 0.0f, 0.0f, 0.0f); + rot = AxisAnglef(q); + TEST(fabs(rot(0)) < eps); + TEST(fabs(rot(1)) < eps); + TEST(fabs(rot(2)) < eps); + + // from axis angle (zero rotation) + rot(0) = rot(1) = rot(2) = 0.0f; + q = Quatf(AxisAnglef(rot)); + q_true = Quatf(1.0f, 0.0f, 0.0f, 0.0f); + TEST(isEqual(q, q_true)); + + // from axis angle, with length of vector the rotation + float n = float(sqrt(4*M_PI*M_PI/3)); + q = AxisAnglef(n, n, n); + TEST(isEqual(q, Quatf(-1, 0, 0, 0))); + q = AxisAnglef(0, 0, 0); + TEST(isEqual(q, Quatf(1, 0, 0, 0))); + + // Quaternion initialisation per array + float q_array[] = {0.9833f, -0.0343f, -0.1060f, -0.1436f}; + Quaternionq_from_array(q_array); + + for (size_t i = 0; i < 4; i++) { + TEST(fabs(q_from_array(i) - q_array[i]) < eps); + } + + // axis angle + AxisAnglef aa_true(Vector3f(1.0f, 2.0f, 3.0f)); + TEST(isEqual(aa_true, Vector3f(1.0f, 2.0f, 3.0f))); + AxisAnglef aa_empty; + TEST(isEqual(aa_empty, AxisAnglef(0.0f, 0.0f, 0.0f))); + float aa_data[] = {4.0f, 5.0f, 6.0f}; + AxisAnglef aa_data_init(aa_data); + TEST(isEqual(aa_data_init, AxisAnglef(4.0f, 5.0f, 6.0f))); + + AxisAnglef aa_norm_check(Vector3f(0.0f, 0.0f, 0.0f)); + TEST(isEqual(aa_norm_check.axis(), Vector3f(1, 0, 0))); + TEST(isEqualF(aa_norm_check.angle(), 0.0f)); + + q = Quatf(-0.29555112749297824f, 0.25532186f, 0.51064372f, 0.76596558f); + float r_array[9] = {-0.6949206f, 0.713521f, 0.089292854f, -0.19200698f, -0.30378509f, 0.93319237f, 0.69297814f, 0.63134968f, 0.34810752f}; + R = Dcmf(r_array); + TEST(isEqual(q.imag(), Vector3f(0.25532186f, 0.51064372f, 0.76596558f))); + + // from dcm + TEST(isEqual(Quatf(R), q)); + TEST(isEqual(Quatf(Dcmf(q)), q)); + + // to dcm + TEST(isEqual(Dcmf(q), R)); + TEST(isEqual(Dcmf(Quatf(R)), R)); + + // conjugate + v = Vector3f(1.5f, 2.2f, 3.2f); + TEST(isEqual(q.conjugate_inversed(v1), Dcmf(q).T()*v1)); + TEST(isEqual(q.conjugate(v1), Dcmf(q)*v1)); + + AxisAnglef aa_q_init(q); + TEST(isEqual(aa_q_init, AxisAnglef(1.0f, 2.0f, 3.0f))); + + AxisAnglef aa_euler_init(Eulerf(0.0f, 0.0f, 0.0f)); + TEST(isEqual(aa_euler_init, Vector3f(0.0f, 0.0f, 0.0f))); + + Dcmf dcm_aa_check = AxisAnglef(dcm_check); + TEST(isEqual(dcm_aa_check, dcm_check)); + + AxisAnglef aa_axis_angle_init(Vector3f(1.0f, 2.0f, 3.0f), 3.0f); + TEST(isEqual(aa_axis_angle_init, Vector3f(0.80178373f, 1.60356745f, 2.40535118f))); + TEST(isEqual(aa_axis_angle_init.axis(), Vector3f(0.26726124f, 0.53452248f, 0.80178373f))); + TEST(isEqualF(aa_axis_angle_init.angle(), 3.0f)); + TEST(isEqual(Quatf((AxisAnglef(Vector3f(0.0f, 0.0f, 1.0f), 0.0f))), + Quatf(1.0f, 0.0f, 0.0f, 0.0f))); + + + // check consistentcy of quaternion and dcm product + Dcmf dcm3(Eulerf(1, 2, 3)); + Dcmf dcm4(Eulerf(4, 5, 6)); + Dcmf dcm34 = dcm3 * dcm4; + TEST(isEqual(Eulerf(Quatf(dcm3)*Quatf(dcm4)), Eulerf(dcm34))); + + // check corner cases of matrix to quaternion conversion + q = Quatf(0,1,0,0); // 180 degree rotation around the x axis + R = Dcmf(q); + TEST(isEqual(q, Quatf(R))); + q = Quatf(0,0,1,0); // 180 degree rotation around the y axis + R = Dcmf(q); + TEST(isEqual(q, Quatf(R))); + q = Quatf(0,0,0,1); // 180 degree rotation around the z axis + R = Dcmf(q); + TEST(isEqual(q, Quatf(R))); + +#if defined(SUPPORT_STDIOSTREAM) + std::cout << "q:" << q; +#endif + return 0; +} + +/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */ diff --git a/src/lib/matrix/test/copyto.cpp b/src/lib/matrix/test/copyto.cpp new file mode 100644 index 0000000000..4e72a1a91d --- /dev/null +++ b/src/lib/matrix/test/copyto.cpp @@ -0,0 +1,72 @@ +#include "test_macros.hpp" +#include + +using namespace matrix; + +namespace { +void doTheCopy(const Matrix& A, float array_A[6]) +{ + A.copyTo(array_A); +} +} + +int main() +{ + float eps = 1e-6f; + + // Vector3 copyTo + const Vector3f v(1, 2, 3); + float dst3[3] = {}; + v.copyTo(dst3); + for (size_t i = 0; i < 3; i++) { + TEST(fabs(v(i) - dst3[i]) < eps); + } + + // Quaternion copyTo + Quatf q(1, 2, 3, 4); + float dst4[4] = {}; + q.copyTo(dst4); + for (size_t i = 0; i < 4; i++) { + TEST(fabs(q(i) - dst4[i]) < eps); + } + + // Matrix copyTo + Matrix A; + A(0,0) = 1; + A(0,1) = 2; + A(0,2) = 3; + A(1,0) = 4; + A(1,1) = 5; + A(1,2) = 6; + float array_A[6] = {}; + doTheCopy(A, array_A); + float array_row[6] = {1, 2, 3, 4, 5, 6}; + for (size_t i = 0; i < 6; i++) { + TEST(fabs(array_A[i] - array_row[i]) < eps); + } + + // Matrix copyToColumnMajor + A.copyToColumnMajor(array_A); + float array_column[6] = {1, 4, 2, 5, 3, 6}; + for (size_t i = 0; i < 6; i++) { + TEST(fabs(array_A[i] - array_column[i]) < eps); + } + + // Slice copyTo + float dst5[2] = {}; + v.slice<2,1>(0,0).copyTo(dst5); + for (size_t i = 0; i < 2; i++) { + TEST(fabs(v(i) - dst5[i]) < eps); + } + + float subarray_A[4] = {}; + A.slice<2,2>(0,0).copyToColumnMajor(subarray_A); + float subarray_column[4] = {1,4,2,5}; + for (size_t i = 0; i < 4; i++) { + TEST(fabs(subarray_A[i] - subarray_column[i]) < eps); + } + + return 0; +} + +/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */ diff --git a/src/lib/matrix/test/dual.cpp b/src/lib/matrix/test/dual.cpp new file mode 100644 index 0000000000..7d5fbb9db7 --- /dev/null +++ b/src/lib/matrix/test/dual.cpp @@ -0,0 +1,310 @@ +#include "test_macros.hpp" +#include +#include + +using namespace matrix; + +template +bool isEqualAll(Dual a, Dual b) +{ + return isEqualF(a.value, b.value) && a.derivative == b.derivative; +} + +template +T testFunction(const Vector& point) { + // function is f(x,y,z) = x^2 + 2xy + 3y^2 + z + return point(0)*point(0) + + 2.f * point(0) * point(1) + + 3.f * point(1) * point(1) + + point(2); +} + +template +Vector positionError(const Vector& positionState, + const Vector& velocityStateBody, + const Quaternion& bodyOrientation, + const Vector& positionMeasurement, + Scalar dt + ) +{ + return positionMeasurement - (positionState + bodyOrientation.conjugate(velocityStateBody) * dt); +} + +int main() +{ + const Dual a(3,0); + const Dual b(6,0); + + { + TEST(isEqualF(a.value, 3.f)); + TEST(isEqualF(a.derivative(0), 1.f)); + } + + { + // addition + Dual c = a + b; + TEST(isEqualF(c.value, 9.f)); + TEST(isEqualF(c.derivative(0), 2.f)); + + Dual d = +a; + TEST(isEqualAll(d, a)); + d += b; + TEST(isEqualAll(d, c)); + + Dual e = a; + e += b.value; + TEST(isEqualF(e.value, c.value)); + TEST(isEqual(e.derivative, a.derivative)); + + Dual f = b.value + a; + TEST(isEqualAll(f, e)); + } + + { + // subtraction + Dual c = b - a; + TEST(isEqualF(c.value, 3.f)); + TEST(isEqualF(c.derivative(0), 0.f)); + + Dual d = b; + TEST(isEqualAll(d, b)); + d -= a; + TEST(isEqualAll(d, c)); + + Dual e = b; + e -= a.value; + TEST(isEqualF(e.value, c.value)); + TEST(isEqual(e.derivative, b.derivative)); + + Dual f = a.value - b; + TEST(isEqualAll(f, -e)); + } + + { + // multiplication + Dual c = a*b; + TEST(isEqualF(c.value, 18.f)); + TEST(isEqualF(c.derivative(0), 9.f)); + + Dual d = a; + TEST(isEqualAll(d, a)); + d *= b; + TEST(isEqualAll(d, c)); + + Dual e = a; + e *= b.value; + TEST(isEqualF(e.value, c.value)); + TEST(isEqual(e.derivative, a.derivative * b.value)); + + Dual f = b.value * a; + TEST(isEqualAll(f, e)); + } + + { + // division + Dual c = b/a; + TEST(isEqualF(c.value, 2.f)); + TEST(isEqualF(c.derivative(0), -1.f/3.f)); + + Dual d = b; + TEST(isEqualAll(d, b)); + d /= a; + TEST(isEqualAll(d, c)); + + Dual e = b; + e /= a.value; + TEST(isEqualF(e.value, c.value)); + TEST(isEqual(e.derivative, b.derivative / a.value)); + + Dual f = a.value / b; + TEST(isEqualAll(f, 1.f/e)); + } + + { + Dual blank; + TEST(isEqualF(blank.value, 0.f)); + TEST(isEqualF(blank.derivative(0), 0.f)); + } + + { + // sqrt + TEST(isEqualF(sqrt(a).value, sqrt(a.value))); + TEST(isEqualF(sqrt(a).derivative(0), 1.f/sqrt(12.f))); + } + + { + // abs + TEST(isEqualAll(a, abs(-a))); + TEST(!isEqualAll(-a, abs(a))); + TEST(isEqualAll(-a, -abs(a))); + } + + { + // ceil + Dual c(1.5,0); + TEST(isEqualF(ceil(c).value, ceil(c.value))); + TEST(isEqualF(ceil(c).derivative(0), 0.f)); + } + + { + // floor + Dual c(1.5,0); + TEST(isEqualF(floor(c).value, floor(c.value))); + TEST(isEqualF(floor(c).derivative(0), 0.f)); + } + + { + // fmod + TEST(isEqualF(fmod(a, 0.8f).value, fmod(a.value, 0.8f))); + TEST(isEqual(fmod(a, 0.8f).derivative, a.derivative)); + } + + { + // max/min + TEST(isEqualAll(b, max(a, b))); + TEST(isEqualAll(a, min(a, b))); + } + + { + // isnan + TEST(!IsNan(a)); + Dual c(sqrt(-1.f),0); + TEST(IsNan(c)); + } + + { + // isfinite/isinf + TEST(IsFinite(a)); + TEST(!IsInf(a)); + Dual c(sqrt(-1.f),0); + TEST(!IsFinite(c)); + TEST(!IsInf(c)); + Dual d(INFINITY,0); + TEST(!IsFinite(d)); + TEST(IsInf(d)); + } + + { + // sin/cos/tan + TEST(isEqualF(sin(a).value, sin(a.value))); + TEST(isEqualF(sin(a).derivative(0), cos(a.value))); // sin'(x) = cos(x) + + TEST(isEqualF(cos(a).value, cos(a.value))); + TEST(isEqualF(cos(a).derivative(0), -sin(a.value))); // cos'(x) = -sin(x) + + TEST(isEqualF(tan(a).value, tan(a.value))); + TEST(isEqualF(tan(a).derivative(0), 1.f + tan(a.value)*tan(a.value))); // tan'(x) = 1 + tan^2(x) + } + + { + // asin/acos/atan + Dual c(0.3f, 0); + TEST(isEqualF(asin(c).value, asin(c.value))); + TEST(isEqualF(asin(c).derivative(0), 1.f/sqrt(1.f - 0.3f*0.3f))); // asin'(x) = 1/sqrt(1-x^2) + + TEST(isEqualF(acos(c).value, acos(c.value))); + TEST(isEqualF(acos(c).derivative(0), -1.f/sqrt(1.f - 0.3f*0.3f))); // acos'(x) = -1/sqrt(1-x^2) + + TEST(isEqualF(atan(c).value, atan(c.value))); + TEST(isEqualF(atan(c).derivative(0), 1.f/(1.f + 0.3f*0.3f))); // tan'(x) = 1 + x^2 + } + + { + // atan2 + TEST(isEqualF(atan2(a, b).value, atan2(a.value, b.value))); + TEST(isEqualAll(atan2(a, Dual(b.value)), atan(a/b.value))); // atan2'(y, x) = atan'(y/x) + } + + { + // partial derivatives + // function is f(x,y,z) = x^2 + 2xy + 3y^2 + z, we need with respect to d/dx and d/dy at the point (0.5, -0.8, 2) + + using D = Dual; + + // set our starting point, requesting partial derivatives of x and y in column 0 and 1 + Vector3 dualPoint(D(0.5f, 0), D(-0.8f, 1), D(2.f)); + + Dual dualResult = testFunction(dualPoint); + + // compare to a numerical derivative: + Vector floatPoint = collectReals(dualPoint); + float floatResult = testFunction(floatPoint); + + float h = 0.0001f; + Vector floatPoint_plusDX = floatPoint; + floatPoint_plusDX(0) += h; + float floatResult_plusDX = testFunction(floatPoint_plusDX); + + Vector floatPoint_plusDY = floatPoint; + floatPoint_plusDY(1) += h; + float floatResult_plusDY = testFunction(floatPoint_plusDY); + + Vector2f numerical_derivative((floatResult_plusDX - floatResult)/h, + (floatResult_plusDY - floatResult)/h); + + TEST(isEqualF(dualResult.value, floatResult, 0.0f)); + TEST(isEqual(dualResult.derivative, numerical_derivative, 1e-2f)); + + } + + { + // jacobian + // get residual of x/y/z with partial derivatives of rotation + + Vector3f direct_error; + Matrix numerical_jacobian; + { + Vector3f positionState(5,6,7); + Vector3f velocityState(-1,0,1); + Quaternionf velocityOrientation(0.2f,-0.1f,0,1); + Vector3f positionMeasurement(4.5f, 6.2f, 7.9f); + float dt = 0.1f; + + direct_error = positionError(positionState, + velocityState, + velocityOrientation, + positionMeasurement, + dt); + float h = 0.001f; + for (size_t i = 0; i < 4; i++) + { + Quaternion h4 = velocityOrientation; + h4(i) += h; + numerical_jacobian.col(i) = (positionError(positionState, + velocityState, + h4, + positionMeasurement, + dt) + - direct_error)/h; + } + } + Vector3f auto_error; + Matrix auto_jacobian; + { + using D4 = Dual; + using Vector3d4 = Vector3; + Vector3d4 positionState(D4(5), D4(6), D4(7)); + Vector3d4 velocityState(D4(-1), D4(0), D4(1)); + + // request partial derivatives of velocity orientation + // by setting these variables' derivatives in corresponding columns [0...3] + Quaternion velocityOrientation(D4(0.2f, 0),D4(-0.1f, 1),D4(0, 2),D4(1, 3)); + + Vector3d4 positionMeasurement(D4(4.5f), D4(6.2f), D4(7.9f)); + D4 dt(0.1f); + + + Vector3d4 error = positionError(positionState, + velocityState, + velocityOrientation, + positionMeasurement, + dt); + auto_error = collectReals(error); + auto_jacobian = collectDerivatives(error); + } + TEST(isEqual(direct_error, auto_error, 0.0f)); + TEST(isEqual(numerical_jacobian, auto_jacobian, 1e-3f)); + + } + return 0; +} diff --git a/src/lib/matrix/test/filter.cpp b/src/lib/matrix/test/filter.cpp new file mode 100644 index 0000000000..342da22824 --- /dev/null +++ b/src/lib/matrix/test/filter.cpp @@ -0,0 +1,29 @@ +#include "test_macros.hpp" +#include + +using namespace matrix; + +int main() +{ + const size_t n_x = 6; + const size_t n_y = 5; + SquareMatrix P = eye(); + SquareMatrix R = eye(); + Matrix C; + C.setIdentity(); + float data[] = {1,2,3,4,5}; + Vector r(data); + + Vector dx; + SquareMatrix dP; + float beta = 0; + kalman_correct(P, C, R, r, dx, dP, beta); + + float data_check[] = {0.5,1,1.5,2,2.5,0}; + Vector dx_check(data_check); + TEST(isEqual(dx, dx_check)); + + return 0; +} + +/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */ diff --git a/src/lib/matrix/test/gtest.cmake b/src/lib/matrix/test/gtest.cmake new file mode 100644 index 0000000000..f1c07b3576 --- /dev/null +++ b/src/lib/matrix/test/gtest.cmake @@ -0,0 +1,12 @@ +set_directory_properties(PROPERTIES COMPILE_OPTIONS "") + +# Download and unpack googletest at configure time +configure_file(${CMAKE_CURRENT_LIST_DIR}/CMakeLists.txt.in googletest-download/CMakeLists.txt) +execute_process(COMMAND ${CMAKE_COMMAND} -G "${CMAKE_GENERATOR}" . RESULT_VARIABLE result1 WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/googletest-download) +execute_process(COMMAND ${CMAKE_COMMAND} --build . RESULT_VARIABLE result2 WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/googletest-download) +if(result1 OR result2) + message(FATAL_ERROR "Preparing googletest failed: ${result1} ${result2}") +endif() + +# Add googletest, defines gtest and gtest_main targets +add_subdirectory(${CMAKE_CURRENT_BINARY_DIR}/googletest-src ${CMAKE_CURRENT_BINARY_DIR}/googletest-build EXCLUDE_FROM_ALL) diff --git a/src/lib/matrix/test/hatvee.cpp b/src/lib/matrix/test/hatvee.cpp new file mode 100644 index 0000000000..aa2c2cf90e --- /dev/null +++ b/src/lib/matrix/test/hatvee.cpp @@ -0,0 +1,19 @@ +#include "test_macros.hpp" +#include + +using namespace matrix; + +int main() +{ + Euler euler(0.1f, 0.2f, 0.3f); + Dcm R(euler); + Dcm skew = R - R.T(); + Vector3 w = skew.vee(); + Vector3 w_check(0.1348f, 0.4170f, 0.5647f); + + TEST(isEqual(w, w_check)); + TEST(isEqual(skew, w.hat())); + return 0; +} + +/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */ diff --git a/src/lib/matrix/test/helper.cpp b/src/lib/matrix/test/helper.cpp new file mode 100644 index 0000000000..6d8b539f3a --- /dev/null +++ b/src/lib/matrix/test/helper.cpp @@ -0,0 +1,78 @@ +#include "test_macros.hpp" +#include + +using namespace matrix; + +int main() +{ + // general wraps + TEST(fabs(wrap(4., 0., 10.) - 4.) < FLT_EPSILON); + TEST(fabs(wrap(4., 0., 1.)) < FLT_EPSILON); + TEST(fabs(wrap(-4., 0., 10.) - 6.) < FLT_EPSILON); + TEST(fabs(wrap(-18., 0., 10.) - 2.) < FLT_EPSILON); + TEST(fabs(wrap(-1.5, 3., 5.) - 4.5) < FLT_EPSILON); + TEST(fabs(wrap(15.5, 3., 5.) - 3.5) < FLT_EPSILON); + TEST(fabs(wrap(-1., 30., 40.) - 39.) < FLT_EPSILON); + TEST(fabs(wrap(-8000., -555., 1.) - (-216.)) < FLT_EPSILON); + TEST(fabs(wrap(0., 0., 360.)) < FLT_EPSILON); + TEST(fabs(wrap(0. - FLT_EPSILON, 0., 360.) - (360. - FLT_EPSILON)) < FLT_EPSILON); + TEST(fabs(wrap(0. + FLT_EPSILON, 0., 360.) - FLT_EPSILON) < FLT_EPSILON); + TEST(fabs(wrap(360., 0., 360.)) < FLT_EPSILON); + TEST(fabs(wrap(360. - FLT_EPSILON, 0., 360.) - (360. - FLT_EPSILON)) < FLT_EPSILON); + TEST(fabs(wrap(360. + FLT_EPSILON, 0., 360.) - FLT_EPSILON) < FLT_EPSILON); + + // integer wraps + TEST(wrap(-10, 0, 10) == 0); + TEST(wrap(-4, 0, 10) == 6); + TEST(wrap(0, 0, 10) == 0) + TEST(wrap(4, 0, 10) == 4); + TEST(wrap(10, 0, 10) == 0); + + // wrap pi + TEST(fabs(wrap_pi(0.)) < FLT_EPSILON); + TEST(fabs(wrap_pi(4.) - (4. - M_TWOPI)) < FLT_EPSILON); + TEST(fabs(wrap_pi(-4.) - (-4. + M_TWOPI)) < FLT_EPSILON); + TEST(fabs(wrap_pi(3.) - (3.)) < FLT_EPSILON); + TEST(fabs(wrap_pi(100.) - (100. - 32. * M_PI)) < FLT_EPSILON); + TEST(fabs(wrap_pi(-100.) - (-100. + 32. * M_PI)) < FLT_EPSILON); + TEST(fabs(wrap_pi(-101.) - (-101. + 32. * M_PI)) < FLT_EPSILON); + TEST(!is_finite(wrap_pi(NAN))); + + // wrap 2pi + TEST(fabs(wrap_2pi(0.)) < FLT_EPSILON); + TEST(fabs(wrap_2pi(-4.) - (-4. + 2. * M_PI)) < FLT_EPSILON); + TEST(fabs(wrap_2pi(3.) - (3.)) < FLT_EPSILON); + TEST(fabs(wrap_2pi(200.) - (200. - 31. * M_TWOPI)) < FLT_EPSILON); + TEST(fabs(wrap_2pi(-201.) - (-201. + 32. * M_TWOPI)) < FLT_EPSILON); + TEST(!is_finite(wrap_2pi(NAN))); + + // Equality checks + TEST(isEqualF(1., 1.)); + TEST(!isEqualF(1., 2.)); + TEST(!isEqualF(NAN, 1.f)); + TEST(!isEqualF(1.f, NAN)); + TEST(!isEqualF(INFINITY, 1.f)); + TEST(!isEqualF(1.f, INFINITY)); + TEST(isEqualF(NAN, NAN)); + TEST(isEqualF(NAN, -NAN)); + TEST(isEqualF(-NAN, NAN)); + TEST(isEqualF(INFINITY, INFINITY)); + TEST(!isEqualF(INFINITY, -INFINITY)); + TEST(!isEqualF(-INFINITY, INFINITY)); + TEST(isEqualF(-INFINITY, -INFINITY)); + + Vector3f a(1, 2, 3); + Vector3f b(4, 5, 6); + TEST(!isEqual(a, b)); + TEST(isEqual(a, a)); + + Vector3f c(1, 2, 3); + Vector3f d(1, 2, NAN); + TEST(!isEqual(c, d)); + TEST(isEqual(c, c)); + TEST(isEqual(d, d)); + + return 0; +} + +/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */ diff --git a/src/lib/matrix/test/integration.cpp b/src/lib/matrix/test/integration.cpp new file mode 100644 index 0000000000..6dbd807205 --- /dev/null +++ b/src/lib/matrix/test/integration.cpp @@ -0,0 +1,26 @@ +#include "test_macros.hpp" +#include + +using namespace matrix; + +Vector f(float t, const Matrix & /*y*/, const Matrix & /*u*/); + +Vector f(float t, const Matrix & /*y*/, const Matrix & /*u*/) { + float v = -sin(t); + return v*ones(); +} + +int main() +{ + Vector y = ones(); + Vector u = ones(); + float t0 = 0; + float tf = 2; + float h = 0.001f; + integrate_rk4(f, y, u, t0, tf, h, y); + float v = 1 + cos(tf) - cos(t0); + TEST(isEqual(y, (ones()*v))); + return 0; +} + +/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */ diff --git a/src/lib/matrix/test/inverse.cpp b/src/lib/matrix/test/inverse.cpp new file mode 100644 index 0000000000..865c79db32 --- /dev/null +++ b/src/lib/matrix/test/inverse.cpp @@ -0,0 +1,164 @@ +#include "test_macros.hpp" +#include + +using namespace matrix; + +static const size_t n_large = 50; + +int main() +{ + float data[9] = {0, 2, 3, + 4, 5, 6, + 7, 8, 10 + }; + float data_check[9] = { + -0.4f, -0.8f, 0.6f, + -0.4f, 4.2f, -2.4f, + 0.6f, -2.8f, 1.6f + }; + + SquareMatrix A(data); + SquareMatrix A_I = inv(A); + SquareMatrix A_I_check(data_check); + TEST((A_I - A_I_check).abs().max() < 1e-6f); + + float data_2x2[4] = {12, 2, + -7, 5 + }; + float data_2x2_check[4] = { + 0.0675675675f, -0.02702702f, + 0.0945945945f, 0.162162162f + }; + + SquareMatrix A2x2(data_2x2); + SquareMatrix A2x2_I = inv(A2x2); + SquareMatrix A2x2_I_check(data_2x2_check); + TEST(isEqual(A2x2_I, A2x2_I_check)); + + SquareMatrix A2x2_sing = ones(); + SquareMatrix A2x2_sing_I; + TEST(inv(A2x2_sing, A2x2_sing_I) == false); + + SquareMatrix A3x3_sing = ones(); + SquareMatrix A3x3_sing_I; + TEST(inv(A3x3_sing, A3x3_sing_I) == false) + + // stess test + SquareMatrix A_large; + A_large.setIdentity(); + SquareMatrix A_large_I; + A_large_I.setZero(); + + for (size_t i = 0; i < n_large; i++) { + A_large_I = inv(A_large); + TEST(isEqual(A_large, A_large_I)); + } + + SquareMatrix zero_test = zeros(); + TEST(isEqual(inv(zero_test), zeros())); + + // test pivotting + float data2[81] = { + -2, 1, 1, -1, -5, 1, 2, -1, 0, + -3, 2, -1, 0, 2, 2, -1, -5, 3, + 0, 0, 0, 1, 4, -3, 3, 0, -2, + 2, 2, -1, -2, -1, 0, 3, 0, 1, + -1, 2, -1, -1, -3, 3, 0, -2, 3, + 0, 1, 1, -3, 3, -2, 0, -4, 0, + 1, 0, 0, 0, 0, 0, -2, 4, -3, + 1, -1, 0, -1, -1, 1, -1, -3, 4, + 0, 3, -1, -2, 2, 1, -2, 0, -1 + }; + + float data2_check[81] = { + 6, -4, 3, -3, -9, -8, -10, 8, 14, + -2, -7, -5, -3, -2, -2, -16, -5, 8, + -2, 0, -23, 7, -24, -5, -28, -14, 9, + 3, -7, 2, -5, -4, -6, -13, 4, 13, + -1, 4, -8, 5, -8, 0, -3, -5, -2, + 6, 7, -7, 7, -21, -7, -5, 3, 6, + 1, 4, -4, 4, -7, -1, 0, -1, -1, + -7, 3, -11, 5, 1, 6, -1, -13, -10, + -8, 0, -11, 3, 3, 6, -5, -14, -8 + }; + SquareMatrix A2(data2); + SquareMatrix A2_I = inv(A2); + SquareMatrix A2_I_check(data2_check); + TEST((A2_I - A2_I_check).abs().max() < 1e-3f); + + float data3[9] = { + 0, 1, 2, + 3, 4, 5, + 6, 7, 9 + }; + float data3_check[9] = { + -0.3333333f, -1.6666666f, 1, + -1, 4, -2, + 1, -2, 1 + }; + SquareMatrix A3(data3); + SquareMatrix A3_I = inv(A3); + SquareMatrix A3_I_check(data3_check); + TEST(isEqual(inv(A3), A3_I_check)); + TEST(isEqual(A3_I, A3_I_check)); + TEST(A3.I(A3_I)); + TEST(isEqual(A3_I, A3_I_check)); + + // cover singular matrices + A3(0, 0) = 0; + A3(0, 1) = 0; + A3(0, 2) = 0; + A3_I = inv(A3); + SquareMatrix Z3 = zeros(); + TEST(!A3.I(A3_I)); + TEST(!Z3.I(A3_I)); + TEST(isEqual(A3_I, Z3)); + TEST(isEqual(A3.I(), Z3)); + + for(size_t i = 0; i < 9; i++) { + A2(0, i) = 0; + } + A2_I = inv(A2); + SquareMatrix Z9 = zeros(); + TEST(!A2.I(A2_I)); + TEST(!Z9.I(A2_I)); + TEST(isEqual(A2_I, Z9)); + TEST(isEqual(A2.I(), Z9)); + + // cover NaN + A3(0, 0) = NAN; + A3(0, 1) = 0; + A3(0, 2) = 0; + A3_I = inv(A3); + TEST(isEqual(A3_I, Z3)); + TEST(isEqual(A3.I(), Z3)); + + A2(0, 0) = NAN; + A2_I = inv(A2); + TEST(isEqual(A2_I, Z9)); + TEST(isEqual(A2.I(), Z9)); + + float data4[9] = { + 1.33471626f, 0.74946721f, -0.0531679f, + 0.74946721f, 1.07519593f, 0.08036323f, + -0.0531679f, 0.08036323f, 1.01618474f + }; + SquareMatrix A4(data4); + + float data4_cholesky[9] = { + 1.15529921f, 0.f, 0.f, + 0.6487213f, 0.80892311f, 0.f, + -0.04602089f, 0.13625271f, 0.99774847f + }; + SquareMatrix A4_cholesky_check(data4_cholesky); + SquareMatrix A4_cholesky = cholesky(A4); + TEST(isEqual(A4_cholesky_check, A4_cholesky)); + + SquareMatrix I3; + I3.setIdentity(); + TEST(isEqual(choleskyInv(A4)*A4, I3)); + TEST(isEqual(cholesky(Z3), Z3)); + return 0; +} + +/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */ diff --git a/src/lib/matrix/test/least_squares.cpp b/src/lib/matrix/test/least_squares.cpp new file mode 100644 index 0000000000..573715caa2 --- /dev/null +++ b/src/lib/matrix/test/least_squares.cpp @@ -0,0 +1,100 @@ +#include "test_macros.hpp" +#include + +using namespace matrix; + +int test_4x3(void); +template int test_4x4(void); +int test_4x4_type_double(void); +int test_div_zero(void); + +int main() +{ + int ret; + + ret = test_4x4(); + if (ret != 0) return ret; + + ret = test_4x4(); + if (ret != 0) return ret; + + ret = test_4x3(); + if (ret != 0) return ret; + + ret = test_div_zero(); + if (ret != 0) return ret; + + return 0; +} + +int test_4x3() { + // Start with an (m x n) A matrix + float data[12] = {20.f, -10.f, -13.f, + 17.f, 16.f, -18.f, + 0.7f, -0.8f, 0.9f, + -1.f, -1.1f, -1.2f + }; + Matrix A(data); + + float b_data[4] = {2.0f, 3.0f, 4.0f, 5.0f}; + Vector b(b_data); + + float x_check_data[3] = {-0.69168233f, + -0.26227593f, + -1.03767522f + }; + Vector x_check(x_check_data); + + LeastSquaresSolver qrd = LeastSquaresSolver(A); + + Vector x = qrd.solve(b); + TEST(isEqual(x, x_check)); + return 0; +} + +template +int test_4x4() { + // Start with an (m x n) A matrix + const Type data[16] = { 20.f, -10.f, -13.f, 21.f, + 17.f, 16.f, -18.f, -14.f, + 0.7f, -0.8f, 0.9f, -0.5f, + -1.f, -1.1f, -1.2f, -1.3f + }; + Matrix A(data); + + Type b_data[4] = {2.0f, 3.0f, 4.0f, 5.0f}; + Vector b(b_data); + + Type x_check_data[4] = { 0.97893433f, + -2.80798701f, + -0.03175765f, + -2.19387649f + }; + Vector x_check(x_check_data); + + LeastSquaresSolver qrd = LeastSquaresSolver(A); + + Vector x = qrd.solve(b); + TEST(isEqual(x, x_check)); + return 0; +} + +int test_div_zero() { + float data[4] = {0.0f, 0.0f, 0.0f, 0.0f}; + Matrix A(data); + + float b_data[2] = {1.0f, 1.0f}; + Vector b(b_data); + + // Implement such that x returns zeros if it reaches div by zero + float x_check_data[2] = {0.0f, 0.0f}; + Vector x_check(x_check_data); + + LeastSquaresSolver qrd = LeastSquaresSolver(A); + + Vector x = qrd.solve(b); + TEST(isEqual(x, x_check)); + return 0; +} + +/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */ diff --git a/src/lib/matrix/test/matrixAssignment.cpp b/src/lib/matrix/test/matrixAssignment.cpp new file mode 100644 index 0000000000..6e5a0783c6 --- /dev/null +++ b/src/lib/matrix/test/matrixAssignment.cpp @@ -0,0 +1,260 @@ +#include "test_macros.hpp" +#include + +using namespace matrix; + +template class matrix::Matrix; + +int main() +{ + Matrix3f m; + m.setZero(); + m.zero(); + m(0, 0) = 1; + m(0, 1) = 2; + m(0, 2) = 3; + m(1, 0) = 4; + m(1, 1) = 5; + m(1, 2) = 6; + m(2, 0) = 7; + m(2, 1) = 8; + m(2, 2) = 9; + + float data[9] = {1, 2, 3, 4, 5, 6, 7, 8, 9}; + Matrix3f m2(data); + + for(size_t i=0; i<3; i++) { + for (size_t j = 0; j < 3; j++) { + TEST(fabs(data[i*3 + j] - m2(i,j)) < FLT_EPSILON); + } + } + + Matrix3f m_nan; + m_nan.setNaN(); + for(size_t i=0; i<3; i++) { + for (size_t j = 0; j < 3; j++) { + TEST(isnan(m_nan(i,j))); + } + } + TEST(m_nan.isAllNan()); + + float data2d[3][3] = { + {1, 2, 3}, + {4, 5, 6}, + {7, 8, 9} + }; + m2 = Matrix3f(data2d); + for(size_t i=0; i<3; i++) { + for (size_t j = 0; j < 3; j++) { + TEST(fabs(data[i*3 + j] - m2(i,j)) < FLT_EPSILON); + } + } + TEST(!m2.isAllNan()); + + float data_times_2[9] = {2, 4, 6, 8, 10, 12, 14, 16, 18}; + Matrix3f m3(data_times_2); + + TEST(isEqual(m, m2)); + TEST(!(isEqual(m, m3))); + + m2 *= 2; + TEST(isEqual(m2, m3)); + + m2 /= 2; + m2 -= 1; + float data_minus_1[9] = {0, 1, 2, 3, 4, 5, 6, 7, 8}; + TEST(isEqual(Matrix3f(data_minus_1), m2)); + + m2 += 1; + TEST(isEqual(Matrix3f(data), m2)); + + m3 -= m2; + + TEST(isEqual(m3, m2)); + + // set rows and columns to value + Matrix3f m2e(data2d); + + float data2e_check1[3][3] = { + {1, 11, 3}, + {4, 11, 6}, + {7, 11, 9} + }; + Matrix3f m2e_check1(data2e_check1); + + float data2e_check2[3][3] = { + {1, 11, 3}, + {4, 11, 6}, + {0, 0, 0} + }; + Matrix3f m2e_check2(data2e_check2); + + m2e.setCol(1, 11); + TEST(isEqual(m2e, m2e_check1)); + m2e.setRow(2, 0); + TEST(isEqual(m2e, m2e_check2)); + + float data_row_02_swap[9] = { + 7, 8, 9, + 4, 5, 6, + 1, 2, 3, + }; + + float data_col_02_swap[9] = { + 3, 2, 1, + 6, 5, 4, + 9, 8, 7 + }; + + Matrix3f m4(data); + + TEST(isEqual(-m4, m4*(-1))); + + // col swap + m4.swapCols(0, 2); + TEST(isEqual(m4, Matrix3f(data_col_02_swap))); + m4.swapCols(0, 2); + + // row swap + m4.swapRows(0, 2); + TEST(isEqual(m4, Matrix3f(data_row_02_swap))); + m4.swapRows(0, 2); + + // swapping with same row should do nothing + m4.swapRows(0, 0); + m4.swapRows(1, 1); + m4.swapRows(2, 2); + TEST(isEqual(m4, Matrix3f(data))); + + // swapping with same col should do nothing + m4.swapCols(0, 0); + m4.swapCols(1, 1); + m4.swapCols(2, 2); + TEST(isEqual(m4, Matrix3f(data))); + + TEST(fabs(m4.min() - 1) < FLT_EPSILON); + TEST(fabs((-m4).min() + 9) < FLT_EPSILON); + + Scalar s = 1; + const Vector & s_vect = s; + TEST(fabs(s - 1) < FLT_EPSILON); + TEST(fabs(s_vect(0) - 1.0f) < FLT_EPSILON); + + Matrix m5 = s; + TEST(fabs(m5(0,0) - s) < FLT_EPSILON); + + Matrix m6; + m6.setRow(0, Vector2f(1, 2)); + float m7_array[] = {1,2,0,0}; + Matrix m7(m7_array); + TEST(isEqual(m6, m7)); + m6.setCol(0, Vector2f(3, 4)); + float m8_array[] = {3,2,4,0}; + Matrix m8(m8_array); + TEST(isEqual(m6, m8)); + + m7.setNaN(); + TEST(m7 != m8); + + // min, max, constrain matrix values with scalar + float data_m9[9] = {2, 4, 6, 8, 10, 12, 14, 16, 18}; + float lower_bound = 7; + float upper_bound = 11; + float data_m9_lower_bounded[9] = {7, 7, 7, 8, 10, 12, 14, 16, 18}; + float data_m9_upper_bounded[9] = {2, 4, 6, 8, 10, 11, 11, 11, 11}; + float data_m9_lower_constrained[9] = {7, 7, 7, 8, 10, 11, 11, 11, 11}; + Matrix3f m9(data_m9); + Matrix3f m9_lower_bounded(data_m9_lower_bounded); + Matrix3f m9_upper_bounded(data_m9_upper_bounded); + Matrix3f m9_lower_upper_constrained(data_m9_lower_constrained); + TEST(isEqual(max(m9, lower_bound), m9_lower_bounded)); + TEST(isEqual(max(lower_bound, m9), m9_lower_bounded)); + TEST(isEqual(min(m9, upper_bound), m9_upper_bounded)); + TEST(isEqual(min(upper_bound, m9), m9_upper_bounded)); + TEST(isEqual(constrain(m9, lower_bound, upper_bound), m9_lower_upper_constrained)); + TEST(isEqual(constrain(m9, 8.0f, 7.0f), m_nan)); + + // min, max, constrain matrix values with matrix of same size + float data_m10[9] = {2, 4, 6, 8, 10, 12, 14, 16, 18}; + float data_m10_lower_bound[9] = {5, 7, 4, 8, 19, 10, 20, 16, 18}; + float data_m10_lower_bounded_ref[9] = {5, 7, 6, 8, 19, 12, 20, 16, 18}; + float data_m10_upper_bound[9] = {6, 4, 8, 18, 20, 11, 30, 16, 18}; + float data_m10_upper_bounded_ref[9] = {2, 4, 6, 8, 10, 11, 14, 16, 18}; + float data_m10_constrained_ref[9] = {5, NAN, 6, 8, 19, 11, 20, 16, 18}; + Matrix3f m10(data_m10); + Matrix3f m10_lower_bound(data_m10_lower_bound); + Matrix3f m10_lower_bounded_ref(data_m10_lower_bounded_ref); + Matrix3f m10_upper_bound(data_m10_upper_bound); + Matrix3f m10_upper_bounded_ref(data_m10_upper_bounded_ref); + Matrix3f m10_constrained_ref(data_m10_constrained_ref); + TEST(isEqual(max(m10, m10_lower_bound), m10_lower_bounded_ref)); + TEST(isEqual(max(m10_lower_bound, m10), m10_lower_bounded_ref)); + TEST(isEqual(min(m10, m10_upper_bound), m10_upper_bounded_ref)); + TEST(isEqual(min(m10_upper_bound, m9), m10_upper_bounded_ref)); + TEST(isEqual(constrain(m10, m10_lower_bound, m10_upper_bound), m10_constrained_ref)); + + // min, max, constrain with NAN + TEST(isEqualF(matrix::typeFunction::min(5.f, NAN), 5.f)); + TEST(isEqualF(matrix::typeFunction::min(NAN, 5.f), 5.f)); + TEST(isEqualF(matrix::typeFunction::min(NAN, NAN), NAN)); + TEST(isEqualF(matrix::typeFunction::max(5.f, NAN), 5.f)); + TEST(isEqualF(matrix::typeFunction::max(NAN, 5.f), 5.f)); + TEST(isEqualF(matrix::typeFunction::max(NAN, NAN), NAN)); + TEST(isEqualF(matrix::typeFunction::constrain(NAN, 5.f, 6.f), NAN)); + TEST(isEqualF(matrix::typeFunction::constrain(1.f, 5.f, 4.f), NAN)); + TEST(isEqualF(matrix::typeFunction::constrain(6.f, NAN, 5.f), 5.f)); + TEST(isEqualF(matrix::typeFunction::constrain(1.f, 5.f, NAN), 5.f)); + Vector2f v1{NAN, 5.0f}; + Vector2f v1_min = min(v1, 1.f); + Matrix3f m11 = min(m10_constrained_ref,NAN); + TEST(isEqualF(fmin(NAN, 1.f), float(v1_min(0)))); + TEST(isEqual(m11, m10_constrained_ref)); + + // check write_string() + float comma[6] = { + 1.f, 12345.123f, + 12345.1228f, .1234567891011f, + 12345678910.123456789f, 1234567891011.123456789101112f + }; + Matrix Comma(comma); + const size_t len = 15*2*3 + 2 + 1; + char buffer[len]; + Comma.print(); // for debugging in case of failure + Comma.write_string(buffer, len); + printf("%s\n", buffer); // for debugging in case of failure + char output[] = "\t 1\t12345.123\n\t12345.123\t0.12345679\n\t1.2345679e+10\t1.234568e+12\n"; + printf("%s\n", output); // for debugging in case of failure + for (size_t i = 0; i < len; i++) { + if(buffer[i] != output[i]) { // for debugging in case of failure + printf("%d: \"%c\" != \"%c\"", int(i), buffer[i], output[i]); // LCOV_EXCL_LINE only print on failure + } + TEST(buffer[i] == output[i]); + if (buffer[i] == '\0') { + break; + } + } + + // check print() + // Redirect stdout + TEST(freopen("testoutput.txt", "w", stdout) != NULL); + // write + Comma.print(); + fclose(stdout); + // read + FILE *fp = fopen("testoutput.txt", "r"); + TEST(fp != nullptr); + TEST(!fseek(fp, 0, SEEK_SET)); + for (size_t i = 0; i < len; i++) { + char c = static_cast(fgetc(fp)); + if (c == '\n') { + break; + } + printf("%d %d %d\n", static_cast(i), output[i], c); + TEST(c == output[i]); + } + TEST(!fclose(fp)); + + return 0; +} + +/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */ diff --git a/src/lib/matrix/test/matrixMult.cpp b/src/lib/matrix/test/matrixMult.cpp new file mode 100644 index 0000000000..ca00d0d100 --- /dev/null +++ b/src/lib/matrix/test/matrixMult.cpp @@ -0,0 +1,71 @@ +#include "test_macros.hpp" +#include + +using namespace matrix; + +int main() +{ + float data[9] = {1, 0, 0, 0, 1, 0, 1, 0, 1}; + Matrix3f A(data); + + float data_check[9] = {1, 0, 0, 0, 1, 0, -1, 0, 1}; + Matrix3f A_I(data_check); + Matrix3f I; + I.setIdentity(); + Matrix3f R = A * A_I; + TEST(isEqual(R, I)); + + Matrix3f R2 = A; + R2 *= A_I; + TEST(isEqual(R2, I)); + + TEST(R2==I); + TEST(A!=A_I); + Matrix3f A2 = eye()*2; + Matrix3f B = A2.emult(A2); + Matrix3f B_check = eye()*4; + Matrix3f C_check = eye()*2; + TEST(isEqual(B, B_check)); + Matrix3f C = B_check.edivide(C_check); + + float off_diagonal_nan[9] = {2, NAN, NAN, NAN, 2, NAN, NAN, NAN, 2}; + // off diagonal are NANs because division by 0 + TEST(C == Matrix3f(off_diagonal_nan)); + + // Test non-square matrix + float data_43[12] = {1,3,2, + 2,2,1, + 5,2,1, + 2,3,4 + }; + float data_32[6] = {2,3, + 1,7, + 5,4 + }; + + Matrix m43(data_43); + Matrix m32(data_32); + + Matrix m42 = m43 * m32; + + float data_42[8] = {15,32, + 11,24, + 17,33, + 27,43 + }; + Matrix m42_check(data_42); + TEST(isEqual(m42, m42_check)) + + float data_42_plus2[8] = {17,34, + 13,26, + 19,35, + 29,45 + }; + Matrix m42_plus2_check(data_42_plus2); + Matrix m42_plus2 = m42 - (-2); + TEST(isEqual(m42_plus2, m42_plus2_check)); + + return 0; +} + +/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */ diff --git a/src/lib/matrix/test/matrixScalarMult.cpp b/src/lib/matrix/test/matrixScalarMult.cpp new file mode 100644 index 0000000000..320f0c8b58 --- /dev/null +++ b/src/lib/matrix/test/matrixScalarMult.cpp @@ -0,0 +1,18 @@ +#include "test_macros.hpp" + +#include + +using namespace matrix; + +int main() +{ + float data[9] = {1, 2, 3, 4, 5, 6, 7, 8, 9}; + Matrix3f A(data); + A = A * 2; + float data_check[9] = {2, 4, 6, 8, 10, 12, 14, 16, 18}; + Matrix3f A_check(data_check); + TEST(isEqual(A, A_check)); + return 0; +} + +/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */ diff --git a/src/lib/matrix/test/pseudoInverse.cpp b/src/lib/matrix/test/pseudoInverse.cpp new file mode 100644 index 0000000000..a313a58bcd --- /dev/null +++ b/src/lib/matrix/test/pseudoInverse.cpp @@ -0,0 +1,153 @@ +#include "test_macros.hpp" +#include + +using namespace matrix; + +static const size_t n_large = 20; + +int main() +{ + // 3x4 Matrix test + float data0[12] = { + 0.f, 1.f, 2.f, 3.f, + 4.f, 5.f, 6.f, 7.f, + 8.f, 9.f, 10.f, 11.f + }; + + float data0_check[12] = { + -0.3375f, -0.1f, 0.1375f, + -0.13333333f, -0.03333333f, 0.06666667f, + 0.07083333f, 0.03333333f, -0.00416667f, + 0.275f, 0.1f, -0.075f + }; + + Matrix A0(data0); + Matrix A0_I; + bool ret = geninv(A0, A0_I); + TEST(ret); + Matrix A0_I_check(data0_check); + + TEST((A0_I - A0_I_check).abs().max() < 1e-5); + + // 4x3 Matrix test + float data1[12] = { + 0.f, 4.f, 8.f, + 1.f, 5.f, 9.f, + 2.f, 6.f, 10.f, + 3.f, 7.f, 11.f + }; + + float data1_check[12] = { + -0.3375f, -0.13333333f, 0.07083333f, 0.275f, + -0.1f, -0.03333333f, 0.03333333f, 0.1f, + 0.1375f, 0.06666667f, -0.00416667f, -0.075f + }; + + Matrix A1(data1); + Matrix A1_I; + ret = geninv(A1, A1_I); + TEST(ret); + Matrix A1_I_check(data1_check); + + TEST((A1_I - A1_I_check).abs().max() < 1e-5); + + // Stess test + Matrix A_large; + A_large.setIdentity(); + Matrix A_large_I; + + for (size_t i = 0; i < n_large; i++) { + ret = geninv(A_large, A_large_I); + TEST(ret); + TEST(isEqual(A_large, A_large_I.T())); + } + + // Square matrix test + float data2[9] = {0, 2, 3, + 4, 5, 6, + 7, 8, 10 + }; + float data2_check[9] = { + -0.4f, -0.8f, 0.6f, + -0.4f, 4.2f, -2.4f, + 0.6f, -2.8f, 1.6f + }; + + SquareMatrix A2(data2); + SquareMatrix A2_I; + ret = geninv(A2, A2_I); + TEST(ret); + SquareMatrix A2_I_check(data2_check); + TEST((A2_I - A2_I_check).abs().max() < 1e-3); + + // Null matrix test + Matrix A3; + Matrix A3_I; + ret = geninv(A3, A3_I); + TEST(ret); + Matrix A3_I_check; + TEST((A3_I - A3_I_check).abs().max() < 1e-5); + + // Mock-up effectiveness matrix + const float B_quad_w[6][16] = { + {-0.5717536f, 0.43756646f, 0.5717536f, -0.43756646f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.35355328f, -0.35355328f, 0.35355328f, -0.35355328f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.28323701f, 0.28323701f, -0.28323701f, -0.28323701f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + {-0.25f, -0.25f, -0.25f, -0.25f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f} + }; + Matrix B = Matrix(B_quad_w); + const float A_quad_w[16][6] = { + { -0.495383f, 0.707107f, 0.765306f, 0.0f, 0.0f, -1.000000f }, + { 0.495383f, -0.707107f, 1.000000f, 0.0f, 0.0f, -1.000000f }, + { 0.495383f, 0.707107f, -0.765306f, 0.0f, 0.0f, -1.000000f }, + { -0.495383f, -0.707107f, -1.000000f, 0.0f, 0.0f, -1.000000f }, + { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, + { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, + { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, + { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, + { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, + { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, + { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, + { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, + { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, + { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, + { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, + { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f} + }; + Matrix A_check = Matrix(A_quad_w); + Matrix A; + ret = geninv(B, A); + TEST(ret); + TEST((A - A_check).abs().max() < 1e-5); + + // Real-world test case + const float real_alloc[5][6] = { + { 0.794079, 0.794079, 0.794079, 0.794079, 0.0000, 0.0000}, + { 0.607814, 0.607814, 0.607814, 0.607814, 1.0000, 1.0000}, + {-0.672516, 0.915642, -0.915642, 0.672516, 0.0000, 0.0000}, + { 0.159704, 0.159704, 0.159704, 0.159704, -0.2500, -0.2500}, + { 0.607814, -0.607814, 0.607814, -0.607814, 1.0000, 1.0000} + }; + Matrix real ( real_alloc); + Matrix real_pinv; + ret = geninv(real, real_pinv); + TEST(ret); + + // from SVD-based inverse + const float real_pinv_expected_alloc[6][5] = { + { 2.096205, -2.722267, 2.056547, 1.503279, 3.098087}, + { 1.612621, -1.992694, 2.056547, 1.131090, 2.275467}, + {-1.062688, 2.043479, -2.056547, -0.927950, -2.275467}, + {-1.546273, 2.773052, -2.056547, -1.300139, -3.098087}, + {-0.293930, 0.443445, 0.000000, -0.226222, 0.000000}, + {-0.293930, 0.443445, 0.000000, -0.226222, 0.000000} + }; + Matrix real_pinv_expected(real_pinv_expected_alloc); + TEST((real_pinv - real_pinv_expected).abs().max() < 1e-4); + + return 0; +} + +/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */ diff --git a/src/lib/matrix/test/setIdentity.cpp b/src/lib/matrix/test/setIdentity.cpp new file mode 100644 index 0000000000..289e286464 --- /dev/null +++ b/src/lib/matrix/test/setIdentity.cpp @@ -0,0 +1,39 @@ +#include "test_macros.hpp" +#include + +using namespace matrix; + +int main() +{ + Matrix3f A; + A.setIdentity(); + + for (size_t i = 0; i < 3; i++) { + for (size_t j = 0; j < 3; j++) { + if (i == j) { + TEST(fabs(A(i, j) - 1) < FLT_EPSILON); + + } else { + TEST(fabs(A(i, j) - 0) < FLT_EPSILON); + } + } + } + + Matrix3f B; + B.identity(); + + for (size_t i = 0; i < 3; i++) { + for (size_t j = 0; j < 3; j++) { + if (i == j) { + TEST(fabs(B(i, j) - 1) < FLT_EPSILON); + + } else { + TEST(fabs(B(i, j) - 0) < FLT_EPSILON); + } + } + } + + return 0; +} + +/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */ diff --git a/src/lib/matrix/test/slice.cpp b/src/lib/matrix/test/slice.cpp new file mode 100644 index 0000000000..6f838f0ccf --- /dev/null +++ b/src/lib/matrix/test/slice.cpp @@ -0,0 +1,235 @@ +#include "test_macros.hpp" +#include + +using namespace matrix; + +template class matrix::Slice; // so that we get full coverage results + + +int main() +{ + float data[9] = {0, 2, 3, + 4, 5, 6, + 7, 8, 10 + }; + SquareMatrix A(data); + + // Test row slicing + Matrix B_rowslice(A.slice<2, 3>(1, 0)); + float data_check_rowslice[6] = { + 4, 5, 6, + 7, 8, 10 + }; + Matrix B_check_rowslice(data_check_rowslice); + TEST(isEqual(B_rowslice, B_check_rowslice)); + + // Test column slicing + Matrix B_colslice(A.slice<3, 2>(0, 1)); + float data_check_colslice[6] = { + 2, 3, + 5, 6, + 8, 10 + }; + Matrix B_check_colslice(data_check_colslice); + TEST(isEqual(B_colslice, B_check_colslice)); + + // Test slicing both + Matrix B_bothslice(A.slice<2, 2>(1, 1)); + float data_check_bothslice[4] = { + 5, 6, + 8, 10 + }; + Matrix B_check_bothslice(data_check_bothslice); + TEST(isEqual(B_bothslice, B_check_bothslice)); + + //Test block writing + float data_2[4] = { + 11, 12, + 13, 14 + }; + + Matrix C(data_2); + A.slice<2, 2>(1, 1) = C; + + float data_2_check[9] = { + 0, 2, 3, + 4, 11, 12, + 7, 13, 14 + }; + Matrix D(data_2_check); + TEST(isEqual(A, D)); + + //Test writing to slices + Matrix E; + E(0,0) = -1; + E(1,0) = 1; + E(2,0) = 3; + + Matrix F; + F(0,0) = 9; + F(1,0) = 11; + + E.slice<2,1>(0,0) = F; + + float data_3_check[3] = {9, 11, 3}; + Matrix G (data_3_check); + TEST(isEqual(E, G)); + TEST(isEqual(E, Matrix(E.slice<3,1>(0,0)))); + + Matrix H = E.slice<2,1>(0,0); + TEST(isEqual(H,F)); + + float data_4_check[5] = {3, 11, 9, 0, 0}; + { // assigning row slices to each other + const Matrix J (data_3_check); + Matrix K; + K.row(2) = J.row(0); + K.row(1) = J.row(1); + K.row(0) = J.row(2); + + Matrix K_check(data_4_check); + TEST(isEqual(K, K_check)); + } + { // assigning col slices to each other + const Matrix J (data_3_check); + Matrix K; + K.col(2) = J.col(0); + K.col(1) = J.col(1); + K.col(0) = J.col(2); + + Matrix K_check(data_4_check); + TEST(isEqual(K, K_check)); + } + + // check that slice of a slice works for reading + const Matrix cm33(data); + Matrix topRight = cm33.slice<2,3>(0,0).slice<2,1>(0,2); + float top_right_check[2] = {3,6}; + TEST(isEqual(topRight, Matrix(top_right_check))); + + // check that slice of a slice works for writing + Matrix m33(data); + m33.slice<2,3>(0,0).slice<2,1>(0,2) = Matrix(); + const float data_check[9] = {0, 2, 0, + 4, 5, 0, + 7, 8, 10 + }; + TEST(isEqual(m33, Matrix(data_check))); + + // longerThan + Vector3f v5; + v5(0) = 3; + v5(1) = 4; + v5(2) = 9; + TEST(v5.xy().longerThan(4.99f)); + TEST(!v5.xy().longerThan(5.f)); + TEST(isEqualF(5.f, v5.xy().norm())); + + // min/max + TEST(m33.row(1).max() == 5); + TEST(m33.col(0).min() == 0); + TEST((m33.slice<2,2>(1,1).max()) == 10); + + // assign scalar value to slice + Matrix L; + L(0,0) = -1; + L(1,0) = 1; + L(2,0) = 3; + + L.slice<2,1>(0,0) = 0.0f; + + float data_5_check[3] = {0, 0, 3}; + Matrix M (data_5_check); + TEST(isEqual(L, M)); + + // return diagonal elements + float data_6[9] = {0, 2, 3, + 4, 5, 6, + 7, 8, 10 + }; + SquareMatrix N(data_6); + + Vector3f v6 = N.slice<3,3>(0,0).diag(); + Vector3f v6_check = {0, 5, 10}; + TEST(isEqual(v6,v6_check)); + Vector2f v7 = N.slice<2,3>(1,0).diag(); + Vector2f v7_check = {4, 8}; + TEST(isEqual(v7,v7_check)); + Vector2f v8 = N.slice<3,2>(0,1).diag(); + Vector2f v8_check = {2, 6}; + TEST(isEqual(v8,v8_check)); + Vector2f v9(N.slice<1,2>(1,1)); + Vector2f v9_check = {5, 6}; + TEST(isEqual(v9,v9_check)); + Vector3f v10(N.slice<1,3>(1,0)); + Vector3f v10_check = {4, 5, 6}; + TEST(isEqual(v10,v10_check)); + + // Different assignment operators + SquareMatrix3f O(data); + float operand_data [4] = {2, 1, -3, -1}; + const SquareMatrix operand(operand_data); + + O.slice<2,2>(1,0) += operand; + float O_check_data_1 [9] = {0, 2, 3, 6, 6, 6, 4, 7, 10}; + TEST(isEqual(O, SquareMatrix3f(O_check_data_1))); + + O = SquareMatrix3f(data); + O.slice<2,1>(1,1) += operand.slice<2,1>(0,0); + float O_check_data_2 [9] = {0, 2, 3, 4, 7, 6, 7, 5, 10}; + TEST(isEqual(O, SquareMatrix3f(O_check_data_2))); + + O = SquareMatrix3f(data); + O.slice<3,3>(0,0) += -1; + float O_check_data_3 [9] = {-1, 1, 2, 3, 4, 5, 6, 7, 9}; + TEST(isEqual(O, SquareMatrix3f(O_check_data_3))); + + O = SquareMatrix3f(data); + O.col(1) += Vector3f{1, -2, 3}; + float O_check_data_4 [9] = {0, 3, 3, 4, 3, 6, 7, 11, 10}; + TEST(isEqual(O, SquareMatrix3f(O_check_data_4))); + + O = SquareMatrix3f(data); + O.slice<2,2>(1,0) -= operand; + float O_check_data_5 [9] = {0, 2, 3, 2, 4, 6, 10, 9, 10}; + TEST(isEqual(O, SquareMatrix3f(O_check_data_5))); + + O = SquareMatrix3f(data); + O.slice<2,1>(1,1) -= operand.slice<2,1>(0,0); + float O_check_data_6 [9] = {0, 2, 3, 4, 3, 6, 7, 11, 10}; + TEST(isEqual(O, SquareMatrix3f(O_check_data_6))); + + O = SquareMatrix3f(data); + O.slice<3,3>(0,0) -= -1; + float O_check_data_7 [9] = {1, 3, 4, 5, 6, 7, 8, 9, 11}; + TEST(isEqual(O, SquareMatrix3f(O_check_data_7))); + + O = SquareMatrix3f(data); + O.col(1) -= Vector3f{1, -2, 3}; + float O_check_data_8 [9] = {0, 1, 3, 4, 7, 6, 7, 5, 10}; + TEST(isEqual(O, SquareMatrix3f(O_check_data_8))); + + O = SquareMatrix3f(data); + O.slice<2,1>(1,1) *= 5.f; + float O_check_data_9 [9] = {0, 2, 3, 4, 25, 6, 7, 40, 10}; + TEST(isEqual(O, SquareMatrix3f(O_check_data_9))); + + O = SquareMatrix3f(data); + O.slice<2,1>(1,1) /= 2.f; + float O_check_data_10 [9] = {0, 2, 3, 4, 2.5, 6, 7, 4, 10}; + TEST(isEqual(O, SquareMatrix3f(O_check_data_10))); + + // Different operations + O = SquareMatrix3f(data); + SquareMatrix res_11(O.slice<2,2>(1,1) * 2.f); + float O_check_data_11 [4] = {10, 12, 16, 20}; + TEST(isEqual(res_11, SquareMatrix(O_check_data_11))); + + O = SquareMatrix3f(data); + SquareMatrix res_12(O.slice<2,2>(1,1) / 2.f); + float O_check_data_12 [4] = {2.5, 3, 4, 5}; + TEST(isEqual(res_12, SquareMatrix(O_check_data_12))); + return 0; +} + +/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */ diff --git a/src/lib/matrix/test/sparseVector.cpp b/src/lib/matrix/test/sparseVector.cpp new file mode 100644 index 0000000000..9c2dbd1224 --- /dev/null +++ b/src/lib/matrix/test/sparseVector.cpp @@ -0,0 +1,119 @@ +#include +#include + +using namespace matrix; + +TEST(sparseVectorTest, defaultConstruction) { + SparseVectorf<24, 4, 6> a; + EXPECT_EQ(a.non_zeros(), 2); + EXPECT_EQ(a.index(0), 4); + EXPECT_EQ(a.index(1), 6); + a.at<4>() = 1.f; + a.at<6>() = 2.f; +} + +TEST(sparseVectorTest, initializationWithData) { + const float data[3] = {1.f, 2.f, 3.f}; + SparseVectorf<24, 4, 6, 22> a(data); + EXPECT_EQ(a.non_zeros(), 3); + EXPECT_EQ(a.index(0), 4); + EXPECT_EQ(a.index(1), 6); + EXPECT_EQ(a.index(2), 22); + EXPECT_FLOAT_EQ(a.at<4>(), data[0]); + EXPECT_FLOAT_EQ(a.at<6>(), data[1]); + EXPECT_FLOAT_EQ(a.at<22>(), data[2]); +} + +TEST(sparseVectorTest, initialisationFromVector) { + const Vector3f vec(1.f, 2.f, 3.f); + const SparseVectorf<3, 0, 2> a(vec); + EXPECT_FLOAT_EQ(a.at<0>(), vec(0)); + EXPECT_FLOAT_EQ(a.at<2>(), vec(2)); +} + +TEST(sparseVectorTest, accessDataWithCompressedIndices) { + const Vector3f vec(1.f, 2.f, 3.f); + SparseVectorf<3, 0, 2> a(vec); + for (size_t i = 0; i < a.non_zeros(); i++) { + a.atCompressedIndex(i) = static_cast(i); + } + EXPECT_FLOAT_EQ(a.at<0>(), a.atCompressedIndex(0)); + EXPECT_FLOAT_EQ(a.at<2>(), a.atCompressedIndex(1)); +} + +TEST(sparseVectorTest, setZero) { + const float data[3] = {1.f, 2.f, 3.f}; + SparseVectorf<24, 4, 6, 22> a(data); + a.setZero(); + EXPECT_FLOAT_EQ(a.at<4>(), 0.f); + EXPECT_FLOAT_EQ(a.at<6>(), 0.f); + EXPECT_FLOAT_EQ(a.at<22>(), 0.f); +} + +TEST(sparseVectorTest, additionWithDenseVector) { + Vector dense_vec; + dense_vec.setAll(1.f); + const float data[3] = {1.f, 2.f, 3.f}; + const SparseVectorf<4, 1, 2, 3> sparse_vec(data); + const Vector res = sparse_vec + dense_vec; + EXPECT_FLOAT_EQ(res(0), 1.f); + EXPECT_FLOAT_EQ(res(1), 2.f); + EXPECT_FLOAT_EQ(res(2), 3.f); + EXPECT_FLOAT_EQ(res(3), 4.f); +} + +TEST(sparseVectorTest, addScalar) { + const float data[3] = {1.f, 2.f, 3.f}; + SparseVectorf<4, 1, 2, 3> sparse_vec(data); + sparse_vec += 2.f; + EXPECT_FLOAT_EQ(sparse_vec.at<1>(), 3.f); + EXPECT_FLOAT_EQ(sparse_vec.at<2>(), 4.f); + EXPECT_FLOAT_EQ(sparse_vec.at<3>(), 5.f); +} + +TEST(sparseVectorTest, dotProductWithDenseVector) { + Vector dense_vec; + dense_vec.setAll(3.f); + const float data[3] = {1.f, 2.f, 3.f}; + const SparseVectorf<4, 1, 2, 3> sparse_vec(data); + float res = sparse_vec.dot(dense_vec); + EXPECT_FLOAT_EQ(res, 18.f); +} + +TEST(sparseVectorTest, multiplicationWithDenseMatrix) { + Matrix dense_matrix; + dense_matrix.setAll(2.f); + dense_matrix(1, 1) = 3.f; + const Vector3f dense_vec(0.f, 1.f, 5.f); + const SparseVectorf<3, 1, 2> sparse_vec(dense_vec); + const Vector res_sparse = dense_matrix * sparse_vec; + const Vector res_dense = dense_matrix * dense_vec; + EXPECT_TRUE(isEqual(res_dense, res_sparse)); +} + +TEST(sparseVectorTest, quadraticForm) { + float matrix_data[9] = {1, 2, 3, + 2, 4, 5, + 3, 5, 6 + }; + const SquareMatrix dense_matrix(matrix_data); + const Vector3f dense_vec(0.f, 1.f, 5.f); + const SparseVectorf<3, 1, 2> sparse_vec(dense_vec); + EXPECT_FLOAT_EQ(quadraticForm(dense_matrix, sparse_vec), 204.f); +} + +TEST(sparseVectorTest, norms) { + const float data[2] = {3.f, 4.f}; + const SparseVectorf<4, 1, 3> sparse_vec(data); + EXPECT_FLOAT_EQ(sparse_vec.norm_squared(), 25.f); + EXPECT_FLOAT_EQ(sparse_vec.norm(), 5.f); + EXPECT_TRUE(sparse_vec.longerThan(4.5f)); + EXPECT_FALSE(sparse_vec.longerThan(5.5f)); +} + + +int main(int argc, char **argv) { + testing::InitGoogleTest(&argc, argv); + std::cout << "Run SparseVector tests" << std::endl; + return RUN_ALL_TESTS(); +} diff --git a/src/lib/matrix/test/squareMatrix.cpp b/src/lib/matrix/test/squareMatrix.cpp new file mode 100644 index 0000000000..0c87debc0b --- /dev/null +++ b/src/lib/matrix/test/squareMatrix.cpp @@ -0,0 +1,148 @@ +#include "test_macros.hpp" + +#include + +using namespace matrix; + +int main() +{ + float data[9] = {1, 2, 3, + 4, 5, 6, + 7, 8, 10 + }; + SquareMatrix A(data); + Vector3 diag_check(1, 5, 10); + + TEST(isEqual(A.diag(), diag_check)); + TEST(A.trace() - 16 < FLT_EPSILON); + + float data_check[9] = { + 1.01158503f, 0.02190432f, 0.03238144f, + 0.04349195f, 1.05428524f, 0.06539627f, + 0.07576783f, 0.08708946f, 1.10894048f + }; + + float dt = 0.01f; + SquareMatrix eA = expm(SquareMatrix(A*dt), 5); + SquareMatrix eA_check(data_check); + TEST((eA - eA_check).abs().max() < 1e-3f); + + SquareMatrix A_bottomright = A.slice<2,2>(1,1); + SquareMatrix A_bottomright2; + A_bottomright2 = A.slice<2,2>(1,1); + + float data_bottomright[4] = {5, 6, + 8, 10 + }; + SquareMatrix bottomright_check(data_bottomright); + TEST(isEqual(A_bottomright, bottomright_check)); + TEST(isEqual(A_bottomright2, bottomright_check)); + + // test diagonal functions + float data_4x4[16] = {1, 2, 3, 4, + 5, 6, 7, 8, + 9, 10, 11, 12, + 13, 14,15, 16 + }; + + SquareMatrix B(data_4x4); + B.uncorrelateCovariance<1>(1); + float data_B_check[16] = {1, 0, 3, 4, + 0, 6, 0, 0, + 9, 0, 11, 12, + 13, 0,15, 16 + }; + SquareMatrix B_check(data_B_check); + TEST(isEqual(B, B_check)) + + SquareMatrix C(data_4x4); + C.uncorrelateCovariance<2>(1); + float data_C_check[16] = {1, 0, 0, 4, + 0, 6, 0, 0, + 0, 0, 11, 0, + 13, 0,0, 16 + }; + SquareMatrix C_check(data_C_check); + TEST(isEqual(C, C_check)) + + SquareMatrix D(data_4x4); + D.uncorrelateCovarianceSetVariance<2>(0, Vector2f{20,21}); + float data_D_check[16] = {20, 0, 0, 0, + 0, 21, 0, 0, + 0, 0, 11, 12, + 0, 0,15, 16 + }; + SquareMatrix D_check(data_D_check); + TEST(isEqual(D, D_check)) + + SquareMatrix E(data_4x4); + E.uncorrelateCovarianceSetVariance<3>(1, 33); + float data_E_check[16] = {1, 0, 0, 0, + 0, 33, 0, 0, + 0, 0, 33, 0, + 0, 0,0, 33 + }; + SquareMatrix E_check(data_E_check); + TEST(isEqual(E, E_check)) + + // test symmetric functions + SquareMatrix F(data_4x4); + F.makeBlockSymmetric<2>(1); + float data_F_check[16] = {1, 2, 3, 4, + 5, 6, 8.5, 8, + 9, 8.5, 11, 12, + 13, 14,15, 16 + }; + SquareMatrix F_check(data_F_check); + TEST(isEqual(F, F_check)) + TEST(F.isBlockSymmetric<2>(1)); + TEST(!F.isRowColSymmetric<2>(1)); + + SquareMatrix G(data_4x4); + G.makeRowColSymmetric<2>(1); + float data_G_check[16] = {1, 3.5, 6, 4, + 3.5, 6, 8.5, 11, + 6, 8.5, 11, 13.5, + 13, 11,13.5, 16 + }; + SquareMatrix G_check(data_G_check); + TEST(isEqual(G, G_check)); + TEST(G.isBlockSymmetric<2>(1)); + TEST(G.isRowColSymmetric<2>(1)); + + SquareMatrix H(data_4x4); + H.makeBlockSymmetric<1>(1); + float data_H_check[16] = {1, 2, 3, 4, + 5, 6, 7, 8, + 9, 10, 11, 12, + 13, 14,15, 16 + }; + SquareMatrix H_check(data_H_check); + TEST(isEqual(H, H_check)) + TEST(H.isBlockSymmetric<1>(1)); + TEST(!H.isRowColSymmetric<1>(1)); + + SquareMatrix J(data_4x4); + J.makeRowColSymmetric<1>(1); + float data_J_check[16] = {1, 3.5, 3, 4, + 3.5, 6, 8.5, 11, + 9, 8.5, 11, 12, + 13, 11,15, 16 + }; + SquareMatrix J_check(data_J_check); + TEST(isEqual(J, J_check)); + TEST(J.isBlockSymmetric<1>(1)); + TEST(J.isRowColSymmetric<1>(1)); + TEST(!J.isBlockSymmetric<3>(1)); + + float data_K[16] = {1, 2, 3, 4, + 2, 3, 4, 11, + 3, 4, 11, 12, + 4, 11,15, 16 + }; + SquareMatrix K(data_K); + TEST(!K.isRowColSymmetric<1>(2)); + return 0; +} + +/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */ diff --git a/src/lib/matrix/test/test_data.py b/src/lib/matrix/test/test_data.py new file mode 100644 index 0000000000..61ef1d0964 --- /dev/null +++ b/src/lib/matrix/test/test_data.py @@ -0,0 +1,149 @@ +from __future__ import print_function +from pylab import * +from pprint import pprint +import scipy.linalg + +# test cases, derived from doc/nasa_rotation_def.pdf + +#pylint: disable=all + +def euler_to_quat(phi, theta, psi): + "Quaternion from (body 3(psi)-2(theta)-1(phi) euler angles" + s1 = sin(psi/2) + c1 = cos(psi/2) + s2 = sin(theta/2) + c2 = cos(theta/2) + s3 = sin(phi/2) + c3 = cos(phi/2) + return array([ + s1*s2*s3 + c1*c2*c3, + -s1*s2*c3 + s3*c1*c2, + s1*s3*c2 + s2*c1*c3, + s1*c2*c3 - s2*s3*c1 + ]) + +def euler_to_dcm(phi, theta, psi): + s1 = sin(psi) + c1 = cos(psi) + s2 = sin(theta) + c2 = cos(theta) + s3 = sin(phi) + c3 = cos(phi) + return array([ + [c1*c2, c1*s2*s3 - s1*c3, c1*s2*c3 + s1*s3], + [s1*c2, s1*s2*s3 + c1*c3, s1*s2*c3 - c1*s3], + [-s2, c2*s3, c2*c3], + ]) + +def quat_prod(q, r): + "Quaternion product" + return array([ + r[0]*q[0] - r[1]*q[1] - r[2]*q[2] - r[3]*q[3], + r[0]*q[1] + r[1]*q[0] - r[2]*q[3] + r[3]*q[2], + r[0]*q[2] + r[1]*q[3] + r[2]*q[0] - r[3]*q[1], + r[0]*q[3] - r[1]*q[2] + r[2]*q[1] + r[3]*q[0] + ]) + +def dcm_to_euler(dcm): + return array([ + arctan(dcm[2,1]/ dcm[2,2]), + arctan(-dcm[2,0]/ sqrt(1 - dcm[2,0]**2)), + arctan(dcm[1,0]/ dcm[0,0]), + ]) + +def dcm_from_quat(q): + q1 = q[0] + q2 = q[1] + q3 = q[2] + q4 = q[3] + return array([ + [q1*q1 + q2*q2 - q3*q3 - q4*q4, 2*(q2*q3 - q1*q4), 2*(q2*q4 + q1*q3)], + [2*(q2*q3 + q1*q4), q1*q1 - q2*q2 + q3*q3 - q4*q4, 2*(q3*q4 - q1*q2)], + [2*(q2*q4 - q1*q3), 2*(q3*q4 + q1*q2), q1*q1 - q2*q2 - q3*q3 + q4*q4] + ]) + +def quat_to_euler(q): + "Quaternion to (body 3(psi)-2(theta)-1(phi) euler angles" + return dcm_to_euler(dcm_from_quat(q)) + +def quat_to_dcm(q): + return euler_to_dcm(quat_to_euler(q)) + +phi = 0.1 +theta = 0.2 +psi = 0.3 +print('euler', phi, theta, psi) + +q = euler_to_quat(phi, theta, psi) +assert(abs(norm(q) - 1) < FLT_EPSILON) +assert(abs(norm(q) - 1) < FLT_EPSILON) +assert(norm(array(quat_to_euler(q)) - array([phi, theta, psi])) < FLT_EPSILON) +print('\nq:') +pprint(q) + +dcm = euler_to_dcm(phi, theta, psi) +assert(norm(dcm[:,0]) == 1) +assert(norm(dcm[:,1]) == 1) +assert(norm(dcm[:,2]) == 1) +assert(abs(dcm[:,0].dot(dcm[:,1])) < FLT_EPSILON) +assert(abs(dcm[:,0].dot(dcm[:,2])) < FLT_EPSILON) +print('\ndcm:') +pprint(dcm) + +print('\nq*q', quat_prod(q, q)) + +q2 = quat_prod(q, q) +pprint(q2) +print(norm(q2)) + +print('\nq3:') +q3 = array([1,2,3,4]) +pprint(q3) +print('\nq3_norm:') +q3_norm =q3 / norm(q3) +pprint(q3_norm) + +print('\ninverse') +A = array([[0,2,3], [4,5,6], [7,8,10]]) +pprint(A) +pprint(inv(A)) + +print('\nmatrix exponential') +A = 0.01*array([[1.0,2.0,3.0], [4.0,5.0,6.0], [7.0,8.0,10.0]]) +eA_check = scipy.linalg.expm(A) + +pprint(eA_check) + +eA_approx = eye(3) +k = 1.0 +A_pow = A +for i in range(1,3): + k *= i + # print(i, k, '\n', A_pow/k, '\n') + eA_approx += A_pow/k + A_pow = A_pow.dot(A) +print(eA_approx) + +print('\nqr decomposition 4x4') +A = array([[20.0, -10.0, -13.0, 21.0], [ 17.0, 16.0, -18.0, -14], [0.7, -0.8, 0.9, -0.5], [-1.0, -1.1, -1.2, -1.3]]) +b = array([[2.], [3.], [4.], [5.]]) +x = scipy.linalg.lstsq(A,b)[0] +print('A:') +pprint(A) +print('b:') +pprint(b) +print('x:') +pprint(scipy.linalg.lstsq(A,b)[0]) + +print('\nqr decomposition 4x3') +A = array([[20.0, -10.0, -13.0], [ 17.0, 16.0, -18.0], [0.7, -0.8, 0.9], [-1.0, -1.1, -1.2]]) +b = array([[2.], [3.], [4.], [5.]]) +x = scipy.linalg.lstsq(A,b)[0] +print('A:') +pprint(A) +print('b:') +pprint(b) +print('x:') +pprint(x) + +# vim: set et ft=python fenc=utf-8 ff=unix sts=4 sw=4 ts=8 : diff --git a/src/lib/matrix/test/test_macros.hpp b/src/lib/matrix/test/test_macros.hpp new file mode 100644 index 0000000000..d6ca2d2fb1 --- /dev/null +++ b/src/lib/matrix/test/test_macros.hpp @@ -0,0 +1,46 @@ +/** + * @file test_marcos.hpp + * + * Helps with cmake testing. + * + * @author James Goppert + * Pavel Kirienko + */ +#pragma once + +#include +#include // cmath has to be introduced BEFORE we poison the C library identifiers + +#define TEST(X) if(!(X)) { fprintf(stderr, "test failed on %s:%d\n", __FILE__, __LINE__); return -1;} + +/** + * This construct is needed to catch any unintended use of the C standard library. + * Feel free to extend the list of poisoned identifiers with as many C functions as possible. + * The current list was constructed by means of automated parsing of http://en.cppreference.com/w/c/numeric/math + */ +#ifdef __GNUC__ + +// float functions +# pragma GCC poison fabsf fmodf +# pragma GCC poison remainderf remquof fmaf fmaxf fminf fdimf fnanf expf +# pragma GCC poison exp2f expm1f logf log10f log2f log1pf powf sqrtf cbrtf +# pragma GCC poison hypotf sinf cosf tanf asinf acosf atanf atan2f sinhf +# pragma GCC poison coshf tanhf asinhf acoshf atanhf erff erfcf tgammaf +# pragma GCC poison lgammaf ceilf floorf truncf roundf nearbyintf rintf +# pragma GCC poison frexpf ldexpf modff scalbnf ilogbf logbf nextafterf +# pragma GCC poison copysignf + +// the list of double functions is missing because otherwise most functions from std:: would be also poisoned, +// which we don't want + +// long double functions +# pragma GCC poison fabsl fabsl fabsl fmodl +# pragma GCC poison remainderl remquol fmal fmaxl fminl fdiml fnanl expl +# pragma GCC poison exp2l expm1l logl log10l log2l log1pl powl sqrtl cbrtl +# pragma GCC poison hypotl sinl cosl tanl asinl acosl atanl atan2l sinhl +# pragma GCC poison coshl tanhl asinhl acoshl atanhl erfl erfcl tgammal +# pragma GCC poison lgammal ceill floorl truncl roundl nearbyintl rintl +# pragma GCC poison frexpl ldexpl modfl scalbnl ilogbl logbl nextafterl +# pragma GCC poison copysignl + +#endif diff --git a/src/lib/matrix/test/transpose.cpp b/src/lib/matrix/test/transpose.cpp new file mode 100644 index 0000000000..41627a6b0c --- /dev/null +++ b/src/lib/matrix/test/transpose.cpp @@ -0,0 +1,19 @@ +#include "test_macros.hpp" + +#include + +using namespace matrix; + +int main() +{ + float data[6] = {1, 2, 3, 4, 5, 6}; + Matrix A(data); + Matrix A_T = A.transpose(); + float data_check[6] = {1, 4, 2, 5, 3, 6}; + Matrix A_T_check(data_check); + TEST(isEqual(A_T, A_T_check)); + + return 0; +} + +/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */ diff --git a/src/lib/matrix/test/upperRightTriangle.cpp b/src/lib/matrix/test/upperRightTriangle.cpp new file mode 100644 index 0000000000..1db52d936c --- /dev/null +++ b/src/lib/matrix/test/upperRightTriangle.cpp @@ -0,0 +1,23 @@ +#include "test_macros.hpp" +#include + +using namespace matrix; + +int main() +{ + float data[9] = {1, 2, 3, + 4, 5, 6, + 7, 8, 10 + }; + float urt[6] = {1, 2, 3, 5, 6, 10}; + + SquareMatrix A(data); + + for(size_t i=0; i<6; i++) { + TEST(fabs(urt[i] - A.upper_right_triangle()(i)) < FLT_EPSILON); + } + + return 0; +} + +/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */ diff --git a/src/lib/matrix/test/vector.cpp b/src/lib/matrix/test/vector.cpp new file mode 100644 index 0000000000..638ae1220f --- /dev/null +++ b/src/lib/matrix/test/vector.cpp @@ -0,0 +1,48 @@ +#include "test_macros.hpp" + +#include + +using namespace matrix; + +int main() +{ + // test data + float data1[] = {1,2,3,4,5}; + float data2[] = {6,7,8,9,10}; + Vector v1(data1); + Vector v2(data2); + + // copy constructor + Vector v3(v2); + TEST(isEqual(v2, v3)); + + // norm, dot product + TEST(isEqualF(v1.norm(), 7.416198487095663f)); + TEST(isEqualF(v1.norm_squared(), v1.norm() * v1.norm())); + TEST(isEqualF(v1.norm(), v1.length())); + TEST(isEqualF(v1.dot(v2), 130.0f)); + TEST(isEqualF(v1.dot(v2), v1 * v2)); + + // unit, unit_zero, normalize + TEST(isEqualF(v2.unit().norm(), 1.f)); + TEST(isEqualF(v2.unit_or_zero().norm(), 1.f)); + TEST(isEqualF(Vector().unit_or_zero().norm(), 0.f)); + v2.normalize(); + TEST(isEqualF(v2.norm(), 1.f)); + + // sqrt + float data1_sq[] = {1,4,9,16,25}; + Vector v4(data1_sq); + TEST(isEqual(v1, v4.sqrt())); + + // longerThan + Vector v5; + v5(0) = 3; + v5(1) = 4; + TEST(v5.longerThan(4.99f)); + TEST(!v5.longerThan(5.f)); + + return 0; +} + +/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */ diff --git a/src/lib/matrix/test/vector2.cpp b/src/lib/matrix/test/vector2.cpp new file mode 100644 index 0000000000..1ea88fc933 --- /dev/null +++ b/src/lib/matrix/test/vector2.cpp @@ -0,0 +1,40 @@ + + +#include + +#include "test_macros.hpp" + +using namespace matrix; + +int main() +{ + Vector2f a(1, 0); + Vector2f b(0, 1); + TEST(fabs(a % b - 1.0f) < FLT_EPSILON); + + Vector2f c; + TEST(fabs(c(0) - 0) < FLT_EPSILON); + TEST(fabs(c(1) - 0) < FLT_EPSILON); + + Matrix d(a); + TEST(fabs(d(0,0) - 1) < FLT_EPSILON); + TEST(fabs(d(1,0) - 0) < FLT_EPSILON); + + Vector2f e(d); + TEST(fabs(e(0) - 1) < FLT_EPSILON); + TEST(fabs(e(1) - 0) < FLT_EPSILON); + + float data[] = {4,5}; + Vector2f f(data); + TEST(fabs(f(0) - 4) < FLT_EPSILON); + TEST(fabs(f(1) - 5) < FLT_EPSILON); + + Vector3f g(1.23f, 423.4f, 3221.f); + Vector2f h(g); + TEST(fabs(h(0) - 1.23f) < FLT_EPSILON); + TEST(fabs(h(1) - 423.4f) < FLT_EPSILON); + + return 0; +} + +/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */ diff --git a/src/lib/matrix/test/vector3.cpp b/src/lib/matrix/test/vector3.cpp new file mode 100644 index 0000000000..8143c8ab32 --- /dev/null +++ b/src/lib/matrix/test/vector3.cpp @@ -0,0 +1,61 @@ +#include "test_macros.hpp" + +#include + +using namespace matrix; + +int main() +{ + Vector3f a(1, 0, 0); + Vector3f b(0, 1, 0); + Vector3f c = a.cross(b); + TEST(isEqual(c, Vector3f(0,0,1))); + c = a % b; + TEST(isEqual(c, Vector3f(0,0,1))); + Matrix d(c); + Vector3f e(d); + TEST(isEqual(e, d)); + float data[] = {4, 5, 6}; + Vector3f f(data); + TEST(isEqual(f, Vector3f(4, 5, 6))); + + TEST(isEqual(a + b, Vector3f(1, 1, 0))); + TEST(isEqual(a - b, Vector3f(1, -1, 0))); + TEST(isEqualF(a * b, 0.0f)); + TEST(isEqual(-a, Vector3f(-1, 0, 0))); + TEST(isEqual(a.unit(), a)); + TEST(isEqual(a.unit(), a.normalized())); + TEST(isEqual(a*2.0, Vector3f(2, 0, 0))); + + Vector2f g2(1,3); + Vector3f g3(7, 11, 17); + g3.xy() = g2; + TEST(isEqual(g3, Vector3f(1, 3, 17))); + + const Vector3f g4(g3); + Vector2f g5 = g4.xy(); + TEST(isEqual(g5,g2)); + TEST(isEqual(g2,Vector2f(g4.xy()))); + + Vector3f h; + TEST(isEqual(h,Vector3f(0,0,0))); + + Vector j; + j(0) = 1; + j(1) = 2; + j(2) = 3; + j(3) = 4; + + Vector3f k = j.slice<3,1>(0,0); + Vector3f k_test(1,2,3); + TEST(isEqual(k,k_test)); + + Vector3f m1(1, 2, 3); + Vector3f m2(3.1f, 4.1f, 5.1f); + TEST(isEqual(m2, m1 + 2.1f)); + TEST(isEqual(m2 - 2.1f, m1)); + + return 0; +} + +/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */ diff --git a/src/lib/matrix/test/vectorAssignment.cpp b/src/lib/matrix/test/vectorAssignment.cpp new file mode 100644 index 0000000000..e8e63462a2 --- /dev/null +++ b/src/lib/matrix/test/vectorAssignment.cpp @@ -0,0 +1,34 @@ +#include + +#include "test_macros.hpp" + +using namespace matrix; + +int main() +{ + Vector3f v; + v(0) = 1; + v(1) = 2; + v(2) = 3; + + static const float eps = 1e-7f; + + TEST(fabs(v(0) - 1) < eps); + TEST(fabs(v(1) - 2) < eps); + TEST(fabs(v(2) - 3) < eps); + + Vector3f v2(4, 5, 6); + + TEST(fabs(v2(0) - 4) < eps); + TEST(fabs(v2(1) - 5) < eps); + TEST(fabs(v2(2) - 6) < eps); + + SquareMatrix m = diag(Vector3f(1,2,3)); + TEST(fabs(m(0, 0) - 1) < eps); + TEST(fabs(m(1, 1) - 2) < eps); + TEST(fabs(m(2, 2) - 3) < eps); + + return 0; +} + +/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */