mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-31 18:47:21 +08:00
matrix: apply PX4 astyle
This commit is contained in:
@@ -17,7 +17,6 @@ exec find boards msg src platforms test \
|
|||||||
-path src/drivers/uavcannode_gps_demo/libcanard -prune -o \
|
-path src/drivers/uavcannode_gps_demo/libcanard -prune -o \
|
||||||
-path src/lib/crypto/monocypher -prune -o \
|
-path src/lib/crypto/monocypher -prune -o \
|
||||||
-path src/lib/events/libevents -prune -o \
|
-path src/lib/events/libevents -prune -o \
|
||||||
-path src/lib/matrix -prune -o \
|
|
||||||
-path src/lib/parameters/uthash -prune -o \
|
-path src/lib/parameters/uthash -prune -o \
|
||||||
-path src/modules/ekf2/EKF -prune -o \
|
-path src/modules/ekf2/EKF -prune -o \
|
||||||
-path src/modules/gyro_fft/CMSIS_5 -prune -o \
|
-path src/modules/gyro_fft/CMSIS_5 -prune -o \
|
||||||
|
|||||||
@@ -70,8 +70,10 @@ public:
|
|||||||
{
|
{
|
||||||
AxisAngle &v = *this;
|
AxisAngle &v = *this;
|
||||||
Type mag = q.imag().norm();
|
Type mag = q.imag().norm();
|
||||||
|
|
||||||
if (fabs(mag) >= Type(1e-10)) {
|
if (fabs(mag) >= Type(1e-10)) {
|
||||||
v = q.imag() * Type(Type(2) * atan2(mag, q(0)) / mag);
|
v = q.imag() * Type(Type(2) * atan2(mag, q(0)) / mag);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
v = q.imag() * Type(Type(2) * Type(sign(q(0))));
|
v = q.imag() * Type(Type(2) * Type(sign(q(0))));
|
||||||
}
|
}
|
||||||
@@ -139,15 +141,18 @@ public:
|
|||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
Vector<Type, 3> axis() {
|
Vector<Type, 3> axis()
|
||||||
|
{
|
||||||
if (Vector<Type, 3>::norm() > 0) {
|
if (Vector<Type, 3>::norm() > 0) {
|
||||||
return Vector<Type, 3>::unit();
|
return Vector<Type, 3>::unit();
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
return Vector3<Type>(1, 0, 0);
|
return Vector3<Type>(1, 0, 0);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
Type angle() {
|
Type angle()
|
||||||
|
{
|
||||||
return Vector<Type, 3>::norm();
|
return Vector<Type, 3>::norm();
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
@@ -156,5 +161,3 @@ using AxisAnglef = AxisAngle<float>;
|
|||||||
using AxisAngled = AxisAngle<double>;
|
using AxisAngled = AxisAngle<double>;
|
||||||
|
|
||||||
} // namespace matrix
|
} // namespace matrix
|
||||||
|
|
||||||
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
|
|
||||||
|
|||||||
@@ -183,5 +183,3 @@ using Dcmf = Dcm<float>;
|
|||||||
using Dcmd = Dcm<double>;
|
using Dcmd = Dcm<double>;
|
||||||
|
|
||||||
} // namespace matrix
|
} // namespace matrix
|
||||||
|
|
||||||
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
|
|
||||||
|
|||||||
@@ -22,8 +22,7 @@ template <typename Type, size_t M>
|
|||||||
class Vector;
|
class Vector;
|
||||||
|
|
||||||
template <typename Scalar, size_t N>
|
template <typename Scalar, size_t N>
|
||||||
struct Dual
|
struct Dual {
|
||||||
{
|
|
||||||
static constexpr size_t WIDTH = N;
|
static constexpr size_t WIDTH = N;
|
||||||
|
|
||||||
Scalar value {};
|
Scalar value {};
|
||||||
@@ -34,6 +33,7 @@ struct Dual
|
|||||||
explicit Dual(Scalar v, size_t inputDimension = 65535)
|
explicit Dual(Scalar v, size_t inputDimension = 65535)
|
||||||
{
|
{
|
||||||
value = v;
|
value = v;
|
||||||
|
|
||||||
if (inputDimension < N) {
|
if (inputDimension < N) {
|
||||||
derivative(inputDimension) = Scalar(1);
|
derivative(inputDimension) = Scalar(1);
|
||||||
}
|
}
|
||||||
@@ -334,9 +334,11 @@ template <typename Scalar, size_t M, size_t N>
|
|||||||
Matrix<Scalar, M, N> collectDerivatives(const Matrix<Dual<Scalar, N>, M, 1> &input)
|
Matrix<Scalar, M, N> collectDerivatives(const Matrix<Dual<Scalar, N>, M, 1> &input)
|
||||||
{
|
{
|
||||||
Matrix<Scalar, M, N> jac;
|
Matrix<Scalar, M, N> jac;
|
||||||
|
|
||||||
for (size_t i = 0; i < M; i++) {
|
for (size_t i = 0; i < M; i++) {
|
||||||
jac.row(i) = input(i, 0).derivative;
|
jac.row(i) = input(i, 0).derivative;
|
||||||
}
|
}
|
||||||
|
|
||||||
return jac;
|
return jac;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -345,11 +347,13 @@ template <typename Scalar, size_t M, size_t N, size_t D>
|
|||||||
Matrix<Scalar, M, N> collectReals(const Matrix<Dual<Scalar, D>, M, N> &input)
|
Matrix<Scalar, M, N> collectReals(const Matrix<Dual<Scalar, D>, M, N> &input)
|
||||||
{
|
{
|
||||||
Matrix<Scalar, M, N> r;
|
Matrix<Scalar, M, N> r;
|
||||||
|
|
||||||
for (size_t i = 0; i < M; i++) {
|
for (size_t i = 0; i < M; i++) {
|
||||||
for (size_t j = 0; j < N; j++) {
|
for (size_t j = 0; j < N; j++) {
|
||||||
r(i, j) = input(i, j).value;
|
r(i, j) = input(i, j).value;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
return r;
|
return r;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -360,10 +364,12 @@ std::ostream& operator<<(std::ostream& os,
|
|||||||
{
|
{
|
||||||
os << "[";
|
os << "[";
|
||||||
os << std::setw(10) << dual.value << ";";
|
os << std::setw(10) << dual.value << ";";
|
||||||
|
|
||||||
for (size_t j = 0; j < N; ++j) {
|
for (size_t j = 0; j < N; ++j) {
|
||||||
os << "\t";
|
os << "\t";
|
||||||
os << std::setw(10) << static_cast<double>(dual.derivative(j));
|
os << std::setw(10) << static_cast<double>(dual.derivative(j));
|
||||||
}
|
}
|
||||||
|
|
||||||
os << "]";
|
os << "]";
|
||||||
return os;
|
return os;
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -153,5 +153,3 @@ using Eulerf = Euler<float>;
|
|||||||
using Eulerd = Euler<double>;
|
using Eulerd = Euler<double>;
|
||||||
|
|
||||||
} // namespace matrix
|
} // namespace matrix
|
||||||
|
|
||||||
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
|
|
||||||
|
|||||||
@@ -16,7 +16,8 @@
|
|||||||
|
|
||||||
#include "math.hpp"
|
#include "math.hpp"
|
||||||
|
|
||||||
namespace matrix {
|
namespace matrix
|
||||||
|
{
|
||||||
|
|
||||||
template<typename Type, size_t M, size_t N>
|
template<typename Type, size_t M, size_t N>
|
||||||
class LeastSquaresSolver
|
class LeastSquaresSolver
|
||||||
@@ -41,31 +42,39 @@ public:
|
|||||||
|
|
||||||
for (size_t j = 0; j < N; j++) {
|
for (size_t j = 0; j < N; j++) {
|
||||||
Type normx = Type(0);
|
Type normx = Type(0);
|
||||||
|
|
||||||
for (size_t i = j; i < M; i++) {
|
for (size_t i = j; i < M; i++) {
|
||||||
normx += _A(i, j) * _A(i, j);
|
normx += _A(i, j) * _A(i, j);
|
||||||
}
|
}
|
||||||
|
|
||||||
normx = sqrt(normx);
|
normx = sqrt(normx);
|
||||||
Type s = _A(j, j) > 0 ? Type(-1) : Type(1);
|
Type s = _A(j, j) > 0 ? Type(-1) : Type(1);
|
||||||
Type u1 = _A(j, j) - s * normx;
|
Type u1 = _A(j, j) - s * normx;
|
||||||
|
|
||||||
// prevent divide by zero
|
// prevent divide by zero
|
||||||
// also covers u1. normx is never negative
|
// also covers u1. normx is never negative
|
||||||
if (normx < Type(1e-8)) {
|
if (normx < Type(1e-8)) {
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
Type w[M] = {};
|
Type w[M] = {};
|
||||||
w[0] = Type(1);
|
w[0] = Type(1);
|
||||||
|
|
||||||
for (size_t i = j + 1; i < M; i++) {
|
for (size_t i = j + 1; i < M; i++) {
|
||||||
w[i - j] = _A(i, j) / u1;
|
w[i - j] = _A(i, j) / u1;
|
||||||
_A(i, j) = w[i - j];
|
_A(i, j) = w[i - j];
|
||||||
}
|
}
|
||||||
|
|
||||||
_A(j, j) = s * normx;
|
_A(j, j) = s * normx;
|
||||||
_tau(j) = -s * u1 / normx;
|
_tau(j) = -s * u1 / normx;
|
||||||
|
|
||||||
for (size_t k = j + 1; k < N; k++) {
|
for (size_t k = j + 1; k < N; k++) {
|
||||||
Type tmp = Type(0);
|
Type tmp = Type(0);
|
||||||
|
|
||||||
for (size_t i = j; i < M; i++) {
|
for (size_t i = j; i < M; i++) {
|
||||||
tmp += w[i - j] * _A(i, k);
|
tmp += w[i - j] * _A(i, k);
|
||||||
}
|
}
|
||||||
|
|
||||||
for (size_t i = j; i < M; i++) {
|
for (size_t i = j; i < M; i++) {
|
||||||
_A(i, k) -= _tau(j) * w[i - j] * tmp;
|
_A(i, k) -= _tau(j) * w[i - j] * tmp;
|
||||||
}
|
}
|
||||||
@@ -82,17 +91,21 @@ public:
|
|||||||
* This function calculates Q^T * b. This is useful for the solver
|
* This function calculates Q^T * b. This is useful for the solver
|
||||||
* because R*x = Q^T*b.
|
* because R*x = Q^T*b.
|
||||||
*/
|
*/
|
||||||
Vector<Type, M> qtb(const Vector<Type, M> &b) {
|
Vector<Type, M> qtb(const Vector<Type, M> &b)
|
||||||
|
{
|
||||||
Vector<Type, M> qtbv = b;
|
Vector<Type, M> qtbv = b;
|
||||||
|
|
||||||
for (size_t j = 0; j < N; j++) {
|
for (size_t j = 0; j < N; j++) {
|
||||||
Type w[M];
|
Type w[M];
|
||||||
w[0] = Type(1);
|
w[0] = Type(1);
|
||||||
|
|
||||||
// fill vector w
|
// fill vector w
|
||||||
for (size_t i = j + 1; i < M; i++) {
|
for (size_t i = j + 1; i < M; i++) {
|
||||||
w[i - j] = _A(i, j);
|
w[i - j] = _A(i, j);
|
||||||
}
|
}
|
||||||
|
|
||||||
Type tmp = Type(0);
|
Type tmp = Type(0);
|
||||||
|
|
||||||
for (size_t i = j; i < M; i++) {
|
for (size_t i = j; i < M; i++) {
|
||||||
tmp += w[i - j] * qtbv(i);
|
tmp += w[i - j] * qtbv(i);
|
||||||
}
|
}
|
||||||
@@ -101,6 +114,7 @@ public:
|
|||||||
qtbv(i) -= _tau(j) * w[i - j] * tmp;
|
qtbv(i) -= _tau(j) * w[i - j] * tmp;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
return qtbv;
|
return qtbv;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -112,7 +126,8 @@ public:
|
|||||||
* Find x in the equation Ax = b.
|
* Find x in the equation Ax = b.
|
||||||
* A is provided in the initializer of the class.
|
* A is provided in the initializer of the class.
|
||||||
*/
|
*/
|
||||||
Vector<Type, N> solve(const Vector<Type, M> &b) {
|
Vector<Type, N> solve(const Vector<Type, M> &b)
|
||||||
|
{
|
||||||
Vector<Type, M> qtbv = qtb(b);
|
Vector<Type, M> qtbv = qtb(b);
|
||||||
Vector<Type, N> x;
|
Vector<Type, N> x;
|
||||||
|
|
||||||
@@ -120,18 +135,23 @@ public:
|
|||||||
for (size_t i = N - 1; i < N; i--) {
|
for (size_t i = N - 1; i < N; i--) {
|
||||||
printf("i %d\n", static_cast<int>(i));
|
printf("i %d\n", static_cast<int>(i));
|
||||||
x(i) = qtbv(i);
|
x(i) = qtbv(i);
|
||||||
|
|
||||||
for (size_t r = i + 1; r < N; r++) {
|
for (size_t r = i + 1; r < N; r++) {
|
||||||
x(i) -= _A(i, r) * x(r);
|
x(i) -= _A(i, r) * x(r);
|
||||||
}
|
}
|
||||||
|
|
||||||
// divide by zero, return vector of zeros
|
// divide by zero, return vector of zeros
|
||||||
if (isEqualF(_A(i, i), Type(0), Type(1e-8))) {
|
if (isEqualF(_A(i, i), Type(0), Type(1e-8))) {
|
||||||
for (size_t z = 0; z < N; z++) {
|
for (size_t z = 0; z < N; z++) {
|
||||||
x(z) = Type(0);
|
x(z) = Type(0);
|
||||||
}
|
}
|
||||||
|
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
x(i) /= _A(i, i);
|
x(i) /= _A(i, i);
|
||||||
}
|
}
|
||||||
|
|
||||||
return x;
|
return x;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -142,5 +162,3 @@ private:
|
|||||||
};
|
};
|
||||||
|
|
||||||
} // namespace matrix
|
} // namespace matrix
|
||||||
|
|
||||||
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
|
|
||||||
|
|||||||
@@ -71,6 +71,7 @@ public:
|
|||||||
Matrix(const Slice<Type, M, N, P, Q> &in_slice)
|
Matrix(const Slice<Type, M, N, P, Q> &in_slice)
|
||||||
{
|
{
|
||||||
Matrix<Type, M, N> &self = *this;
|
Matrix<Type, M, N> &self = *this;
|
||||||
|
|
||||||
for (size_t i = 0; i < M; i++) {
|
for (size_t i = 0; i < M; i++) {
|
||||||
for (size_t j = 0; j < N; j++) {
|
for (size_t j = 0; j < N; j++) {
|
||||||
self(i, j) = in_slice(i, j);
|
self(i, j) = in_slice(i, j);
|
||||||
@@ -103,18 +104,21 @@ public:
|
|||||||
{
|
{
|
||||||
if (this != &other) {
|
if (this != &other) {
|
||||||
Matrix<Type, M, N> &self = *this;
|
Matrix<Type, M, N> &self = *this;
|
||||||
|
|
||||||
for (size_t i = 0; i < M; i++) {
|
for (size_t i = 0; i < M; i++) {
|
||||||
for (size_t j = 0; j < N; j++) {
|
for (size_t j = 0; j < N; j++) {
|
||||||
self(i, j) = other(i, j);
|
self(i, j) = other(i, j);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
return (*this);
|
return (*this);
|
||||||
}
|
}
|
||||||
|
|
||||||
void copyTo(Type dst[M * N]) const
|
void copyTo(Type dst[M * N]) const
|
||||||
{
|
{
|
||||||
const Matrix<Type, M, N> &self = *this;
|
const Matrix<Type, M, N> &self = *this;
|
||||||
|
|
||||||
for (size_t i = 0; i < M; i++) {
|
for (size_t i = 0; i < M; i++) {
|
||||||
for (size_t j = 0; j < N; j++) {
|
for (size_t j = 0; j < N; j++) {
|
||||||
dst[N * i + j] = self(i, j);
|
dst[N * i + j] = self(i, j);
|
||||||
@@ -232,6 +236,7 @@ public:
|
|||||||
void operator+=(const Matrix<Type, M, N> &other)
|
void operator+=(const Matrix<Type, M, N> &other)
|
||||||
{
|
{
|
||||||
Matrix<Type, M, N> &self = *this;
|
Matrix<Type, M, N> &self = *this;
|
||||||
|
|
||||||
for (size_t i = 0; i < M; i++) {
|
for (size_t i = 0; i < M; i++) {
|
||||||
for (size_t j = 0; j < N; j++) {
|
for (size_t j = 0; j < N; j++) {
|
||||||
self(i, j) += other(i, j);
|
self(i, j) += other(i, j);
|
||||||
@@ -242,6 +247,7 @@ public:
|
|||||||
void operator-=(const Matrix<Type, M, N> &other)
|
void operator-=(const Matrix<Type, M, N> &other)
|
||||||
{
|
{
|
||||||
Matrix<Type, M, N> &self = *this;
|
Matrix<Type, M, N> &self = *this;
|
||||||
|
|
||||||
for (size_t i = 0; i < M; i++) {
|
for (size_t i = 0; i < M; i++) {
|
||||||
for (size_t j = 0; j < N; j++) {
|
for (size_t j = 0; j < N; j++) {
|
||||||
self(i, j) -= other(i, j);
|
self(i, j) -= other(i, j);
|
||||||
@@ -318,6 +324,7 @@ public:
|
|||||||
inline void operator+=(Type scalar)
|
inline void operator+=(Type scalar)
|
||||||
{
|
{
|
||||||
Matrix<Type, M, N> &self = *this;
|
Matrix<Type, M, N> &self = *this;
|
||||||
|
|
||||||
for (size_t i = 0; i < M; i++) {
|
for (size_t i = 0; i < M; i++) {
|
||||||
for (size_t j = 0; j < N; j++) {
|
for (size_t j = 0; j < N; j++) {
|
||||||
self(i, j) += scalar;
|
self(i, j) += scalar;
|
||||||
@@ -350,10 +357,12 @@ public:
|
|||||||
{
|
{
|
||||||
buf[0] = '\0'; // make an empty string to begin with (we need the '\0' for strlen to work)
|
buf[0] = '\0'; // make an empty string to begin with (we need the '\0' for strlen to work)
|
||||||
const Matrix<Type, M, N> &self = *this;
|
const Matrix<Type, M, N> &self = *this;
|
||||||
|
|
||||||
for (size_t i = 0; i < M; i++) {
|
for (size_t i = 0; i < M; i++) {
|
||||||
for (size_t j = 0; j < N; j++) {
|
for (size_t j = 0; j < N; j++) {
|
||||||
snprintf(buf + strlen(buf), n - strlen(buf), "\t%8.8g", double(self(i, j))); // directly append to the string buffer
|
snprintf(buf + strlen(buf), n - strlen(buf), "\t%8.8g", double(self(i, j))); // directly append to the string buffer
|
||||||
}
|
}
|
||||||
|
|
||||||
snprintf(buf + strlen(buf), n - strlen(buf), "\n");
|
snprintf(buf + strlen(buf), n - strlen(buf), "\n");
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -477,6 +486,7 @@ public:
|
|||||||
Matrix<Type, M, N> &self = *this;
|
Matrix<Type, M, N> &self = *this;
|
||||||
|
|
||||||
const size_t min_i = M > N ? N : M;
|
const size_t min_i = M > N ? N : M;
|
||||||
|
|
||||||
for (size_t i = 0; i < min_i; i++) {
|
for (size_t i = 0; i < min_i; i++) {
|
||||||
self(i, i) = 1;
|
self(i, i) = 1;
|
||||||
}
|
}
|
||||||
@@ -526,70 +536,84 @@ public:
|
|||||||
Matrix<Type, M, N> abs() const
|
Matrix<Type, M, N> abs() const
|
||||||
{
|
{
|
||||||
Matrix<Type, M, N> r;
|
Matrix<Type, M, N> r;
|
||||||
|
|
||||||
for (size_t i = 0; i < M; i++) {
|
for (size_t i = 0; i < M; i++) {
|
||||||
for (size_t j = 0; j < N; j++) {
|
for (size_t j = 0; j < N; j++) {
|
||||||
r(i, j) = Type(fabs((*this)(i, j)));
|
r(i, j) = Type(fabs((*this)(i, j)));
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
return r;
|
return r;
|
||||||
}
|
}
|
||||||
|
|
||||||
Type max() const
|
Type max() const
|
||||||
{
|
{
|
||||||
Type max_val = (*this)(0, 0);
|
Type max_val = (*this)(0, 0);
|
||||||
|
|
||||||
for (size_t i = 0; i < M; i++) {
|
for (size_t i = 0; i < M; i++) {
|
||||||
for (size_t j = 0; j < N; j++) {
|
for (size_t j = 0; j < N; j++) {
|
||||||
Type val = (*this)(i, j);
|
Type val = (*this)(i, j);
|
||||||
|
|
||||||
if (val > max_val) {
|
if (val > max_val) {
|
||||||
max_val = val;
|
max_val = val;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
return max_val;
|
return max_val;
|
||||||
}
|
}
|
||||||
|
|
||||||
Type min() const
|
Type min() const
|
||||||
{
|
{
|
||||||
Type min_val = (*this)(0, 0);
|
Type min_val = (*this)(0, 0);
|
||||||
|
|
||||||
for (size_t i = 0; i < M; i++) {
|
for (size_t i = 0; i < M; i++) {
|
||||||
for (size_t j = 0; j < N; j++) {
|
for (size_t j = 0; j < N; j++) {
|
||||||
Type val = (*this)(i, j);
|
Type val = (*this)(i, j);
|
||||||
|
|
||||||
if (val < min_val) {
|
if (val < min_val) {
|
||||||
min_val = val;
|
min_val = val;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
return min_val;
|
return min_val;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool isAllNan() const {
|
bool isAllNan() const
|
||||||
|
{
|
||||||
const Matrix<float, M, N> &self = *this;
|
const Matrix<float, M, N> &self = *this;
|
||||||
bool result = true;
|
bool result = true;
|
||||||
|
|
||||||
for (size_t i = 0; i < M; i++) {
|
for (size_t i = 0; i < M; i++) {
|
||||||
for (size_t j = 0; j < N; j++) {
|
for (size_t j = 0; j < N; j++) {
|
||||||
result = result && isnan(self(i, j));
|
result = result && isnan(self(i, j));
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
return result;
|
return result;
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
template<typename Type, size_t M, size_t N>
|
template<typename Type, size_t M, size_t N>
|
||||||
Matrix<Type, M, N> zeros() {
|
Matrix<Type, M, N> zeros()
|
||||||
|
{
|
||||||
Matrix<Type, M, N> m;
|
Matrix<Type, M, N> m;
|
||||||
m.setZero();
|
m.setZero();
|
||||||
return m;
|
return m;
|
||||||
}
|
}
|
||||||
|
|
||||||
template<typename Type, size_t M, size_t N>
|
template<typename Type, size_t M, size_t N>
|
||||||
Matrix<Type, M, N> ones() {
|
Matrix<Type, M, N> ones()
|
||||||
|
{
|
||||||
Matrix<Type, M, N> m;
|
Matrix<Type, M, N> m;
|
||||||
m.setOne();
|
m.setOne();
|
||||||
return m;
|
return m;
|
||||||
}
|
}
|
||||||
|
|
||||||
template<size_t M, size_t N>
|
template<size_t M, size_t N>
|
||||||
Matrix<float, M, N> nans() {
|
Matrix<float, M, N> nans()
|
||||||
|
{
|
||||||
Matrix<float, M, N> m;
|
Matrix<float, M, N> m;
|
||||||
m.setNaN();
|
m.setNaN();
|
||||||
return m;
|
return m;
|
||||||
@@ -603,7 +627,8 @@ Matrix<Type, M, N> operator*(Type scalar, const Matrix<Type, M, N> &other)
|
|||||||
|
|
||||||
template<typename Type, size_t M, size_t N>
|
template<typename Type, size_t M, size_t N>
|
||||||
bool isEqual(const Matrix<Type, M, N> &x,
|
bool isEqual(const Matrix<Type, M, N> &x,
|
||||||
const Matrix<Type, M, N> &y, const Type eps=Type(1e-4f)) {
|
const Matrix<Type, M, N> &y, const Type eps = Type(1e-4f))
|
||||||
|
{
|
||||||
for (size_t i = 0; i < M; i++) {
|
for (size_t i = 0; i < M; i++) {
|
||||||
for (size_t j = 0; j < N; j++) {
|
for (size_t j = 0; j < N; j++) {
|
||||||
if (!isEqualF(x(i, j), y(i, j), eps)) {
|
if (!isEqualF(x(i, j), y(i, j), eps)) {
|
||||||
@@ -611,47 +636,59 @@ bool isEqual(const Matrix<Type, M, N> &x,
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
namespace typeFunction
|
namespace typeFunction
|
||||||
{
|
{
|
||||||
template<typename Type>
|
template<typename Type>
|
||||||
Type min(const Type x, const Type y) {
|
Type min(const Type x, const Type y)
|
||||||
|
{
|
||||||
bool x_is_nan = isnan(x);
|
bool x_is_nan = isnan(x);
|
||||||
bool y_is_nan = isnan(y);
|
bool y_is_nan = isnan(y);
|
||||||
|
|
||||||
// take the non-nan value if there is one
|
// take the non-nan value if there is one
|
||||||
if (x_is_nan || y_is_nan) {
|
if (x_is_nan || y_is_nan) {
|
||||||
if (x_is_nan && !y_is_nan) {
|
if (x_is_nan && !y_is_nan) {
|
||||||
return y;
|
return y;
|
||||||
}
|
}
|
||||||
|
|
||||||
// either !x_is_nan && y_is_nan or both are NAN anyways
|
// either !x_is_nan && y_is_nan or both are NAN anyways
|
||||||
return x;
|
return x;
|
||||||
}
|
}
|
||||||
|
|
||||||
return (x < y) ? x : y;
|
return (x < y) ? x : y;
|
||||||
}
|
}
|
||||||
|
|
||||||
template<typename Type>
|
template<typename Type>
|
||||||
Type max(const Type x, const Type y) {
|
Type max(const Type x, const Type y)
|
||||||
|
{
|
||||||
bool x_is_nan = isnan(x);
|
bool x_is_nan = isnan(x);
|
||||||
bool y_is_nan = isnan(y);
|
bool y_is_nan = isnan(y);
|
||||||
|
|
||||||
// take the non-nan value if there is one
|
// take the non-nan value if there is one
|
||||||
if (x_is_nan || y_is_nan) {
|
if (x_is_nan || y_is_nan) {
|
||||||
if (x_is_nan && !y_is_nan) {
|
if (x_is_nan && !y_is_nan) {
|
||||||
return y;
|
return y;
|
||||||
}
|
}
|
||||||
|
|
||||||
// either !x_is_nan && y_is_nan or both are NAN anyways
|
// either !x_is_nan && y_is_nan or both are NAN anyways
|
||||||
return x;
|
return x;
|
||||||
}
|
}
|
||||||
|
|
||||||
return (x > y) ? x : y;
|
return (x > y) ? x : y;
|
||||||
}
|
}
|
||||||
|
|
||||||
template<typename Type>
|
template<typename Type>
|
||||||
Type constrain(const Type x, const Type lower_bound, const Type upper_bound) {
|
Type constrain(const Type x, const Type lower_bound, const Type upper_bound)
|
||||||
|
{
|
||||||
if (lower_bound > upper_bound) {
|
if (lower_bound > upper_bound) {
|
||||||
return NAN;
|
return NAN;
|
||||||
|
|
||||||
} else if (isnan(x)) {
|
} else if (isnan(x)) {
|
||||||
return NAN;
|
return NAN;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
return typeFunction::max(lower_bound, typeFunction::min(upper_bound, x));
|
return typeFunction::max(lower_bound, typeFunction::min(upper_bound, x));
|
||||||
}
|
}
|
||||||
@@ -659,66 +696,83 @@ Type constrain(const Type x, const Type lower_bound, const Type upper_bound) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
template<typename Type, size_t M, size_t N>
|
template<typename Type, size_t M, size_t N>
|
||||||
Matrix<Type, M, N> min(const Matrix<Type, M, N> &x, const Type scalar_upper_bound) {
|
Matrix<Type, M, N> min(const Matrix<Type, M, N> &x, const Type scalar_upper_bound)
|
||||||
|
{
|
||||||
Matrix<Type, M, N> m;
|
Matrix<Type, M, N> m;
|
||||||
|
|
||||||
for (size_t i = 0; i < M; i++) {
|
for (size_t i = 0; i < M; i++) {
|
||||||
for (size_t j = 0; j < N; j++) {
|
for (size_t j = 0; j < N; j++) {
|
||||||
m(i, j) = typeFunction::min(x(i, j), scalar_upper_bound);
|
m(i, j) = typeFunction::min(x(i, j), scalar_upper_bound);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
return m;
|
return m;
|
||||||
}
|
}
|
||||||
|
|
||||||
template<typename Type, size_t M, size_t N>
|
template<typename Type, size_t M, size_t N>
|
||||||
Matrix<Type, M, N> min(const Type scalar_upper_bound, const Matrix<Type, M, N> &x) {
|
Matrix<Type, M, N> min(const Type scalar_upper_bound, const Matrix<Type, M, N> &x)
|
||||||
|
{
|
||||||
return min(x, scalar_upper_bound);
|
return min(x, scalar_upper_bound);
|
||||||
}
|
}
|
||||||
|
|
||||||
template<typename Type, size_t M, size_t N>
|
template<typename Type, size_t M, size_t N>
|
||||||
Matrix<Type, M, N> min(const Matrix<Type, M, N> &x1, const Matrix<Type, M, N> &x2) {
|
Matrix<Type, M, N> min(const Matrix<Type, M, N> &x1, const Matrix<Type, M, N> &x2)
|
||||||
|
{
|
||||||
Matrix<Type, M, N> m;
|
Matrix<Type, M, N> m;
|
||||||
|
|
||||||
for (size_t i = 0; i < M; i++) {
|
for (size_t i = 0; i < M; i++) {
|
||||||
for (size_t j = 0; j < N; j++) {
|
for (size_t j = 0; j < N; j++) {
|
||||||
m(i, j) = typeFunction::min(x1(i, j), x2(i, j));
|
m(i, j) = typeFunction::min(x1(i, j), x2(i, j));
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
return m;
|
return m;
|
||||||
}
|
}
|
||||||
|
|
||||||
template<typename Type, size_t M, size_t N>
|
template<typename Type, size_t M, size_t N>
|
||||||
Matrix<Type, M, N> max(const Matrix<Type, M, N> &x, const Type scalar_lower_bound) {
|
Matrix<Type, M, N> max(const Matrix<Type, M, N> &x, const Type scalar_lower_bound)
|
||||||
|
{
|
||||||
Matrix<Type, M, N> m;
|
Matrix<Type, M, N> m;
|
||||||
|
|
||||||
for (size_t i = 0; i < M; i++) {
|
for (size_t i = 0; i < M; i++) {
|
||||||
for (size_t j = 0; j < N; j++) {
|
for (size_t j = 0; j < N; j++) {
|
||||||
m(i, j) = typeFunction::max(x(i, j), scalar_lower_bound);
|
m(i, j) = typeFunction::max(x(i, j), scalar_lower_bound);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
return m;
|
return m;
|
||||||
}
|
}
|
||||||
|
|
||||||
template<typename Type, size_t M, size_t N>
|
template<typename Type, size_t M, size_t N>
|
||||||
Matrix<Type, M, N> max(const Type scalar_lower_bound, const Matrix<Type, M, N> &x) {
|
Matrix<Type, M, N> max(const Type scalar_lower_bound, const Matrix<Type, M, N> &x)
|
||||||
|
{
|
||||||
return max(x, scalar_lower_bound);
|
return max(x, scalar_lower_bound);
|
||||||
}
|
}
|
||||||
|
|
||||||
template<typename Type, size_t M, size_t N>
|
template<typename Type, size_t M, size_t N>
|
||||||
Matrix<Type, M, N> max(const Matrix<Type, M, N> &x1, const Matrix<Type, M, N> &x2) {
|
Matrix<Type, M, N> max(const Matrix<Type, M, N> &x1, const Matrix<Type, M, N> &x2)
|
||||||
|
{
|
||||||
Matrix<Type, M, N> m;
|
Matrix<Type, M, N> m;
|
||||||
|
|
||||||
for (size_t i = 0; i < M; i++) {
|
for (size_t i = 0; i < M; i++) {
|
||||||
for (size_t j = 0; j < N; j++) {
|
for (size_t j = 0; j < N; j++) {
|
||||||
m(i, j) = typeFunction::max(x1(i, j), x2(i, j));
|
m(i, j) = typeFunction::max(x1(i, j), x2(i, j));
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
return m;
|
return m;
|
||||||
}
|
}
|
||||||
|
|
||||||
template<typename Type, size_t M, size_t N>
|
template<typename Type, size_t M, size_t N>
|
||||||
Matrix<Type, M, N> constrain(const Matrix<Type, M, N> &x,
|
Matrix<Type, M, N> constrain(const Matrix<Type, M, N> &x,
|
||||||
const Type scalar_lower_bound,
|
const Type scalar_lower_bound,
|
||||||
const Type scalar_upper_bound) {
|
const Type scalar_upper_bound)
|
||||||
|
{
|
||||||
Matrix<Type, M, N> m;
|
Matrix<Type, M, N> m;
|
||||||
|
|
||||||
if (scalar_lower_bound > scalar_upper_bound) {
|
if (scalar_lower_bound > scalar_upper_bound) {
|
||||||
m.setNaN();
|
m.setNaN();
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
for (size_t i = 0; i < M; i++) {
|
for (size_t i = 0; i < M; i++) {
|
||||||
for (size_t j = 0; j < N; j++) {
|
for (size_t j = 0; j < N; j++) {
|
||||||
@@ -726,19 +780,23 @@ Matrix<Type, M, N> constrain(const Matrix<Type, M, N> &x,
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
return m;
|
return m;
|
||||||
}
|
}
|
||||||
|
|
||||||
template<typename Type, size_t M, size_t N>
|
template<typename Type, size_t M, size_t N>
|
||||||
Matrix<Type, M, N> constrain(const Matrix<Type, M, N> &x,
|
Matrix<Type, M, N> constrain(const Matrix<Type, M, N> &x,
|
||||||
const Matrix<Type, M, N> &x_lower_bound,
|
const Matrix<Type, M, N> &x_lower_bound,
|
||||||
const Matrix<Type, M, N> &x_upper_bound) {
|
const Matrix<Type, M, N> &x_upper_bound)
|
||||||
|
{
|
||||||
Matrix<Type, M, N> m;
|
Matrix<Type, M, N> m;
|
||||||
|
|
||||||
for (size_t i = 0; i < M; i++) {
|
for (size_t i = 0; i < M; i++) {
|
||||||
for (size_t j = 0; j < N; j++) {
|
for (size_t j = 0; j < N; j++) {
|
||||||
m(i, j) = typeFunction::constrain(x(i, j), x_lower_bound(i, j), x_upper_bound(i, j));
|
m(i, j) = typeFunction::constrain(x(i, j), x_lower_bound(i, j), x_upper_bound(i, j));
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
return m;
|
return m;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -749,16 +807,17 @@ std::ostream& operator<<(std::ostream& os,
|
|||||||
{
|
{
|
||||||
for (size_t i = 0; i < M; ++i) {
|
for (size_t i = 0; i < M; ++i) {
|
||||||
os << "[";
|
os << "[";
|
||||||
|
|
||||||
for (size_t j = 0; j < N; ++j) {
|
for (size_t j = 0; j < N; ++j) {
|
||||||
os << std::setw(10) << matrix(i, j);
|
os << std::setw(10) << matrix(i, j);
|
||||||
os << "\t";
|
os << "\t";
|
||||||
}
|
}
|
||||||
|
|
||||||
os << "]" << std::endl;
|
os << "]" << std::endl;
|
||||||
}
|
}
|
||||||
|
|
||||||
return os;
|
return os;
|
||||||
}
|
}
|
||||||
#endif // defined(SUPPORT_STDIOSTREAM)
|
#endif // defined(SUPPORT_STDIOSTREAM)
|
||||||
|
|
||||||
} // namespace matrix
|
} // namespace matrix
|
||||||
|
|
||||||
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
|
|
||||||
|
|||||||
@@ -24,16 +24,19 @@ 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;
|
size_t rank;
|
||||||
|
|
||||||
if (M <= N) {
|
if (M <= N) {
|
||||||
SquareMatrix<Type, M> A = G * G.transpose();
|
SquareMatrix<Type, M> A = G * G.transpose();
|
||||||
SquareMatrix<Type, M> L = fullRankCholesky(A, rank);
|
SquareMatrix<Type, M> L = fullRankCholesky(A, rank);
|
||||||
|
|
||||||
A = L.transpose() * L;
|
A = L.transpose() * L;
|
||||||
SquareMatrix<Type, M> X;
|
SquareMatrix<Type, M> X;
|
||||||
|
|
||||||
if (!inv(A, X, rank)) {
|
if (!inv(A, X, rank)) {
|
||||||
res = Matrix<Type, N, M>();
|
res = Matrix<Type, N, M>();
|
||||||
return false; // LCOV_EXCL_LINE -- this can only be hit from numerical issues
|
return false; // LCOV_EXCL_LINE -- this can only be hit from numerical issues
|
||||||
}
|
}
|
||||||
|
|
||||||
// doing an intermediate assignment reduces stack usage
|
// doing an intermediate assignment reduces stack usage
|
||||||
A = X * X * L.transpose();
|
A = X * X * L.transpose();
|
||||||
res = G.transpose() * (L * A);
|
res = G.transpose() * (L * A);
|
||||||
@@ -44,14 +47,17 @@ bool geninv(const Matrix<Type, M, N> & G, Matrix<Type, N, M>& res)
|
|||||||
|
|
||||||
A = L.transpose() * L;
|
A = L.transpose() * L;
|
||||||
SquareMatrix<Type, N> X;
|
SquareMatrix<Type, N> X;
|
||||||
|
|
||||||
if (!inv(A, X, rank)) {
|
if (!inv(A, X, rank)) {
|
||||||
res = Matrix<Type, N, M>();
|
res = Matrix<Type, N, M>();
|
||||||
return false; // LCOV_EXCL_LINE -- this can only be hit from numerical issues
|
return false; // LCOV_EXCL_LINE -- this can only be hit from numerical issues
|
||||||
}
|
}
|
||||||
|
|
||||||
// doing an intermediate assignment reduces stack usage
|
// doing an intermediate assignment reduces stack usage
|
||||||
A = X * X * L.transpose();
|
A = X * X * L.transpose();
|
||||||
res = (L * A) * G.transpose();
|
res = (L * A) * G.transpose();
|
||||||
}
|
}
|
||||||
|
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -78,6 +84,7 @@ SquareMatrix<Type, N> fullRankCholesky(const SquareMatrix<Type, N> & A,
|
|||||||
Matrix<Type, N, N> L;
|
Matrix<Type, N, N> L;
|
||||||
|
|
||||||
size_t r = 0;
|
size_t r = 0;
|
||||||
|
|
||||||
for (size_t k = 0; k < N; k++) {
|
for (size_t k = 0; k < N; k++) {
|
||||||
|
|
||||||
if (r == 0) {
|
if (r == 0) {
|
||||||
@@ -89,12 +96,15 @@ SquareMatrix<Type, N> fullRankCholesky(const SquareMatrix<Type, N> & A,
|
|||||||
for (size_t i = k; i < N; i++) {
|
for (size_t i = k; i < N; i++) {
|
||||||
// Compute LL = L[k:n, :r] * L[k, :r].T
|
// Compute LL = L[k:n, :r] * L[k, :r].T
|
||||||
Type LL = Type();
|
Type LL = Type();
|
||||||
|
|
||||||
for (size_t j = 0; j < r; j++) {
|
for (size_t j = 0; j < r; j++) {
|
||||||
LL += L(i, j) * L(k, j);
|
LL += L(i, j) * L(k, j);
|
||||||
}
|
}
|
||||||
|
|
||||||
L(i, r) = A(i, k) - LL;
|
L(i, r) = A(i, k) - LL;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
if (L(k, r) > tol) {
|
if (L(k, r) > tol) {
|
||||||
L(k, r) = sqrt(L(k, r));
|
L(k, r) = sqrt(L(k, r));
|
||||||
|
|
||||||
@@ -115,5 +125,3 @@ SquareMatrix<Type, N> fullRankCholesky(const SquareMatrix<Type, N> & A,
|
|||||||
}
|
}
|
||||||
|
|
||||||
} // namespace matrix
|
} // namespace matrix
|
||||||
|
|
||||||
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
|
|
||||||
|
|||||||
@@ -100,6 +100,7 @@ public:
|
|||||||
{
|
{
|
||||||
Quaternion &q = *this;
|
Quaternion &q = *this;
|
||||||
Type t = R.trace();
|
Type t = R.trace();
|
||||||
|
|
||||||
if (t > Type(0)) {
|
if (t > Type(0)) {
|
||||||
t = sqrt(Type(1) + t);
|
t = sqrt(Type(1) + t);
|
||||||
q(0) = Type(0.5) * t;
|
q(0) = Type(0.5) * t;
|
||||||
@@ -107,6 +108,7 @@ public:
|
|||||||
q(1) = (R(2, 1) - R(1, 2)) * t;
|
q(1) = (R(2, 1) - R(1, 2)) * t;
|
||||||
q(2) = (R(0, 2) - R(2, 0)) * t;
|
q(2) = (R(0, 2) - R(2, 0)) * t;
|
||||||
q(3) = (R(1, 0) - R(0, 1)) * t;
|
q(3) = (R(1, 0) - R(0, 1)) * t;
|
||||||
|
|
||||||
} else if (R(0, 0) > R(1, 1) && R(0, 0) > R(2, 2)) {
|
} else if (R(0, 0) > R(1, 1) && R(0, 0) > R(2, 2)) {
|
||||||
t = sqrt(Type(1) + R(0, 0) - R(1, 1) - R(2, 2));
|
t = sqrt(Type(1) + R(0, 0) - R(1, 1) - R(2, 2));
|
||||||
q(1) = Type(0.5) * t;
|
q(1) = Type(0.5) * t;
|
||||||
@@ -114,6 +116,7 @@ public:
|
|||||||
q(0) = (R(2, 1) - R(1, 2)) * t;
|
q(0) = (R(2, 1) - R(1, 2)) * t;
|
||||||
q(2) = (R(1, 0) + R(0, 1)) * t;
|
q(2) = (R(1, 0) + R(0, 1)) * t;
|
||||||
q(3) = (R(0, 2) + R(2, 0)) * t;
|
q(3) = (R(0, 2) + R(2, 0)) * t;
|
||||||
|
|
||||||
} else if (R(1, 1) > R(2, 2)) {
|
} else if (R(1, 1) > R(2, 2)) {
|
||||||
t = sqrt(Type(1) - R(0, 0) + R(1, 1) - R(2, 2));
|
t = sqrt(Type(1) - R(0, 0) + R(1, 1) - R(2, 2));
|
||||||
q(2) = Type(0.5) * t;
|
q(2) = Type(0.5) * t;
|
||||||
@@ -121,6 +124,7 @@ public:
|
|||||||
q(0) = (R(0, 2) - R(2, 0)) * t;
|
q(0) = (R(0, 2) - R(2, 0)) * t;
|
||||||
q(1) = (R(1, 0) + R(0, 1)) * t;
|
q(1) = (R(1, 0) + R(0, 1)) * t;
|
||||||
q(3) = (R(2, 1) + R(1, 2)) * t;
|
q(3) = (R(2, 1) + R(1, 2)) * t;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
t = sqrt(Type(1) - R(0, 0) - R(1, 1) + R(2, 2));
|
t = sqrt(Type(1) - R(0, 0) - R(1, 1) + R(2, 2));
|
||||||
q(3) = Type(0.5) * t;
|
q(3) = Type(0.5) * t;
|
||||||
@@ -169,9 +173,11 @@ public:
|
|||||||
Quaternion &q = *this;
|
Quaternion &q = *this;
|
||||||
Type angle = aa.norm();
|
Type angle = aa.norm();
|
||||||
Vector<Type, 3> axis = aa.unit();
|
Vector<Type, 3> axis = aa.unit();
|
||||||
|
|
||||||
if (angle < Type(1e-10)) {
|
if (angle < Type(1e-10)) {
|
||||||
q(0) = Type(1);
|
q(0) = Type(1);
|
||||||
q(1) = q(2) = q(3) = 0;
|
q(1) = q(2) = q(3) = 0;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
Type magnitude = sin(angle / Type(2));
|
Type magnitude = sin(angle / Type(2));
|
||||||
q(0) = cos(angle / Type(2));
|
q(0) = cos(angle / Type(2));
|
||||||
@@ -194,30 +200,38 @@ public:
|
|||||||
Quaternion &q = *this;
|
Quaternion &q = *this;
|
||||||
Vector3<Type> cr = src.cross(dst);
|
Vector3<Type> cr = src.cross(dst);
|
||||||
const float dt = src.dot(dst);
|
const float dt = src.dot(dst);
|
||||||
|
|
||||||
if (cr.norm() < eps && dt < 0) {
|
if (cr.norm() < eps && dt < 0) {
|
||||||
// handle corner cases with 180 degree rotations
|
// handle corner cases with 180 degree rotations
|
||||||
// if the two vectors are parallel, cross product is zero
|
// if the two vectors are parallel, cross product is zero
|
||||||
// if they point opposite, the dot product is negative
|
// if they point opposite, the dot product is negative
|
||||||
cr = src.abs();
|
cr = src.abs();
|
||||||
|
|
||||||
if (cr(0) < cr(1)) {
|
if (cr(0) < cr(1)) {
|
||||||
if (cr(0) < cr(2)) {
|
if (cr(0) < cr(2)) {
|
||||||
cr = Vector3<Type>(1, 0, 0);
|
cr = Vector3<Type>(1, 0, 0);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
cr = Vector3<Type>(0, 0, 1);
|
cr = Vector3<Type>(0, 0, 1);
|
||||||
}
|
}
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
if (cr(1) < cr(2)) {
|
if (cr(1) < cr(2)) {
|
||||||
cr = Vector3<Type>(0, 1, 0);
|
cr = Vector3<Type>(0, 1, 0);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
cr = Vector3<Type>(0, 0, 1);
|
cr = Vector3<Type>(0, 0, 1);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
q(0) = Type(0);
|
q(0) = Type(0);
|
||||||
cr = src.cross(cr);
|
cr = src.cross(cr);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
// normal case, do half-way quaternion solution
|
// normal case, do half-way quaternion solution
|
||||||
q(0) = dt + sqrt(src.norm_squared() * dst.norm_squared());
|
q(0) = dt + sqrt(src.norm_squared() * dst.norm_squared());
|
||||||
}
|
}
|
||||||
|
|
||||||
q(1) = cr(0);
|
q(1) = cr(0);
|
||||||
q(2) = cr(1);
|
q(2) = cr(1);
|
||||||
q(3) = cr(2);
|
q(3) = cr(2);
|
||||||
@@ -366,10 +380,12 @@ public:
|
|||||||
// compute the first 4 terms of the Taylor serie
|
// compute the first 4 terms of the Taylor serie
|
||||||
sinc_u = Type(1.0) - u2 * c3 + u4 * c5 - u6 * c7;
|
sinc_u = Type(1.0) - u2 * c3 + u4 * c5 - u6 * c7;
|
||||||
cos_u = Type(1.0) - u2 * c2 + u4 * c4 - u6 * c6;
|
cos_u = Type(1.0) - u2 * c2 + u4 * c4 - u6 * c6;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
sinc_u = Type(sin(u_norm) / u_norm);
|
sinc_u = Type(sin(u_norm) / u_norm);
|
||||||
cos_u = Type(cos(u_norm));
|
cos_u = Type(cos(u_norm));
|
||||||
}
|
}
|
||||||
|
|
||||||
Vector<Type, 3> v = sinc_u * u;
|
Vector<Type, 3> v = sinc_u * u;
|
||||||
return Quaternion<Type> (cos_u, v(0), v(1), v(2));
|
return Quaternion<Type> (cos_u, v(0), v(1), v(2));
|
||||||
}
|
}
|
||||||
@@ -392,8 +408,10 @@ public:
|
|||||||
|
|
||||||
if (u_norm < tol) { // result smaller than O(||.||^3)
|
if (u_norm < tol) { // result smaller than O(||.||^3)
|
||||||
return Type(0.5) * (Dcm<Type>() + u_hat + (Type(1.0 / 3.0) + u_norm * u_norm / Type(45.0)) * u_hat * u_hat);
|
return Type(0.5) * (Dcm<Type>() + u_hat + (Type(1.0 / 3.0) + u_norm * u_norm / Type(45.0)) * u_hat * u_hat);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
return Type(0.5) * (Dcm<Type>() + u_hat + (Type(1.0) - u_norm * Type(cos(u_norm) / sin(u_norm))) / (u_norm * u_norm) * u_hat * u_hat);
|
return Type(0.5) * (Dcm<Type>() + u_hat + (Type(1.0) - u_norm * Type(cos(u_norm) / sin(u_norm))) /
|
||||||
|
(u_norm * u_norm) * u_hat * u_hat);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -443,6 +461,7 @@ public:
|
|||||||
return q * Type(matrix::sign(q(i)));
|
return q * Type(matrix::sign(q(i)));
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
return q;
|
return q;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -466,7 +485,8 @@ public:
|
|||||||
* @param vec vector to rotate in frame 1 (typically body frame)
|
* @param vec vector to rotate in frame 1 (typically body frame)
|
||||||
* @return rotated vector in frame 2 (typically reference frame)
|
* @return rotated vector in frame 2 (typically reference frame)
|
||||||
*/
|
*/
|
||||||
Vector3<Type> conjugate(const Vector3<Type> &vec) const {
|
Vector3<Type> conjugate(const Vector3<Type> &vec) const
|
||||||
|
{
|
||||||
const Quaternion &q = *this;
|
const Quaternion &q = *this;
|
||||||
Quaternion v(Type(0), vec(0), vec(1), vec(2));
|
Quaternion v(Type(0), vec(0), vec(1), vec(2));
|
||||||
Quaternion res = q * v * q.inversed();
|
Quaternion res = q * v * q.inversed();
|
||||||
@@ -528,5 +548,3 @@ using Quatd = Quaternion<double>;
|
|||||||
using Quaterniond = Quaternion<double>;
|
using Quaterniond = Quaternion<double>;
|
||||||
|
|
||||||
} // namespace matrix
|
} // namespace matrix
|
||||||
|
|
||||||
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
|
|
||||||
|
|||||||
@@ -33,13 +33,15 @@ public:
|
|||||||
return _value;
|
return _value;
|
||||||
}
|
}
|
||||||
|
|
||||||
operator Matrix<Type, 1, 1>() const {
|
operator Matrix<Type, 1, 1>() const
|
||||||
|
{
|
||||||
Matrix<Type, 1, 1> m;
|
Matrix<Type, 1, 1> m;
|
||||||
m(0, 0) = _value;
|
m(0, 0) = _value;
|
||||||
return m;
|
return m;
|
||||||
}
|
}
|
||||||
|
|
||||||
operator Vector<Type, 1>() const {
|
operator Vector<Type, 1>() const
|
||||||
|
{
|
||||||
Vector<Type, 1> m;
|
Vector<Type, 1> m;
|
||||||
m(0) = _value;
|
m(0) = _value;
|
||||||
return m;
|
return m;
|
||||||
@@ -54,5 +56,3 @@ using Scalarf = Scalar<float>;
|
|||||||
using Scalard = Scalar<double>;
|
using Scalard = Scalar<double>;
|
||||||
|
|
||||||
} // namespace matrix
|
} // namespace matrix
|
||||||
|
|
||||||
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
|
|
||||||
|
|||||||
@@ -11,7 +11,8 @@
|
|||||||
#include "math.hpp"
|
#include "math.hpp"
|
||||||
|
|
||||||
|
|
||||||
namespace matrix {
|
namespace matrix
|
||||||
|
{
|
||||||
|
|
||||||
template<typename Type, size_t M, size_t N>
|
template<typename Type, size_t M, size_t N>
|
||||||
class Matrix;
|
class Matrix;
|
||||||
@@ -20,12 +21,14 @@ template<typename Type, size_t M>
|
|||||||
class Vector;
|
class Vector;
|
||||||
|
|
||||||
template <typename Type, size_t P, size_t Q, size_t M, size_t N>
|
template <typename Type, size_t P, size_t Q, size_t M, size_t N>
|
||||||
class Slice {
|
class Slice
|
||||||
|
{
|
||||||
public:
|
public:
|
||||||
Slice(size_t x0, size_t y0, const Matrix<Type, M, N> *data) :
|
Slice(size_t x0, size_t y0, const Matrix<Type, M, N> *data) :
|
||||||
_x0(x0),
|
_x0(x0),
|
||||||
_y0(y0),
|
_y0(y0),
|
||||||
_data(const_cast<Matrix<Type, M, N>*>(data)) {
|
_data(const_cast<Matrix<Type, M, N>*>(data))
|
||||||
|
{
|
||||||
static_assert(P <= M, "Slice rows bigger than backing matrix");
|
static_assert(P <= M, "Slice rows bigger than backing matrix");
|
||||||
static_assert(Q <= N, "Slice cols bigger than backing matrix");
|
static_assert(Q <= N, "Slice cols bigger than backing matrix");
|
||||||
assert(x0 + P <= M);
|
assert(x0 + P <= M);
|
||||||
@@ -53,33 +56,39 @@ public:
|
|||||||
Slice<Type, P, Q, M, N> &operator=(const Slice<Type, P, Q, MM, NN> &other)
|
Slice<Type, P, Q, M, N> &operator=(const Slice<Type, P, Q, MM, NN> &other)
|
||||||
{
|
{
|
||||||
Slice<Type, P, Q, M, N> &self = *this;
|
Slice<Type, P, Q, M, N> &self = *this;
|
||||||
|
|
||||||
for (size_t i = 0; i < P; i++) {
|
for (size_t i = 0; i < P; i++) {
|
||||||
for (size_t j = 0; j < Q; j++) {
|
for (size_t j = 0; j < Q; j++) {
|
||||||
self(i, j) = other(i, j);
|
self(i, j) = other(i, j);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
return self;
|
return self;
|
||||||
}
|
}
|
||||||
|
|
||||||
Slice<Type, P, Q, M, N> &operator=(const Matrix<Type, P, Q> &other)
|
Slice<Type, P, Q, M, N> &operator=(const Matrix<Type, P, Q> &other)
|
||||||
{
|
{
|
||||||
Slice<Type, P, Q, M, N> &self = *this;
|
Slice<Type, P, Q, M, N> &self = *this;
|
||||||
|
|
||||||
for (size_t i = 0; i < P; i++) {
|
for (size_t i = 0; i < P; i++) {
|
||||||
for (size_t j = 0; j < Q; j++) {
|
for (size_t j = 0; j < Q; j++) {
|
||||||
self(i, j) = other(i, j);
|
self(i, j) = other(i, j);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
return self;
|
return self;
|
||||||
}
|
}
|
||||||
|
|
||||||
Slice<Type, P, Q, M, N> &operator=(const Type &other)
|
Slice<Type, P, Q, M, N> &operator=(const Type &other)
|
||||||
{
|
{
|
||||||
Slice<Type, P, Q, M, N> &self = *this;
|
Slice<Type, P, Q, M, N> &self = *this;
|
||||||
|
|
||||||
for (size_t i = 0; i < P; i++) {
|
for (size_t i = 0; i < P; i++) {
|
||||||
for (size_t j = 0; j < Q; j++) {
|
for (size_t j = 0; j < Q; j++) {
|
||||||
self(i, j) = other;
|
self(i, j) = other;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
return self;
|
return self;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -88,9 +97,11 @@ public:
|
|||||||
Slice<Type, 1, Q, M, N> &operator=(const Vector<Type, Q> &other)
|
Slice<Type, 1, Q, M, N> &operator=(const Vector<Type, Q> &other)
|
||||||
{
|
{
|
||||||
Slice<Type, 1, Q, M, N> &self = *this;
|
Slice<Type, 1, Q, M, N> &self = *this;
|
||||||
|
|
||||||
for (size_t j = 0; j < Q; j++) {
|
for (size_t j = 0; j < Q; j++) {
|
||||||
self(0, j) = other(j);
|
self(0, j) = other(j);
|
||||||
}
|
}
|
||||||
|
|
||||||
return self;
|
return self;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -98,33 +109,39 @@ public:
|
|||||||
Slice<Type, P, Q, M, N> &operator+=(const Slice<Type, P, Q, MM, NN> &other)
|
Slice<Type, P, Q, M, N> &operator+=(const Slice<Type, P, Q, MM, NN> &other)
|
||||||
{
|
{
|
||||||
Slice<Type, P, Q, M, N> &self = *this;
|
Slice<Type, P, Q, M, N> &self = *this;
|
||||||
|
|
||||||
for (size_t i = 0; i < P; i++) {
|
for (size_t i = 0; i < P; i++) {
|
||||||
for (size_t j = 0; j < Q; j++) {
|
for (size_t j = 0; j < Q; j++) {
|
||||||
self(i, j) += other(i, j);
|
self(i, j) += other(i, j);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
return self;
|
return self;
|
||||||
}
|
}
|
||||||
|
|
||||||
Slice<Type, P, Q, M, N> &operator+=(const Matrix<Type, P, Q> &other)
|
Slice<Type, P, Q, M, N> &operator+=(const Matrix<Type, P, Q> &other)
|
||||||
{
|
{
|
||||||
Slice<Type, P, Q, M, N> &self = *this;
|
Slice<Type, P, Q, M, N> &self = *this;
|
||||||
|
|
||||||
for (size_t i = 0; i < P; i++) {
|
for (size_t i = 0; i < P; i++) {
|
||||||
for (size_t j = 0; j < Q; j++) {
|
for (size_t j = 0; j < Q; j++) {
|
||||||
self(i, j) += other(i, j);
|
self(i, j) += other(i, j);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
return self;
|
return self;
|
||||||
}
|
}
|
||||||
|
|
||||||
Slice<Type, P, Q, M, N> &operator+=(const Type &other)
|
Slice<Type, P, Q, M, N> &operator+=(const Type &other)
|
||||||
{
|
{
|
||||||
Slice<Type, P, Q, M, N> &self = *this;
|
Slice<Type, P, Q, M, N> &self = *this;
|
||||||
|
|
||||||
for (size_t i = 0; i < P; i++) {
|
for (size_t i = 0; i < P; i++) {
|
||||||
for (size_t j = 0; j < Q; j++) {
|
for (size_t j = 0; j < Q; j++) {
|
||||||
self(i, j) += other;
|
self(i, j) += other;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
return self;
|
return self;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -132,44 +149,52 @@ public:
|
|||||||
Slice<Type, P, Q, M, N> &operator-=(const Slice<Type, P, Q, MM, NN> &other)
|
Slice<Type, P, Q, M, N> &operator-=(const Slice<Type, P, Q, MM, NN> &other)
|
||||||
{
|
{
|
||||||
Slice<Type, P, Q, M, N> &self = *this;
|
Slice<Type, P, Q, M, N> &self = *this;
|
||||||
|
|
||||||
for (size_t i = 0; i < P; i++) {
|
for (size_t i = 0; i < P; i++) {
|
||||||
for (size_t j = 0; j < Q; j++) {
|
for (size_t j = 0; j < Q; j++) {
|
||||||
self(i, j) -= other(i, j);
|
self(i, j) -= other(i, j);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
return self;
|
return self;
|
||||||
}
|
}
|
||||||
|
|
||||||
Slice<Type, P, Q, M, N> &operator-=(const Matrix<Type, P, Q> &other)
|
Slice<Type, P, Q, M, N> &operator-=(const Matrix<Type, P, Q> &other)
|
||||||
{
|
{
|
||||||
Slice<Type, P, Q, M, N> &self = *this;
|
Slice<Type, P, Q, M, N> &self = *this;
|
||||||
|
|
||||||
for (size_t i = 0; i < P; i++) {
|
for (size_t i = 0; i < P; i++) {
|
||||||
for (size_t j = 0; j < Q; j++) {
|
for (size_t j = 0; j < Q; j++) {
|
||||||
self(i, j) -= other(i, j);
|
self(i, j) -= other(i, j);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
return self;
|
return self;
|
||||||
}
|
}
|
||||||
|
|
||||||
Slice<Type, P, Q, M, N> &operator-=(const Type &other)
|
Slice<Type, P, Q, M, N> &operator-=(const Type &other)
|
||||||
{
|
{
|
||||||
Slice<Type, P, Q, M, N> &self = *this;
|
Slice<Type, P, Q, M, N> &self = *this;
|
||||||
|
|
||||||
for (size_t i = 0; i < P; i++) {
|
for (size_t i = 0; i < P; i++) {
|
||||||
for (size_t j = 0; j < Q; j++) {
|
for (size_t j = 0; j < Q; j++) {
|
||||||
self(i, j) -= other;
|
self(i, j) -= other;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
return self;
|
return self;
|
||||||
}
|
}
|
||||||
|
|
||||||
Slice<Type, P, Q, M, N> &operator*=(const Type &other)
|
Slice<Type, P, Q, M, N> &operator*=(const Type &other)
|
||||||
{
|
{
|
||||||
Slice<Type, P, Q, M, N> &self = *this;
|
Slice<Type, P, Q, M, N> &self = *this;
|
||||||
|
|
||||||
for (size_t i = 0; i < P; i++) {
|
for (size_t i = 0; i < P; i++) {
|
||||||
for (size_t j = 0; j < Q; j++) {
|
for (size_t j = 0; j < Q; j++) {
|
||||||
self(i, j) *= other;
|
self(i, j) *= other;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
return self;
|
return self;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -182,11 +207,13 @@ public:
|
|||||||
{
|
{
|
||||||
const Slice<Type, P, Q, M, N> &self = *this;
|
const Slice<Type, P, Q, M, N> &self = *this;
|
||||||
Matrix<Type, P, Q> res;
|
Matrix<Type, P, Q> res;
|
||||||
|
|
||||||
for (size_t i = 0; i < P; i++) {
|
for (size_t i = 0; i < P; i++) {
|
||||||
for (size_t j = 0; j < Q; j++) {
|
for (size_t j = 0; j < Q; j++) {
|
||||||
res(i, j) = self(i, j) * other;
|
res(i, j) = self(i, j) * other;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
return res;
|
return res;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -234,9 +261,11 @@ public:
|
|||||||
{
|
{
|
||||||
const Slice<Type, P, Q, M, N> &self = *this;
|
const Slice<Type, P, Q, M, N> &self = *this;
|
||||||
Vector < Type, P < Q ? P : Q > res;
|
Vector < Type, P < Q ? P : Q > res;
|
||||||
|
|
||||||
for (size_t j = 0; j < (P < Q ? P : Q); j++) {
|
for (size_t j = 0; j < (P < Q ? P : Q); j++) {
|
||||||
res(j) = self(j, j);
|
res(j) = self(j, j);
|
||||||
}
|
}
|
||||||
|
|
||||||
return res;
|
return res;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -244,11 +273,13 @@ public:
|
|||||||
{
|
{
|
||||||
const Slice<Type, P, Q, M, N> &self = *this;
|
const Slice<Type, P, Q, M, N> &self = *this;
|
||||||
Type accum(0);
|
Type accum(0);
|
||||||
|
|
||||||
for (size_t i = 0; i < P; i++) {
|
for (size_t i = 0; i < P; i++) {
|
||||||
for (size_t j = 0; j < Q; j++) {
|
for (size_t j = 0; j < Q; j++) {
|
||||||
accum += self(i, j) * self(i, j);
|
accum += self(i, j) * self(i, j);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
return accum;
|
return accum;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -12,7 +12,8 @@
|
|||||||
|
|
||||||
#include "math.hpp"
|
#include "math.hpp"
|
||||||
|
|
||||||
namespace matrix {
|
namespace matrix
|
||||||
|
{
|
||||||
template<int N> struct force_constexpr_eval {
|
template<int N> struct force_constexpr_eval {
|
||||||
static const int value = N;
|
static const int value = N;
|
||||||
};
|
};
|
||||||
@@ -20,12 +21,14 @@ template<int N> struct force_constexpr_eval {
|
|||||||
// Vector that only store nonzero elements,
|
// Vector that only store nonzero elements,
|
||||||
// which indices are specified as parameter pack
|
// which indices are specified as parameter pack
|
||||||
template<typename Type, size_t M, size_t... Idxs>
|
template<typename Type, size_t M, size_t... Idxs>
|
||||||
class SparseVector {
|
class SparseVector
|
||||||
|
{
|
||||||
private:
|
private:
|
||||||
static constexpr size_t N = sizeof...(Idxs);
|
static constexpr size_t N = sizeof...(Idxs);
|
||||||
static constexpr size_t _indices[N] {Idxs...};
|
static constexpr size_t _indices[N] {Idxs...};
|
||||||
|
|
||||||
static constexpr bool duplicateIndices() {
|
static constexpr bool duplicateIndices()
|
||||||
|
{
|
||||||
for (size_t i = 0; i < N; i++) {
|
for (size_t i = 0; i < N; i++) {
|
||||||
for (size_t j = 0; j < i; j++) {
|
for (size_t j = 0; j < i; j++) {
|
||||||
if (_indices[i] == _indices[j]) {
|
if (_indices[i] == _indices[j]) {
|
||||||
@@ -33,15 +36,19 @@ private:
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
static constexpr size_t findMaxIndex() {
|
static constexpr size_t findMaxIndex()
|
||||||
|
{
|
||||||
size_t maxIndex = 0;
|
size_t maxIndex = 0;
|
||||||
|
|
||||||
for (size_t i = 0; i < N; i++) {
|
for (size_t i = 0; i < N; i++) {
|
||||||
if (maxIndex < _indices[i]) {
|
if (maxIndex < _indices[i]) {
|
||||||
maxIndex = _indices[i];
|
maxIndex = _indices[i];
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
return maxIndex;
|
return maxIndex;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -52,96 +59,118 @@ private:
|
|||||||
|
|
||||||
Type _data[N] {};
|
Type _data[N] {};
|
||||||
|
|
||||||
static constexpr int findCompressedIndex(size_t index) {
|
static constexpr int findCompressedIndex(size_t index)
|
||||||
|
{
|
||||||
int compressedIndex = -1;
|
int compressedIndex = -1;
|
||||||
|
|
||||||
for (size_t i = 0; i < N; i++) {
|
for (size_t i = 0; i < N; i++) {
|
||||||
if (index == _indices[i]) {
|
if (index == _indices[i]) {
|
||||||
compressedIndex = static_cast<int>(i);
|
compressedIndex = static_cast<int>(i);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
return compressedIndex;
|
return compressedIndex;
|
||||||
}
|
}
|
||||||
|
|
||||||
public:
|
public:
|
||||||
constexpr size_t non_zeros() const {
|
constexpr size_t non_zeros() const
|
||||||
|
{
|
||||||
return N;
|
return N;
|
||||||
}
|
}
|
||||||
|
|
||||||
constexpr size_t index(size_t i) const {
|
constexpr size_t index(size_t i) const
|
||||||
|
{
|
||||||
return SparseVector::_indices[i];
|
return SparseVector::_indices[i];
|
||||||
}
|
}
|
||||||
|
|
||||||
SparseVector() = default;
|
SparseVector() = default;
|
||||||
|
|
||||||
SparseVector(const matrix::Vector<Type, M>& data) {
|
SparseVector(const matrix::Vector<Type, M> &data)
|
||||||
|
{
|
||||||
for (size_t i = 0; i < N; i++) {
|
for (size_t i = 0; i < N; i++) {
|
||||||
_data[i] = data(_indices[i]);
|
_data[i] = data(_indices[i]);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
explicit SparseVector(const Type data[N]) {
|
explicit SparseVector(const Type data[N])
|
||||||
|
{
|
||||||
memcpy(_data, data, sizeof(_data));
|
memcpy(_data, data, sizeof(_data));
|
||||||
}
|
}
|
||||||
|
|
||||||
template <size_t i>
|
template <size_t i>
|
||||||
inline Type at() const {
|
inline Type at() const
|
||||||
|
{
|
||||||
static constexpr int compressed_index = force_constexpr_eval<findCompressedIndex(i)>::value;
|
static constexpr int compressed_index = force_constexpr_eval<findCompressedIndex(i)>::value;
|
||||||
static_assert(compressed_index >= 0, "cannot access unpopulated indices");
|
static_assert(compressed_index >= 0, "cannot access unpopulated indices");
|
||||||
return _data[compressed_index];
|
return _data[compressed_index];
|
||||||
}
|
}
|
||||||
|
|
||||||
template <size_t i>
|
template <size_t i>
|
||||||
inline Type& at() {
|
inline Type &at()
|
||||||
|
{
|
||||||
static constexpr int compressed_index = force_constexpr_eval<findCompressedIndex(i)>::value;
|
static constexpr int compressed_index = force_constexpr_eval<findCompressedIndex(i)>::value;
|
||||||
static_assert(compressed_index >= 0, "cannot access unpopulated indices");
|
static_assert(compressed_index >= 0, "cannot access unpopulated indices");
|
||||||
return _data[compressed_index];
|
return _data[compressed_index];
|
||||||
}
|
}
|
||||||
|
|
||||||
inline Type atCompressedIndex(size_t i) const {
|
inline Type atCompressedIndex(size_t i) const
|
||||||
|
{
|
||||||
assert(i < N);
|
assert(i < N);
|
||||||
return _data[i];
|
return _data[i];
|
||||||
}
|
}
|
||||||
|
|
||||||
inline Type& atCompressedIndex(size_t i) {
|
inline Type &atCompressedIndex(size_t i)
|
||||||
|
{
|
||||||
assert(i < N);
|
assert(i < N);
|
||||||
return _data[i];
|
return _data[i];
|
||||||
}
|
}
|
||||||
|
|
||||||
void setZero() {
|
void setZero()
|
||||||
|
{
|
||||||
for (size_t i = 0; i < N; i++) {
|
for (size_t i = 0; i < N; i++) {
|
||||||
_data[i] = Type(0);
|
_data[i] = Type(0);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
Type dot(const matrix::Vector<Type, M>& other) const {
|
Type dot(const matrix::Vector<Type, M> &other) const
|
||||||
|
{
|
||||||
Type accum(0);
|
Type accum(0);
|
||||||
|
|
||||||
for (size_t i = 0; i < N; i++) {
|
for (size_t i = 0; i < N; i++) {
|
||||||
accum += _data[i] * other(_indices[i]);
|
accum += _data[i] * other(_indices[i]);
|
||||||
}
|
}
|
||||||
|
|
||||||
return accum;
|
return accum;
|
||||||
}
|
}
|
||||||
|
|
||||||
matrix::Vector<Type, M> operator+(const matrix::Vector<Type, M>& other) const {
|
matrix::Vector<Type, M> operator+(const matrix::Vector<Type, M> &other) const
|
||||||
|
{
|
||||||
matrix::Vector<Type, M> vec = other;
|
matrix::Vector<Type, M> vec = other;
|
||||||
|
|
||||||
for (size_t i = 0; i < N; i++) {
|
for (size_t i = 0; i < N; i++) {
|
||||||
vec(_indices[i]) += _data[i];
|
vec(_indices[i]) += _data[i];
|
||||||
}
|
}
|
||||||
|
|
||||||
return vec;
|
return vec;
|
||||||
}
|
}
|
||||||
|
|
||||||
SparseVector& operator+=(Type t) {
|
SparseVector &operator+=(Type t)
|
||||||
|
{
|
||||||
for (size_t i = 0; i < N; i++) {
|
for (size_t i = 0; i < N; i++) {
|
||||||
_data[i] += t;
|
_data[i] += t;
|
||||||
}
|
}
|
||||||
|
|
||||||
return *this;
|
return *this;
|
||||||
}
|
}
|
||||||
|
|
||||||
Type norm_squared() const
|
Type norm_squared() const
|
||||||
{
|
{
|
||||||
Type accum(0);
|
Type accum(0);
|
||||||
|
|
||||||
for (size_t i = 0; i < N; i++) {
|
for (size_t i = 0; i < N; i++) {
|
||||||
accum += _data[i] * _data[i];
|
accum += _data[i] * _data[i];
|
||||||
}
|
}
|
||||||
|
|
||||||
return accum;
|
return accum;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -157,26 +186,35 @@ public:
|
|||||||
};
|
};
|
||||||
|
|
||||||
template<typename Type, size_t Q, size_t M, size_t ... Idxs>
|
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> operator*(const matrix::Matrix<Type, Q, M> &mat,
|
||||||
|
const matrix::SparseVector<Type, M, Idxs...> &vec)
|
||||||
|
{
|
||||||
matrix::Vector<Type, Q> res;
|
matrix::Vector<Type, Q> res;
|
||||||
|
|
||||||
for (size_t i = 0; i < Q; i++) {
|
for (size_t i = 0; i < Q; i++) {
|
||||||
const Vector<Type, M> row = mat.row(i);
|
const Vector<Type, M> row = mat.row(i);
|
||||||
res(i) = vec.dot(row);
|
res(i) = vec.dot(row);
|
||||||
}
|
}
|
||||||
|
|
||||||
return res;
|
return res;
|
||||||
}
|
}
|
||||||
|
|
||||||
// returns x.T * A * x
|
// returns x.T * A * x
|
||||||
template<typename Type, size_t M, size_t ... Idxs>
|
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 quadraticForm(const matrix::SquareMatrix<Type, M> &A, const matrix::SparseVector<Type, M, Idxs...> &x)
|
||||||
|
{
|
||||||
Type res = Type(0);
|
Type res = Type(0);
|
||||||
|
|
||||||
for (size_t i = 0; i < x.non_zeros(); i++) {
|
for (size_t i = 0; i < x.non_zeros(); i++) {
|
||||||
Type tmp = Type(0);
|
Type tmp = Type(0);
|
||||||
|
|
||||||
for (size_t j = 0; j < x.non_zeros(); j++) {
|
for (size_t j = 0; j < x.non_zeros(); j++) {
|
||||||
tmp += A(x.index(i), x.index(j)) * x.atCompressedIndex(j);
|
tmp += A(x.index(i), x.index(j)) * x.atCompressedIndex(j);
|
||||||
}
|
}
|
||||||
|
|
||||||
res += x.atCompressedIndex(i) * tmp;
|
res += x.atCompressedIndex(i) * tmp;
|
||||||
}
|
}
|
||||||
|
|
||||||
return res;
|
return res;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -77,8 +77,10 @@ public:
|
|||||||
inline SquareMatrix<Type, M> I() const
|
inline SquareMatrix<Type, M> I() const
|
||||||
{
|
{
|
||||||
SquareMatrix<Type, M> i;
|
SquareMatrix<Type, M> i;
|
||||||
|
|
||||||
if (inv(*this, i)) {
|
if (inv(*this, i)) {
|
||||||
return i;
|
return i;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
i.setZero();
|
i.setZero();
|
||||||
return i;
|
return i;
|
||||||
@@ -100,6 +102,7 @@ public:
|
|||||||
for (size_t i = 0; i < M; i++) {
|
for (size_t i = 0; i < M; i++) {
|
||||||
res(i) = self(i, i);
|
res(i) = self(i, i);
|
||||||
}
|
}
|
||||||
|
|
||||||
return res;
|
return res;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -110,6 +113,7 @@ public:
|
|||||||
const SquareMatrix<Type, M> &self = *this;
|
const SquareMatrix<Type, M> &self = *this;
|
||||||
|
|
||||||
unsigned idx = 0;
|
unsigned idx = 0;
|
||||||
|
|
||||||
for (size_t x = 0; x < M; x++) {
|
for (size_t x = 0; x < M; x++) {
|
||||||
for (size_t y = x; y < M; y++) {
|
for (size_t y = x; y < M; y++) {
|
||||||
res(idx) = self(x, y);
|
res(idx) = self(x, y);
|
||||||
@@ -128,6 +132,7 @@ public:
|
|||||||
for (size_t i = 0; i < M; i++) {
|
for (size_t i = 0; i < M; i++) {
|
||||||
res += self(i, i);
|
res += self(i, i);
|
||||||
}
|
}
|
||||||
|
|
||||||
return res;
|
return res;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -156,6 +161,7 @@ public:
|
|||||||
|
|
||||||
// set diagonals
|
// set diagonals
|
||||||
unsigned vec_idx = 0;
|
unsigned vec_idx = 0;
|
||||||
|
|
||||||
for (size_t idx = first; idx < first + Width; idx++) {
|
for (size_t idx = first; idx < first + Width; idx++) {
|
||||||
self(idx, idx) = vec(vec_idx);
|
self(idx, idx) = vec(vec_idx);
|
||||||
vec_idx ++;
|
vec_idx ++;
|
||||||
@@ -187,6 +193,7 @@ public:
|
|||||||
assert(first + Width <= M);
|
assert(first + Width <= M);
|
||||||
|
|
||||||
SquareMatrix<Type, M> &self = *this;
|
SquareMatrix<Type, M> &self = *this;
|
||||||
|
|
||||||
if (Width > 1) {
|
if (Width > 1) {
|
||||||
for (size_t row_idx = first + 1; row_idx < first + Width; row_idx++) {
|
for (size_t row_idx = first + 1; row_idx < first + Width; row_idx++) {
|
||||||
for (size_t col_idx = first; col_idx < row_idx; col_idx++) {
|
for (size_t col_idx = first; col_idx < row_idx; col_idx++) {
|
||||||
@@ -207,12 +214,14 @@ public:
|
|||||||
|
|
||||||
SquareMatrix<Type, M> &self = *this;
|
SquareMatrix<Type, M> &self = *this;
|
||||||
self.makeBlockSymmetric<Width>(first);
|
self.makeBlockSymmetric<Width>(first);
|
||||||
|
|
||||||
for (size_t row_idx = first; row_idx < first + Width; row_idx++) {
|
for (size_t row_idx = first; row_idx < first + Width; row_idx++) {
|
||||||
for (size_t col_idx = 0; col_idx < first; col_idx++) {
|
for (size_t col_idx = 0; col_idx < first; col_idx++) {
|
||||||
Type tmp = (self(row_idx, col_idx) + self(col_idx, row_idx)) / Type(2);
|
Type tmp = (self(row_idx, col_idx) + self(col_idx, row_idx)) / Type(2);
|
||||||
self(row_idx, col_idx) = tmp;
|
self(row_idx, col_idx) = tmp;
|
||||||
self(col_idx, row_idx) = tmp;
|
self(col_idx, row_idx) = tmp;
|
||||||
}
|
}
|
||||||
|
|
||||||
for (size_t col_idx = first + Width; col_idx < M; col_idx++) {
|
for (size_t col_idx = first + Width; col_idx < M; col_idx++) {
|
||||||
Type tmp = (self(row_idx, col_idx) + self(col_idx, row_idx)) / Type(2);
|
Type tmp = (self(row_idx, col_idx) + self(col_idx, row_idx)) / Type(2);
|
||||||
self(row_idx, col_idx) = tmp;
|
self(row_idx, col_idx) = tmp;
|
||||||
@@ -229,6 +238,7 @@ public:
|
|||||||
assert(first + Width <= M);
|
assert(first + Width <= M);
|
||||||
|
|
||||||
SquareMatrix<Type, M> &self = *this;
|
SquareMatrix<Type, M> &self = *this;
|
||||||
|
|
||||||
if (Width > 1) {
|
if (Width > 1) {
|
||||||
for (size_t row_idx = first + 1; row_idx < first + Width; row_idx++) {
|
for (size_t row_idx = first + 1; row_idx < first + Width; row_idx++) {
|
||||||
for (size_t col_idx = first; col_idx < row_idx; col_idx++) {
|
for (size_t col_idx = first; col_idx < row_idx; col_idx++) {
|
||||||
@@ -238,6 +248,7 @@ public:
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -249,18 +260,21 @@ public:
|
|||||||
assert(first + Width <= M);
|
assert(first + Width <= M);
|
||||||
|
|
||||||
SquareMatrix<Type, M> &self = *this;
|
SquareMatrix<Type, M> &self = *this;
|
||||||
|
|
||||||
for (size_t row_idx = first; row_idx < first + Width; row_idx++) {
|
for (size_t row_idx = first; row_idx < first + Width; row_idx++) {
|
||||||
for (size_t col_idx = 0; col_idx < first; col_idx++) {
|
for (size_t col_idx = 0; col_idx < first; col_idx++) {
|
||||||
if (!isEqualF(self(row_idx, col_idx), self(col_idx, row_idx), eps)) {
|
if (!isEqualF(self(row_idx, col_idx), self(col_idx, row_idx), eps)) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
for (size_t col_idx = first + Width; col_idx < M; col_idx++) {
|
for (size_t col_idx = first + Width; col_idx < M; col_idx++) {
|
||||||
if (!isEqualF(self(row_idx, col_idx), self(col_idx, row_idx), eps)) {
|
if (!isEqualF(self(row_idx, col_idx), self(col_idx, row_idx), eps)) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
return self.isBlockSymmetric<Width>(first, eps);
|
return self.isBlockSymmetric<Width>(first, eps);
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -270,18 +284,22 @@ using SquareMatrix3f = SquareMatrix<float, 3>;
|
|||||||
using SquareMatrix3d = SquareMatrix<double, 3>;
|
using SquareMatrix3d = SquareMatrix<double, 3>;
|
||||||
|
|
||||||
template<typename Type, size_t M>
|
template<typename Type, size_t M>
|
||||||
SquareMatrix<Type, M> eye() {
|
SquareMatrix<Type, M> eye()
|
||||||
|
{
|
||||||
SquareMatrix<Type, M> m;
|
SquareMatrix<Type, M> m;
|
||||||
m.setIdentity();
|
m.setIdentity();
|
||||||
return m;
|
return m;
|
||||||
}
|
}
|
||||||
|
|
||||||
template<typename Type, size_t M>
|
template<typename Type, size_t M>
|
||||||
SquareMatrix<Type, M> diag(Vector<Type, M> d) {
|
SquareMatrix<Type, M> diag(Vector<Type, M> d)
|
||||||
|
{
|
||||||
SquareMatrix<Type, M> m;
|
SquareMatrix<Type, M> m;
|
||||||
|
|
||||||
for (size_t i = 0; i < M; i++) {
|
for (size_t i = 0; i < M; i++) {
|
||||||
m(i, i) = d(i);
|
m(i, i) = d(i);
|
||||||
}
|
}
|
||||||
|
|
||||||
return m;
|
return m;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -292,6 +310,7 @@ SquareMatrix<Type, M> expm(const Matrix<Type, M, M> & A, size_t order=5)
|
|||||||
SquareMatrix<Type, M> A_pow = A;
|
SquareMatrix<Type, M> A_pow = A;
|
||||||
res.setIdentity();
|
res.setIdentity();
|
||||||
size_t i_factorial = 1;
|
size_t i_factorial = 1;
|
||||||
|
|
||||||
for (size_t i = 1; i <= order; i++) {
|
for (size_t i = 1; i <= order; i++) {
|
||||||
i_factorial *= i;
|
i_factorial *= i;
|
||||||
res += A_pow / Type(i_factorial);
|
res += A_pow / Type(i_factorial);
|
||||||
@@ -424,6 +443,7 @@ bool inv(const SquareMatrix<Type, M> & A, SquareMatrix<Type, M> & inv, size_t ra
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
//printf("X:\n"); X.print();
|
//printf("X:\n"); X.print();
|
||||||
inv = P;
|
inv = P;
|
||||||
return true;
|
return true;
|
||||||
@@ -477,8 +497,10 @@ template<typename Type, size_t M>
|
|||||||
SquareMatrix<Type, M> inv(const SquareMatrix<Type, M> &A)
|
SquareMatrix<Type, M> inv(const SquareMatrix<Type, M> &A)
|
||||||
{
|
{
|
||||||
SquareMatrix<Type, M> i;
|
SquareMatrix<Type, M> i;
|
||||||
|
|
||||||
if (inv(A, i)) {
|
if (inv(A, i)) {
|
||||||
return i;
|
return i;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
i.setZero();
|
i.setZero();
|
||||||
return i;
|
return i;
|
||||||
@@ -494,32 +516,42 @@ template<typename Type, size_t M>
|
|||||||
SquareMatrix <Type, M> cholesky(const SquareMatrix<Type, M> &A)
|
SquareMatrix <Type, M> cholesky(const SquareMatrix<Type, M> &A)
|
||||||
{
|
{
|
||||||
SquareMatrix<Type, M> L;
|
SquareMatrix<Type, M> L;
|
||||||
|
|
||||||
for (size_t j = 0; j < M; j++) {
|
for (size_t j = 0; j < M; j++) {
|
||||||
for (size_t i = j; i < M; i++) {
|
for (size_t i = j; i < M; i++) {
|
||||||
if (i == j) {
|
if (i == j) {
|
||||||
float sum = 0;
|
float sum = 0;
|
||||||
|
|
||||||
for (size_t k = 0; k < j; k++) {
|
for (size_t k = 0; k < j; k++) {
|
||||||
sum += L(j, k) * L(j, k);
|
sum += L(j, k) * L(j, k);
|
||||||
}
|
}
|
||||||
|
|
||||||
Type res = A(j, j) - sum;
|
Type res = A(j, j) - sum;
|
||||||
|
|
||||||
if (res <= 0) {
|
if (res <= 0) {
|
||||||
L(j, j) = 0;
|
L(j, j) = 0;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
L(j, j) = sqrt(res);
|
L(j, j) = sqrt(res);
|
||||||
}
|
}
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
float sum = 0;
|
float sum = 0;
|
||||||
|
|
||||||
for (size_t k = 0; k < j; k++) {
|
for (size_t k = 0; k < j; k++) {
|
||||||
sum += L(i, k) * L(j, k);
|
sum += L(i, k) * L(j, k);
|
||||||
}
|
}
|
||||||
|
|
||||||
if (L(j, j) <= 0) {
|
if (L(j, j) <= 0) {
|
||||||
L(i, j) = 0;
|
L(i, j) = 0;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
L(i, j) = (A(i, j) - sum) / L(j, j);
|
L(i, j) = (A(i, j) - sum) / L(j, j);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
return L;
|
return L;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -540,5 +572,3 @@ using Matrix3f = SquareMatrix<float, 3>;
|
|||||||
using Matrix3d = SquareMatrix<double, 3>;
|
using Matrix3d = SquareMatrix<double, 3>;
|
||||||
|
|
||||||
} // namespace matrix
|
} // namespace matrix
|
||||||
|
|
||||||
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
|
|
||||||
|
|||||||
@@ -44,6 +44,7 @@ public:
|
|||||||
Vector(const Slice<Type, 1, M, P, Q> &slice_in)
|
Vector(const Slice<Type, 1, M, P, Q> &slice_in)
|
||||||
{
|
{
|
||||||
Vector &self(*this);
|
Vector &self(*this);
|
||||||
|
|
||||||
for (size_t i = 0; i < M; i++) {
|
for (size_t i = 0; i < M; i++) {
|
||||||
self(i) = slice_in(0, i);
|
self(i) = slice_in(0, i);
|
||||||
}
|
}
|
||||||
@@ -65,72 +66,88 @@ public:
|
|||||||
return v(i, 0);
|
return v(i, 0);
|
||||||
}
|
}
|
||||||
|
|
||||||
Type dot(const MatrixM1 & b) const {
|
Type dot(const MatrixM1 &b) const
|
||||||
|
{
|
||||||
const Vector &a(*this);
|
const Vector &a(*this);
|
||||||
Type r(0);
|
Type r(0);
|
||||||
|
|
||||||
for (size_t i = 0; i < M; i++) {
|
for (size_t i = 0; i < M; i++) {
|
||||||
r += a(i) * b(i, 0);
|
r += a(i) * b(i, 0);
|
||||||
}
|
}
|
||||||
|
|
||||||
return r;
|
return r;
|
||||||
}
|
}
|
||||||
|
|
||||||
inline Type operator*(const MatrixM1 & b) const {
|
inline Type operator*(const MatrixM1 &b) const
|
||||||
|
{
|
||||||
const Vector &a(*this);
|
const Vector &a(*this);
|
||||||
return a.dot(b);
|
return a.dot(b);
|
||||||
}
|
}
|
||||||
|
|
||||||
inline Vector operator*(Type b) const {
|
inline Vector operator*(Type b) const
|
||||||
|
{
|
||||||
return Vector(MatrixM1::operator*(b));
|
return Vector(MatrixM1::operator*(b));
|
||||||
}
|
}
|
||||||
|
|
||||||
Type norm() const {
|
Type norm() const
|
||||||
|
{
|
||||||
const Vector &a(*this);
|
const Vector &a(*this);
|
||||||
return Type(matrix::sqrt(a.dot(a)));
|
return Type(matrix::sqrt(a.dot(a)));
|
||||||
}
|
}
|
||||||
|
|
||||||
Type norm_squared() const {
|
Type norm_squared() const
|
||||||
|
{
|
||||||
const Vector &a(*this);
|
const Vector &a(*this);
|
||||||
return a.dot(a);
|
return a.dot(a);
|
||||||
}
|
}
|
||||||
|
|
||||||
inline Type length() const {
|
inline Type length() const
|
||||||
|
{
|
||||||
return norm();
|
return norm();
|
||||||
}
|
}
|
||||||
|
|
||||||
inline void normalize() {
|
inline void normalize()
|
||||||
|
{
|
||||||
(*this) /= norm();
|
(*this) /= norm();
|
||||||
}
|
}
|
||||||
|
|
||||||
Vector unit() const {
|
Vector unit() const
|
||||||
|
{
|
||||||
return (*this) / norm();
|
return (*this) / norm();
|
||||||
}
|
}
|
||||||
|
|
||||||
Vector unit_or_zero(const Type eps = Type(1e-5)) const {
|
Vector unit_or_zero(const Type eps = Type(1e-5)) const
|
||||||
|
{
|
||||||
const Type n = norm();
|
const Type n = norm();
|
||||||
|
|
||||||
if (n > eps) {
|
if (n > eps) {
|
||||||
return (*this) / n;
|
return (*this) / n;
|
||||||
}
|
}
|
||||||
|
|
||||||
return Vector();
|
return Vector();
|
||||||
}
|
}
|
||||||
|
|
||||||
inline Vector normalized() const {
|
inline Vector normalized() const
|
||||||
|
{
|
||||||
return unit();
|
return unit();
|
||||||
}
|
}
|
||||||
|
|
||||||
bool longerThan(Type testVal) const {
|
bool longerThan(Type testVal) const
|
||||||
|
{
|
||||||
return norm_squared() > testVal * testVal;
|
return norm_squared() > testVal * testVal;
|
||||||
}
|
}
|
||||||
|
|
||||||
Vector sqrt() const {
|
Vector sqrt() const
|
||||||
|
{
|
||||||
const Vector &a(*this);
|
const Vector &a(*this);
|
||||||
Vector r;
|
Vector r;
|
||||||
|
|
||||||
for (size_t i = 0; i < M; i++) {
|
for (size_t i = 0; i < M; i++) {
|
||||||
r(i) = Type(matrix::sqrt(a(i)));
|
r(i) = Type(matrix::sqrt(a(i)));
|
||||||
}
|
}
|
||||||
|
|
||||||
return r;
|
return r;
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
} // namespace matrix
|
} // namespace matrix
|
||||||
|
|
||||||
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
|
|
||||||
|
|||||||
@@ -60,12 +60,14 @@ public:
|
|||||||
v(1) = other(1);
|
v(1) = other(1);
|
||||||
}
|
}
|
||||||
|
|
||||||
Type cross(const Matrix21 & b) const {
|
Type cross(const Matrix21 &b) const
|
||||||
|
{
|
||||||
const Vector2 &a(*this);
|
const Vector2 &a(*this);
|
||||||
return a(0) * b(1, 0) - a(1) * b(0, 0);
|
return a(0) * b(1, 0) - a(1) * b(0, 0);
|
||||||
}
|
}
|
||||||
|
|
||||||
Type operator%(const Matrix21 & b) const {
|
Type operator%(const Matrix21 &b) const
|
||||||
|
{
|
||||||
return (*this).cross(b);
|
return (*this).cross(b);
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -76,5 +78,3 @@ using Vector2f = Vector2<float>;
|
|||||||
using Vector2d = Vector2<double>;
|
using Vector2d = Vector2<double>;
|
||||||
|
|
||||||
} // namespace matrix
|
} // namespace matrix
|
||||||
|
|
||||||
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
|
|
||||||
|
|||||||
@@ -44,7 +44,8 @@ public:
|
|||||||
{
|
{
|
||||||
}
|
}
|
||||||
|
|
||||||
Vector3(Type x, Type y, Type z) {
|
Vector3(Type x, Type y, Type z)
|
||||||
|
{
|
||||||
Vector3 &v(*this);
|
Vector3 &v(*this);
|
||||||
v(0) = x;
|
v(0) = x;
|
||||||
v(1) = y;
|
v(1) = y;
|
||||||
@@ -61,7 +62,8 @@ public:
|
|||||||
{
|
{
|
||||||
}
|
}
|
||||||
|
|
||||||
Vector3 cross(const Matrix31 & b) const {
|
Vector3 cross(const Matrix31 &b) const
|
||||||
|
{
|
||||||
const Vector3 &a(*this);
|
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)};
|
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)};
|
||||||
}
|
}
|
||||||
@@ -105,18 +107,21 @@ public:
|
|||||||
return Vector<Type, 3>::operator*(b);
|
return Vector<Type, 3>::operator*(b);
|
||||||
}
|
}
|
||||||
|
|
||||||
inline Vector3 operator%(const Matrix31 & b) const {
|
inline Vector3 operator%(const Matrix31 &b) const
|
||||||
|
{
|
||||||
return (*this).cross(b);
|
return (*this).cross(b);
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Override vector ops so Vector3 type is returned
|
* Override vector ops so Vector3 type is returned
|
||||||
*/
|
*/
|
||||||
inline Vector3 unit() const {
|
inline Vector3 unit() const
|
||||||
|
{
|
||||||
return Vector3(Vector<Type, 3>::unit());
|
return Vector3(Vector<Type, 3>::unit());
|
||||||
}
|
}
|
||||||
|
|
||||||
inline Vector3 normalized() const {
|
inline Vector3 normalized() const
|
||||||
|
{
|
||||||
return unit();
|
return unit();
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -131,7 +136,8 @@ public:
|
|||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
Dcm<Type> hat() const { // inverse to Dcm.vee() operation
|
Dcm<Type> hat() const // inverse to Dcm.vee() operation
|
||||||
|
{
|
||||||
const Vector3 &v(*this);
|
const Vector3 &v(*this);
|
||||||
Dcm<Type> A;
|
Dcm<Type> A;
|
||||||
A(0, 0) = 0;
|
A(0, 0) = 0;
|
||||||
@@ -152,5 +158,3 @@ using Vector3f = Vector3<float>;
|
|||||||
using Vector3d = Vector3<double>;
|
using Vector3d = Vector3<double>;
|
||||||
|
|
||||||
} // namespace matrix
|
} // namespace matrix
|
||||||
|
|
||||||
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
|
|
||||||
|
|||||||
@@ -2,7 +2,8 @@
|
|||||||
|
|
||||||
#include "math.hpp"
|
#include "math.hpp"
|
||||||
|
|
||||||
namespace matrix {
|
namespace matrix
|
||||||
|
{
|
||||||
|
|
||||||
template<typename Type, size_t M, size_t N>
|
template<typename Type, size_t M, size_t N>
|
||||||
int kalman_correct(
|
int kalman_correct(
|
||||||
|
|||||||
@@ -10,7 +10,8 @@ namespace matrix
|
|||||||
{
|
{
|
||||||
|
|
||||||
template<typename Type>
|
template<typename Type>
|
||||||
bool is_finite(Type x) {
|
bool is_finite(Type x)
|
||||||
|
{
|
||||||
#if defined (__PX4_NUTTX)
|
#if defined (__PX4_NUTTX)
|
||||||
return PX4_ISFINITE(x);
|
return PX4_ISFINITE(x);
|
||||||
#elif defined (__PX4_QURT)
|
#elif defined (__PX4_QURT)
|
||||||
@@ -43,7 +44,8 @@ namespace detail
|
|||||||
{
|
{
|
||||||
|
|
||||||
template<typename Floating>
|
template<typename Floating>
|
||||||
Floating wrap_floating(Floating x, Floating low, Floating high) {
|
Floating wrap_floating(Floating x, Floating low, Floating high)
|
||||||
|
{
|
||||||
// already in range
|
// already in range
|
||||||
if (low <= x && x < high) {
|
if (low <= x && x < high) {
|
||||||
return x;
|
return x;
|
||||||
@@ -65,7 +67,8 @@ Floating wrap_floating(Floating x, Floating low, Floating high) {
|
|||||||
* @param high upper limit of the allowed range
|
* @param high upper limit of the allowed range
|
||||||
* @return wrapped value inside the range
|
* @return wrapped value inside the range
|
||||||
*/
|
*/
|
||||||
inline float wrap(float x, float low, float high) {
|
inline float wrap(float x, float low, float high)
|
||||||
|
{
|
||||||
return matrix::detail::wrap_floating(x, low, high);
|
return matrix::detail::wrap_floating(x, low, high);
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -77,7 +80,8 @@ inline float wrap(float x, float low, float high) {
|
|||||||
* @param high upper limit of the allowed range
|
* @param high upper limit of the allowed range
|
||||||
* @return wrapped value inside the range
|
* @return wrapped value inside the range
|
||||||
*/
|
*/
|
||||||
inline double wrap(double x, double low, double high) {
|
inline double wrap(double x, double low, double high)
|
||||||
|
{
|
||||||
return matrix::detail::wrap_floating(x, low, high);
|
return matrix::detail::wrap_floating(x, low, high);
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -90,7 +94,8 @@ inline double wrap(double x, double low, double high) {
|
|||||||
* @return wrapped value inside the range
|
* @return wrapped value inside the range
|
||||||
*/
|
*/
|
||||||
template<typename Integer>
|
template<typename Integer>
|
||||||
Integer wrap(Integer x, Integer low, Integer high) {
|
Integer wrap(Integer x, Integer low, Integer high)
|
||||||
|
{
|
||||||
const auto range = high - low;
|
const auto range = high - low;
|
||||||
|
|
||||||
if (x < low) {
|
if (x < low) {
|
||||||
|
|||||||
@@ -2,7 +2,8 @@
|
|||||||
|
|
||||||
#include "math.hpp"
|
#include "math.hpp"
|
||||||
|
|
||||||
namespace matrix {
|
namespace matrix
|
||||||
|
{
|
||||||
|
|
||||||
template<typename Type, size_t M, size_t N>
|
template<typename Type, size_t M, size_t N>
|
||||||
int integrate_rk4(
|
int integrate_rk4(
|
||||||
@@ -20,13 +21,17 @@ int integrate_rk4(
|
|||||||
y1 = y0;
|
y1 = y0;
|
||||||
Type h = h0;
|
Type h = h0;
|
||||||
Vector<Type, M> k1, k2, k3, k4;
|
Vector<Type, M> k1, k2, k3, k4;
|
||||||
if (tf < t0) return -1; // make sure t1 > t0
|
|
||||||
|
if (tf < t0) { return -1; } // make sure t1 > t0
|
||||||
|
|
||||||
while (t1 < tf) {
|
while (t1 < tf) {
|
||||||
if (t1 + h0 < tf) {
|
if (t1 + h0 < tf) {
|
||||||
h = h0;
|
h = h0;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
h = tf - t1;
|
h = tf - t1;
|
||||||
}
|
}
|
||||||
|
|
||||||
k1 = f(t1, y1, u);
|
k1 = f(t1, y1, u);
|
||||||
k2 = f(t1 + h / 2, y1 + k1 * h / 2, u);
|
k2 = f(t1 + h / 2, y1 + k1 * h / 2, u);
|
||||||
k3 = f(t1 + h / 2, y1 + k2 * h / 2, u);
|
k3 = f(t1 + h / 2, y1 + k2 * h / 2, u);
|
||||||
@@ -34,9 +39,8 @@ int integrate_rk4(
|
|||||||
y1 += (k1 + k2 * 2 + k3 * 2 + k4) * (h / 6);
|
y1 += (k1 + k2 * 2 + k3 * 2 + k4) * (h / 6);
|
||||||
t1 += h;
|
t1 += h;
|
||||||
}
|
}
|
||||||
|
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
} // namespace matrix
|
} // namespace matrix
|
||||||
|
|
||||||
// vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 :
|
|
||||||
|
|||||||
@@ -20,7 +20,8 @@
|
|||||||
#define M_TWOPI (M_PI * 2.0)
|
#define M_TWOPI (M_PI * 2.0)
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
namespace matrix {
|
namespace matrix
|
||||||
|
{
|
||||||
|
|
||||||
#if !defined(FLT_EPSILON)
|
#if !defined(FLT_EPSILON)
|
||||||
#define FLT_EPSILON __FLT_EPSILON__
|
#define FLT_EPSILON __FLT_EPSILON__
|
||||||
|
|||||||
@@ -7,7 +7,8 @@ using namespace matrix;
|
|||||||
// manually instantiated all files we intend to test
|
// manually instantiated all files we intend to test
|
||||||
// so that coverage works correctly
|
// so that coverage works correctly
|
||||||
// doesn't matter what test this is in
|
// doesn't matter what test this is in
|
||||||
namespace matrix {
|
namespace matrix
|
||||||
|
{
|
||||||
template class Matrix<float, 3, 3>;
|
template class Matrix<float, 3, 3>;
|
||||||
template class Vector3<float>;
|
template class Vector3<float>;
|
||||||
template class Vector2<float>;
|
template class Vector2<float>;
|
||||||
@@ -171,6 +172,7 @@ int main()
|
|||||||
// dcm renormalize
|
// dcm renormalize
|
||||||
Dcmf A = eye<float, 3>();
|
Dcmf A = eye<float, 3>();
|
||||||
Dcmf R(euler_check);
|
Dcmf R(euler_check);
|
||||||
|
|
||||||
for (size_t i = 0; i < 1000; i++) {
|
for (size_t i = 0; i < 1000; i++) {
|
||||||
A = R * A;
|
A = R * A;
|
||||||
}
|
}
|
||||||
@@ -182,6 +184,7 @@ int main()
|
|||||||
Vector3f rvec(matrix::Matrix<float, 1, 3>(A.row(r)).transpose());
|
Vector3f rvec(matrix::Matrix<float, 1, 3>(A.row(r)).transpose());
|
||||||
err += fabs(1.0f - rvec.length());
|
err += fabs(1.0f - rvec.length());
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST(err < eps);
|
TEST(err < eps);
|
||||||
|
|
||||||
// constants
|
// constants
|
||||||
@@ -486,5 +489,3 @@ int main()
|
|||||||
#endif
|
#endif
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
|
|
||||||
|
|||||||
@@ -3,7 +3,8 @@
|
|||||||
|
|
||||||
using namespace matrix;
|
using namespace matrix;
|
||||||
|
|
||||||
namespace {
|
namespace
|
||||||
|
{
|
||||||
void doTheCopy(const Matrix<float, 2, 3> &A, float array_A[6])
|
void doTheCopy(const Matrix<float, 2, 3> &A, float array_A[6])
|
||||||
{
|
{
|
||||||
A.copyTo(array_A);
|
A.copyTo(array_A);
|
||||||
@@ -18,6 +19,7 @@ int main()
|
|||||||
const Vector3f v(1, 2, 3);
|
const Vector3f v(1, 2, 3);
|
||||||
float dst3[3] = {};
|
float dst3[3] = {};
|
||||||
v.copyTo(dst3);
|
v.copyTo(dst3);
|
||||||
|
|
||||||
for (size_t i = 0; i < 3; i++) {
|
for (size_t i = 0; i < 3; i++) {
|
||||||
TEST(fabs(v(i) - dst3[i]) < eps);
|
TEST(fabs(v(i) - dst3[i]) < eps);
|
||||||
}
|
}
|
||||||
@@ -26,6 +28,7 @@ int main()
|
|||||||
Quatf q(1, 2, 3, 4);
|
Quatf q(1, 2, 3, 4);
|
||||||
float dst4[4] = {};
|
float dst4[4] = {};
|
||||||
q.copyTo(dst4);
|
q.copyTo(dst4);
|
||||||
|
|
||||||
for (size_t i = 0; i < 4; i++) {
|
for (size_t i = 0; i < 4; i++) {
|
||||||
TEST(fabs(q(i) - dst4[i]) < eps);
|
TEST(fabs(q(i) - dst4[i]) < eps);
|
||||||
}
|
}
|
||||||
@@ -41,6 +44,7 @@ int main()
|
|||||||
float array_A[6] = {};
|
float array_A[6] = {};
|
||||||
doTheCopy(A, array_A);
|
doTheCopy(A, array_A);
|
||||||
float array_row[6] = {1, 2, 3, 4, 5, 6};
|
float array_row[6] = {1, 2, 3, 4, 5, 6};
|
||||||
|
|
||||||
for (size_t i = 0; i < 6; i++) {
|
for (size_t i = 0; i < 6; i++) {
|
||||||
TEST(fabs(array_A[i] - array_row[i]) < eps);
|
TEST(fabs(array_A[i] - array_row[i]) < eps);
|
||||||
}
|
}
|
||||||
@@ -48,6 +52,7 @@ int main()
|
|||||||
// Matrix copyToColumnMajor
|
// Matrix copyToColumnMajor
|
||||||
A.copyToColumnMajor(array_A);
|
A.copyToColumnMajor(array_A);
|
||||||
float array_column[6] = {1, 4, 2, 5, 3, 6};
|
float array_column[6] = {1, 4, 2, 5, 3, 6};
|
||||||
|
|
||||||
for (size_t i = 0; i < 6; i++) {
|
for (size_t i = 0; i < 6; i++) {
|
||||||
TEST(fabs(array_A[i] - array_column[i]) < eps);
|
TEST(fabs(array_A[i] - array_column[i]) < eps);
|
||||||
}
|
}
|
||||||
@@ -55,6 +60,7 @@ int main()
|
|||||||
// Slice copyTo
|
// Slice copyTo
|
||||||
float dst5[2] = {};
|
float dst5[2] = {};
|
||||||
v.slice<2, 1>(0, 0).copyTo(dst5);
|
v.slice<2, 1>(0, 0).copyTo(dst5);
|
||||||
|
|
||||||
for (size_t i = 0; i < 2; i++) {
|
for (size_t i = 0; i < 2; i++) {
|
||||||
TEST(fabs(v(i) - dst5[i]) < eps);
|
TEST(fabs(v(i) - dst5[i]) < eps);
|
||||||
}
|
}
|
||||||
@@ -62,6 +68,7 @@ int main()
|
|||||||
float subarray_A[4] = {};
|
float subarray_A[4] = {};
|
||||||
A.slice<2, 2>(0, 0).copyToColumnMajor(subarray_A);
|
A.slice<2, 2>(0, 0).copyToColumnMajor(subarray_A);
|
||||||
float subarray_column[4] = {1, 4, 2, 5};
|
float subarray_column[4] = {1, 4, 2, 5};
|
||||||
|
|
||||||
for (size_t i = 0; i < 4; i++) {
|
for (size_t i = 0; i < 4; i++) {
|
||||||
TEST(fabs(subarray_A[i] - subarray_column[i]) < eps);
|
TEST(fabs(subarray_A[i] - subarray_column[i]) < eps);
|
||||||
}
|
}
|
||||||
@@ -69,4 +76,3 @@ int main()
|
|||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
|
|
||||||
|
|||||||
@@ -11,7 +11,8 @@ bool isEqualAll(Dual<Scalar, N> a, Dual<Scalar, N> b)
|
|||||||
}
|
}
|
||||||
|
|
||||||
template <typename T>
|
template <typename T>
|
||||||
T testFunction(const Vector<T, 3>& point) {
|
T testFunction(const Vector<T, 3> &point)
|
||||||
|
{
|
||||||
// function is f(x,y,z) = x^2 + 2xy + 3y^2 + z
|
// function is f(x,y,z) = x^2 + 2xy + 3y^2 + z
|
||||||
return point(0) * point(0)
|
return point(0) * point(0)
|
||||||
+ 2.f * point(0) * point(1)
|
+ 2.f * point(0) * point(1)
|
||||||
@@ -266,8 +267,8 @@ int main()
|
|||||||
positionMeasurement,
|
positionMeasurement,
|
||||||
dt);
|
dt);
|
||||||
float h = 0.001f;
|
float h = 0.001f;
|
||||||
for (size_t i = 0; i < 4; i++)
|
|
||||||
{
|
for (size_t i = 0; i < 4; i++) {
|
||||||
Quaternion<float> h4 = velocityOrientation;
|
Quaternion<float> h4 = velocityOrientation;
|
||||||
h4(i) += h;
|
h4(i) += h;
|
||||||
numerical_jacobian.col(i) = (positionError(positionState,
|
numerical_jacobian.col(i) = (positionError(positionState,
|
||||||
|
|||||||
@@ -26,4 +26,3 @@ int main()
|
|||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
|
|
||||||
|
|||||||
@@ -16,4 +16,3 @@ int main()
|
|||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
|
|
||||||
|
|||||||
@@ -75,4 +75,3 @@ int main()
|
|||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
|
|
||||||
|
|||||||
@@ -5,7 +5,8 @@ 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*/);
|
||||||
|
|
||||||
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);
|
float v = -sin(t);
|
||||||
return v * ones<float, 6, 1>();
|
return v * ones<float, 6, 1>();
|
||||||
}
|
}
|
||||||
@@ -23,4 +24,3 @@ int main()
|
|||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
|
|
||||||
|
|||||||
@@ -118,6 +118,7 @@ int main()
|
|||||||
for (size_t i = 0; i < 9; i++) {
|
for (size_t i = 0; i < 9; i++) {
|
||||||
A2(0, i) = 0;
|
A2(0, i) = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
A2_I = inv(A2);
|
A2_I = inv(A2);
|
||||||
SquareMatrix<float, 9> Z9 = zeros<float, 9, 9>();
|
SquareMatrix<float, 9> Z9 = zeros<float, 9, 9>();
|
||||||
TEST(!A2.I(A2_I));
|
TEST(!A2.I(A2_I));
|
||||||
@@ -161,4 +162,3 @@ int main()
|
|||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
|
|
||||||
|
|||||||
@@ -13,21 +13,26 @@ int main()
|
|||||||
int ret;
|
int ret;
|
||||||
|
|
||||||
ret = test_4x4<float>();
|
ret = test_4x4<float>();
|
||||||
if (ret != 0) return ret;
|
|
||||||
|
if (ret != 0) { return ret; }
|
||||||
|
|
||||||
ret = test_4x4<double>();
|
ret = test_4x4<double>();
|
||||||
if (ret != 0) return ret;
|
|
||||||
|
if (ret != 0) { return ret; }
|
||||||
|
|
||||||
ret = test_4x3();
|
ret = test_4x3();
|
||||||
if (ret != 0) return ret;
|
|
||||||
|
if (ret != 0) { return ret; }
|
||||||
|
|
||||||
ret = test_div_zero();
|
ret = test_div_zero();
|
||||||
if (ret != 0) return ret;
|
|
||||||
|
if (ret != 0) { return ret; }
|
||||||
|
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
int test_4x3() {
|
int test_4x3()
|
||||||
|
{
|
||||||
// Start with an (m x n) A matrix
|
// Start with an (m x n) A matrix
|
||||||
float data[12] = {20.f, -10.f, -13.f,
|
float data[12] = {20.f, -10.f, -13.f,
|
||||||
17.f, 16.f, -18.f,
|
17.f, 16.f, -18.f,
|
||||||
@@ -53,7 +58,8 @@ int test_4x3() {
|
|||||||
}
|
}
|
||||||
|
|
||||||
template<typename Type>
|
template<typename Type>
|
||||||
int test_4x4() {
|
int test_4x4()
|
||||||
|
{
|
||||||
// Start with an (m x n) A matrix
|
// Start with an (m x n) A matrix
|
||||||
const Type data[16] = { 20.f, -10.f, -13.f, 21.f,
|
const Type data[16] = { 20.f, -10.f, -13.f, 21.f,
|
||||||
17.f, 16.f, -18.f, -14.f,
|
17.f, 16.f, -18.f, -14.f,
|
||||||
@@ -79,7 +85,8 @@ int test_4x4() {
|
|||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
int test_div_zero() {
|
int test_div_zero()
|
||||||
|
{
|
||||||
float data[4] = {0.0f, 0.0f, 0.0f, 0.0f};
|
float data[4] = {0.0f, 0.0f, 0.0f, 0.0f};
|
||||||
Matrix<float, 2, 2> A(data);
|
Matrix<float, 2, 2> A(data);
|
||||||
|
|
||||||
@@ -97,4 +104,3 @@ int test_div_zero() {
|
|||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
|
|
||||||
|
|||||||
@@ -31,11 +31,13 @@ int main()
|
|||||||
|
|
||||||
Matrix3f m_nan;
|
Matrix3f m_nan;
|
||||||
m_nan.setNaN();
|
m_nan.setNaN();
|
||||||
|
|
||||||
for (size_t i = 0; i < 3; i++) {
|
for (size_t i = 0; i < 3; i++) {
|
||||||
for (size_t j = 0; j < 3; j++) {
|
for (size_t j = 0; j < 3; j++) {
|
||||||
TEST(isnan(m_nan(i, j)));
|
TEST(isnan(m_nan(i, j)));
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST(m_nan.isAllNan());
|
TEST(m_nan.isAllNan());
|
||||||
|
|
||||||
float data2d[3][3] = {
|
float data2d[3][3] = {
|
||||||
@@ -44,11 +46,13 @@ int main()
|
|||||||
{7, 8, 9}
|
{7, 8, 9}
|
||||||
};
|
};
|
||||||
m2 = Matrix3f(data2d);
|
m2 = Matrix3f(data2d);
|
||||||
|
|
||||||
for (size_t i = 0; i < 3; i++) {
|
for (size_t i = 0; i < 3; i++) {
|
||||||
for (size_t j = 0; j < 3; j++) {
|
for (size_t j = 0; j < 3; j++) {
|
||||||
TEST(fabs(data[i * 3 + j] - m2(i, j)) < FLT_EPSILON);
|
TEST(fabs(data[i * 3 + j] - m2(i, j)) < FLT_EPSILON);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST(!m2.isAllNan());
|
TEST(!m2.isAllNan());
|
||||||
|
|
||||||
float data_times_2[9] = {2, 4, 6, 8, 10, 12, 14, 16, 18};
|
float data_times_2[9] = {2, 4, 6, 8, 10, 12, 14, 16, 18};
|
||||||
@@ -224,11 +228,14 @@ int main()
|
|||||||
printf("%s\n", buffer); // for debugging in case of failure
|
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";
|
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
|
printf("%s\n", output); // for debugging in case of failure
|
||||||
|
|
||||||
for (size_t i = 0; i < len; i++) {
|
for (size_t i = 0; i < len; i++) {
|
||||||
if (buffer[i] != output[i]) { // for debugging in case of failure
|
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
|
printf("%d: \"%c\" != \"%c\"", int(i), buffer[i], output[i]); // LCOV_EXCL_LINE only print on failure
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST(buffer[i] == output[i]);
|
TEST(buffer[i] == output[i]);
|
||||||
|
|
||||||
if (buffer[i] == '\0') {
|
if (buffer[i] == '\0') {
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
@@ -244,17 +251,20 @@ int main()
|
|||||||
FILE *fp = fopen("testoutput.txt", "r");
|
FILE *fp = fopen("testoutput.txt", "r");
|
||||||
TEST(fp != nullptr);
|
TEST(fp != nullptr);
|
||||||
TEST(!fseek(fp, 0, SEEK_SET));
|
TEST(!fseek(fp, 0, SEEK_SET));
|
||||||
|
|
||||||
for (size_t i = 0; i < len; i++) {
|
for (size_t i = 0; i < len; i++) {
|
||||||
char c = static_cast<char>(fgetc(fp));
|
char c = static_cast<char>(fgetc(fp));
|
||||||
|
|
||||||
if (c == '\n') {
|
if (c == '\n') {
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
printf("%d %d %d\n", static_cast<int>(i), output[i], c);
|
printf("%d %d %d\n", static_cast<int>(i), output[i], c);
|
||||||
TEST(c == output[i]);
|
TEST(c == output[i]);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST(!fclose(fp));
|
TEST(!fclose(fp));
|
||||||
|
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
|
|
||||||
|
|||||||
@@ -68,4 +68,3 @@ int main()
|
|||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
|
|
||||||
|
|||||||
@@ -15,4 +15,3 @@ int main()
|
|||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
|
|
||||||
|
|||||||
@@ -150,4 +150,3 @@ int main()
|
|||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
|
|
||||||
|
|||||||
@@ -36,4 +36,3 @@ int main()
|
|||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
|
|
||||||
|
|||||||
@@ -80,7 +80,8 @@ int main()
|
|||||||
TEST(isEqual(H, F));
|
TEST(isEqual(H, F));
|
||||||
|
|
||||||
float data_4_check[5] = {3, 11, 9, 0, 0};
|
float data_4_check[5] = {3, 11, 9, 0, 0};
|
||||||
{ // assigning row slices to each other
|
{
|
||||||
|
// assigning row slices to each other
|
||||||
const Matrix<float, 3, 1> J(data_3_check);
|
const Matrix<float, 3, 1> J(data_3_check);
|
||||||
Matrix<float, 5, 1> K;
|
Matrix<float, 5, 1> K;
|
||||||
K.row(2) = J.row(0);
|
K.row(2) = J.row(0);
|
||||||
@@ -90,7 +91,8 @@ int main()
|
|||||||
Matrix<float, 5, 1> K_check(data_4_check);
|
Matrix<float, 5, 1> K_check(data_4_check);
|
||||||
TEST(isEqual(K, K_check));
|
TEST(isEqual(K, K_check));
|
||||||
}
|
}
|
||||||
{ // assigning col slices to each other
|
{
|
||||||
|
// assigning col slices to each other
|
||||||
const Matrix<float, 1, 3> J(data_3_check);
|
const Matrix<float, 1, 3> J(data_3_check);
|
||||||
Matrix<float, 1, 5> K;
|
Matrix<float, 1, 5> K;
|
||||||
K.col(2) = J.col(0);
|
K.col(2) = J.col(0);
|
||||||
@@ -232,4 +234,3 @@ int main()
|
|||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
|
|
||||||
|
|||||||
@@ -3,7 +3,8 @@
|
|||||||
|
|
||||||
using namespace matrix;
|
using namespace matrix;
|
||||||
|
|
||||||
TEST(sparseVectorTest, defaultConstruction) {
|
TEST(sparseVectorTest, defaultConstruction)
|
||||||
|
{
|
||||||
SparseVectorf<24, 4, 6> a;
|
SparseVectorf<24, 4, 6> a;
|
||||||
EXPECT_EQ(a.non_zeros(), 2);
|
EXPECT_EQ(a.non_zeros(), 2);
|
||||||
EXPECT_EQ(a.index(0), 4);
|
EXPECT_EQ(a.index(0), 4);
|
||||||
@@ -12,7 +13,8 @@ TEST(sparseVectorTest, defaultConstruction) {
|
|||||||
a.at<6>() = 2.f;
|
a.at<6>() = 2.f;
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST(sparseVectorTest, initializationWithData) {
|
TEST(sparseVectorTest, initializationWithData)
|
||||||
|
{
|
||||||
const float data[3] = {1.f, 2.f, 3.f};
|
const float data[3] = {1.f, 2.f, 3.f};
|
||||||
SparseVectorf<24, 4, 6, 22> a(data);
|
SparseVectorf<24, 4, 6, 22> a(data);
|
||||||
EXPECT_EQ(a.non_zeros(), 3);
|
EXPECT_EQ(a.non_zeros(), 3);
|
||||||
@@ -24,24 +26,29 @@ TEST(sparseVectorTest, initializationWithData) {
|
|||||||
EXPECT_FLOAT_EQ(a.at<22>(), data[2]);
|
EXPECT_FLOAT_EQ(a.at<22>(), data[2]);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST(sparseVectorTest, initialisationFromVector) {
|
TEST(sparseVectorTest, initialisationFromVector)
|
||||||
|
{
|
||||||
const Vector3f vec(1.f, 2.f, 3.f);
|
const Vector3f vec(1.f, 2.f, 3.f);
|
||||||
const SparseVectorf<3, 0, 2> a(vec);
|
const SparseVectorf<3, 0, 2> a(vec);
|
||||||
EXPECT_FLOAT_EQ(a.at<0>(), vec(0));
|
EXPECT_FLOAT_EQ(a.at<0>(), vec(0));
|
||||||
EXPECT_FLOAT_EQ(a.at<2>(), vec(2));
|
EXPECT_FLOAT_EQ(a.at<2>(), vec(2));
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST(sparseVectorTest, accessDataWithCompressedIndices) {
|
TEST(sparseVectorTest, accessDataWithCompressedIndices)
|
||||||
|
{
|
||||||
const Vector3f vec(1.f, 2.f, 3.f);
|
const Vector3f vec(1.f, 2.f, 3.f);
|
||||||
SparseVectorf<3, 0, 2> a(vec);
|
SparseVectorf<3, 0, 2> a(vec);
|
||||||
|
|
||||||
for (size_t i = 0; i < a.non_zeros(); i++) {
|
for (size_t i = 0; i < a.non_zeros(); i++) {
|
||||||
a.atCompressedIndex(i) = static_cast<float>(i);
|
a.atCompressedIndex(i) = static_cast<float>(i);
|
||||||
}
|
}
|
||||||
|
|
||||||
EXPECT_FLOAT_EQ(a.at<0>(), a.atCompressedIndex(0));
|
EXPECT_FLOAT_EQ(a.at<0>(), a.atCompressedIndex(0));
|
||||||
EXPECT_FLOAT_EQ(a.at<2>(), a.atCompressedIndex(1));
|
EXPECT_FLOAT_EQ(a.at<2>(), a.atCompressedIndex(1));
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST(sparseVectorTest, setZero) {
|
TEST(sparseVectorTest, setZero)
|
||||||
|
{
|
||||||
const float data[3] = {1.f, 2.f, 3.f};
|
const float data[3] = {1.f, 2.f, 3.f};
|
||||||
SparseVectorf<24, 4, 6, 22> a(data);
|
SparseVectorf<24, 4, 6, 22> a(data);
|
||||||
a.setZero();
|
a.setZero();
|
||||||
@@ -50,7 +57,8 @@ TEST(sparseVectorTest, setZero) {
|
|||||||
EXPECT_FLOAT_EQ(a.at<22>(), 0.f);
|
EXPECT_FLOAT_EQ(a.at<22>(), 0.f);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST(sparseVectorTest, additionWithDenseVector) {
|
TEST(sparseVectorTest, additionWithDenseVector)
|
||||||
|
{
|
||||||
Vector<float, 4> dense_vec;
|
Vector<float, 4> dense_vec;
|
||||||
dense_vec.setAll(1.f);
|
dense_vec.setAll(1.f);
|
||||||
const float data[3] = {1.f, 2.f, 3.f};
|
const float data[3] = {1.f, 2.f, 3.f};
|
||||||
@@ -62,7 +70,8 @@ TEST(sparseVectorTest, additionWithDenseVector) {
|
|||||||
EXPECT_FLOAT_EQ(res(3), 4.f);
|
EXPECT_FLOAT_EQ(res(3), 4.f);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST(sparseVectorTest, addScalar) {
|
TEST(sparseVectorTest, addScalar)
|
||||||
|
{
|
||||||
const float data[3] = {1.f, 2.f, 3.f};
|
const float data[3] = {1.f, 2.f, 3.f};
|
||||||
SparseVectorf<4, 1, 2, 3> sparse_vec(data);
|
SparseVectorf<4, 1, 2, 3> sparse_vec(data);
|
||||||
sparse_vec += 2.f;
|
sparse_vec += 2.f;
|
||||||
@@ -71,7 +80,8 @@ TEST(sparseVectorTest, addScalar) {
|
|||||||
EXPECT_FLOAT_EQ(sparse_vec.at<3>(), 5.f);
|
EXPECT_FLOAT_EQ(sparse_vec.at<3>(), 5.f);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST(sparseVectorTest, dotProductWithDenseVector) {
|
TEST(sparseVectorTest, dotProductWithDenseVector)
|
||||||
|
{
|
||||||
Vector<float, 4> dense_vec;
|
Vector<float, 4> dense_vec;
|
||||||
dense_vec.setAll(3.f);
|
dense_vec.setAll(3.f);
|
||||||
const float data[3] = {1.f, 2.f, 3.f};
|
const float data[3] = {1.f, 2.f, 3.f};
|
||||||
@@ -80,7 +90,8 @@ TEST(sparseVectorTest, dotProductWithDenseVector) {
|
|||||||
EXPECT_FLOAT_EQ(res, 18.f);
|
EXPECT_FLOAT_EQ(res, 18.f);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST(sparseVectorTest, multiplicationWithDenseMatrix) {
|
TEST(sparseVectorTest, multiplicationWithDenseMatrix)
|
||||||
|
{
|
||||||
Matrix<float, 2, 3> dense_matrix;
|
Matrix<float, 2, 3> dense_matrix;
|
||||||
dense_matrix.setAll(2.f);
|
dense_matrix.setAll(2.f);
|
||||||
dense_matrix(1, 1) = 3.f;
|
dense_matrix(1, 1) = 3.f;
|
||||||
@@ -91,7 +102,8 @@ TEST(sparseVectorTest, multiplicationWithDenseMatrix) {
|
|||||||
EXPECT_TRUE(isEqual(res_dense, res_sparse));
|
EXPECT_TRUE(isEqual(res_dense, res_sparse));
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST(sparseVectorTest, quadraticForm) {
|
TEST(sparseVectorTest, quadraticForm)
|
||||||
|
{
|
||||||
float matrix_data[9] = {1, 2, 3,
|
float matrix_data[9] = {1, 2, 3,
|
||||||
2, 4, 5,
|
2, 4, 5,
|
||||||
3, 5, 6
|
3, 5, 6
|
||||||
@@ -102,7 +114,8 @@ TEST(sparseVectorTest, quadraticForm) {
|
|||||||
EXPECT_FLOAT_EQ(quadraticForm(dense_matrix, sparse_vec), 204.f);
|
EXPECT_FLOAT_EQ(quadraticForm(dense_matrix, sparse_vec), 204.f);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST(sparseVectorTest, norms) {
|
TEST(sparseVectorTest, norms)
|
||||||
|
{
|
||||||
const float data[2] = {3.f, 4.f};
|
const float data[2] = {3.f, 4.f};
|
||||||
const SparseVectorf<4, 1, 3> sparse_vec(data);
|
const SparseVectorf<4, 1, 3> sparse_vec(data);
|
||||||
EXPECT_FLOAT_EQ(sparse_vec.norm_squared(), 25.f);
|
EXPECT_FLOAT_EQ(sparse_vec.norm_squared(), 25.f);
|
||||||
@@ -112,7 +125,8 @@ TEST(sparseVectorTest, norms) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
int main(int argc, char **argv) {
|
int main(int argc, char **argv)
|
||||||
|
{
|
||||||
testing::InitGoogleTest(&argc, argv);
|
testing::InitGoogleTest(&argc, argv);
|
||||||
std::cout << "Run SparseVector tests" << std::endl;
|
std::cout << "Run SparseVector tests" << std::endl;
|
||||||
return RUN_ALL_TESTS();
|
return RUN_ALL_TESTS();
|
||||||
|
|||||||
@@ -145,4 +145,3 @@ int main()
|
|||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
|
|
||||||
|
|||||||
@@ -145,5 +145,3 @@ print('b:')
|
|||||||
pprint(b)
|
pprint(b)
|
||||||
print('x:')
|
print('x:')
|
||||||
pprint(x)
|
pprint(x)
|
||||||
|
|
||||||
# vim: set et ft=python fenc=utf-8 ff=unix sts=4 sw=4 ts=8 :
|
|
||||||
|
|||||||
@@ -16,4 +16,3 @@ int main()
|
|||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
|
|
||||||
|
|||||||
@@ -20,4 +20,3 @@ int main()
|
|||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
|
|
||||||
|
|||||||
@@ -45,4 +45,3 @@ int main()
|
|||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
|
|
||||||
|
|||||||
@@ -37,4 +37,3 @@ int main()
|
|||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
|
|
||||||
|
|||||||
@@ -58,4 +58,3 @@ int main()
|
|||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
|
|
||||||
|
|||||||
@@ -31,4 +31,3 @@ int main()
|
|||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
|
|
||||||
|
|||||||
Reference in New Issue
Block a user