From 96bf3aa5d0e62183b25ffc1c376236dfc7e8550b Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 16 Nov 2021 12:30:51 -0500 Subject: [PATCH] matrix: apply PX4 astyle --- Tools/astyle/files_to_check_code_style.sh | 1 - src/lib/matrix/matrix/AxisAngle.hpp | 225 +-- src/lib/matrix/matrix/Dcm.hpp | 252 ++-- src/lib/matrix/matrix/Dual.hpp | 312 ++-- src/lib/matrix/matrix/Euler.hpp | 202 ++- src/lib/matrix/matrix/LeastSquaresSolver.hpp | 232 +-- src/lib/matrix/matrix/Matrix.hpp | 1381 +++++++++--------- src/lib/matrix/matrix/PseudoInverse.hpp | 138 +- src/lib/matrix/matrix/Quaternion.hpp | 894 ++++++------ src/lib/matrix/matrix/Scalar.hpp | 50 +- src/lib/matrix/matrix/Slice.hpp | 511 ++++--- src/lib/matrix/matrix/SparseVector.hpp | 302 ++-- src/lib/matrix/matrix/SquareMatrix.hpp | 832 ++++++----- src/lib/matrix/matrix/Vector.hpp | 197 +-- src/lib/matrix/matrix/Vector2.hpp | 80 +- src/lib/matrix/matrix/Vector3.hpp | 194 +-- src/lib/matrix/matrix/filter.hpp | 29 +- src/lib/matrix/matrix/helper_functions.hpp | 63 +- src/lib/matrix/matrix/integration.hpp | 64 +- src/lib/matrix/matrix/stdlib_imports.hpp | 3 +- src/lib/matrix/test/attitude.cpp | 931 ++++++------ src/lib/matrix/test/copyto.cpp | 110 +- src/lib/matrix/test/dual.cpp | 477 +++--- src/lib/matrix/test/filter.cpp | 33 +- src/lib/matrix/test/hatvee.cpp | 17 +- src/lib/matrix/test/helper.cpp | 123 +- src/lib/matrix/test/integration.cpp | 26 +- src/lib/matrix/test/inverse.cpp | 270 ++-- src/lib/matrix/test/least_squares.cpp | 130 +- src/lib/matrix/test/matrixAssignment.cpp | 440 +++--- src/lib/matrix/test/matrixMult.cpp | 103 +- src/lib/matrix/test/matrixScalarMult.cpp | 15 +- src/lib/matrix/test/pseudoInverse.cpp | 251 ++-- src/lib/matrix/test/setIdentity.cpp | 47 +- src/lib/matrix/test/slice.cpp | 375 ++--- src/lib/matrix/test/sparseVector.cpp | 194 +-- src/lib/matrix/test/squareMatrix.cpp | 245 ++-- src/lib/matrix/test/test_data.py | 2 - src/lib/matrix/test/transpose.cpp | 15 +- src/lib/matrix/test/upperRightTriangle.cpp | 21 +- src/lib/matrix/test/vector.cpp | 63 +- src/lib/matrix/test/vector2.cpp | 43 +- src/lib/matrix/test/vector3.cpp | 85 +- src/lib/matrix/test/vectorAssignment.cpp | 35 +- 44 files changed, 5137 insertions(+), 4876 deletions(-) diff --git a/Tools/astyle/files_to_check_code_style.sh b/Tools/astyle/files_to_check_code_style.sh index f9033e35ca..963c8a51cf 100755 --- a/Tools/astyle/files_to_check_code_style.sh +++ b/Tools/astyle/files_to_check_code_style.sh @@ -17,7 +17,6 @@ exec find boards msg src platforms test \ -path src/drivers/uavcannode_gps_demo/libcanard -prune -o \ -path src/lib/crypto/monocypher -prune -o \ -path src/lib/events/libevents -prune -o \ - -path src/lib/matrix -prune -o \ -path src/lib/parameters/uthash -prune -o \ -path src/modules/ekf2/EKF -prune -o \ -path src/modules/gyro_fft/CMSIS_5 -prune -o \ diff --git a/src/lib/matrix/matrix/AxisAngle.hpp b/src/lib/matrix/matrix/AxisAngle.hpp index 41a9711541..bc935ec20f 100644 --- a/src/lib/matrix/matrix/AxisAngle.hpp +++ b/src/lib/matrix/matrix/AxisAngle.hpp @@ -30,131 +30,134 @@ template class AxisAngle : public Vector { public: - using Matrix31 = Matrix; + using Matrix31 = Matrix; - /** - * Constructor from array - * - * @param data_ array - */ - explicit AxisAngle(const Type data_[3]) : - Vector(data_) - { - } + /** + * Constructor from array + * + * @param data_ array + */ + explicit AxisAngle(const Type data_[3]) : + Vector(data_) + { + } - /** - * Standard constructor - */ - AxisAngle() = default; + /** + * Standard constructor + */ + AxisAngle() = default; - /** - * Constructor from Matrix31 - * - * @param other Matrix31 to copy - */ - AxisAngle(const Matrix31 &other) : - Vector(other) - { - } + /** + * 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 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(); - /** - * 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)); - } + if (fabs(mag) >= Type(1e-10)) { + v = q.imag() * Type(Type(2) * atan2(mag, q(0)) / mag); - /** - * 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)); - } + } else { + v = q.imag() * Type(Type(2) * Type(sign(q(0)))); + } + } - /** - * 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 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 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_; - } + /** + * 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); - } - } + Vector axis() + { + if (Vector::norm() > 0) { + return Vector::unit(); - Type angle() { - return Vector::norm(); - } + } 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 index 0e1cf49960..87d5e2f354 100644 --- a/src/lib/matrix/matrix/Dcm.hpp +++ b/src/lib/matrix/matrix/Dcm.hpp @@ -39,149 +39,147 @@ template class Dcm : public SquareMatrix { public: - using Vector3 = Matrix; + using Vector3 = Matrix; - /** - * Standard constructor - * - * Initializes to identity - */ - Dcm() : SquareMatrix(eye()) {} + /** + * 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_[3][3]) : SquareMatrix(data_) + { + } - /** - * Constructor from array - * - * @param _data pointer to array - */ - explicit Dcm(const Type data_[9]) : 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) - { - } + /** + * 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 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())); + /** + * 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(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(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; - } + 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); - } + /** + * 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; - } + 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(); - } - } + 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 index da8eabf9e8..284b45d012 100644 --- a/src/lib/matrix/matrix/Dual.hpp +++ b/src/lib/matrix/matrix/Dual.hpp @@ -22,350 +22,356 @@ template class Vector; template -struct Dual -{ - static constexpr size_t WIDTH = N; +struct Dual { + static constexpr size_t WIDTH = N; - Scalar value {}; - Vector derivative; + Scalar value {}; + Vector derivative; - Dual() = default; + Dual() = default; - explicit Dual(Scalar v, size_t inputDimension = 65535) - { - value = v; - if (inputDimension < N) { - derivative(inputDimension) = Scalar(1); - } - } + explicit Dual(Scalar v, size_t inputDimension = 65535) + { + value = v; - explicit Dual(Scalar v, const Vector& d) : - value(v), derivative(d) - {} + if (inputDimension < N) { + derivative(inputDimension) = Scalar(1); + } + } - Dual& operator=(const Scalar& a) - { - derivative.setZero(); - value = a; - return *this; - } + explicit Dual(Scalar v, const Vector &d) : + value(v), derivative(d) + {} - Dual& operator +=(const Dual& a) - { - return (*this = *this + a); - } + 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 /=(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 /=(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); + } - 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) +Dual operator+(const Dual &a) { - return a; + return a; } template -Dual operator-(const Dual& a) +Dual operator-(const Dual &a) { - return Dual(-a.value, -a.derivative); + return Dual(-a.value, -a.derivative); } template -Dual operator+(const Dual& a, const Dual& b) +Dual operator+(const Dual &a, const Dual &b) { - return Dual(a.value + b.value, a.derivative + b.derivative); + return Dual(a.value + b.value, a.derivative + b.derivative); } template -Dual operator-(const Dual& a, const Dual& b) +Dual operator-(const Dual &a, const Dual &b) { - return a + (-b); + return a + (-b); } template -Dual operator+(const Dual& a, Scalar b) +Dual operator+(const Dual &a, Scalar b) { - return Dual(a.value + b, a.derivative); + return Dual(a.value + b, a.derivative); } template -Dual operator-(const Dual& a, Scalar b) +Dual operator-(const Dual &a, Scalar b) { - return a + (-b); + return a + (-b); } template -Dual operator+(Scalar a, const Dual& b) +Dual operator+(Scalar a, const Dual &b) { - return Dual(a + b.value, b.derivative); + return Dual(a + b.value, b.derivative); } template -Dual operator-(Scalar a, const Dual& b) +Dual operator-(Scalar a, const Dual &b) { - return a + (-b); + return a + (-b); } template -Dual operator*(const Dual& a, const Dual& b) +Dual operator*(const Dual &a, const Dual &b) { - return Dual(a.value * b.value, a.value * b.derivative + b.value * a.derivative); + return Dual(a.value * b.value, a.value * b.derivative + b.value * a.derivative); } template -Dual operator*(const Dual& a, Scalar b) +Dual operator*(const Dual &a, Scalar b) { - return Dual(a.value * b, a.derivative * b); + return Dual(a.value * b, a.derivative * b); } template -Dual operator*(Scalar a, const Dual& b) +Dual operator*(Scalar a, const Dual &b) { - return b * a; + return b * a; } template -Dual operator/(const Dual& a, const Dual& b) +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); + 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) +Dual operator/(const Dual &a, Scalar b) { - return a * (Scalar(1) / b); + return a * (Scalar(1) / b); } template -Dual operator/(Scalar a, const Dual& b) +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); + 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) +Dual sqrt(const Dual &a) { - Scalar real = sqrt(a.value); - return Dual(real, a.derivative * (Scalar(1) / (Scalar(2) * real))); + Scalar real = sqrt(a.value); + return Dual(real, a.derivative * (Scalar(1) / (Scalar(2) * real))); } // abs template -Dual abs(const Dual& a) +Dual abs(const Dual &a) { - return a.value >= Scalar(0) ? a : -a; + return a.value >= Scalar(0) ? a : -a; } // ceil template -Dual ceil(const Dual& a) +Dual ceil(const Dual &a) { - return Dual(ceil(a.value)); + return Dual(ceil(a.value)); } // floor template -Dual floor(const Dual& a) +Dual floor(const Dual &a) { - return Dual(floor(a.value)); + return Dual(floor(a.value)); } // fmod template -Dual fmod(const Dual& a, Scalar mod) +Dual fmod(const Dual &a, Scalar mod) { - return Dual(a.value - Scalar(size_t(a.value / mod)) * mod, a.derivative); + return Dual(a.value - Scalar(size_t(a.value / mod)) * mod, a.derivative); } // max template -Dual max(const Dual& a, const Dual& b) +Dual max(const Dual &a, const Dual &b) { - return a.value >= b.value ? a : b; + return a.value >= b.value ? a : b; } // min template -Dual min(const Dual& a, const Dual& b) +Dual min(const Dual &a, const Dual &b) { - return a.value < b.value ? a : b; + return a.value < b.value ? a : b; } // isnan template bool IsNan(Scalar a) { - return isnan(a); + return isnan(a); } template -bool IsNan(const Dual& a) +bool IsNan(const Dual &a) { - return IsNan(a.value); + return IsNan(a.value); } // isfinite template bool IsFinite(Scalar a) { - return isfinite(a); + return isfinite(a); } template -bool IsFinite(const Dual& a) +bool IsFinite(const Dual &a) { - return IsFinite(a.value); + return IsFinite(a.value); } // isinf template bool IsInf(Scalar a) { - return isinf(a); + return isinf(a); } template -bool IsInf(const Dual& a) +bool IsInf(const Dual &a) { - return IsInf(a.value); + return IsInf(a.value); } // trig // sin template -Dual sin(const Dual& a) +Dual sin(const Dual &a) { - return Dual(sin(a.value), cos(a.value) * a.derivative); + return Dual(sin(a.value), cos(a.value) * a.derivative); } // cos template -Dual cos(const Dual& a) +Dual cos(const Dual &a) { - return Dual(cos(a.value), -sin(a.value) * a.derivative); + return Dual(cos(a.value), -sin(a.value) * a.derivative); } // tan template -Dual tan(const Dual& a) +Dual tan(const Dual &a) { - Scalar real = tan(a.value); - return Dual(real, (Scalar(1) + real * real) * a.derivative); + Scalar real = tan(a.value); + return Dual(real, (Scalar(1) + real * real) * a.derivative); } // asin template -Dual asin(const Dual& a) +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); + 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) +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); + 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) +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); + 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) +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); + // 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 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; + 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 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; + 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) +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; + 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 index 6145cffd38..b1fce355ec 100644 --- a/src/lib/matrix/matrix/Euler.hpp +++ b/src/lib/matrix/matrix/Euler.hpp @@ -36,116 +36,116 @@ template class Euler : public Vector { public: - /** - * Standard constructor - */ - Euler() = default; + /** + * Standard constructor + */ + Euler() = default; - /** - * Copy constructor - * - * @param other vector to copy - */ - Euler(const Vector &other) : - Vector(other) - { - } + /** + * 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 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 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)); + /** + * 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)); + 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 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)); - } - } + } 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))) - { - } + /** + * 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() 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); - } + inline Type &phi() + { + return (*this)(0); + } + inline Type &theta() + { + return (*this)(1); + } + inline Type &psi() + { + return (*this)(2); + } }; @@ -153,5 +153,3 @@ 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 index 1f518b4d69..0756e05ee3 100644 --- a/src/lib/matrix/matrix/LeastSquaresSolver.hpp +++ b/src/lib/matrix/matrix/LeastSquaresSolver.hpp @@ -16,131 +16,149 @@ #include "math.hpp" -namespace matrix { +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"); + /** + * @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; + // 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 j = 0; j < N; j++) { + Type normx = Type(0); - 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; - } - } + 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; - /** - * @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; + // prevent divide by zero + // also covers u1. normx is never negative + if (normx < Type(1e-8)) { + break; + } - 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); - } + Type w[M] = {}; + w[0] = Type(1); - for (size_t i = j; i < M; i++) { - qtbv(i) -= _tau(j) * w[i-j] * tmp; - } - } - return qtbv; - } + for (size_t i = j + 1; i < M; i++) { + w[i - j] = _A(i, j) / u1; + _A(i, j) = w[i - j]; + } - /** - * @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; + _A(j, j) = s * normx; + _tau(j) = -s * u1 / normx; - // 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; - } + 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; + 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 index 2b865f4ab3..3f91b27a74 100644 --- a/src/lib/matrix/matrix/Matrix.hpp +++ b/src/lib/matrix/matrix/Matrix.hpp @@ -33,732 +33,791 @@ class Slice; template class Matrix { - Type _data[M][N] {}; + 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; - } + // 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 < M; i++) { + for (size_t j = 0; j < N; j++) { + r(i, j) = Type(fabs((*this)(i, j))); + } + } + + return r; + } + + Type max() const + { + Type max_val = (*this)(0, 0); + + for (size_t i = 0; i < M; i++) { + for (size_t j = 0; j < N; 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 < M; i++) { + for (size_t j = 0; j < N; j++) { + Type val = (*this)(i, j); + + if (val < min_val) { + min_val = val; + } + } + } + + return min_val; + } + + bool isAllNan() const + { + const Matrix &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; +Matrix zeros() +{ + Matrix m; + m.setZero(); + return m; } template -Matrix ones() { - Matrix m; - m.setOne(); - return m; +Matrix ones() +{ + Matrix m; + m.setOne(); + return m; } template -Matrix nans() { - Matrix m; - m.setNaN(); - return m; +Matrix nans() +{ + Matrix m; + m.setNaN(); + return m; } template Matrix operator*(Type scalar, const Matrix &other) { - return other * scalar; + 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; + 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; +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; +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)); - } +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; +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); +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; +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; +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); +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; +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; + 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; + 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) +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; + 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 index 1590efecfd..e4a2e6548a 100644 --- a/src/lib/matrix/matrix/PseudoInverse.hpp +++ b/src/lib/matrix/matrix/PseudoInverse.hpp @@ -21,38 +21,44 @@ namespace matrix * 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) +bool geninv(const Matrix &G, Matrix &res) { - size_t rank; - if (M <= N) { - SquareMatrix A = G * G.transpose(); - SquareMatrix L = fullRankCholesky(A, rank); + size_t 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); + if (M <= N) { + SquareMatrix A = G * G.transpose(); + SquareMatrix L = fullRankCholesky(A, rank); - } else { - SquareMatrix A = G.transpose() * G; - SquareMatrix L = fullRankCholesky(A, rank); + A = L.transpose() * L; + SquareMatrix X; - 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; + 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; } @@ -62,58 +68,60 @@ Type typeEpsilon(); template<> inline float typeEpsilon() { - return FLT_EPSILON; + return FLT_EPSILON; } /** * Full rank Cholesky factorization of A */ template -SquareMatrix fullRankCholesky(const SquareMatrix & A, - size_t& rank) +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(); + // Loses one ulp accuracy per row of diag, relative to largest magnitude + const Type tol = N * typeEpsilon() * A.diag().max(); - Matrix L; + Matrix L; - size_t r = 0; - for (size_t k = 0; k < N; k++) { + size_t r = 0; - if (r == 0) { - for (size_t i = k; i < N; i++) { - L(i, r) = A(i, k); - } + for (size_t k = 0; k < N; 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 (r == 0) { + for (size_t i = k; i < N; i++) { + L(i, r) = A(i, k); + } - if (k < N - 1) { - for (size_t i = k + 1; i < N; i++) { - L(i, r) = L(i, r) / L(k, r); - } - } + } else { + for (size_t i = k; i < N; i++) { + // Compute LL = L[k:n, :r] * L[k, :r].T + Type LL = Type(); - r = r + 1; - } - } + for (size_t j = 0; j < r; j++) { + LL += L(i, j) * L(k, j); + } - // Return rank - rank = r; + L(i, r) = A(i, k) - LL; + } + } - return L; + 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 index ddcbdf9ec0..dfe5860b9a 100644 --- a/src/lib/matrix/matrix/Quaternion.hpp +++ b/src/lib/matrix/matrix/Quaternion.hpp @@ -53,472 +53,492 @@ template class Quaternion : public Vector { public: - using Matrix41 = Matrix; - using Matrix31 = Matrix; + using Matrix41 = Matrix; + using Matrix31 = Matrix; - /** - * Constructor from array - * - * @param data_ array - */ - explicit Quaternion(const Type data_[4]) : - Vector(data_) - { - } + /** + * 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; - } + /** + * 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 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 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(); - /** - * 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; - } + 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; - /** - * 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; - } - } + } 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; - /** - * 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(); - } + } 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; - /** - * 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; - } + } 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; + } + } - /** - * 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) }; - } + /** + * 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; + } - /** - * Self-multiplication operator - * - * @param other quaternion to multiply with - */ - void operator*=(const Quaternion &other) - { - Quaternion &self = *this; - self = self * other; - } + /** + * Quaternion from AxisAngle + * + * @param aa axis-angle vector + */ + Quaternion(const AxisAngle &aa) + { + Quaternion &q = *this; + Type angle = aa.norm(); + Vector axis = aa.unit(); - /** - * Scalar multiplication operator - * - * @param scalar scalar to multiply with - * @return product - */ - Quaternion operator*(Type scalar) const - { - const Quaternion &q = *this; - return scalar * q; - } + if (angle < Type(1e-10)) { + q(0) = Type(1); + q(1) = q(2) = q(3) = 0; - /** - * Scalar self-multiplication operator - * - * @param scalar scalar to multiply with - */ - void operator*=(Type scalar) - { - Quaternion &q = *this; - q = q * scalar; - } + } 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; + } + } - /** - * 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); - } + /** + * 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); - /** - * 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); - } + 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(); - /** - * 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! + if (cr(0) < cr(1)) { + if (cr(0) < cr(2)) { + cr = Vector3(1, 0, 0); - Type u_norm = u.norm(); - Type sinc_u, cos_u; + } else { + cr = Vector3(0, 0, 1); + } - if (u_norm < tol) { - Type u2 = u_norm * u_norm; - Type u4 = u2 * u2; - Type u6 = u4 * u2; + } else { + if (cr(1) < cr(2)) { + cr = Vector3(0, 1, 0); - // 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)); - } + } else { + cr = Vector3(0, 0, 1); + } + } - /** 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(); + q(0) = Type(0); + cr = src.cross(cr); - 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); - } - } + } else { + // normal case, do half-way quaternion solution + q(0) = dt + sqrt(src.norm_squared() * dst.norm_squared()); + } - /** - * Invert quaternion in place - */ - void invert() - { - *this = this->inversed(); - } + q(1) = cr(0); + q(2) = cr(1); + q(3) = cr(2); + q.normalize(); + } - /** - * 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); - } + /** + * 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; + } - /** - * Bring quaternion to canonical form - */ - void canonicalize() - { - *this = this->canonical(); - } + /** + * 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) }; + } - /** - * Return canonical form of the quaternion - * - * @return quaternion in canonical from - */ - Quaternion canonical() const - { - const Quaternion &q = *this; + /** + * Self-multiplication operator + * + * @param other quaternion to multiply with + */ + void operator*=(const Quaternion &other) + { + Quaternion &self = *this; + self = self * other; + } - for (size_t i = 0; i < 4; i++) { - if (fabs(q(i)) > FLT_EPSILON) { - return q * Type(matrix::sign(q(i))); - } - } - return q; - } + /** + * Scalar multiplication operator + * + * @param scalar scalar to multiply with + * @return product + */ + Quaternion operator*(Type scalar) const + { + const Quaternion &q = *this; + return scalar * q; + } - /** - * Rotate quaternion from rotation vector - * - * @param vec rotation vector - */ - void rotate(const AxisAngle &vec) - { - Quaternion res(vec); - (*this) = res * (*this); - } + /** + * Scalar self-multiplication operator + * + * @param scalar scalar to multiply with + */ + void operator*=(Type scalar) + { + Quaternion &q = *this; + q = q * scalar; + } - /** - * 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)); - } + /** + * 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); + } - /** - * 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)); - } + /** + * 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); + } - /** - * Imaginary components of quaternion - */ - Vector3 imag() const - { - const Quaternion &q = *this; - return Vector3(q(1), q(2), q(3)); - } + /** + * 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! - /** - * 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; - } + 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; @@ -528,5 +548,3 @@ 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 index 182e0f7786..26538f976c 100644 --- a/src/lib/matrix/matrix/Scalar.hpp +++ b/src/lib/matrix/matrix/Scalar.hpp @@ -17,36 +17,38 @@ template class Scalar { public: - Scalar() = delete; + Scalar() = delete; - Scalar(const Matrix & other) : - _value{other(0,0)} - { - } + Scalar(const Matrix &other) : + _value{other(0, 0)} + { + } - Scalar(Type other) : _value(other) - { - } + Scalar(Type other) : _value(other) + { + } - operator const Type &() - { - return _value; - } + operator const Type &() + { + return _value; + } - operator Matrix() const { - Matrix m; - m(0, 0) = _value; - return m; - } + operator Matrix() const + { + Matrix m; + m(0, 0) = _value; + return m; + } - operator Vector() const { - Vector m; - m(0) = _value; - return m; - } + operator Vector() const + { + Vector m; + m(0) = _value; + return m; + } private: - const Type _value; + const Type _value; }; @@ -54,5 +56,3 @@ 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 index f98ac483c0..f730c548f9 100644 --- a/src/lib/matrix/matrix/Slice.hpp +++ b/src/lib/matrix/matrix/Slice.hpp @@ -11,7 +11,8 @@ #include "math.hpp" -namespace matrix { +namespace matrix +{ template class Matrix; @@ -20,285 +21,315 @@ template class Vector; template -class Slice { +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); - } + 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); + const Type &operator()(size_t i, size_t j) const + { + assert(i < P); + assert(j < Q); - return (*_data)(_x0 + i, _y0 + j); - } + return (*_data)(_x0 + i, _y0 + j); + } - Type &operator()(size_t i, size_t j) + Type &operator()(size_t i, size_t j) - { - assert(i < P); - assert(j < Q); + { + assert(i < P); + assert(j < Q); - return (*_data)(_x0 + i, _y0 + j); - } + 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; - } + template + Slice &operator=(const Slice &other) + { + Slice &self = *this; - 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; - } + for (size_t i = 0; i < P; i++) { + for (size_t j = 0; j < Q; j++) { + self(i, j) = other(i, j); + } + } - 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; - } + 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; - } + Slice &operator=(const Matrix &other) + { + Slice &self = *this; - 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; - } + for (size_t i = 0; i < P; i++) { + for (size_t j = 0; j < Q; j++) { + self(i, j) = other(i, j); + } + } - 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; - } + 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; - 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; - } + for (size_t i = 0; i < P; i++) { + for (size_t j = 0; j < Q; j++) { + self(i, j) = other; + } + } - 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; - } + 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; - 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; - } + for (size_t j = 0; j < Q; j++) { + self(0, j) = other(j); + } - Slice& operator/=(const Type& other) - { - return operator*=(Type(1) / other); - } + return self; + } - 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; - } + template + Slice &operator+=(const Slice &other) + { + Slice &self = *this; - Matrix operator/(const Type& other) const - { - const Slice& self = *this; - return self * (Type(1) / other); - } + for (size_t i = 0; i < P; i++) { + for (size_t j = 0; j < Q; j++) { + self(i, j) += other(i, j); + } + } - template - const Slice slice(size_t x0, size_t y0) const - { - return Slice(x0 + _x0, y0 + _y0, _data); - } + return self; + } - template - Slice slice(size_t x0, size_t y0) - { - return Slice(x0 + _x0, y0 + _y0, _data); - } + Slice &operator+=(const Matrix &other) + { + Slice &self = *this; - 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++) { + self(i, j) += other(i, j); + } + } - for (size_t i = 0; i < P; i++) { - for (size_t j = 0; j < Q; j++) { - dst[i*N+j] = self(i, j); - } - } - } + return self; + } - void copyToColumnMajor(Type dst[P*Q]) const - { - const Slice &self = *this; + Slice &operator+=(const Type &other) + { + 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); - } - } - } + for (size_t i = 0; i < P; i++) { + for (size_t j = 0; j < Q; j++) { + self(i, j) += other; + } + } - 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; - } + template + Slice &operator-=(const Slice &other) + { + Slice &self = *this; - Type norm() const - { - return matrix::sqrt(norm_squared()); - } + for (size_t i = 0; i < P; i++) { + for (size_t j = 0; j < Q; j++) { + self(i, j) -= other(i, j); + } + } - bool longerThan(Type testVal) const - { - return norm_squared() > testVal*testVal; - } + return self; + } - Type max() const - { - Type max_val = (*this)(0,0); + Slice &operator-=(const Matrix &other) + { + Slice &self = *this; - for (size_t i = 0; i < P; i++) { - for (size_t j = 0; j < Q; j++) { - Type val = (*this)(i,j); + for (size_t i = 0; i < P; i++) { + for (size_t j = 0; j < Q; j++) { + self(i, j) -= other(i, j); + } + } - if (val > max_val) { - max_val = val; - } - } - } + return self; + } - return max_val; - } + Slice &operator-=(const Type &other) + { + Slice &self = *this; - 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++) { + self(i, j) -= other; + } + } - for (size_t i = 0; i < P; i++) { - for (size_t j = 0; j < Q; j++) { - Type val = (*this)(i,j); + return self; + } - if (val < min_val) { - min_val = val; - } - } - } + Slice &operator*=(const Type &other) + { + Slice &self = *this; - return min_val; - } + 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 < Type, P < Q ? P : Q > diag() const + { + const Slice &self = *this; + Vector < Type, P < Q ? P : Q > res; + + for (size_t j = 0; j < (P < Q ? P : Q); j++) { + res(j) = self(j, j); + } + + return res; + } + + Type norm_squared() const + { + const Slice &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; + size_t _x0, _y0; + Matrix *_data; }; } diff --git a/src/lib/matrix/matrix/SparseVector.hpp b/src/lib/matrix/matrix/SparseVector.hpp index ca4659a7a3..4af242516e 100644 --- a/src/lib/matrix/matrix/SparseVector.hpp +++ b/src/lib/matrix/matrix/SparseVector.hpp @@ -12,175 +12,213 @@ #include "math.hpp" -namespace matrix { +namespace matrix +{ template struct force_constexpr_eval { - static const int value = N; + static const int value = N; }; // Vector that only store nonzero elements, // which indices are specified as parameter pack template -class SparseVector { +class SparseVector +{ private: - static constexpr size_t N = sizeof...(Idxs); - static constexpr size_t _indices[N] {Idxs...}; + 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 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; + } + } + } - 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"); + return false; + } + static constexpr size_t findMaxIndex() + { + size_t maxIndex = 0; - Type _data[N] {}; + for (size_t i = 0; i < N; i++) { + if (maxIndex < _indices[i]) { + maxIndex = _indices[i]; + } + } - 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; - } + 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 non_zeros() const + { + return N; + } - constexpr size_t index(size_t i) const { - return SparseVector::_indices[i]; - } + constexpr size_t index(size_t i) const + { + return SparseVector::_indices[i]; + } - SparseVector() = default; + SparseVector() = default; - SparseVector(const matrix::Vector& data) { - for (size_t i = 0; i < N; i++) { - _data[i] = data(_indices[i]); - } - } + 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)); - } + 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() 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]; - } + 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) const + { + assert(i < N); + return _data[i]; + } - inline Type& atCompressedIndex(size_t i) { - 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); - } - } + 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; - } + Type dot(const matrix::Vector &other) const + { + Type accum(0); - 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; - } + for (size_t i = 0; i < N; i++) { + accum += _data[i] * other(_indices[i]); + } - SparseVector& operator+=(Type t) { - for (size_t i = 0; i < N; i++) { - _data[i] += t; - } - return *this; - } + return accum; + } - Type norm_squared() const - { - Type accum(0); - for (size_t i = 0; i < N; i++) { - accum += _data[i] * _data[i]; - } - return accum; - } + matrix::Vector operator+(const matrix::Vector &other) const + { + matrix::Vector vec = other; - Type norm() const - { - return matrix::sqrt(norm_squared()); - } + for (size_t i = 0; i < N; i++) { + vec(_indices[i]) += _data[i]; + } - bool longerThan(Type testVal) const - { - return norm_squared() > testVal*testVal; - } + 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; +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; +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 +template constexpr size_t SparseVector::_indices[SparseVector::N]; template diff --git a/src/lib/matrix/matrix/SquareMatrix.hpp b/src/lib/matrix/matrix/SquareMatrix.hpp index 127e89b2f1..fd6754a1de 100644 --- a/src/lib/matrix/matrix/SquareMatrix.hpp +++ b/src/lib/matrix/matrix/SquareMatrix.hpp @@ -26,243 +26,257 @@ template class SquareMatrix : public Matrix { public: - SquareMatrix() = default; + SquareMatrix() = default; - explicit SquareMatrix(const Type data_[M][M]) : - Matrix(data_) - { - } + explicit SquareMatrix(const Type data_[M][M]) : + Matrix(data_) + { + } - explicit SquareMatrix(const Type data_[M*M]) : - Matrix(data_) - { - } + explicit SquareMatrix(const Type data_[M * M]) : + Matrix(data_) + { + } - SquareMatrix(const Matrix &other) : - Matrix(other) - { - } + SquareMatrix(const Matrix &other) : + Matrix(other) + { + } - template - SquareMatrix(const Slice& in_slice) : Matrix(in_slice) - { - } + template + SquareMatrix(const Slice &in_slice) : Matrix(in_slice) + { + } - SquareMatrix& operator=(const Matrix& other) - { - Matrix::operator=(other); - return *this; - } + SquareMatrix &operator=(const Matrix &other) + { + Matrix::operator=(other); + return *this; + } - template - SquareMatrix & operator=(const Slice& in_slice) - { - Matrix::operator=(in_slice); - 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 + 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); - } + 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 SquareMatrix I() const + { + SquareMatrix i; - // inverse alias - inline bool I(SquareMatrix &i) const - { - return inv(*this, 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; + Vector diag() const + { + Vector res; + const SquareMatrix &self = *this; - for (size_t i = 0; i < M; i++) { - res(i) = self(i, i); - } - return res; - } + for (size_t i = 0; i < M; i++) { + res(i) = self(i, i); + } - // get matrix upper right triangle in a row-major vector format - Vector upper_right_triangle() const - { - Vector res; - const SquareMatrix &self = *this; + return res; + } - 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; - } - } + // get matrix upper right triangle in a row-major vector format + Vector < Type, M *(M + 1) / 2 > upper_right_triangle() const + { + Vector < Type, M * (M + 1) / 2 > res; + const SquareMatrix &self = *this; - return res; - } + unsigned idx = 0; - Type trace() const - { - Type res = 0; - const SquareMatrix &self = *this; + for (size_t x = 0; x < M; x++) { + for (size_t y = x; y < M; y++) { + res(idx) = self(x, y); + ++idx; + } + } - for (size_t i = 0; i < M; i++) { - res += self(i, i); - } - return res; - } + 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); + Type trace() const + { + Type res = 0; + const SquareMatrix &self = *this; - SquareMatrix &self = *this; - Vector diag_elements = self.slice(first, first).diag(); - self.uncorrelateCovarianceSetVariance(first, diag_elements); - } + for (size_t i = 0; i < M; i++) { + res += self(i, i); + } - template - void uncorrelateCovarianceSetVariance(size_t first, const Vector &vec) - { - static_assert(Width <= M, "Width bigger than matrix"); - assert(first + Width <= M); + return res; + } - SquareMatrix &self = *this; - // zero rows and columns - self.slice(first, 0) = Type(0); - self.slice(0, first) = Type(0); + // 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); - // set diagonals - unsigned vec_idx = 0; - for (size_t idx = first; idx < first+Width; idx++) { - self(idx,idx) = vec(vec_idx); - vec_idx ++; - } - } + SquareMatrix &self = *this; + Vector diag_elements = self.slice(first, first).diag(); + self.uncorrelateCovarianceSetVariance(first, diag_elements); + } - template - void uncorrelateCovarianceSetVariance(size_t first, Type val) - { - static_assert(Width <= M, "Width bigger than matrix"); - assert(first + Width <= M); + 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); + 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; - } - } + // set diagonals + unsigned vec_idx = 0; - // 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); + for (size_t idx = first; idx < first + Width; idx++) { + self(idx, idx) = vec(vec_idx); + vec_idx ++; + } + } - 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; - } - } - } - } + template + void uncorrelateCovarianceSetVariance(size_t first, Type val) + { + static_assert(Width <= M, "Width bigger than matrix"); + assert(first + Width <= M); - // 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; + // zero rows and columns + self.slice(first, 0) = Type(0); + self.slice(0, first) = Type(0); - 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; - } - } - } + // set diagonals + for (size_t idx = first; idx < first + Width; idx++) { + self(idx, idx) = val; + } + } - // 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); + // 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++) { - if(!isEqualF(self(row_idx,col_idx), self(col_idx,row_idx), eps)) { - return false; - } - } - } - } - return true; - } + SquareMatrix &self = *this; - // 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); + 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; + } + } + } + } - 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); - } + // 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); + } }; @@ -270,35 +284,40 @@ 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 eye() { - 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; - } + SquareMatrix m; + m.setIdentity(); + return m; +} - return res; +template +SquareMatrix diag(Vector d) +{ + SquareMatrix m; + + for (size_t i = 0; i < M; i++) { + m(i, i) = d(i); + } + + return m; +} + +template +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; } @@ -306,183 +325,186 @@ SquareMatrix expm(const Matrix & A, size_t order=5) * inverse based on LU factorization with partial pivotting */ template -bool inv(const SquareMatrix & A, SquareMatrix & inv, size_t rank = M) +bool inv(const SquareMatrix &A, SquareMatrix &inv, size_t rank = M) { - SquareMatrix L; - L.setIdentity(); - SquareMatrix U = A; - SquareMatrix P; - P.setIdentity(); + SquareMatrix L; + L.setIdentity(); + SquareMatrix U = A; + SquareMatrix P; + P.setIdentity(); - //printf("A:\n"); A.print(); + //printf("A:\n"); A.print(); - // for all diagonal elements - for (size_t n = 0; n < rank; n++) { + // 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++) { + // 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; - } - } - } + //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); + //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; - } + // 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); + // 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); - } - } - } + // 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(); + //printf("L:\n"); L.print(); + //printf("U:\n"); U.print(); - // solve LY=P*I for Y by forward subst - //SquareMatrix Y = P; + // 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); - } + // 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 - } - } + // 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(); + //printf("Y:\n"); Y.print(); - // solve Ux=y for x by back subst - //SquareMatrix X = Y; + // 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 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); - } + // 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); - } - } + // 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; + //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) +bool inv(const SquareMatrix &A, SquareMatrix &inv) { - Type det = A(0, 0) * A(1, 1) - A(1, 0) * A(0, 1); + 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; - } + 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; + 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) +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)); + 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; - } + 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; + 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 inv(const SquareMatrix &A) { - SquareMatrix i; - if (inv(A, i)) { - return i; - } else { - i.setZero(); - return i; - } + SquareMatrix i; + + if (inv(A, i)) { + return i; + + } else { + i.setZero(); + return i; + } } /** @@ -491,36 +513,46 @@ SquareMatrix inv(const SquareMatrix & A) * Note: A must be positive definite */ template -SquareMatrix cholesky(const SquareMatrix & A) +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; + 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; } /** @@ -530,15 +562,13 @@ SquareMatrix cholesky(const SquareMatrix & A) * for L or we need to do it manually. Will impact speed otherwise. */ template -SquareMatrix choleskyInv(const SquareMatrix & A) +SquareMatrix choleskyInv(const SquareMatrix &A) { - SquareMatrix L_inv = inv(cholesky(A)); - return L_inv.T()*L_inv; + 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 index 09e79d1bbb..336e31cef8 100644 --- a/src/lib/matrix/matrix/Vector.hpp +++ b/src/lib/matrix/matrix/Vector.hpp @@ -20,117 +20,134 @@ template class Vector : public Matrix { public: - using MatrixM1 = Matrix; + using MatrixM1 = Matrix; - Vector() = default; + Vector() = default; - Vector(const MatrixM1 & other) : - MatrixM1(other) - { - } + Vector(const MatrixM1 &other) : + MatrixM1(other) + { + } - explicit Vector(const Type data_[M]) : - MatrixM1(data_) - { - } + explicit Vector(const Type data_[M]) : + MatrixM1(data_) + { + } - template - Vector(const Slice& slice_in) : - Matrix(slice_in) - { - } + template + Vector(const Slice &slice_in) : + Matrix(slice_in) + { + } - template - Vector(const Slice& slice_in) - { - Vector &self(*this); - for (size_t i = 0; i + Vector(const Slice &slice_in) + { + Vector &self(*this); - inline const Type &operator()(size_t i) const - { - assert(i < M); + for (size_t i = 0; i < M; i++) { + self(i) = slice_in(0, i); + } + } - const MatrixM1 &v = *this; - return v(i, 0); - } + inline const Type &operator()(size_t i) const + { + assert(i < M); - inline Type &operator()(size_t i) - { - assert(i < M); + const MatrixM1 &v = *this; + return v(i, 0); + } - MatrixM1 &v = *this; - return v(i, 0); - } + inline Type &operator()(size_t i) + { + assert(i < M); - Type dot(const MatrixM1 & b) const { - const Vector &a(*this); - Type r(0); - for (size_t i = 0; i eps) { - return (*this) / n; - } - return Vector(); - } + inline Type length() const + { + return norm(); + } - inline Vector normalized() const { - return unit(); - } + inline void normalize() + { + (*this) /= norm(); + } - bool longerThan(Type testVal) const { - return norm_squared() > testVal*testVal; - } + Vector unit() const + { + return (*this) / norm(); + } - Vector sqrt() const { - const Vector &a(*this); - Vector r; - 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 < M; i++) { + r(i) = Type(matrix::sqrt(a(i))); + } + + return r; + } }; } // namespace matrix - -/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */ diff --git a/src/lib/matrix/matrix/Vector2.hpp b/src/lib/matrix/matrix/Vector2.hpp index 33d0cda467..35ceb7c1ac 100644 --- a/src/lib/matrix/matrix/Vector2.hpp +++ b/src/lib/matrix/matrix/Vector2.hpp @@ -21,53 +21,55 @@ class Vector2 : public Vector { public: - using Matrix21 = Matrix; - using Vector3 = Vector; + using Matrix21 = Matrix; + using Vector3 = Vector; - Vector2() = default; + Vector2() = default; - Vector2(const Matrix21 & other) : - Vector(other) - { - } + Vector2(const Matrix21 &other) : + Vector(other) + { + } - explicit Vector2(const Type data_[2]) : - Vector(data_) - { - } + explicit Vector2(const Type data_[2]) : + Vector(data_) + { + } - Vector2(Type x, Type y) - { - Vector2 &v(*this); - v(0) = x; - v(1) = y; - } + 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) + { + } - 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); - } + 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 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); - } + Type operator%(const Matrix21 &b) const + { + return (*this).cross(b); + } }; @@ -76,5 +78,3 @@ 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 index f67ade4618..b67ab39a8b 100644 --- a/src/lib/matrix/matrix/Vector3.hpp +++ b/src/lib/matrix/matrix/Vector3.hpp @@ -30,121 +30,127 @@ class Vector3 : public Vector { public: - using Matrix31 = Matrix; + using Matrix31 = Matrix; - Vector3() = default; + Vector3() = default; - Vector3(const Matrix31 & other) : - Vector(other) - { - } + Vector3(const Matrix31 &other) : + Vector(other) + { + } - explicit Vector3(const Type data_[3]) : - Vector(data_) - { - } + 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; - } + 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) + { + } - 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)}; - } + 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 - */ + /** + * Override matrix ops so Vector3 type is returned + */ - inline Vector3 operator+(Vector3 other) const - { - return Matrix31::operator+(other); - } + inline Vector3 operator+(Vector3 other) const + { + return Matrix31::operator+(other); + } - inline Vector3 operator+(Type scalar) const - { - return Matrix31::operator+(scalar); - } + inline Vector3 operator+(Type scalar) const + { + return Matrix31::operator+(scalar); + } - inline Vector3 operator-(Vector3 other) const - { - return Matrix31::operator-(other); - } + inline Vector3 operator-(Vector3 other) const + { + return Matrix31::operator-(other); + } - inline Vector3 operator-(Type scalar) const - { - return Matrix31::operator-(scalar); - } + inline Vector3 operator-(Type scalar) const + { + return Matrix31::operator-(scalar); + } - inline Vector3 operator-() const - { - return Matrix31::operator-(); - } + inline Vector3 operator-() const + { + return Matrix31::operator-(); + } - inline Vector3 operator*(Type scalar) const - { - return Matrix31::operator*(scalar); - } + inline Vector3 operator*(Type scalar) const + { + return Matrix31::operator*(scalar); + } - inline Type operator*(Vector3 b) const - { - return Vector::operator*(b); - } + inline Type operator*(Vector3 b) const + { + return Vector::operator*(b); + } - inline Vector3 operator%(const Matrix31 & b) const { - return (*this).cross(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()); - } + /** + * Override vector ops so Vector3 type is returned + */ + inline Vector3 unit() const + { + return Vector3(Vector::unit()); + } - inline Vector3 normalized() const { - return unit(); - } + inline Vector3 normalized() const + { + return unit(); + } - const Slice xy() const - { - return Slice(0, 0, this); - } + const Slice xy() const + { + return Slice(0, 0, this); + } - Slice xy() - { - 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; - } + 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; + } }; @@ -152,5 +158,3 @@ 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 index 7951cb3cb0..76c091d44a 100644 --- a/src/lib/matrix/matrix/filter.hpp +++ b/src/lib/matrix/matrix/filter.hpp @@ -2,25 +2,26 @@ #include "math.hpp" -namespace matrix { +namespace matrix +{ template int kalman_correct( - const Matrix & P, - const Matrix & C, - const Matrix & R, - const Matrix &r, - Matrix & dx, - Matrix & dP, - Type & beta + 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; + 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 index 191ff12228..dd2b28ea1b 100644 --- a/src/lib/matrix/matrix/helper_functions.hpp +++ b/src/lib/matrix/matrix/helper_functions.hpp @@ -10,13 +10,14 @@ namespace matrix { template -bool is_finite(Type x) { +bool is_finite(Type x) +{ #if defined (__PX4_NUTTX) - return PX4_ISFINITE(x); + return PX4_ISFINITE(x); #elif defined (__PX4_QURT) - return __builtin_isfinite(x); + return __builtin_isfinite(x); #else - return std::isfinite(x); + return std::isfinite(x); #endif } @@ -34,25 +35,26 @@ bool is_finite(Type x) { 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)); + 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; - } +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; + 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 @@ -65,8 +67,9 @@ Floating wrap_floating(Floating x, Floating low, Floating high) { * @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); +inline float wrap(float x, float low, float high) +{ + return matrix::detail::wrap_floating(x, low, high); } /** @@ -77,8 +80,9 @@ inline float wrap(float x, float low, float high) { * @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); +inline double wrap(double x, double low, double high) +{ + return matrix::detail::wrap_floating(x, low, high); } /** @@ -90,14 +94,15 @@ inline double wrap(double x, double low, double high) { * @return wrapped value inside the range */ template -Integer wrap(Integer x, Integer low, Integer high) { - const auto range = high - low; +Integer wrap(Integer x, Integer low, Integer high) +{ + const auto range = high - low; - if (x < low) { - x += range * ((low - x) / range + 1); - } + if (x < low) { + x += range * ((low - x) / range + 1); + } - return low + (x - low) % range; + return low + (x - low) % range; } /** @@ -106,7 +111,7 @@ Integer wrap(Integer x, Integer low, Integer high) { template Type wrap_pi(Type x) { - return wrap(x, Type(-M_PI), Type(M_PI)); + return wrap(x, Type(-M_PI), Type(M_PI)); } /** @@ -115,13 +120,13 @@ Type wrap_pi(Type x) template Type wrap_2pi(Type x) { - return wrap(x, Type(0), Type(M_TWOPI)); + return wrap(x, Type(0), Type(M_TWOPI)); } template int sign(T val) { - return (T(FLT_EPSILON) < val) - (val < T(FLT_EPSILON)); + 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 index 8909a17c60..338958ed4b 100644 --- a/src/lib/matrix/matrix/integration.hpp +++ b/src/lib/matrix/matrix/integration.hpp @@ -2,41 +2,45 @@ #include "math.hpp" -namespace matrix { +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 + 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; + // 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/stdlib_imports.hpp b/src/lib/matrix/matrix/stdlib_imports.hpp index 547b5fed75..94b9c79689 100644 --- a/src/lib/matrix/matrix/stdlib_imports.hpp +++ b/src/lib/matrix/matrix/stdlib_imports.hpp @@ -20,7 +20,8 @@ #define M_TWOPI (M_PI * 2.0) #endif -namespace matrix { +namespace matrix +{ #if !defined(FLT_EPSILON) #define FLT_EPSILON __FLT_EPSILON__ diff --git a/src/lib/matrix/test/attitude.cpp b/src/lib/matrix/test/attitude.cpp index d8954965d3..1d4a7f1868 100644 --- a/src/lib/matrix/test/attitude.cpp +++ b/src/lib/matrix/test/attitude.cpp @@ -7,7 +7,8 @@ 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 { +namespace matrix +{ template class Matrix; template class Vector3; template class Vector2; @@ -20,471 +21,471 @@ 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))); + // 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; + std::cout << "q:" << q; #endif - return 0; + 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 index 4e72a1a91d..c8fae3620e 100644 --- a/src/lib/matrix/test/copyto.cpp +++ b/src/lib/matrix/test/copyto.cpp @@ -3,70 +3,76 @@ using namespace matrix; -namespace { -void doTheCopy(const Matrix& A, float array_A[6]) +namespace { - A.copyTo(array_A); +void doTheCopy(const Matrix &A, float array_A[6]) +{ + A.copyTo(array_A); } } int main() { - float eps = 1e-6f; + 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); - } + // Vector3 copyTo + const Vector3f v(1, 2, 3); + float dst3[3] = {}; + v.copyTo(dst3); - // 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); - } + for (size_t i = 0; i < 3; i++) { + TEST(fabs(v(i) - dst3[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); - } + // Quaternion copyTo + Quatf q(1, 2, 3, 4); + float dst4[4] = {}; + q.copyTo(dst4); - // 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); - } + for (size_t i = 0; i < 4; i++) { + TEST(fabs(q(i) - dst4[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); - } + // 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}; - 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); - } + for (size_t i = 0; i < 6; i++) { + TEST(fabs(array_A[i] - array_row[i]) < eps); + } - return 0; + // 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 index 7d5fbb9db7..d92ff72712 100644 --- a/src/lib/matrix/test/dual.cpp +++ b/src/lib/matrix/test/dual.cpp @@ -7,304 +7,305 @@ using namespace matrix; template bool isEqualAll(Dual a, Dual b) { - return isEqualF(a.value, b.value) && a.derivative == b.derivative; + 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); +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 - ) +Vector positionError(const Vector &positionState, + const Vector &velocityStateBody, + const Quaternion &bodyOrientation, + const Vector &positionMeasurement, + Scalar dt + ) { - return positionMeasurement - (positionState + bodyOrientation.conjugate(velocityStateBody) * dt); + return positionMeasurement - (positionState + bodyOrientation.conjugate(velocityStateBody) * dt); } int main() { - const Dual a(3,0); - const Dual b(6,0); + const Dual a(3, 0); + const Dual b(6, 0); - { - TEST(isEqualF(a.value, 3.f)); - TEST(isEqualF(a.derivative(0), 1.f)); - } + { + 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)); + { + // 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 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 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)); - } + 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)); + { + // 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 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 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)); - } + 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)); + { + // 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 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 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)); - } + 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)); + { + // 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 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 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 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)); - } + { + 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))); - } + { + // 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))); - } + { + // 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)); - } + { + // 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)); - } + { + // 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)); - } + { + // 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))); - } + { + // 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)); - } + { + // 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)); - } + { + // 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) + { + // 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(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) - } + 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) + { + // 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(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 - } + 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) - } + { + // 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) + { + // 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; + 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)); + // 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); + Dual dualResult = testFunction(dualPoint); - // compare to a numerical derivative: - Vector floatPoint = collectReals(dualPoint); - float floatResult = testFunction(floatPoint); + // 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); + 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); + 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); + 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)); + 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 + { + // 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; + 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)); + direct_error = positionError(positionState, + velocityState, + velocityOrientation, + positionMeasurement, + dt); + float h = 0.001f; - // 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)); + 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)); - Vector3d4 positionMeasurement(D4(4.5f), D4(6.2f), D4(7.9f)); - D4 dt(0.1f); + // 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)); + 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; + } + return 0; } diff --git a/src/lib/matrix/test/filter.cpp b/src/lib/matrix/test/filter.cpp index 342da22824..f334c66ad9 100644 --- a/src/lib/matrix/test/filter.cpp +++ b/src/lib/matrix/test/filter.cpp @@ -5,25 +5,24 @@ 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); + 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); + 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)); + float data_check[] = {0.5, 1, 1.5, 2, 2.5, 0}; + Vector dx_check(data_check); + TEST(isEqual(dx, dx_check)); - return 0; + return 0; } -/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */ diff --git a/src/lib/matrix/test/hatvee.cpp b/src/lib/matrix/test/hatvee.cpp index aa2c2cf90e..bb90e3157d 100644 --- a/src/lib/matrix/test/hatvee.cpp +++ b/src/lib/matrix/test/hatvee.cpp @@ -5,15 +5,14 @@ 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); + 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; + 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 index 6d8b539f3a..7bb914da8f 100644 --- a/src/lib/matrix/test/helper.cpp +++ b/src/lib/matrix/test/helper.cpp @@ -5,74 +5,73 @@ 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); + // 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); + // 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 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))); + // 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)); + // 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 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)); + Vector3f c(1, 2, 3); + Vector3f d(1, 2, NAN); + TEST(!isEqual(c, d)); + TEST(isEqual(c, c)); + TEST(isEqual(d, d)); - return 0; + 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 index 6dbd807205..7bec5dac28 100644 --- a/src/lib/matrix/test/integration.cpp +++ b/src/lib/matrix/test/integration.cpp @@ -5,22 +5,22 @@ 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(); +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; + 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 index 865c79db32..8885011a51 100644 --- a/src/lib/matrix/test/inverse.cpp +++ b/src/lib/matrix/test/inverse.cpp @@ -7,158 +7,158 @@ 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 - }; + 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); + 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 - }; + 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(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 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) + 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(); + // 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)); - } + 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())); + 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 - }; + // 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 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)); + 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)); + // 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)); + for (size_t i = 0; i < 9; i++) { + A2(0, i) = 0; + } - // 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_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)); - A2(0, 0) = NAN; - A2_I = inv(A2); - 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)); - float data4[9] = { - 1.33471626f, 0.74946721f, -0.0531679f, - 0.74946721f, 1.07519593f, 0.08036323f, - -0.0531679f, 0.08036323f, 1.01618474f - }; - SquareMatrix A4(data4); + A2(0, 0) = NAN; + A2_I = inv(A2); + TEST(isEqual(A2_I, Z9)); + TEST(isEqual(A2.I(), Z9)); - 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)); + float data4[9] = { + 1.33471626f, 0.74946721f, -0.0531679f, + 0.74946721f, 1.07519593f, 0.08036323f, + -0.0531679f, 0.08036323f, 1.01618474f + }; + SquareMatrix A4(data4); - SquareMatrix I3; - I3.setIdentity(); - TEST(isEqual(choleskyInv(A4)*A4, I3)); - TEST(isEqual(cholesky(Z3), Z3)); - return 0; + 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 index 573715caa2..2009cc59a2 100644 --- a/src/lib/matrix/test/least_squares.cpp +++ b/src/lib/matrix/test/least_squares.cpp @@ -10,91 +10,97 @@ int test_div_zero(void); int main() { - int ret; + int ret; - ret = test_4x4(); - if (ret != 0) return ret; + ret = test_4x4(); - ret = test_4x4(); - if (ret != 0) return ret; + if (ret != 0) { return ret; } - ret = test_4x3(); - if (ret != 0) return ret; + ret = test_4x4(); - ret = test_div_zero(); - if (ret != 0) return ret; + if (ret != 0) { return ret; } - return 0; + 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); +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 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); + float x_check_data[3] = {-0.69168233f, + -0.26227593f, + -1.03767522f + }; + Vector x_check(x_check_data); - LeastSquaresSolver qrd = LeastSquaresSolver(A); + LeastSquaresSolver qrd = LeastSquaresSolver(A); - Vector x = qrd.solve(b); - TEST(isEqual(x, x_check)); - return 0; + 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); +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 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); + Type x_check_data[4] = { 0.97893433f, + -2.80798701f, + -0.03175765f, + -2.19387649f + }; + Vector x_check(x_check_data); - LeastSquaresSolver qrd = LeastSquaresSolver(A); + LeastSquaresSolver qrd = LeastSquaresSolver(A); - Vector x = qrd.solve(b); - TEST(isEqual(x, x_check)); - return 0; + 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); +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); + 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); + // 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); + LeastSquaresSolver qrd = LeastSquaresSolver(A); - Vector x = qrd.solve(b); - TEST(isEqual(x, x_check)); - return 0; + 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 index 6e5a0783c6..ff8e43304e 100644 --- a/src/lib/matrix/test/matrixAssignment.cpp +++ b/src/lib/matrix/test/matrixAssignment.cpp @@ -7,254 +7,264 @@ 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; + 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); + 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); - } - } + 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()); + Matrix3f m_nan; + m_nan.setNaN(); - 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()); + for (size_t i = 0; i < 3; i++) { + for (size_t j = 0; j < 3; j++) { + TEST(isnan(m_nan(i, j))); + } + } - float data_times_2[9] = {2, 4, 6, 8, 10, 12, 14, 16, 18}; - Matrix3f m3(data_times_2); + TEST(m_nan.isAllNan()); - TEST(isEqual(m, m2)); - TEST(!(isEqual(m, m3))); + float data2d[3][3] = { + {1, 2, 3}, + {4, 5, 6}, + {7, 8, 9} + }; + m2 = Matrix3f(data2d); - m2 *= 2; - TEST(isEqual(m2, m3)); + 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); + } + } - 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)); + TEST(!m2.isAllNan()); - m2 += 1; - TEST(isEqual(Matrix3f(data), m2)); + float data_times_2[9] = {2, 4, 6, 8, 10, 12, 14, 16, 18}; + Matrix3f m3(data_times_2); - m3 -= m2; + TEST(isEqual(m, m2)); + TEST(!(isEqual(m, m3))); - TEST(isEqual(m3, m2)); + m2 *= 2; + TEST(isEqual(m2, m3)); - // set rows and columns to value - Matrix3f m2e(data2d); + 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)); - float data2e_check1[3][3] = { - {1, 11, 3}, - {4, 11, 6}, - {7, 11, 9} - }; - Matrix3f m2e_check1(data2e_check1); + m2 += 1; + TEST(isEqual(Matrix3f(data), m2)); - float data2e_check2[3][3] = { - {1, 11, 3}, - {4, 11, 6}, - {0, 0, 0} - }; - Matrix3f m2e_check2(data2e_check2); + m3 -= m2; - m2e.setCol(1, 11); - TEST(isEqual(m2e, m2e_check1)); - m2e.setRow(2, 0); - TEST(isEqual(m2e, m2e_check2)); + TEST(isEqual(m3, m2)); - float data_row_02_swap[9] = { - 7, 8, 9, - 4, 5, 6, - 1, 2, 3, - }; + // set rows and columns to value + Matrix3f m2e(data2d); - float data_col_02_swap[9] = { - 3, 2, 1, - 6, 5, 4, - 9, 8, 7 - }; + float data2e_check1[3][3] = { + {1, 11, 3}, + {4, 11, 6}, + {7, 11, 9} + }; + Matrix3f m2e_check1(data2e_check1); - Matrix3f m4(data); + float data2e_check2[3][3] = { + {1, 11, 3}, + {4, 11, 6}, + {0, 0, 0} + }; + Matrix3f m2e_check2(data2e_check2); - TEST(isEqual(-m4, m4*(-1))); + m2e.setCol(1, 11); + TEST(isEqual(m2e, m2e_check1)); + m2e.setRow(2, 0); + TEST(isEqual(m2e, m2e_check2)); - // col swap - m4.swapCols(0, 2); - TEST(isEqual(m4, Matrix3f(data_col_02_swap))); - m4.swapCols(0, 2); + float data_row_02_swap[9] = { + 7, 8, 9, + 4, 5, 6, + 1, 2, 3, + }; - // row swap - m4.swapRows(0, 2); - TEST(isEqual(m4, Matrix3f(data_row_02_swap))); - m4.swapRows(0, 2); + float data_col_02_swap[9] = { + 3, 2, 1, + 6, 5, 4, + 9, 8, 7 + }; - // swapping with same row should do nothing - m4.swapRows(0, 0); - m4.swapRows(1, 1); - m4.swapRows(2, 2); - TEST(isEqual(m4, Matrix3f(data))); + Matrix3f m4(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(isEqual(-m4, m4 * (-1))); - TEST(fabs(m4.min() - 1) < FLT_EPSILON); - TEST(fabs((-m4).min() + 9) < FLT_EPSILON); + // col swap + m4.swapCols(0, 2); + TEST(isEqual(m4, Matrix3f(data_col_02_swap))); + m4.swapCols(0, 2); - Scalar s = 1; - const Vector & s_vect = s; - TEST(fabs(s - 1) < FLT_EPSILON); - TEST(fabs(s_vect(0) - 1.0f) < FLT_EPSILON); + // row swap + m4.swapRows(0, 2); + TEST(isEqual(m4, Matrix3f(data_row_02_swap))); + m4.swapRows(0, 2); - Matrix m5 = s; - TEST(fabs(m5(0,0) - s) < FLT_EPSILON); + // swapping with same row should do nothing + m4.swapRows(0, 0); + m4.swapRows(1, 1); + m4.swapRows(2, 2); + TEST(isEqual(m4, Matrix3f(data))); - 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)); + // swapping with same col should do nothing + m4.swapCols(0, 0); + m4.swapCols(1, 1); + m4.swapCols(2, 2); + TEST(isEqual(m4, Matrix3f(data))); - m7.setNaN(); - TEST(m7 != m8); + TEST(fabs(m4.min() - 1) < FLT_EPSILON); + TEST(fabs((-m4).min() + 9) < FLT_EPSILON); - // 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)); + Scalar s = 1; + const Vector &s_vect = s; + TEST(fabs(s - 1) < FLT_EPSILON); + TEST(fabs(s_vect(0) - 1.0f) < FLT_EPSILON); - // 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)); + Matrix m5 = s; + TEST(fabs(m5(0, 0) - s) < FLT_EPSILON); - // 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)); + 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)); - // 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; - } - } + m7.setNaN(); + TEST(m7 != m8); - // 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)); + // 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)); - return 0; + // 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 index ca00d0d100..0e37e3a320 100644 --- a/src/lib/matrix/test/matrixMult.cpp +++ b/src/lib/matrix/test/matrixMult.cpp @@ -5,67 +5,66 @@ using namespace matrix; int main() { - float data[9] = {1, 0, 0, 0, 1, 0, 1, 0, 1}; - Matrix3f A(data); + 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)); + 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)); + 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); + 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)); + 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 - }; + // 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 m43(data_43); + Matrix m32(data_32); - Matrix m42 = m43 * m32; + 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[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)); + 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; + 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 index 320f0c8b58..846cec6ea6 100644 --- a/src/lib/matrix/test/matrixScalarMult.cpp +++ b/src/lib/matrix/test/matrixScalarMult.cpp @@ -6,13 +6,12 @@ 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; + 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 index a313a58bcd..e2f98382fb 100644 --- a/src/lib/matrix/test/pseudoInverse.cpp +++ b/src/lib/matrix/test/pseudoInverse.cpp @@ -7,147 +7,146 @@ 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 - }; + // 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 - }; + 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); + 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); + 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 - }; + // 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 - }; + 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); + 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); + TEST((A1_I - A1_I_check).abs().max() < 1e-5); - // Stess test - Matrix A_large; - A_large.setIdentity(); - Matrix A_large_I; + // Stess test + Matrix < float, n_large, n_large - 1 > A_large; + A_large.setIdentity(); + Matrix < float, n_large - 1, n_large > 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())); - } + 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 - }; + // 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); + 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); + // 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); + // 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); + // 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); + // 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; + 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 index 289e286464..a0193da649 100644 --- a/src/lib/matrix/test/setIdentity.cpp +++ b/src/lib/matrix/test/setIdentity.cpp @@ -5,35 +5,34 @@ using namespace matrix; int main() { - Matrix3f A; - A.setIdentity(); + 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); + 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); - } - } - } + } else { + TEST(fabs(A(i, j) - 0) < FLT_EPSILON); + } + } + } - Matrix3f B; - B.identity(); + 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); + 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); - } - } - } + } else { + TEST(fabs(B(i, j) - 0) < FLT_EPSILON); + } + } + } - return 0; + 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 index 6f838f0ccf..4206059512 100644 --- a/src/lib/matrix/test/slice.cpp +++ b/src/lib/matrix/test/slice.cpp @@ -8,228 +8,229 @@ template class matrix::Slice; // so that we get full coverage int main() { - float data[9] = {0, 2, 3, - 4, 5, 6, - 7, 8, 10 - }; - SquareMatrix A(data); + 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 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 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 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 - }; + //Test block writing + float data_2[4] = { + 11, 12, + 13, 14 + }; - Matrix C(data_2); - A.slice<2, 2>(1, 1) = C; + 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)); + 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; + //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; + Matrix F; + F(0, 0) = 9; + F(1, 0) = 11; - E.slice<2,1>(0,0) = F; + 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)))); + 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)); + 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); + 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)); + } + { + // 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)); - } + 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 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))); + // 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())); + // 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); + // 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; + // 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; + 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)); + 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); + // 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)); + 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); + // 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.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<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.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.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, 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<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.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.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) *= 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))); + 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))); + // 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; + 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 index 9c2dbd1224..c2a7d80476 100644 --- a/src/lib/matrix/test/sparseVector.cpp +++ b/src/lib/matrix/test/sparseVector.cpp @@ -3,117 +3,131 @@ 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, 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, 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, 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, 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, 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, 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, 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, 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, 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, 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)); +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(); +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 index 0c87debc0b..e42f8f4b53 100644 --- a/src/lib/matrix/test/squareMatrix.cpp +++ b/src/lib/matrix/test/squareMatrix.cpp @@ -6,143 +6,142 @@ 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); + 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); + 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 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); + 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); + 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)); + 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 - }; + // 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 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 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 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)) + 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)); + // 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 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 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)); + 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; + 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 index 61ef1d0964..f7d7cd6539 100644 --- a/src/lib/matrix/test/test_data.py +++ b/src/lib/matrix/test/test_data.py @@ -145,5 +145,3 @@ 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/transpose.cpp b/src/lib/matrix/test/transpose.cpp index 41627a6b0c..05456fbdc2 100644 --- a/src/lib/matrix/test/transpose.cpp +++ b/src/lib/matrix/test/transpose.cpp @@ -6,14 +6,13 @@ 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)); + 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; + 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 index 1db52d936c..0aa2333386 100644 --- a/src/lib/matrix/test/upperRightTriangle.cpp +++ b/src/lib/matrix/test/upperRightTriangle.cpp @@ -5,19 +5,18 @@ 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}; + float data[9] = {1, 2, 3, + 4, 5, 6, + 7, 8, 10 + }; + float urt[6] = {1, 2, 3, 5, 6, 10}; - SquareMatrix A(data); + SquareMatrix A(data); - for(size_t i=0; i<6; i++) { - TEST(fabs(urt[i] - A.upper_right_triangle()(i)) < FLT_EPSILON); - } + for (size_t i = 0; i < 6; i++) { + TEST(fabs(urt[i] - A.upper_right_triangle()(i)) < FLT_EPSILON); + } - return 0; + 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 index 638ae1220f..f5c5b198a5 100644 --- a/src/lib/matrix/test/vector.cpp +++ b/src/lib/matrix/test/vector.cpp @@ -6,43 +6,42 @@ 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); + // 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)); + // 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)); + // 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)); + // 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())); + // 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)); + // longerThan + Vector v5; + v5(0) = 3; + v5(1) = 4; + TEST(v5.longerThan(4.99f)); + TEST(!v5.longerThan(5.f)); - return 0; + 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 index 1ea88fc933..5b7d923d4e 100644 --- a/src/lib/matrix/test/vector2.cpp +++ b/src/lib/matrix/test/vector2.cpp @@ -8,33 +8,32 @@ using namespace matrix; int main() { - Vector2f a(1, 0); - Vector2f b(0, 1); - TEST(fabs(a % b - 1.0f) < FLT_EPSILON); + 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); + 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); + 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); + 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); + 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); + 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; + 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 index 8143c8ab32..75eaa11038 100644 --- a/src/lib/matrix/test/vector3.cpp +++ b/src/lib/matrix/test/vector3.cpp @@ -6,56 +6,55 @@ 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))); + 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))); + 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))); + 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()))); + 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))); + Vector3f h; + TEST(isEqual(h, Vector3f(0, 0, 0))); - Vector j; - j(0) = 1; - j(1) = 2; - j(2) = 3; - j(3) = 4; + 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 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)); + 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; + 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 index e8e63462a2..83081532c4 100644 --- a/src/lib/matrix/test/vectorAssignment.cpp +++ b/src/lib/matrix/test/vectorAssignment.cpp @@ -6,29 +6,28 @@ using namespace matrix; int main() { - Vector3f v; - v(0) = 1; - v(1) = 2; - v(2) = 3; + Vector3f v; + v(0) = 1; + v(1) = 2; + v(2) = 3; - static const float eps = 1e-7f; + static const float eps = 1e-7f; - TEST(fabs(v(0) - 1) < eps); - TEST(fabs(v(1) - 2) < eps); - TEST(fabs(v(2) - 3) < eps); + TEST(fabs(v(0) - 1) < eps); + TEST(fabs(v(1) - 2) < eps); + TEST(fabs(v(2) - 3) < eps); - Vector3f v2(4, 5, 6); + Vector3f v2(4, 5, 6); - TEST(fabs(v2(0) - 4) < eps); - TEST(fabs(v2(1) - 5) < eps); - TEST(fabs(v2(2) - 6) < eps); + 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); + 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; + return 0; } -/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */