Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-10 06:39:25 +08:00
Code Issues Actions 4 Packages Projects Releases Wiki Activity

matrix: apply PX4 astyle

Browse Source
This commit is contained in:
Daniel Agar
2021-11-16 12:30:51 -05:00
parent ab07f5300b
commit 96bf3aa5d0
44 changed files with 5137 additions and 4876 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Tools/astyle/files_to_check_code_style.sh
-1
View File
@@ -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 \
src/lib/matrix/matrix/AxisAngle.hpp
+114 -111
View File
@@ -30,131 +30,134 @@ template<typename Type>
class AxisAngle : public Vector<Type, 3>
{
public:
using Matrix31 = Matrix<Type, 3, 1>;
using Matrix31 = Matrix<Type, 3, 1>;
/**
* Constructor from array
*
* @param data_ array
*/
explicit AxisAngle(const Type data_[3]) :
Vector<Type, 3>(data_)
{
}
/**
* Constructor from array
*
* @param data_ array
*/
explicit AxisAngle(const Type data_[3]) :
Vector<Type, 3>(data_)
{
}
/**
* Standard constructor
*/
AxisAngle() = default;
/**
* Standard constructor
*/
AxisAngle() = default;
/**
* Constructor from Matrix31
*
* @param other Matrix31 to copy
*/
AxisAngle(const Matrix31 &other) :
Vector<Type, 3>(other)
{
}
/**
* Constructor from Matrix31
*
* @param other Matrix31 to copy
*/
AxisAngle(const Matrix31 &other) :
Vector<Type, 3>(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<Type> &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<Type> &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<Type> &dcm)
{
AxisAngle &v = *this;
v = AxisAngle<Type>(Quaternion<Type>(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<Type> &euler)
{
AxisAngle &v = *this;
v = AxisAngle<Type>(Quaternion<Type>(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<Type> &dcm)
{
AxisAngle &v = *this;
v = AxisAngle<Type>(Quaternion<Type>(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<Type, 3> 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<Type> &euler)
{
AxisAngle &v = *this;
v = AxisAngle<Type>(Quaternion<Type>(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<Type, 3> a = axis_;
a = a.unit();
v(0) = a(0) * angle_;
v(1) = a(1) * angle_;
v(2) = a(2) * angle_;
}
Vector<Type, 3> axis() {
if (Vector<Type, 3>::norm() > 0) {
return Vector<Type, 3>::unit();
} else {
return Vector3<Type>(1, 0, 0);
}
}
Vector<Type, 3> axis()
{
if (Vector<Type, 3>::norm() > 0) {
return Vector<Type, 3>::unit();
Type angle() {
return Vector<Type, 3>::norm();
}
} else {
return Vector3<Type>(1, 0, 0);
}
}
Type angle()
{
return Vector<Type, 3>::norm();
}
};
using AxisAnglef = AxisAngle<float>;
using AxisAngled = AxisAngle<double>;
} // namespace matrix
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
src/lib/matrix/matrix/Dcm.hpp
+125 -127
View File
@@ -39,149 +39,147 @@ template<typename Type>
class Dcm : public SquareMatrix<Type, 3>
{
public:
using Vector3 = Matrix<Type, 3, 1>;
using Vector3 = Matrix<Type, 3, 1>;
/**
* Standard constructor
*
* Initializes to identity
*/
Dcm() : SquareMatrix<Type, 3>(eye<Type, 3>()) {}
/**
* Standard constructor
*
* Initializes to identity
*/
Dcm() : SquareMatrix<Type, 3>(eye<Type, 3>()) {}
/**
* Constructor from array
*
* @param _data pointer to array
*/
explicit Dcm(const Type data_[3][3]) : SquareMatrix<Type, 3>(data_)
{
}
/**
* Constructor from array
*
* @param _data pointer to array
*/
explicit Dcm(const Type data_[3][3]) : SquareMatrix<Type, 3>(data_)
{
}
/**
* Constructor from array
*
* @param _data pointer to array
*/
explicit Dcm(const Type data_[9]) : SquareMatrix<Type, 3>(data_)
{
}
/**
* Constructor from array
*
* @param _data pointer to array
*/
explicit Dcm(const Type data_[9]) : SquareMatrix<Type, 3>(data_)
{
}
/**
* Copy constructor
*
* @param other Matrix33 to set dcm to
*/
Dcm(const Matrix<Type, 3, 3> &other) : SquareMatrix<Type, 3>(other)
{
}
/**
* Copy constructor
*
* @param other Matrix33 to set dcm to
*/
Dcm(const Matrix<Type, 3, 3> &other) : SquareMatrix<Type, 3>(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<Type> &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<Type> &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<Type> &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<Type> &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<Type> &aa)
{
Dcm &dcm = *this;
dcm = Quaternion<Type>(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<Type> &aa)
{
Dcm &dcm = *this;
dcm = Quaternion<Type>(aa);
}
Vector<Type, 3> vee() const // inverse to Vector.hat() operation
{
const Dcm &A(*this);
Vector<Type, 3> v;
v(0) = -A(1, 2);
v(1) = A(0, 2);
v(2) = -A(0, 1);
return v;
}
Vector<Type, 3> vee() const // inverse to Vector.hat() operation
{
const Dcm &A(*this);
Vector<Type, 3> 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<Type> rvec(Matrix<Type,1,3>(this->Matrix<Type,3,3>::row(r)).transpose());
this->Matrix<Type,3,3>::row(r) = rvec.normalized();
}
}
void renormalize()
{
/* renormalize rows */
for (size_t r = 0; r < 3; r++) {
matrix::Vector3<Type> rvec(Matrix<Type, 1, 3>(this->Matrix<Type, 3, 3>::row(r)).transpose());
this->Matrix<Type, 3, 3>::row(r) = rvec.normalized();
}
}
};
using Dcmf = Dcm<float>;
using Dcmd = Dcm<double>;
} // namespace matrix
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
src/lib/matrix/matrix/Dual.hpp
+159 -153
View File
File diff suppressed because it is too large Load Diff
src/lib/matrix/matrix/Euler.hpp
+100 -102
View File
@@ -36,116 +36,116 @@ template<typename Type>
class Euler : public Vector<Type, 3>
{
public:
/**
* Standard constructor
*/
Euler() = default;
/**
* Standard constructor
*/
Euler() = default;
/**
* Copy constructor
*
* @param other vector to copy
*/
Euler(const Vector<Type, 3> &other) :
Vector<Type, 3>(other)
{
}
/**
* Copy constructor
*
* @param other vector to copy
*/
Euler(const Vector<Type, 3> &other) :
Vector<Type, 3>(other)
{
}
/**
* Constructor from Matrix31
*
* @param other Matrix31 to copy
*/
Euler(const Matrix<Type, 3, 1> &other) :
Vector<Type, 3>(other)
{
}
/**
* Constructor from Matrix31
*
* @param other Matrix31 to copy
*/
Euler(const Matrix<Type, 3, 1> &other) :
Vector<Type, 3>(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<Type, 3>()
{
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<Type, 3>()
{
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<Type> &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<Type> &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<Type> &q) : Vector<Type, 3>(Euler(Dcm<Type>(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<Type> &q) : Vector<Type, 3>(Euler(Dcm<Type>(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<float>;
using Eulerd = Euler<double>;
} // namespace matrix
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
src/lib/matrix/matrix/LeastSquaresSolver.hpp
+125 -107
View File
@@ -16,131 +16,149 @@
#include "math.hpp"
namespace matrix {
namespace matrix
{
template<typename Type, size_t M, size_t N>
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<Type, M, N> &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<Type, M, N> &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<Type, M> qtb(const Vector<Type, M> &b) {
Vector<Type, M> 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<Type, N> solve(const Vector<Type, M> &b) {
Vector<Type, M> qtbv = qtb(b);
Vector<Type, N> 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<int>(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<Type, M> qtb(const Vector<Type, M> &b)
{
Vector<Type, M> 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<Type, N> solve(const Vector<Type, M> &b)
{
Vector<Type, M> qtbv = qtb(b);
Vector<Type, N> 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<int>(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<Type, M, N> _A;
Vector<Type, N> _tau;
Matrix<Type, M, N> _A;
Vector<Type, N> _tau;
};
} // namespace matrix
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
src/lib/matrix/matrix/Matrix.hpp
+720 -661
View File
File diff suppressed because it is too large Load Diff
src/lib/matrix/matrix/PseudoInverse.hpp
+73 -65
View File
@@ -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<typename Type, size_t M, size_t N>
bool geninv(const Matrix<Type, M, N> & G, Matrix<Type, N, M>& res)
bool geninv(const Matrix<Type, M, N> &G, Matrix<Type, N, M> &res)
{
size_t rank;
if (M <= N) {
SquareMatrix<Type, M> A = G * G.transpose();
SquareMatrix<Type, M> L = fullRankCholesky(A, rank);
size_t rank;
A = L.transpose() * L;
SquareMatrix<Type, M> X;
if (!inv(A, X, rank)) {
res = Matrix<Type, N, M>();
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<Type, M> A = G * G.transpose();
SquareMatrix<Type, M> L = fullRankCholesky(A, rank);
} else {
SquareMatrix<Type, N> A = G.transpose() * G;
SquareMatrix<Type, N> L = fullRankCholesky(A, rank);
A = L.transpose() * L;
SquareMatrix<Type, M> X;
A = L.transpose() * L;
SquareMatrix<Type, N> X;
if(!inv(A, X, rank)) {
res = Matrix<Type, N, M>();
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<Type, N, M>();
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<Type, N> A = G.transpose() * G;
SquareMatrix<Type, N> L = fullRankCholesky(A, rank);
A = L.transpose() * L;
SquareMatrix<Type, N> X;
if (!inv(A, X, rank)) {
res = Matrix<Type, N, M>();
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<float>()
{
return FLT_EPSILON;
return FLT_EPSILON;
}
/**
* Full rank Cholesky factorization of A
*/
template<typename Type, size_t N>
SquareMatrix<Type, N> fullRankCholesky(const SquareMatrix<Type, N> & A,
size_t& rank)
SquareMatrix<Type, N> fullRankCholesky(const SquareMatrix<Type, N> &A,
size_t &rank)
{
// Loses one ulp accuracy per row of diag, relative to largest magnitude
const Type tol = N * typeEpsilon<Type>() * A.diag().max();
// Loses one ulp accuracy per row of diag, relative to largest magnitude
const Type tol = N * typeEpsilon<Type>() * A.diag().max();
Matrix<Type, N, N> L;
Matrix<Type, N, N> 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 : */
src/lib/matrix/matrix/Quaternion.hpp
+456 -438
View File
File diff suppressed because it is too large Load Diff
src/lib/matrix/matrix/Scalar.hpp
+25 -25
View File
@@ -17,36 +17,38 @@ template<typename Type>
class Scalar
{
public:
Scalar() = delete;
Scalar() = delete;
Scalar(const Matrix<Type, 1, 1> & other) :
_value{other(0,0)}
{
}
Scalar(const Matrix<Type, 1, 1> &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<Type, 1, 1>() const {
Matrix<Type, 1, 1> m;
m(0, 0) = _value;
return m;
}
operator Matrix<Type, 1, 1>() const
{
Matrix<Type, 1, 1> m;
m(0, 0) = _value;
return m;
}
operator Vector<Type, 1>() const {
Vector<Type, 1> m;
m(0) = _value;
return m;
}
operator Vector<Type, 1>() const
{
Vector<Type, 1> m;
m(0) = _value;
return m;
}
private:
const Type _value;
const Type _value;
};
@@ -54,5 +56,3 @@ using Scalarf = Scalar<float>;
using Scalard = Scalar<double>;
} // namespace matrix
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
src/lib/matrix/matrix/Slice.hpp
+271 -240
View File
File diff suppressed because it is too large Load Diff
src/lib/matrix/matrix/SparseVector.hpp
+170 -132
View File
@@ -12,175 +12,213 @@
#include "math.hpp"
namespace matrix {
namespace matrix
{
template<int N> 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<typename Type, size_t M, size_t... Idxs>
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<int>(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<int>(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<Type, M>& data) {
for (size_t i = 0; i < N; i++) {
_data[i] = data(_indices[i]);
}
}
SparseVector(const matrix::Vector<Type, M> &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 <size_t i>
inline Type at() const {
static constexpr int compressed_index = force_constexpr_eval<findCompressedIndex(i)>::value;
static_assert(compressed_index >= 0, "cannot access unpopulated indices");
return _data[compressed_index];
}
template <size_t i>
inline Type at() const
{
static constexpr int compressed_index = force_constexpr_eval<findCompressedIndex(i)>::value;
static_assert(compressed_index >= 0, "cannot access unpopulated indices");
return _data[compressed_index];
}
template <size_t i>
inline Type& at() {
static constexpr int compressed_index = force_constexpr_eval<findCompressedIndex(i)>::value;
static_assert(compressed_index >= 0, "cannot access unpopulated indices");
return _data[compressed_index];
}
template <size_t i>
inline Type &at()
{
static constexpr int compressed_index = force_constexpr_eval<findCompressedIndex(i)>::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<Type, M>& 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<Type, M> &other) const
{
Type accum(0);
matrix::Vector<Type, M> operator+(const matrix::Vector<Type, M>& other) const {
matrix::Vector<Type, M> 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<Type, M> operator+(const matrix::Vector<Type, M> &other) const
{
matrix::Vector<Type, M> 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<typename Type, size_t Q, size_t M, size_t ... Idxs>
matrix::Vector<Type, Q> operator*(const matrix::Matrix<Type, Q, M>& mat, const matrix::SparseVector<Type, M, Idxs...>& vec) {
matrix::Vector<Type, Q> res;
for (size_t i = 0; i < Q; i++) {
const Vector<Type, M> row = mat.row(i);
res(i) = vec.dot(row);
}
return res;
matrix::Vector<Type, Q> operator*(const matrix::Matrix<Type, Q, M> &mat,
const matrix::SparseVector<Type, M, Idxs...> &vec)
{
matrix::Vector<Type, Q> res;
for (size_t i = 0; i < Q; i++) {
const Vector<Type, M> row = mat.row(i);
res(i) = vec.dot(row);
}
return res;
}
// returns x.T * A * x
template<typename Type, size_t M, size_t ... Idxs>
Type quadraticForm(const matrix::SquareMatrix<Type, M>& A, const matrix::SparseVector<Type, M, Idxs...>& 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<Type, M> &A, const matrix::SparseVector<Type, M, Idxs...> &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<typename Type,size_t M, size_t... Idxs>
template<typename Type, size_t M, size_t... Idxs>
constexpr size_t SparseVector<Type, M, Idxs...>::_indices[SparseVector<Type, M, Idxs...>::N];
template<size_t M, size_t ... Idxs>
src/lib/matrix/matrix/SquareMatrix.hpp
+431 -401
View File
File diff suppressed because it is too large Load Diff
src/lib/matrix/matrix/Vector.hpp
+107 -90
View File
@@ -20,117 +20,134 @@ template<typename Type, size_t M>
class Vector : public Matrix<Type, M, 1>
{
public:
using MatrixM1 = Matrix<Type, M, 1>;
using MatrixM1 = Matrix<Type, M, 1>;
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<size_t P, size_t Q>
Vector(const Slice<Type, M, 1, P, Q>& slice_in) :
Matrix<Type, M, 1>(slice_in)
{
}
template<size_t P, size_t Q>
Vector(const Slice<Type, M, 1, P, Q> &slice_in) :
Matrix<Type, M, 1>(slice_in)
{
}
template<size_t P, size_t Q, size_t DUMMY = 1>
Vector(const Slice<Type, 1, M, P, Q>& slice_in)
{
Vector &self(*this);
for (size_t i = 0; i<M; i++) {
self(i) = slice_in(0, i);
}
}
template<size_t P, size_t Q, size_t DUMMY = 1>
Vector(const Slice<Type, 1, M, P, Q> &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<M; i++) {
r += a(i)*b(i,0);
}
return r;
}
MatrixM1 &v = *this;
return v(i, 0);
}
inline Type operator*(const MatrixM1 & b) const {
const Vector &a(*this);
return a.dot(b);
}
Type dot(const MatrixM1 &b) const
{
const Vector &a(*this);
Type r(0);
inline Vector operator*(Type b) const {
return Vector(MatrixM1::operator*(b));
}
for (size_t i = 0; i < M; i++) {
r += a(i) * b(i, 0);
}
Type norm() const {
const Vector &a(*this);
return Type(matrix::sqrt(a.dot(a)));
}
return r;
}
Type norm_squared() const {
const Vector &a(*this);
return a.dot(a);
}
inline Type operator*(const MatrixM1 &b) const
{
const Vector &a(*this);
return a.dot(b);
}
inline Type length() const {
return norm();
}
inline Vector operator*(Type b) const
{
return Vector(MatrixM1::operator*(b));
}
inline void normalize() {
(*this) /= norm();
}
Type norm() const
{
const Vector &a(*this);
return Type(matrix::sqrt(a.dot(a)));
}
Vector unit() const {
return (*this) / norm();
}
Type norm_squared() const
{
const Vector &a(*this);
return a.dot(a);
}
Vector unit_or_zero(const Type eps = Type(1e-5)) const {
const Type n = norm();
if (n > 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<M; i++) {
r(i) = Type(matrix::sqrt(a(i)));
}
return r;
}
Vector unit_or_zero(const Type eps = Type(1e-5)) const
{
const Type n = norm();
if (n > 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 : */
src/lib/matrix/matrix/Vector2.hpp
+40 -40
View File
@@ -21,53 +21,55 @@ class Vector2 : public Vector<Type, 2>
{
public:
using Matrix21 = Matrix<Type, 2, 1>;
using Vector3 = Vector<Type, 3>;
using Matrix21 = Matrix<Type, 2, 1>;
using Vector3 = Vector<Type, 3>;
Vector2() = default;
Vector2() = default;
Vector2(const Matrix21 & other) :
Vector<Type, 2>(other)
{
}
Vector2(const Matrix21 &other) :
Vector<Type, 2>(other)
{
}
explicit Vector2(const Type data_[2]) :
Vector<Type, 2>(data_)
{
}
explicit Vector2(const Type data_[2]) :
Vector<Type, 2>(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<size_t P, size_t Q>
Vector2(const Slice<Type, 2, 1, P, Q>& slice_in) : Vector<Type, 2>(slice_in)
{
}
template<size_t P, size_t Q>
Vector2(const Slice<Type, 2, 1, P, Q> &slice_in) : Vector<Type, 2>(slice_in)
{
}
template<size_t P, size_t Q>
Vector2(const Slice<Type, 1, 2, P, Q>& slice_in) : Vector<Type, 2>(slice_in)
{
}
template<size_t P, size_t Q>
Vector2(const Slice<Type, 1, 2, P, Q> &slice_in) : Vector<Type, 2>(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<float>;
using Vector2d = Vector2<double>;
} // namespace matrix
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
src/lib/matrix/matrix/Vector3.hpp
+99 -95
View File
@@ -30,121 +30,127 @@ class Vector3 : public Vector<Type, 3>
{
public:
using Matrix31 = Matrix<Type, 3, 1>;
using Matrix31 = Matrix<Type, 3, 1>;
Vector3() = default;
Vector3() = default;
Vector3(const Matrix31 & other) :
Vector<Type, 3>(other)
{
}
Vector3(const Matrix31 &other) :
Vector<Type, 3>(other)
{
}
explicit Vector3(const Type data_[3]) :
Vector<Type, 3>(data_)
{
}
explicit Vector3(const Type data_[3]) :
Vector<Type, 3>(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<size_t P, size_t Q>
Vector3(const Slice<Type, 3, 1, P, Q>& slice_in) : Vector<Type, 3>(slice_in)
{
}
template<size_t P, size_t Q>
Vector3(const Slice<Type, 3, 1, P, Q> &slice_in) : Vector<Type, 3>(slice_in)
{
}
template<size_t P, size_t Q>
Vector3(const Slice<Type, 1, 3, P, Q>& slice_in) : Vector<Type, 3>(slice_in)
{
}
template<size_t P, size_t Q>
Vector3(const Slice<Type, 1, 3, P, Q> &slice_in) : Vector<Type, 3>(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<Type, 3>::operator*(b);
}
inline Type operator*(Vector3 b) const
{
return Vector<Type, 3>::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<Type, 3>::unit());
}
/**
* Override vector ops so Vector3 type is returned
*/
inline Vector3 unit() const
{
return Vector3(Vector<Type, 3>::unit());
}
inline Vector3 normalized() const {
return unit();
}
inline Vector3 normalized() const
{
return unit();
}
const Slice<Type, 2, 1, 3, 1> xy() const
{
return Slice<Type, 2, 1, 3, 1>(0, 0, this);
}
const Slice<Type, 2, 1, 3, 1> xy() const
{
return Slice<Type, 2, 1, 3, 1>(0, 0, this);
}
Slice<Type, 2, 1, 3, 1> xy()
{
return Slice<Type, 2, 1, 3, 1>(0, 0, this);
}
Slice<Type, 2, 1, 3, 1> xy()
{
return Slice<Type, 2, 1, 3, 1>(0, 0, this);
}
Dcm<Type> hat() const { // inverse to Dcm.vee() operation
const Vector3 &v(*this);
Dcm<Type> 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<Type> hat() const // inverse to Dcm.vee() operation
{
const Vector3 &v(*this);
Dcm<Type> 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<float>;
using Vector3d = Vector3<double>;
} // namespace matrix
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
src/lib/matrix/matrix/filter.hpp
+15 -14
View File
@@ -2,25 +2,26 @@
#include "math.hpp"
namespace matrix {
namespace matrix
{
template<typename Type, size_t M, size_t N>
int kalman_correct(
const Matrix<Type, M, M> & P,
const Matrix<Type, N, M> & C,
const Matrix<Type, N, N> & R,
const Matrix<Type, N, 1> &r,
Matrix<Type, M, 1> & dx,
Matrix<Type, M, M> & dP,
Type & beta
const Matrix<Type, M, M> &P,
const Matrix<Type, N, M> &C,
const Matrix<Type, N, N> &R,
const Matrix<Type, N, 1> &r,
Matrix<Type, M, 1> &dx,
Matrix<Type, M, M> &dP,
Type &beta
)
{
SquareMatrix<Type, N> S_I = SquareMatrix<Type, N>(C*P*C.T() + R).I();
Matrix<Type, M, N> K = P*C.T()*S_I;
dx = K*r;
beta = Scalar<Type>(r.T()*S_I*r);
dP = K*C*P*(-1);
return 0;
SquareMatrix<Type, N> S_I = SquareMatrix<Type, N>(C * P * C.T() + R).I();
Matrix<Type, M, N> K = P * C.T() * S_I;
dx = K * r;
beta = Scalar<Type>(r.T() * S_I * r);
dP = K * C * P * (-1);
return 0;
}
} // namespace matrix
src/lib/matrix/matrix/helper_functions.hpp
+34 -29
View File
@@ -10,13 +10,14 @@ namespace matrix
{
template<typename Type>
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<typename Type>
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<typename Floating>
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<typename Integer>
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<typename Type>
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<typename Type>
Type wrap_2pi(Type x)
{
return wrap(x, Type(0), Type(M_TWOPI));
return wrap(x, Type(0), Type(M_TWOPI));
}
template<typename T>
int sign(T val)
{
return (T(FLT_EPSILON) < val) - (val < T(FLT_EPSILON));
return (T(FLT_EPSILON) < val) - (val < T(FLT_EPSILON));
}
} // namespace matrix
src/lib/matrix/matrix/integration.hpp
+34 -30
View File
@@ -2,41 +2,45 @@
#include "math.hpp"
namespace matrix {
namespace matrix
{
template<typename Type, size_t M, size_t N>
int integrate_rk4(
Vector<Type, M> (*f)(Type, const Matrix<Type, M, 1> &x, const Matrix<Type, N, 1> & u),
const Matrix<Type, M, 1> & y0,
const Matrix<Type, N, 1> & u,
Type t0,
Type tf,
Type h0,
Matrix<Type, M, 1> & y1
Vector<Type, M> (*f)(Type, const Matrix<Type, M, 1> &x, const Matrix<Type, N, 1> &u),
const Matrix<Type, M, 1> &y0,
const Matrix<Type, N, 1> &u,
Type t0,
Type tf,
Type h0,
Matrix<Type, M, 1> &y1
)
{
// https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta_methods
Type t1 = t0;
y1 = y0;
Type h = h0;
Vector<Type, M> 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<Type, M> 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 :
src/lib/matrix/matrix/stdlib_imports.hpp
+2 -1
View File
@@ -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__
src/lib/matrix/test/attitude.cpp
+466 -465
View File
File diff suppressed because it is too large Load Diff
src/lib/matrix/test/copyto.cpp
+58 -52
View File
@@ -3,70 +3,76 @@
using namespace matrix;
namespace {
void doTheCopy(const Matrix<float, 2, 3>& A, float array_A[6])
namespace
{
A.copyTo(array_A);
void doTheCopy(const Matrix<float, 2, 3> &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<float, 2, 3> 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<float, 2, 3> 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 : */
src/lib/matrix/test/dual.cpp
+239 -238
View File
File diff suppressed because it is too large Load Diff
src/lib/matrix/test/filter.cpp
+16 -17
View File
@@ -5,25 +5,24 @@ using namespace matrix;
int main()
{
const size_t n_x = 6;
const size_t n_y = 5;
SquareMatrix<float, n_x> P = eye<float, n_x>();
SquareMatrix<float, n_y> R = eye<float, n_y>();
Matrix<float, n_y, n_x> C;
C.setIdentity();
float data[] = {1,2,3,4,5};
Vector<float, n_y> r(data);
const size_t n_x = 6;
const size_t n_y = 5;
SquareMatrix<float, n_x> P = eye<float, n_x>();
SquareMatrix<float, n_y> R = eye<float, n_y>();
Matrix<float, n_y, n_x> C;
C.setIdentity();
float data[] = {1, 2, 3, 4, 5};
Vector<float, n_y> r(data);
Vector<float, n_x> dx;
SquareMatrix<float, n_x> dP;
float beta = 0;
kalman_correct<float, 6, 5>(P, C, R, r, dx, dP, beta);
Vector<float, n_x> dx;
SquareMatrix<float, n_x> dP;
float beta = 0;
kalman_correct<float, 6, 5>(P, C, R, r, dx, dP, beta);
float data_check[] = {0.5,1,1.5,2,2.5,0};
Vector<float, n_x> dx_check(data_check);
TEST(isEqual(dx, dx_check));
float data_check[] = {0.5, 1, 1.5, 2, 2.5, 0};
Vector<float, n_x> 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 : */
src/lib/matrix/test/hatvee.cpp
+8 -9
View File
@@ -5,15 +5,14 @@ using namespace matrix;
int main()
{
Euler<float> euler(0.1f, 0.2f, 0.3f);
Dcm<float> R(euler);
Dcm<float> skew = R - R.T();
Vector3<float> w = skew.vee();
Vector3<float> w_check(0.1348f, 0.4170f, 0.5647f);
Euler<float> euler(0.1f, 0.2f, 0.3f);
Dcm<float> R(euler);
Dcm<float> skew = R - R.T();
Vector3<float> w = skew.vee();
Vector3<float> 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 : */
src/lib/matrix/test/helper.cpp
+61 -62
View File
@@ -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 : */
src/lib/matrix/test/integration.cpp
+13 -13
View File
@@ -5,22 +5,22 @@ using namespace matrix;
Vector<float, 6> f(float t, const Matrix<float, 6, 1> & /*y*/, const Matrix<float, 3, 1> & /*u*/);
Vector<float, 6> f(float t, const Matrix<float, 6, 1> & /*y*/, const Matrix<float, 3, 1> & /*u*/) {
float v = -sin(t);
return v*ones<float, 6, 1>();
Vector<float, 6> f(float t, const Matrix<float, 6, 1> & /*y*/, const Matrix<float, 3, 1> & /*u*/)
{
float v = -sin(t);
return v * ones<float, 6, 1>();
}
int main()
{
Vector<float, 6> y = ones<float, 6, 1>();
Vector<float, 3> u = ones<float, 3, 1>();
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<float, 6, 1>()*v)));
return 0;
Vector<float, 6> y = ones<float, 6, 1>();
Vector<float, 3> u = ones<float, 3, 1>();
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<float, 6, 1>()*v)));
return 0;
}
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
src/lib/matrix/test/inverse.cpp
+135 -135
View File
@@ -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<float, 3> A(data);
SquareMatrix<float, 3> A_I = inv(A);
SquareMatrix<float, 3> A_I_check(data_check);
TEST((A_I - A_I_check).abs().max() < 1e-6f);
SquareMatrix<float, 3> A(data);
SquareMatrix<float, 3> A_I = inv(A);
SquareMatrix<float, 3> 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<float, 2> A2x2(data_2x2);
SquareMatrix<float, 2> A2x2_I = inv(A2x2);
SquareMatrix<float, 2> A2x2_I_check(data_2x2_check);
TEST(isEqual(A2x2_I, A2x2_I_check));
SquareMatrix<float, 2> A2x2(data_2x2);
SquareMatrix<float, 2> A2x2_I = inv(A2x2);
SquareMatrix<float, 2> A2x2_I_check(data_2x2_check);
TEST(isEqual(A2x2_I, A2x2_I_check));
SquareMatrix<float, 2> A2x2_sing = ones<float, 2, 2>();
SquareMatrix<float, 2> A2x2_sing_I;
TEST(inv(A2x2_sing, A2x2_sing_I) == false);
SquareMatrix<float, 2> A2x2_sing = ones<float, 2, 2>();
SquareMatrix<float, 2> A2x2_sing_I;
TEST(inv(A2x2_sing, A2x2_sing_I) == false);
SquareMatrix<float, 3> A3x3_sing = ones<float, 3, 3>();
SquareMatrix<float, 3> A3x3_sing_I;
TEST(inv(A3x3_sing, A3x3_sing_I) == false)
SquareMatrix<float, 3> A3x3_sing = ones<float, 3, 3>();
SquareMatrix<float, 3> A3x3_sing_I;
TEST(inv(A3x3_sing, A3x3_sing_I) == false)
// stess test
SquareMatrix<float, n_large> A_large;
A_large.setIdentity();
SquareMatrix<float, n_large> A_large_I;
A_large_I.setZero();
// stess test
SquareMatrix<float, n_large> A_large;
A_large.setIdentity();
SquareMatrix<float, n_large> 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<float, 3> zero_test = zeros<float, 3, 3>();
TEST(isEqual(inv(zero_test), zeros<float, 3, 3>()));
SquareMatrix<float, 3> zero_test = zeros<float, 3, 3>();
TEST(isEqual(inv(zero_test), zeros<float, 3, 3>()));
// 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<float, 9> A2(data2);
SquareMatrix<float, 9> A2_I = inv(A2);
SquareMatrix<float, 9> 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<float, 9> A2(data2);
SquareMatrix<float, 9> A2_I = inv(A2);
SquareMatrix<float, 9> 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<float, 3> A3(data3);
SquareMatrix<float, 3> A3_I = inv(A3);
SquareMatrix<float, 3> 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<float, 3> A3(data3);
SquareMatrix<float, 3> A3_I = inv(A3);
SquareMatrix<float, 3> 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<float, 3> Z3 = zeros<float, 3, 3>();
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<float, 3> Z3 = zeros<float, 3, 3>();
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<float, 9> Z9 = zeros<float, 9, 9>();
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<float, 9> Z9 = zeros<float, 9, 9>();
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<float, 3> 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<float, 3> A4_cholesky_check(data4_cholesky);
SquareMatrix<float, 3> 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<float, 3> A4(data4);
SquareMatrix<float, 3> 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<float, 3> A4_cholesky_check(data4_cholesky);
SquareMatrix<float, 3> A4_cholesky = cholesky(A4);
TEST(isEqual(A4_cholesky_check, A4_cholesky));
SquareMatrix<float, 3> 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 : */
src/lib/matrix/test/least_squares.cpp
+68 -62
View File
@@ -10,91 +10,97 @@ int test_div_zero(void);
int main()
{
int ret;
int ret;
ret = test_4x4<float>();
if (ret != 0) return ret;
ret = test_4x4<float>();
ret = test_4x4<double>();
if (ret != 0) return ret;
if (ret != 0) { return ret; }
ret = test_4x3();
if (ret != 0) return ret;
ret = test_4x4<double>();
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<float, 4, 3> 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<float, 4, 3> A(data);
float b_data[4] = {2.0f, 3.0f, 4.0f, 5.0f};
Vector<float, 4> b(b_data);
float b_data[4] = {2.0f, 3.0f, 4.0f, 5.0f};
Vector<float, 4> b(b_data);
float x_check_data[3] = {-0.69168233f,
-0.26227593f,
-1.03767522f
};
Vector<float, 3> x_check(x_check_data);
float x_check_data[3] = {-0.69168233f,
-0.26227593f,
-1.03767522f
};
Vector<float, 3> x_check(x_check_data);
LeastSquaresSolver<float, 4, 3> qrd = LeastSquaresSolver<float, 4, 3>(A);
LeastSquaresSolver<float, 4, 3> qrd = LeastSquaresSolver<float, 4, 3>(A);
Vector<float, 3> x = qrd.solve(b);
TEST(isEqual(x, x_check));
return 0;
Vector<float, 3> x = qrd.solve(b);
TEST(isEqual(x, x_check));
return 0;
}
template<typename Type>
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<Type, 4, 4> 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<Type, 4, 4> A(data);
Type b_data[4] = {2.0f, 3.0f, 4.0f, 5.0f};
Vector<Type, 4> b(b_data);
Type b_data[4] = {2.0f, 3.0f, 4.0f, 5.0f};
Vector<Type, 4> b(b_data);
Type x_check_data[4] = { 0.97893433f,
-2.80798701f,
-0.03175765f,
-2.19387649f
};
Vector<Type, 4> x_check(x_check_data);
Type x_check_data[4] = { 0.97893433f,
-2.80798701f,
-0.03175765f,
-2.19387649f
};
Vector<Type, 4> x_check(x_check_data);
LeastSquaresSolver<Type, 4, 4> qrd = LeastSquaresSolver<Type, 4, 4>(A);
LeastSquaresSolver<Type, 4, 4> qrd = LeastSquaresSolver<Type, 4, 4>(A);
Vector<Type, 4> x = qrd.solve(b);
TEST(isEqual(x, x_check));
return 0;
Vector<Type, 4> 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<float, 2, 2> A(data);
int test_div_zero()
{
float data[4] = {0.0f, 0.0f, 0.0f, 0.0f};
Matrix<float, 2, 2> A(data);
float b_data[2] = {1.0f, 1.0f};
Vector<float, 2> b(b_data);
float b_data[2] = {1.0f, 1.0f};
Vector<float, 2> 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<float, 2> 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<float, 2> x_check(x_check_data);
LeastSquaresSolver<float, 2, 2> qrd = LeastSquaresSolver<float, 2, 2>(A);
LeastSquaresSolver<float, 2, 2> qrd = LeastSquaresSolver<float, 2, 2>(A);
Vector<float, 2> x = qrd.solve(b);
TEST(isEqual(x, x_check));
return 0;
Vector<float, 2> 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 : */
src/lib/matrix/test/matrixAssignment.cpp
+225 -215
View File
@@ -7,254 +7,264 @@ template class matrix::Matrix<float, 3, 2>;
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<float> s = 1;
const Vector<float, 1> & 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<float, 1, 1> 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<float, 2, 2> m6;
m6.setRow(0, Vector2f(1, 2));
float m7_array[] = {1,2,0,0};
Matrix<float, 2, 2> m7(m7_array);
TEST(isEqual(m6, m7));
m6.setCol(0, Vector2f(3, 4));
float m8_array[] = {3,2,4,0};
Matrix<float, 2, 2> 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<float> s = 1;
const Vector<float, 1> &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<float, 1, 1> 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<float, 2, 2> m6;
m6.setRow(0, Vector2f(1, 2));
float m7_array[] = {1, 2, 0, 0};
Matrix<float, 2, 2> m7(m7_array);
TEST(isEqual(m6, m7));
m6.setCol(0, Vector2f(3, 4));
float m8_array[] = {3, 2, 4, 0};
Matrix<float, 2, 2> 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<float, 3, 2> 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<char>(fgetc(fp));
if (c == '\n') {
break;
}
printf("%d %d %d\n", static_cast<int>(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<float, 3, 2> 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<char>(fgetc(fp));
if (c == '\n') {
break;
}
printf("%d %d %d\n", static_cast<int>(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 : */
src/lib/matrix/test/matrixMult.cpp
+51 -52
View File
@@ -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<float, 3>()*2;
Matrix3f B = A2.emult(A2);
Matrix3f B_check = eye<float, 3>()*4;
Matrix3f C_check = eye<float, 3>()*2;
TEST(isEqual(B, B_check));
Matrix3f C = B_check.edivide(C_check);
TEST(R2 == I);
TEST(A != A_I);
Matrix3f A2 = eye<float, 3>() * 2;
Matrix3f B = A2.emult(A2);
Matrix3f B_check = eye<float, 3>() * 4;
Matrix3f C_check = eye<float, 3>() * 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<float, 4, 3> m43(data_43);
Matrix<float, 3, 2> m32(data_32);
Matrix<float, 4, 3> m43(data_43);
Matrix<float, 3, 2> m32(data_32);
Matrix<float, 4, 2> m42 = m43 * m32;
Matrix<float, 4, 2> m42 = m43 * m32;
float data_42[8] = {15,32,
11,24,
17,33,
27,43
};
Matrix<float, 4, 2> m42_check(data_42);
TEST(isEqual(m42, m42_check))
float data_42[8] = {15, 32,
11, 24,
17, 33,
27, 43
};
Matrix<float, 4, 2> m42_check(data_42);
TEST(isEqual(m42, m42_check))
float data_42_plus2[8] = {17,34,
13,26,
19,35,
29,45
};
Matrix<float, 4, 2> m42_plus2_check(data_42_plus2);
Matrix<float, 4, 2> 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<float, 4, 2> m42_plus2_check(data_42_plus2);
Matrix<float, 4, 2> 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 : */
src/lib/matrix/test/matrixScalarMult.cpp
+7 -8
View File
@@ -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 : */
src/lib/matrix/test/pseudoInverse.cpp
+125 -126
View File
@@ -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<float, 3, 4> A0(data0);
Matrix<float, 4, 3> A0_I;
bool ret = geninv(A0, A0_I);
TEST(ret);
Matrix<float, 4, 3> A0_I_check(data0_check);
Matrix<float, 3, 4> A0(data0);
Matrix<float, 4, 3> A0_I;
bool ret = geninv(A0, A0_I);
TEST(ret);
Matrix<float, 4, 3> 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<float, 4, 3> A1(data1);
Matrix<float, 3, 4> A1_I;
ret = geninv(A1, A1_I);
TEST(ret);
Matrix<float, 3, 4> A1_I_check(data1_check);
Matrix<float, 4, 3> A1(data1);
Matrix<float, 3, 4> A1_I;
ret = geninv(A1, A1_I);
TEST(ret);
Matrix<float, 3, 4> 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<float, n_large, n_large - 1> A_large;
A_large.setIdentity();
Matrix<float, n_large - 1, n_large> 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<float, 3> A2(data2);
SquareMatrix<float, 3> A2_I;
ret = geninv(A2, A2_I);
TEST(ret);
SquareMatrix<float, 3> A2_I_check(data2_check);
TEST((A2_I - A2_I_check).abs().max() < 1e-3);
SquareMatrix<float, 3> A2(data2);
SquareMatrix<float, 3> A2_I;
ret = geninv(A2, A2_I);
TEST(ret);
SquareMatrix<float, 3> A2_I_check(data2_check);
TEST((A2_I - A2_I_check).abs().max() < 1e-3);
// Null matrix test
Matrix<float, 6, 16> A3;
Matrix<float, 16, 6> A3_I;
ret = geninv(A3, A3_I);
TEST(ret);
Matrix<float, 16, 6> A3_I_check;
TEST((A3_I - A3_I_check).abs().max() < 1e-5);
// Null matrix test
Matrix<float, 6, 16> A3;
Matrix<float, 16, 6> A3_I;
ret = geninv(A3, A3_I);
TEST(ret);
Matrix<float, 16, 6> 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<float, 6, 16> B = Matrix<float, 6, 16>(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<float, 16, 6> A_check = Matrix<float, 16, 6>(A_quad_w);
Matrix<float, 16, 6> 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<float, 6, 16> B = Matrix<float, 6, 16>(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<float, 16, 6> A_check = Matrix<float, 16, 6>(A_quad_w);
Matrix<float, 16, 6> 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<float, 5, 6> real ( real_alloc);
Matrix<float, 6, 5> 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<float, 5, 6> real(real_alloc);
Matrix<float, 6, 5> 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<float, 6, 5> 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<float, 6, 5> 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 : */
src/lib/matrix/test/setIdentity.cpp
+23 -24
View File
@@ -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 : */
src/lib/matrix/test/slice.cpp
+188 -187
View File
@@ -8,228 +8,229 @@ template class matrix::Slice<float, 2, 3, 4, 5>; // so that we get full coverage
int main()
{
float data[9] = {0, 2, 3,
4, 5, 6,
7, 8, 10
};
SquareMatrix<float, 3> A(data);
float data[9] = {0, 2, 3,
4, 5, 6,
7, 8, 10
};
SquareMatrix<float, 3> A(data);
// Test row slicing
Matrix<float, 2, 3> B_rowslice(A.slice<2, 3>(1, 0));
float data_check_rowslice[6] = {
4, 5, 6,
7, 8, 10
};
Matrix<float, 2, 3> B_check_rowslice(data_check_rowslice);
TEST(isEqual(B_rowslice, B_check_rowslice));
// Test row slicing
Matrix<float, 2, 3> B_rowslice(A.slice<2, 3>(1, 0));
float data_check_rowslice[6] = {
4, 5, 6,
7, 8, 10
};
Matrix<float, 2, 3> B_check_rowslice(data_check_rowslice);
TEST(isEqual(B_rowslice, B_check_rowslice));
// Test column slicing
Matrix<float, 3, 2> B_colslice(A.slice<3, 2>(0, 1));
float data_check_colslice[6] = {
2, 3,
5, 6,
8, 10
};
Matrix<float, 3, 2> B_check_colslice(data_check_colslice);
TEST(isEqual(B_colslice, B_check_colslice));
// Test column slicing
Matrix<float, 3, 2> B_colslice(A.slice<3, 2>(0, 1));
float data_check_colslice[6] = {
2, 3,
5, 6,
8, 10
};
Matrix<float, 3, 2> B_check_colslice(data_check_colslice);
TEST(isEqual(B_colslice, B_check_colslice));
// Test slicing both
Matrix<float, 2, 2> B_bothslice(A.slice<2, 2>(1, 1));
float data_check_bothslice[4] = {
5, 6,
8, 10
};
Matrix<float, 2, 2> B_check_bothslice(data_check_bothslice);
TEST(isEqual(B_bothslice, B_check_bothslice));
// Test slicing both
Matrix<float, 2, 2> B_bothslice(A.slice<2, 2>(1, 1));
float data_check_bothslice[4] = {
5, 6,
8, 10
};
Matrix<float, 2, 2> 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<float, 2, 2> C(data_2);
A.slice<2, 2>(1, 1) = C;
Matrix<float, 2, 2> 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<float, 3, 3> D(data_2_check);
TEST(isEqual(A, D));
float data_2_check[9] = {
0, 2, 3,
4, 11, 12,
7, 13, 14
};
Matrix<float, 3, 3> D(data_2_check);
TEST(isEqual(A, D));
//Test writing to slices
Matrix<float, 3, 1> E;
E(0,0) = -1;
E(1,0) = 1;
E(2,0) = 3;
//Test writing to slices
Matrix<float, 3, 1> E;
E(0, 0) = -1;
E(1, 0) = 1;
E(2, 0) = 3;
Matrix<float, 2, 1> F;
F(0,0) = 9;
F(1,0) = 11;
Matrix<float, 2, 1> 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<float, 3, 1> G (data_3_check);
TEST(isEqual(E, G));
TEST(isEqual(E, Matrix<float,3,1>(E.slice<3,1>(0,0))));
float data_3_check[3] = {9, 11, 3};
Matrix<float, 3, 1> G(data_3_check);
TEST(isEqual(E, G));
TEST(isEqual(E, Matrix<float, 3, 1>(E.slice<3, 1>(0, 0))));
Matrix<float, 2, 1> H = E.slice<2,1>(0,0);
TEST(isEqual(H,F));
Matrix<float, 2, 1> 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<float, 3, 1> J (data_3_check);
Matrix<float, 5, 1> 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<float, 3, 1> J(data_3_check);
Matrix<float, 5, 1> K;
K.row(2) = J.row(0);
K.row(1) = J.row(1);
K.row(0) = J.row(2);
Matrix<float, 5, 1> K_check(data_4_check);
TEST(isEqual(K, K_check));
}
{ // assigning col slices to each other
const Matrix<float, 1, 3> J (data_3_check);
Matrix<float, 1, 5> K;
K.col(2) = J.col(0);
K.col(1) = J.col(1);
K.col(0) = J.col(2);
Matrix<float, 5, 1> K_check(data_4_check);
TEST(isEqual(K, K_check));
}
{
// assigning col slices to each other
const Matrix<float, 1, 3> J(data_3_check);
Matrix<float, 1, 5> K;
K.col(2) = J.col(0);
K.col(1) = J.col(1);
K.col(0) = J.col(2);
Matrix<float, 1, 5> K_check(data_4_check);
TEST(isEqual(K, K_check));
}
Matrix<float, 1, 5> K_check(data_4_check);
TEST(isEqual(K, K_check));
}
// check that slice of a slice works for reading
const Matrix<float, 3, 3> cm33(data);
Matrix<float, 2, 1> topRight = cm33.slice<2,3>(0,0).slice<2,1>(0,2);
float top_right_check[2] = {3,6};
TEST(isEqual(topRight, Matrix<float, 2, 1>(top_right_check)));
// check that slice of a slice works for reading
const Matrix<float, 3, 3> cm33(data);
Matrix<float, 2, 1> topRight = cm33.slice<2, 3>(0, 0).slice<2, 1>(0, 2);
float top_right_check[2] = {3, 6};
TEST(isEqual(topRight, Matrix<float, 2, 1>(top_right_check)));
// check that slice of a slice works for writing
Matrix<float, 3, 3> m33(data);
m33.slice<2,3>(0,0).slice<2,1>(0,2) = Matrix<float, 2, 1>();
const float data_check[9] = {0, 2, 0,
4, 5, 0,
7, 8, 10
};
TEST(isEqual(m33, Matrix<float, 3, 3>(data_check)));
// check that slice of a slice works for writing
Matrix<float, 3, 3> m33(data);
m33.slice<2, 3>(0, 0).slice<2, 1>(0, 2) = Matrix<float, 2, 1>();
const float data_check[9] = {0, 2, 0,
4, 5, 0,
7, 8, 10
};
TEST(isEqual(m33, Matrix<float, 3, 3>(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<float, 3, 1> L;
L(0,0) = -1;
L(1,0) = 1;
L(2,0) = 3;
// assign scalar value to slice
Matrix<float, 3, 1> 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<float, 3, 1> M (data_5_check);
TEST(isEqual(L, M));
float data_5_check[3] = {0, 0, 3};
Matrix<float, 3, 1> 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<float, 3> N(data_6);
// return diagonal elements
float data_6[9] = {0, 2, 3,
4, 5, 6,
7, 8, 10
};
SquareMatrix<float, 3> 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<float, 2> operand(operand_data);
// Different assignment operators
SquareMatrix3f O(data);
float operand_data [4] = {2, 1, -3, -1};
const SquareMatrix<float, 2> 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<float, 2> 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<float, 2>(O_check_data_11)));
// Different operations
O = SquareMatrix3f(data);
SquareMatrix<float, 2> 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<float, 2>(O_check_data_11)));
O = SquareMatrix3f(data);
SquareMatrix<float, 2> 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<float, 2>(O_check_data_12)));
return 0;
O = SquareMatrix3f(data);
SquareMatrix<float, 2> 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<float, 2>(O_check_data_12)));
return 0;
}
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
src/lib/matrix/test/sparseVector.cpp
+104 -90
View File
@@ -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<float>(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<float>(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<float, 4> 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<float, 4> 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<float, 4> 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<float, 4> 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<float, 4> 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<float, 4> 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<float, 2, 3> 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<float, 2> res_sparse = dense_matrix * sparse_vec;
const Vector<float, 2> res_dense = dense_matrix * dense_vec;
EXPECT_TRUE(isEqual(res_dense, res_sparse));
TEST(sparseVectorTest, multiplicationWithDenseMatrix)
{
Matrix<float, 2, 3> 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<float, 2> res_sparse = dense_matrix * sparse_vec;
const Vector<float, 2> 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<float, 3> 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<float, 3> 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();
}
src/lib/matrix/test/squareMatrix.cpp
+122 -123
View File
@@ -6,143 +6,142 @@ using namespace matrix;
int main()
{
float data[9] = {1, 2, 3,
4, 5, 6,
7, 8, 10
};
SquareMatrix<float, 3> A(data);
Vector3<float> diag_check(1, 5, 10);
float data[9] = {1, 2, 3,
4, 5, 6,
7, 8, 10
};
SquareMatrix<float, 3> A(data);
Vector3<float> 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<float, 3> eA = expm(SquareMatrix<float, 3>(A*dt), 5);
SquareMatrix<float, 3> eA_check(data_check);
TEST((eA - eA_check).abs().max() < 1e-3f);
float dt = 0.01f;
SquareMatrix<float, 3> eA = expm(SquareMatrix<float, 3>(A * dt), 5);
SquareMatrix<float, 3> eA_check(data_check);
TEST((eA - eA_check).abs().max() < 1e-3f);
SquareMatrix<float, 2> A_bottomright = A.slice<2,2>(1,1);
SquareMatrix<float, 2> A_bottomright2;
A_bottomright2 = A.slice<2,2>(1,1);
SquareMatrix<float, 2> A_bottomright = A.slice<2, 2>(1, 1);
SquareMatrix<float, 2> A_bottomright2;
A_bottomright2 = A.slice<2, 2>(1, 1);
float data_bottomright[4] = {5, 6,
8, 10
};
SquareMatrix<float, 2> 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<float, 2> 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<float, 4> 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<float, 4> B_check(data_B_check);
TEST(isEqual(B, B_check))
SquareMatrix<float, 4> 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<float, 4> B_check(data_B_check);
TEST(isEqual(B, B_check))
SquareMatrix<float, 4> 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<float, 4> C_check(data_C_check);
TEST(isEqual(C, C_check))
SquareMatrix<float, 4> 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<float, 4> C_check(data_C_check);
TEST(isEqual(C, C_check))
SquareMatrix<float, 4> 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<float, 4> D_check(data_D_check);
TEST(isEqual(D, D_check))
SquareMatrix<float, 4> 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<float, 4> D_check(data_D_check);
TEST(isEqual(D, D_check))
SquareMatrix<float, 4> 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<float, 4> E_check(data_E_check);
TEST(isEqual(E, E_check))
SquareMatrix<float, 4> 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<float, 4> E_check(data_E_check);
TEST(isEqual(E, E_check))
// test symmetric functions
SquareMatrix<float, 4> 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<float, 4> F_check(data_F_check);
TEST(isEqual(F, F_check))
TEST(F.isBlockSymmetric<2>(1));
TEST(!F.isRowColSymmetric<2>(1));
// test symmetric functions
SquareMatrix<float, 4> 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<float, 4> F_check(data_F_check);
TEST(isEqual(F, F_check))
TEST(F.isBlockSymmetric<2>(1));
TEST(!F.isRowColSymmetric<2>(1));
SquareMatrix<float, 4> 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<float, 4> G_check(data_G_check);
TEST(isEqual(G, G_check));
TEST(G.isBlockSymmetric<2>(1));
TEST(G.isRowColSymmetric<2>(1));
SquareMatrix<float, 4> 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<float, 4> G_check(data_G_check);
TEST(isEqual(G, G_check));
TEST(G.isBlockSymmetric<2>(1));
TEST(G.isRowColSymmetric<2>(1));
SquareMatrix<float, 4> 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<float, 4> H_check(data_H_check);
TEST(isEqual(H, H_check))
TEST(H.isBlockSymmetric<1>(1));
TEST(!H.isRowColSymmetric<1>(1));
SquareMatrix<float, 4> 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<float, 4> H_check(data_H_check);
TEST(isEqual(H, H_check))
TEST(H.isBlockSymmetric<1>(1));
TEST(!H.isRowColSymmetric<1>(1));
SquareMatrix<float, 4> 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<float, 4> 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<float, 4> 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<float, 4> 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<float, 4> 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<float, 4> K(data_K);
TEST(!K.isRowColSymmetric<1>(2));
return 0;
}
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
src/lib/matrix/test/test_data.py
-2
View File
@@ -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 :
src/lib/matrix/test/transpose.cpp
+7 -8
View File
@@ -6,14 +6,13 @@ using namespace matrix;
int main()
{
float data[6] = {1, 2, 3, 4, 5, 6};
Matrix<float, 2, 3> A(data);
Matrix<float, 3, 2> A_T = A.transpose();
float data_check[6] = {1, 4, 2, 5, 3, 6};
Matrix<float, 3, 2> A_T_check(data_check);
TEST(isEqual(A_T, A_T_check));
float data[6] = {1, 2, 3, 4, 5, 6};
Matrix<float, 2, 3> A(data);
Matrix<float, 3, 2> A_T = A.transpose();
float data_check[6] = {1, 4, 2, 5, 3, 6};
Matrix<float, 3, 2> 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 : */
src/lib/matrix/test/upperRightTriangle.cpp
+10 -11
View File
@@ -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<float, 3> A(data);
SquareMatrix<float, 3> 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 : */
src/lib/matrix/test/vector.cpp
+31 -32
View File
@@ -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<float, 5> v1(data1);
Vector<float, 5> v2(data2);
// test data
float data1[] = {1, 2, 3, 4, 5};
float data2[] = {6, 7, 8, 9, 10};
Vector<float, 5> v1(data1);
Vector<float, 5> v2(data2);
// copy constructor
Vector<float, 5> v3(v2);
TEST(isEqual(v2, v3));
// copy constructor
Vector<float, 5> 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<float, 5>().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<float, 5>().unit_or_zero().norm(), 0.f));
v2.normalize();
TEST(isEqualF(v2.norm(), 1.f));
// sqrt
float data1_sq[] = {1,4,9,16,25};
Vector<float, 5> v4(data1_sq);
TEST(isEqual(v1, v4.sqrt()));
// sqrt
float data1_sq[] = {1, 4, 9, 16, 25};
Vector<float, 5> v4(data1_sq);
TEST(isEqual(v1, v4.sqrt()));
// longerThan
Vector<float, 2> v5;
v5(0) = 3;
v5(1) = 4;
TEST(v5.longerThan(4.99f));
TEST(!v5.longerThan(5.f));
// longerThan
Vector<float, 2> 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 : */
src/lib/matrix/test/vector2.cpp
+21 -22
View File
@@ -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<float, 2, 1> d(a);
TEST(fabs(d(0,0) - 1) < FLT_EPSILON);
TEST(fabs(d(1,0) - 0) < FLT_EPSILON);
Matrix<float, 2, 1> 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 : */
src/lib/matrix/test/vector3.cpp
+42 -43
View File
@@ -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<float, 3, 1> 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<float, 3, 1> 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<float, 4> j;
j(0) = 1;
j(1) = 2;
j(2) = 3;
j(3) = 4;
Vector<float, 4> 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 : */
src/lib/matrix/test/vectorAssignment.cpp
+17 -18
View File
@@ -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<float, 3> 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<float, 3> 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 : */