mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-12 22:37:35 +08:00
Formatting and added Scalar.
This commit is contained in:
jgoppert
@@ -1 +1,2 @@
|
||||
build*/
|
||||
*.orig
|
||||
|
||||
@@ -2,9 +2,15 @@ cmake_minimum_required(VERSION 2.8)
|
||||
|
||||
project(matrix CXX)
|
||||
|
||||
if (NOT CMAKE_BUILD_TYPE)
|
||||
set(CMAKE_BUILD_TYPE "RelWithDebInfo" CACHE STRING "Build type" FORCE)
|
||||
message(STATUS "set build type to ${CMAKE_BUILD_TYPE}")
|
||||
endif()
|
||||
|
||||
list(APPEND CMAKE_CXX_FLAGS
|
||||
-Wall
|
||||
-Weffc++
|
||||
#-Werror
|
||||
-Wfatal-errors
|
||||
)
|
||||
|
||||
@@ -15,3 +21,11 @@ enable_testing()
|
||||
include_directories(matrix)
|
||||
|
||||
add_subdirectory(test)
|
||||
|
||||
add_custom_target(format
|
||||
COMMAND astyle --recursive
|
||||
${CMAKE_SOURCE_DIR}/matrix/*.*pp
|
||||
${CMAKE_SOURCE_DIR}/test/*.*pp
|
||||
WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
|
||||
VERBATIM
|
||||
)
|
||||
|
||||
+46
-31
@@ -22,41 +22,56 @@ template<typename Type>
|
||||
class Dcm : public Matrix<Type, 3, 3>
|
||||
{
|
||||
public:
|
||||
virtual ~Dcm() {};
|
||||
virtual ~Dcm() {};
|
||||
|
||||
typedef Matrix<Type, 3, 1> Vector3;
|
||||
typedef Matrix<Type, 3, 1> Vector3;
|
||||
|
||||
Dcm() : Matrix<Type, 3, 3>()
|
||||
{
|
||||
}
|
||||
Dcm() : Matrix<Type, 3, 3>()
|
||||
{
|
||||
}
|
||||
|
||||
Dcm(const Quaternion<Type> & q) {
|
||||
// TODO
|
||||
Dcm &c = *this;
|
||||
c(0, 0) = 0;
|
||||
c(0, 1) = 0;
|
||||
c(0, 2) = 0;
|
||||
c(1, 0) = 0;
|
||||
c(1, 1) = 0;
|
||||
c(1, 2) = 0;
|
||||
c(2, 0) = 0;
|
||||
c(2, 1) = 0;
|
||||
c(2, 2) = 0;
|
||||
}
|
||||
Dcm(const Quaternion<Type> & q) {
|
||||
Dcm &dcm = *this;
|
||||
Type a = q(0);
|
||||
Type b = q(1);
|
||||
Type c = q(2);
|
||||
Type d = q(3);
|
||||
Type aSq = a*a;
|
||||
Type bSq = b*b;
|
||||
Type cSq = c*c;
|
||||
Type dSq = d*d;
|
||||
dcm(0, 0) = aSq + bSq - cSq - dSq;
|
||||
dcm(0, 1) = 2 * (b * c - a * d);
|
||||
dcm(0, 2) = 2 * (a * c + b * d);
|
||||
dcm(1, 0) = 2 * (b * c + a * d);
|
||||
dcm(1, 1) = aSq - bSq + cSq - dSq;
|
||||
dcm(1, 2) = 2 * (c * d - a * b);
|
||||
dcm(2, 0) = 2 * (b * d - a * c);
|
||||
dcm(2, 1) = 2 * (a * b + c * d);
|
||||
dcm(2, 2) = aSq - bSq - cSq + dSq;
|
||||
}
|
||||
|
||||
Dcm(const Euler<Type> & e) {
|
||||
// TODO
|
||||
Dcm &c = *this;
|
||||
c(0, 0) = 0;
|
||||
c(0, 1) = 0;
|
||||
c(0, 2) = 0;
|
||||
c(1, 0) = 0;
|
||||
c(1, 1) = 0;
|
||||
c(1, 2) = 0;
|
||||
c(2, 0) = 0;
|
||||
c(2, 1) = 0;
|
||||
c(2, 2) = 0;
|
||||
}
|
||||
Dcm(const Euler<Type> & euler) {
|
||||
Dcm &dcm = *this;
|
||||
Type cosPhi = cosf(euler.phi());
|
||||
Type sinPhi = sinf(euler.phi());
|
||||
Type cosThe = cosf(euler.theta());
|
||||
Type sinThe = sinf(euler.theta());
|
||||
Type cosPsi = cosf(euler.psi());
|
||||
Type sinPsi = sinf(euler.psi());
|
||||
|
||||
dcm(0, 0) = cosThe * cosPsi;
|
||||
dcm(0, 1) = -cosPhi * sinPsi + sinPhi * sinThe * cosPsi;
|
||||
dcm(0, 2) = sinPhi * sinPsi + cosPhi * sinThe * cosPsi;
|
||||
|
||||
dcm(1, 0) = cosThe * sinPsi;
|
||||
dcm(1, 1) = cosPhi * cosPsi + sinPhi * sinThe * sinPsi;
|
||||
dcm(1, 2) = -sinPhi * cosPsi + cosPhi * sinThe * sinPsi;
|
||||
|
||||
dcm(2, 0) = -sinThe;
|
||||
dcm(2, 1) = sinPhi * cosThe;
|
||||
dcm(2, 2) = cosPhi * cosThe;
|
||||
}
|
||||
};
|
||||
|
||||
typedef Dcm<float> Dcmf;
|
||||
|
||||
+54
-25
@@ -24,35 +24,64 @@ template<typename Type>
|
||||
class Euler : public Vector<Type, 3>
|
||||
{
|
||||
public:
|
||||
virtual ~Euler() {};
|
||||
virtual ~Euler() {};
|
||||
|
||||
Euler() : Vector<Type, 3>()
|
||||
{
|
||||
}
|
||||
Euler() : Vector<Type, 3>()
|
||||
{
|
||||
}
|
||||
|
||||
Euler(Type roll, Type pitch, Type yaw) : Vector<Type, 3>()
|
||||
{
|
||||
Euler &v(*this);
|
||||
v(0) = roll;
|
||||
v(1) = pitch;
|
||||
v(2) = yaw;
|
||||
}
|
||||
Euler(Type phi, Type theta, Type psi) : Vector<Type, 3>()
|
||||
{
|
||||
this->phi() = phi;
|
||||
this->theta() = theta;
|
||||
this->psi() = psi;
|
||||
}
|
||||
|
||||
Euler(const Dcm<Type> & dcm) {
|
||||
// TODO
|
||||
Euler &e = *this;
|
||||
e(0) = 0;
|
||||
e(1) = 0;
|
||||
e(2) = 0;
|
||||
}
|
||||
Euler(const Dcm<Type> & dcm) {
|
||||
Type theta = asin(-dcm(2, 0));
|
||||
Type phi = 0;
|
||||
Type psi = 0;
|
||||
|
||||
Euler(const Quaternion<Type> & q) {
|
||||
// TODO
|
||||
Euler &e = *this;
|
||||
e(0) = 0;
|
||||
e(1) = 0;
|
||||
e(2) = 0;
|
||||
}
|
||||
if (abs(theta - M_PI_2) < 1.0e-3) {
|
||||
psi = atan2(dcm(1, 2) - dcm(0, 1),
|
||||
dcm(0, 2) + dcm(1, 1));
|
||||
|
||||
} else if (abs(theta + M_PI_2) < 1.0e-3) {
|
||||
psi = atan2(dcm(1, 2) - dcm(0, 1),
|
||||
dcm(0, 2) + dcm(1, 1));
|
||||
|
||||
} else {
|
||||
phi = atan2f(dcm(2, 1), dcm(2, 2));
|
||||
psi = atan2f(dcm(1, 0), dcm(0, 0));
|
||||
}
|
||||
this->phi() = phi;
|
||||
this->theta() = theta;
|
||||
this->psi() = psi;
|
||||
}
|
||||
|
||||
Euler(const Quaternion<Type> & q) {
|
||||
*this = Euler(Dcm<Type>(q));
|
||||
}
|
||||
|
||||
inline Type phi() const {
|
||||
return (*this)(0);
|
||||
}
|
||||
inline Type theta() const {
|
||||
return (*this)(1);
|
||||
}
|
||||
inline Type psi() const {
|
||||
return (*this)(2);
|
||||
}
|
||||
|
||||
inline Type & phi() {
|
||||
return (*this)(0);
|
||||
}
|
||||
inline Type & theta() {
|
||||
return (*this)(1);
|
||||
}
|
||||
inline Type & psi() {
|
||||
return (*this)(2);
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
|
||||
+405
-422
@@ -22,451 +22,434 @@ class Matrix
|
||||
{
|
||||
|
||||
protected:
|
||||
Type _data[M][N];
|
||||
size_t _rows;
|
||||
size_t _cols;
|
||||
Type _data[M][N];
|
||||
|
||||
public:
|
||||
|
||||
virtual ~Matrix() {};
|
||||
|
||||
Matrix() :
|
||||
_data(),
|
||||
_rows(M),
|
||||
_cols(N)
|
||||
{
|
||||
}
|
||||
|
||||
Matrix(const Type *data) :
|
||||
_data(),
|
||||
_rows(M),
|
||||
_cols(N)
|
||||
{
|
||||
memcpy(_data, data, sizeof(_data));
|
||||
}
|
||||
|
||||
Matrix(const Matrix &other) :
|
||||
_data(),
|
||||
_rows(M),
|
||||
_cols(N)
|
||||
{
|
||||
memcpy(_data, other._data, sizeof(_data));
|
||||
}
|
||||
|
||||
/**
|
||||
* Accessors/ Assignment etc.
|
||||
*/
|
||||
|
||||
Type *data()
|
||||
{
|
||||
return _data[0];
|
||||
}
|
||||
|
||||
inline size_t rows() const
|
||||
{
|
||||
return _rows;
|
||||
}
|
||||
|
||||
inline size_t cols() const
|
||||
{
|
||||
return _rows;
|
||||
}
|
||||
|
||||
inline Type operator()(size_t i, size_t j) const
|
||||
{
|
||||
return _data[i][j];
|
||||
}
|
||||
|
||||
inline Type &operator()(size_t i, size_t j)
|
||||
{
|
||||
return _data[i][j];
|
||||
}
|
||||
|
||||
/**
|
||||
* Matrix Operations
|
||||
*/
|
||||
|
||||
// this might use a lot of programming memory
|
||||
// since it instantiates a class for every
|
||||
// required mult pair, but it provides
|
||||
// compile time size_t checking
|
||||
template<size_t P>
|
||||
Matrix<Type, M, P> operator*(const Matrix<Type, N, P> &other) const
|
||||
{
|
||||
const Matrix<Type, M, N> &self = *this;
|
||||
Matrix<Type, M, P> res;
|
||||
res.setZero();
|
||||
|
||||
for (size_t i = 0; i < M; i++) {
|
||||
for (size_t k = 0; k < P; k++) {
|
||||
for (size_t j = 0; j < N; j++) {
|
||||
res(i, k) += self(i, j) * other(j, k);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
Matrix<Type, M, N> operator+(const Matrix<Type, M, N> &other) const
|
||||
{
|
||||
Matrix<Type, M, N> res;
|
||||
const Matrix<Type, M, N> &self = *this;
|
||||
|
||||
for (size_t i = 0; i < M; i++) {
|
||||
for (size_t j = 0; j < N; j++) {
|
||||
res(i , j) = self(i, j) + other(i, j);
|
||||
}
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
bool operator==(const Matrix<Type, M, N> &other) const
|
||||
{
|
||||
Matrix<Type, M, N> res;
|
||||
const Matrix<Type, M, N> &self = *this;
|
||||
|
||||
for (size_t i = 0; i < M; i++) {
|
||||
for (size_t j = 0; j < N; j++) {
|
||||
if (self(i , j) != other(i, j)) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
operator Type();
|
||||
|
||||
Matrix<Type, M, N> operator-(const Matrix<Type, M, N> &other) const
|
||||
{
|
||||
Matrix<Type, M, N> res;
|
||||
const Matrix<Type, M, N> &self = *this;
|
||||
|
||||
for (size_t i = 0; i < M; i++) {
|
||||
for (size_t j = 0; j < N; j++) {
|
||||
res(i , j) = self(i, j) - other(i, j);
|
||||
}
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
void operator+=(const Matrix<Type, M, N> &other)
|
||||
{
|
||||
Matrix<Type, M, N> &self = *this;
|
||||
self = self + other;
|
||||
}
|
||||
|
||||
void operator-=(const Matrix<Type, M, N> &other)
|
||||
{
|
||||
Matrix<Type, M, N> &self = *this;
|
||||
self = self - other;
|
||||
}
|
||||
|
||||
void operator*=(const Matrix<Type, M, N> &other)
|
||||
{
|
||||
Matrix<Type, M, N> &self = *this;
|
||||
self = self * other;
|
||||
}
|
||||
|
||||
/**
|
||||
* Scalar Operations
|
||||
*/
|
||||
|
||||
Matrix<Type, M, N> operator*(Type scalar) const
|
||||
{
|
||||
Matrix<Type, M, N> res;
|
||||
const Matrix<Type, M, N> &self = *this;
|
||||
|
||||
for (size_t i = 0; i < M; i++) {
|
||||
for (size_t j = 0; j < N; j++) {
|
||||
res(i , j) = self(i, j) * scalar;
|
||||
}
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
Matrix<Type, M, N> operator+(Type scalar) const
|
||||
{
|
||||
Matrix<Type, M, N> res;
|
||||
Matrix<Type, M, N> &self = *this;
|
||||
|
||||
for (size_t i = 0; i < M; i++) {
|
||||
for (size_t j = 0; j < N; j++) {
|
||||
res(i , j) = self(i, j) + scalar;
|
||||
}
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
void operator*=(Type scalar)
|
||||
{
|
||||
Matrix<Type, M, N> &self = *this;
|
||||
|
||||
for (size_t i = 0; i < M; i++) {
|
||||
for (size_t j = 0; j < N; j++) {
|
||||
self(i, j) = self(i, j) * scalar;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void operator/=(Type scalar)
|
||||
{
|
||||
Matrix<Type, M, N> &self = *this;
|
||||
self = self * (1.0f / scalar);
|
||||
}
|
||||
|
||||
/**
|
||||
* Misc. Functions
|
||||
*/
|
||||
|
||||
void print()
|
||||
{
|
||||
Matrix<Type, M, N> &self = *this;
|
||||
printf("\n");
|
||||
|
||||
for (size_t i = 0; i < M; i++) {
|
||||
printf("[");
|
||||
|
||||
for (size_t j = 0; j < N; j++) {
|
||||
printf("%10g\t", double(self(i, j)));
|
||||
}
|
||||
|
||||
printf("]\n");
|
||||
}
|
||||
}
|
||||
|
||||
Matrix<Type, N, M> transpose() const
|
||||
{
|
||||
Matrix<Type, N, M> res;
|
||||
const Matrix<Type, M, N> &self = *this;
|
||||
|
||||
for (size_t i = 0; i < M; i++) {
|
||||
for (size_t j = 0; j < N; j++) {
|
||||
res(j, i) = self(i, j);
|
||||
}
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
|
||||
// tranpose alias
|
||||
inline Matrix<Type, N, M> T() const
|
||||
{
|
||||
return transpose();
|
||||
}
|
||||
|
||||
Matrix<Type, M, M> expm(float dt, size_t n) const
|
||||
{
|
||||
Matrix<float, M, M> res;
|
||||
res.setIdentity();
|
||||
Matrix<float, M, M> A_pow = *this;
|
||||
float k_fact = 1;
|
||||
size_t k = 1;
|
||||
|
||||
while (k < n) {
|
||||
res += A_pow * (float(pow(dt, k)) / k_fact);
|
||||
|
||||
if (k == n) { break; }
|
||||
|
||||
A_pow *= A_pow;
|
||||
k_fact *= k;
|
||||
k++;
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
Matrix<Type, M, 1> diagonal() const
|
||||
{
|
||||
Matrix<Type, M, 1> res;
|
||||
// force square for now
|
||||
const Matrix<Type, M, M> &self = *this;
|
||||
|
||||
for (size_t i = 0; i < M; i++) {
|
||||
res(i) = self(i, i);
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
void setZero()
|
||||
{
|
||||
memset(_data, 0, sizeof(_data));
|
||||
}
|
||||
|
||||
void setIdentity()
|
||||
{
|
||||
setZero();
|
||||
Matrix<Type, M, N> &self = *this;
|
||||
|
||||
for (size_t i = 0; i < M and i < N; i++) {
|
||||
self(i, i) = 1;
|
||||
}
|
||||
}
|
||||
|
||||
inline void swapRows(size_t a, size_t b)
|
||||
{
|
||||
if (a == b) { return; }
|
||||
|
||||
Matrix<Type, M, N> &self = *this;
|
||||
|
||||
for (size_t j = 0; j < cols(); j++) {
|
||||
Type tmp = self(a, j);
|
||||
self(a, j) = self(b, j);
|
||||
self(b, j) = tmp;
|
||||
}
|
||||
}
|
||||
|
||||
inline void swapCols(size_t a, size_t b)
|
||||
{
|
||||
if (a == b) { return; }
|
||||
|
||||
Matrix<Type, M, N> &self = *this;
|
||||
|
||||
for (size_t i = 0; i < rows(); i++) {
|
||||
Type tmp = self(i, a);
|
||||
self(i, a) = self(i, b);
|
||||
self(i, b) = tmp;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* inverse based on LU factorization with partial pivotting
|
||||
*/
|
||||
Matrix <Type, M, M> inverse() const
|
||||
{
|
||||
Matrix<Type, M, M> L;
|
||||
L.setIdentity();
|
||||
const Matrix<Type, M, M> &A = (*this);
|
||||
Matrix<Type, M, M> U = A;
|
||||
Matrix<Type, M, M> P;
|
||||
P.setIdentity();
|
||||
|
||||
//printf("A:\n"); A.print();
|
||||
|
||||
// for all diagonal elements
|
||||
for (size_t n = 0; n < N; n++) {
|
||||
|
||||
// if diagonal is zero, swap with row below
|
||||
if (fabsf(U(n, n)) < 1e-8f) {
|
||||
//printf("trying pivot for row %d\n",n);
|
||||
for (size_t i = 0; i < N; i++) {
|
||||
if (i == n) { continue; }
|
||||
|
||||
//printf("\ttrying row %d\n",i);
|
||||
if (fabsf(U(i, n)) > 1e-8f) {
|
||||
//printf("swapped %d\n",i);
|
||||
U.swapRows(i, n);
|
||||
P.swapRows(i, n);
|
||||
}
|
||||
}
|
||||
}
|
||||
virtual ~Matrix() {};
|
||||
|
||||
Matrix() :
|
||||
_data()
|
||||
{
|
||||
}
|
||||
|
||||
Matrix(const Type *data) :
|
||||
_data()
|
||||
{
|
||||
memcpy(_data, data, sizeof(_data));
|
||||
}
|
||||
|
||||
Matrix(const Matrix &other) :
|
||||
_data()
|
||||
{
|
||||
memcpy(_data, other._data, sizeof(_data));
|
||||
}
|
||||
|
||||
/**
|
||||
* Accessors/ Assignment etc.
|
||||
*/
|
||||
|
||||
Type *data()
|
||||
{
|
||||
return _data[0];
|
||||
}
|
||||
|
||||
inline Type operator()(size_t i, size_t j) const
|
||||
{
|
||||
return _data[i][j];
|
||||
}
|
||||
|
||||
inline Type &operator()(size_t i, size_t j)
|
||||
{
|
||||
return _data[i][j];
|
||||
}
|
||||
|
||||
/**
|
||||
* Matrix Operations
|
||||
*/
|
||||
|
||||
// this might use a lot of programming memory
|
||||
// since it instantiates a class for every
|
||||
// required mult pair, but it provides
|
||||
// compile time size_t checking
|
||||
template<size_t P>
|
||||
Matrix<Type, M, P> operator*(const Matrix<Type, N, P> &other) const
|
||||
{
|
||||
const Matrix<Type, M, N> &self = *this;
|
||||
Matrix<Type, M, P> res;
|
||||
res.setZero();
|
||||
|
||||
for (size_t i = 0; i < M; i++) {
|
||||
for (size_t k = 0; k < P; k++) {
|
||||
for (size_t j = 0; j < N; j++) {
|
||||
res(i, k) += self(i, j) * other(j, k);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
Matrix<Type, M, N> operator+(const Matrix<Type, M, N> &other) const
|
||||
{
|
||||
Matrix<Type, M, N> res;
|
||||
const Matrix<Type, M, N> &self = *this;
|
||||
|
||||
for (size_t i = 0; i < M; i++) {
|
||||
for (size_t j = 0; j < N; j++) {
|
||||
res(i , j) = self(i, j) + other(i, j);
|
||||
}
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
bool operator==(const Matrix<Type, M, N> &other) const
|
||||
{
|
||||
Matrix<Type, M, N> res;
|
||||
const Matrix<Type, M, N> &self = *this;
|
||||
|
||||
for (size_t i = 0; i < M; i++) {
|
||||
for (size_t j = 0; j < N; j++) {
|
||||
if (self(i , j) != other(i, j)) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
Matrix<Type, M, N> operator-(const Matrix<Type, M, N> &other) const
|
||||
{
|
||||
Matrix<Type, M, N> res;
|
||||
const Matrix<Type, M, N> &self = *this;
|
||||
|
||||
for (size_t i = 0; i < M; i++) {
|
||||
for (size_t j = 0; j < N; j++) {
|
||||
res(i , j) = self(i, j) - other(i, j);
|
||||
}
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
void operator+=(const Matrix<Type, M, N> &other)
|
||||
{
|
||||
Matrix<Type, M, N> &self = *this;
|
||||
self = self + other;
|
||||
}
|
||||
|
||||
void operator-=(const Matrix<Type, M, N> &other)
|
||||
{
|
||||
Matrix<Type, M, N> &self = *this;
|
||||
self = self - other;
|
||||
}
|
||||
|
||||
void operator*=(const Matrix<Type, M, N> &other)
|
||||
{
|
||||
Matrix<Type, M, N> &self = *this;
|
||||
self = self * other;
|
||||
}
|
||||
|
||||
/**
|
||||
* Scalar Operations
|
||||
*/
|
||||
|
||||
Matrix<Type, M, N> operator*(Type scalar) const
|
||||
{
|
||||
Matrix<Type, M, N> res;
|
||||
const Matrix<Type, M, N> &self = *this;
|
||||
|
||||
for (size_t i = 0; i < M; i++) {
|
||||
for (size_t j = 0; j < N; j++) {
|
||||
res(i , j) = self(i, j) * scalar;
|
||||
}
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
Matrix<Type, M, N> operator+(Type scalar) const
|
||||
{
|
||||
Matrix<Type, M, N> res;
|
||||
Matrix<Type, M, N> &self = *this;
|
||||
|
||||
for (size_t i = 0; i < M; i++) {
|
||||
for (size_t j = 0; j < N; j++) {
|
||||
res(i , j) = self(i, j) + scalar;
|
||||
}
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
void operator*=(Type scalar)
|
||||
{
|
||||
Matrix<Type, M, N> &self = *this;
|
||||
|
||||
for (size_t i = 0; i < M; i++) {
|
||||
for (size_t j = 0; j < N; j++) {
|
||||
self(i, j) = self(i, j) * scalar;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void operator/=(Type scalar)
|
||||
{
|
||||
Matrix<Type, M, N> &self = *this;
|
||||
self = self * (1.0f / scalar);
|
||||
}
|
||||
|
||||
/**
|
||||
* Misc. Functions
|
||||
*/
|
||||
|
||||
void print()
|
||||
{
|
||||
Matrix<Type, M, N> &self = *this;
|
||||
printf("\n");
|
||||
|
||||
for (size_t i = 0; i < M; i++) {
|
||||
printf("[");
|
||||
|
||||
for (size_t j = 0; j < N; j++) {
|
||||
printf("%10g\t", double(self(i, j)));
|
||||
}
|
||||
|
||||
printf("]\n");
|
||||
}
|
||||
}
|
||||
|
||||
Matrix<Type, N, M> transpose() const
|
||||
{
|
||||
Matrix<Type, N, M> res;
|
||||
const Matrix<Type, M, N> &self = *this;
|
||||
|
||||
for (size_t i = 0; i < M; i++) {
|
||||
for (size_t j = 0; j < N; j++) {
|
||||
res(j, i) = self(i, j);
|
||||
}
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
|
||||
// tranpose alias
|
||||
inline Matrix<Type, N, M> T() const
|
||||
{
|
||||
return transpose();
|
||||
}
|
||||
|
||||
Matrix<Type, M, M> expm(float dt, size_t n) const
|
||||
{
|
||||
Matrix<float, M, M> res;
|
||||
res.setIdentity();
|
||||
Matrix<float, M, M> A_pow = *this;
|
||||
float k_fact = 1;
|
||||
size_t k = 1;
|
||||
|
||||
while (k < n) {
|
||||
res += A_pow * (float(pow(dt, k)) / k_fact);
|
||||
|
||||
if (k == n) {
|
||||
break;
|
||||
}
|
||||
|
||||
A_pow *= A_pow;
|
||||
k_fact *= k;
|
||||
k++;
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
Matrix<Type, M, 1> diagonal() const
|
||||
{
|
||||
Matrix<Type, M, 1> res;
|
||||
// force square for now
|
||||
const Matrix<Type, M, M> &self = *this;
|
||||
|
||||
for (size_t i = 0; i < M; i++) {
|
||||
res(i) = self(i, i);
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
void setZero()
|
||||
{
|
||||
memset(_data, 0, sizeof(_data));
|
||||
}
|
||||
|
||||
void setIdentity()
|
||||
{
|
||||
setZero();
|
||||
Matrix<Type, M, N> &self = *this;
|
||||
|
||||
for (size_t i = 0; i < M and i < N; i++) {
|
||||
self(i, i) = 1;
|
||||
}
|
||||
}
|
||||
|
||||
inline void swapRows(size_t a, size_t b)
|
||||
{
|
||||
if (a == b) {
|
||||
return;
|
||||
}
|
||||
|
||||
Matrix<Type, M, N> &self = *this;
|
||||
|
||||
for (size_t j = 0; j < N; j++) {
|
||||
Type tmp = self(a, j);
|
||||
self(a, j) = self(b, j);
|
||||
self(b, j) = tmp;
|
||||
}
|
||||
}
|
||||
|
||||
inline void swapCols(size_t a, size_t b)
|
||||
{
|
||||
if (a == b) {
|
||||
return;
|
||||
}
|
||||
|
||||
Matrix<Type, M, N> &self = *this;
|
||||
|
||||
for (size_t i = 0; i < M; i++) {
|
||||
Type tmp = self(i, a);
|
||||
self(i, a) = self(i, b);
|
||||
self(i, b) = tmp;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* inverse based on LU factorization with partial pivotting
|
||||
*/
|
||||
Matrix <Type, M, M> inverse() const
|
||||
{
|
||||
Matrix<Type, M, M> L;
|
||||
L.setIdentity();
|
||||
const Matrix<Type, M, M> &A = (*this);
|
||||
Matrix<Type, M, M> U = A;
|
||||
Matrix<Type, M, M> P;
|
||||
P.setIdentity();
|
||||
|
||||
//printf("A:\n"); A.print();
|
||||
|
||||
// for all diagonal elements
|
||||
for (size_t n = 0; n < N; n++) {
|
||||
|
||||
// if diagonal is zero, swap with row below
|
||||
if (fabsf(U(n, n)) < 1e-8f) {
|
||||
//printf("trying pivot for row %d\n",n);
|
||||
for (size_t i = 0; i < N; i++) {
|
||||
if (i == n) {
|
||||
continue;
|
||||
}
|
||||
|
||||
//printf("\ttrying row %d\n",i);
|
||||
if (fabsf(U(i, n)) > 1e-8f) {
|
||||
//printf("swapped %d\n",i);
|
||||
U.swapRows(i, n);
|
||||
P.swapRows(i, n);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#ifdef MATRIX_ASSERT
|
||||
//printf("A:\n"); A.print();
|
||||
//printf("U:\n"); U.print();
|
||||
//printf("P:\n"); P.print();
|
||||
//fflush(stdout);
|
||||
ASSERT(fabsf(U(n, n)) > 1e-8f);
|
||||
//printf("A:\n"); A.print();
|
||||
//printf("U:\n"); U.print();
|
||||
//printf("P:\n"); P.print();
|
||||
//fflush(stdout);
|
||||
ASSERT(fabsf(U(n, n)) > 1e-8f);
|
||||
#endif
|
||||
|
||||
// failsafe, return zero matrix
|
||||
if (fabsf(U(n, n)) < 1e-8f) {
|
||||
Matrix<Type, M, M> zero;
|
||||
zero.setZero();
|
||||
return zero;
|
||||
}
|
||||
// failsafe, return zero matrix
|
||||
if (fabsf(U(n, n)) < 1e-8f) {
|
||||
Matrix<Type, M, M> zero;
|
||||
zero.setZero();
|
||||
return zero;
|
||||
}
|
||||
|
||||
// for all rows below diagonal
|
||||
for (size_t i = (n + 1); i < N; i++) {
|
||||
L(i, n) = U(i, n) / U(n, n);
|
||||
// for all rows below diagonal
|
||||
for (size_t i = (n + 1); i < N; i++) {
|
||||
L(i, n) = U(i, n) / U(n, n);
|
||||
|
||||
// add i-th row and n-th row
|
||||
// multiplied by: -a(i,n)/a(n,n)
|
||||
for (size_t k = n; k < N; k++) {
|
||||
U(i, k) -= L(i, n) * U(n, k);
|
||||
}
|
||||
}
|
||||
}
|
||||
// add i-th row and n-th row
|
||||
// multiplied by: -a(i,n)/a(n,n)
|
||||
for (size_t k = n; k < N; k++) {
|
||||
U(i, k) -= L(i, n) * U(n, k);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//printf("L:\n"); L.print();
|
||||
//printf("U:\n"); U.print();
|
||||
//printf("L:\n"); L.print();
|
||||
//printf("U:\n"); U.print();
|
||||
|
||||
// solve LY=P*I for Y by forward subst
|
||||
Matrix<Type, M, M> Y = P;
|
||||
// solve LY=P*I for Y by forward subst
|
||||
Matrix<Type, M, M> Y = P;
|
||||
|
||||
// for all columns of Y
|
||||
for (size_t c = 0; c < N; c++) {
|
||||
// for all rows of L
|
||||
for (size_t i = 0; i < N; i++) {
|
||||
// for all columns of L
|
||||
for (size_t j = 0; j < i; j++) {
|
||||
// for all existing y
|
||||
// subtract the component they
|
||||
// contribute to the solution
|
||||
Y(i, c) -= L(i, j) * Y(j, c);
|
||||
}
|
||||
// for all columns of Y
|
||||
for (size_t c = 0; c < N; c++) {
|
||||
// for all rows of L
|
||||
for (size_t i = 0; i < N; i++) {
|
||||
// for all columns of L
|
||||
for (size_t j = 0; j < i; j++) {
|
||||
// for all existing y
|
||||
// subtract the component they
|
||||
// contribute to the solution
|
||||
Y(i, c) -= L(i, j) * Y(j, c);
|
||||
}
|
||||
|
||||
// divide by the factor
|
||||
// on current
|
||||
// term to be solved
|
||||
// Y(i,c) /= L(i,i);
|
||||
// but L(i,i) = 1.0
|
||||
}
|
||||
}
|
||||
// divide by the factor
|
||||
// on current
|
||||
// term to be solved
|
||||
// Y(i,c) /= L(i,i);
|
||||
// but L(i,i) = 1.0
|
||||
}
|
||||
}
|
||||
|
||||
//printf("Y:\n"); Y.print();
|
||||
//printf("Y:\n"); Y.print();
|
||||
|
||||
// solve Ux=y for x by back subst
|
||||
Matrix<Type, M, M> X = Y;
|
||||
// solve Ux=y for x by back subst
|
||||
Matrix<Type, M, M> X = Y;
|
||||
|
||||
// for all columns of X
|
||||
for (size_t c = 0; c < N; c++) {
|
||||
// for all rows of U
|
||||
for (size_t k = 0; k < N; k++) {
|
||||
// have to go in reverse order
|
||||
size_t i = N - 1 - k;
|
||||
// for all columns of X
|
||||
for (size_t c = 0; c < N; c++) {
|
||||
// for all rows of U
|
||||
for (size_t k = 0; k < N; k++) {
|
||||
// have to go in reverse order
|
||||
size_t i = N - 1 - k;
|
||||
|
||||
// for all columns of U
|
||||
for (size_t j = i + 1; j < N; j++) {
|
||||
// for all existing x
|
||||
// subtract the component they
|
||||
// contribute to the solution
|
||||
X(i, c) -= U(i, j) * X(j, c);
|
||||
}
|
||||
// for all columns of U
|
||||
for (size_t j = i + 1; j < N; j++) {
|
||||
// for all existing x
|
||||
// subtract the component they
|
||||
// contribute to the solution
|
||||
X(i, c) -= U(i, j) * X(j, c);
|
||||
}
|
||||
|
||||
// divide by the factor
|
||||
// on current
|
||||
// term to be solved
|
||||
X(i, c) /= U(i, i);
|
||||
}
|
||||
}
|
||||
// divide by the factor
|
||||
// on current
|
||||
// term to be solved
|
||||
X(i, c) /= U(i, i);
|
||||
}
|
||||
}
|
||||
|
||||
//printf("X:\n"); X.print();
|
||||
return X;
|
||||
}
|
||||
//printf("X:\n"); X.print();
|
||||
return X;
|
||||
}
|
||||
|
||||
// inverse alias
|
||||
inline Matrix<Type, N, M> I() const
|
||||
{
|
||||
return inverse();
|
||||
}
|
||||
// inverse alias
|
||||
inline Matrix<Type, N, M> I() const
|
||||
{
|
||||
return inverse();
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
template <>
|
||||
Matrix<float,1,1>::operator float()
|
||||
{
|
||||
return (*this)(0,0);
|
||||
}
|
||||
|
||||
typedef Matrix<float, 3,3> Matrix3f;
|
||||
typedef Matrix<float, 3, 3> Matrix3f;
|
||||
|
||||
}; // namespace matrix
|
||||
|
||||
+74
-52
@@ -24,64 +24,86 @@ template<typename Type>
|
||||
class Quaternion : public Vector<Type, 4>
|
||||
{
|
||||
public:
|
||||
virtual ~Quaternion() {};
|
||||
virtual ~Quaternion() {};
|
||||
|
||||
Quaternion() : Vector<Type, 4>()
|
||||
{
|
||||
// TODO
|
||||
Quaternion &q = *this;
|
||||
q(0) = 1;
|
||||
q(1) = 0;
|
||||
q(2) = 0;
|
||||
q(3) = 0;
|
||||
}
|
||||
Quaternion() :
|
||||
Vector<Type, 4>()
|
||||
{
|
||||
Quaternion &q = *this;
|
||||
q(0) = 1;
|
||||
q(1) = 2;
|
||||
q(2) = 3;
|
||||
q(3) = 4;
|
||||
}
|
||||
|
||||
Quaternion(const Dcm<Type> & dcm) {
|
||||
// TODO
|
||||
Quaternion &q = *this;
|
||||
q(0) = 0;
|
||||
q(1) = 0;
|
||||
q(2) = 0;
|
||||
q(3) = 0;
|
||||
}
|
||||
Quaternion(const Dcm<Type> & dcm) :
|
||||
Vector<Type, 4>()
|
||||
{
|
||||
Quaternion &q = *this;
|
||||
q(0) = 0.5 * sqrt(1 + dcm(0, 0) +
|
||||
dcm(1, 1) + dcm(2, 2));
|
||||
q(1) = (dcm(2, 1) - dcm(1, 2)) /
|
||||
(4 * q(0));
|
||||
q(2) = (dcm(0, 2) - dcm(2, 0)) /
|
||||
(4 * q(0));
|
||||
q(3) = (dcm(1, 0) - dcm(0, 1)) /
|
||||
(4 * q(0));
|
||||
}
|
||||
|
||||
Quaternion(const Euler<Type> & e) {
|
||||
// TODO
|
||||
Quaternion &q = *this;
|
||||
q(0) = 0;
|
||||
q(1) = 0;
|
||||
q(2) = 0;
|
||||
q(3) = 0;
|
||||
}
|
||||
Quaternion(const Euler<Type> & euler) {
|
||||
Quaternion &q = *this;
|
||||
Type cosPhi_2 = cos(euler.phi() / 2.0);
|
||||
Type cosTheta_2 = cos(euler.theta() / 2.0);
|
||||
Type cosPsi_2 = cos(euler.psi() / 2.0);
|
||||
Type sinPhi_2 = sin(euler.phi() / 2.0);
|
||||
Type sinTheta_2 = sin(euler.theta() / 2.0);
|
||||
Type sinPsi_2 = sin(euler.psi() / 2.0);
|
||||
q(0) = cosPhi_2 * cosTheta_2 * cosPsi_2 +
|
||||
sinPhi_2 * sinTheta_2 * sinPsi_2;
|
||||
q(1) = sinPhi_2 * cosTheta_2 * cosPsi_2 -
|
||||
cosPhi_2 * sinTheta_2 * sinPsi_2;
|
||||
q(2) = cosPhi_2 * sinTheta_2 * cosPsi_2 +
|
||||
sinPhi_2 * cosTheta_2 * sinPsi_2;
|
||||
q(3) = cosPhi_2 * cosTheta_2 * sinPsi_2 +
|
||||
sinPhi_2 * sinTheta_2 * cosPsi_2;
|
||||
}
|
||||
|
||||
Quaternion(Type a, Type b, Type c, Type d) : Vector<Type, 4>()
|
||||
{
|
||||
// TODO
|
||||
Quaternion &q = *this;
|
||||
q(0) = a;
|
||||
q(1) = b;
|
||||
q(2) = c;
|
||||
q(3) = d;
|
||||
}
|
||||
Quaternion(Type a, Type b, Type c, Type d) : Vector<Type, 4>()
|
||||
{
|
||||
Quaternion &q = *this;
|
||||
q(0) = a;
|
||||
q(1) = b;
|
||||
q(2) = c;
|
||||
q(3) = d;
|
||||
}
|
||||
|
||||
Quaternion operator*(const Quaternion &q) const
|
||||
{
|
||||
// TODO
|
||||
const Quaternion &p = *this;
|
||||
Quaternion r;
|
||||
r(0) = 0;
|
||||
r(1) = 0;
|
||||
r(2) = 0;
|
||||
r(3) = 0;
|
||||
return r;
|
||||
}
|
||||
|
||||
Matrix<Type, 4, 1> derivative() const {
|
||||
// TODO
|
||||
Matrix<Type, 4, 1> d;
|
||||
return d;
|
||||
}
|
||||
Quaternion operator*(const Quaternion &q) const
|
||||
{
|
||||
const Quaternion &p = *this;
|
||||
Quaternion r;
|
||||
r(0) = p(0)*q(0) - p(1)*q(1) - p(2)*q(2) - p(3)*q(3);
|
||||
r(1) = p(0)*q(1) + p(1)*q(0) - p(2)*q(3) + p(3)*q(2);
|
||||
r(2) = p(0)*q(2) + p(1)*q(3) + p(2)*q(0) - p(3)*q(1);
|
||||
r(3) = p(0)*q(3) - p(1)*q(2) + p(2)*q(1) + p(3)*q(0);
|
||||
return r;
|
||||
}
|
||||
|
||||
Matrix<Type, 4, 1> derivative(const Matrix<Type, 3, 1> & w) const {
|
||||
const Quaternion &q = *this;
|
||||
Type dataQ[] = {
|
||||
q(0), -q(1), -q(2), -q(3),
|
||||
q(1), q(0), -q(3), q(2),
|
||||
q(2), q(3), q(0), -q(1),
|
||||
q(3), -q(2), q(1), q(0)
|
||||
};
|
||||
Matrix<Type, 4, 4> Q(dataQ);
|
||||
Vector<Type, 4> v;
|
||||
v(0) = 0;
|
||||
v(1) = w(0);
|
||||
v(2) = w(1);
|
||||
v(3) = w(2);
|
||||
return Q * v * 0.5;
|
||||
}
|
||||
};
|
||||
|
||||
typedef Quaternion<float> Quatf;
|
||||
|
||||
@@ -0,0 +1,46 @@
|
||||
/**
|
||||
* @file Scalar.hpp
|
||||
*
|
||||
* Defines conversion of matrix to scalar.
|
||||
*
|
||||
* @author James Goppert <james.goppert@gmail.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stddef.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "Matrix.hpp"
|
||||
|
||||
namespace matrix
|
||||
{
|
||||
|
||||
template<typename Type>
|
||||
class Scalar : public Matrix<Type, 1, 1>
|
||||
{
|
||||
public:
|
||||
virtual ~Scalar() {};
|
||||
|
||||
Scalar() : Matrix<Type, 1, 1>()
|
||||
{
|
||||
}
|
||||
|
||||
Scalar(const Matrix<Type, 1, 1> & other)
|
||||
{
|
||||
(*this)(0,0) = other(0,0);
|
||||
}
|
||||
|
||||
operator Type()
|
||||
{
|
||||
return (*this)(0,0);
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
typedef Scalar<float> Scalarf;
|
||||
|
||||
}; // namespace matrix
|
||||
+26
-26
@@ -16,37 +16,37 @@ template<typename Type, size_t N>
|
||||
class Vector : public Matrix<Type, N, 1>
|
||||
{
|
||||
public:
|
||||
virtual ~Vector() {};
|
||||
virtual ~Vector() {};
|
||||
|
||||
Vector() : Matrix<Type, N, 1>()
|
||||
{
|
||||
}
|
||||
Vector() : Matrix<Type, N, 1>()
|
||||
{
|
||||
}
|
||||
|
||||
inline Type operator()(size_t i) const
|
||||
{
|
||||
const Matrix<Type, N, 1> &v = *this;
|
||||
return v(i, 0);
|
||||
}
|
||||
inline Type operator()(size_t i) const
|
||||
{
|
||||
const Matrix<Type, N, 1> &v = *this;
|
||||
return v(i, 0);
|
||||
}
|
||||
|
||||
inline Type &operator()(size_t i)
|
||||
{
|
||||
Matrix<Type, N, 1> &v = *this;
|
||||
return v(i, 0);
|
||||
}
|
||||
inline Type &operator()(size_t i)
|
||||
{
|
||||
Matrix<Type, N, 1> &v = *this;
|
||||
return v(i, 0);
|
||||
}
|
||||
|
||||
Type dot(const Vector & b) {
|
||||
Vector &a(*this);
|
||||
Type r = 0;
|
||||
for (int i = 0; i<3; i++) {
|
||||
r += a(i)*b(i);
|
||||
}
|
||||
return r;
|
||||
}
|
||||
Type dot(const Vector & b) {
|
||||
Vector &a(*this);
|
||||
Type r = 0;
|
||||
for (int i = 0; i<3; i++) {
|
||||
r += a(i)*b(i);
|
||||
}
|
||||
return r;
|
||||
}
|
||||
|
||||
Type norm() {
|
||||
Vector &a(*this);
|
||||
return sqrt(a.dot(a));
|
||||
}
|
||||
Type norm() {
|
||||
Vector &a(*this);
|
||||
return sqrt(a.dot(a));
|
||||
}
|
||||
};
|
||||
|
||||
}; // namespace matrix
|
||||
|
||||
+21
-20
@@ -16,29 +16,30 @@ template<typename Type>
|
||||
class Vector3 : public Vector<Type, 3>
|
||||
{
|
||||
public:
|
||||
virtual ~Vector3() {};
|
||||
virtual ~Vector3() {};
|
||||
|
||||
Vector3() : Vector<Type, 3>()
|
||||
{
|
||||
}
|
||||
Vector3() : Vector<Type, 3>()
|
||||
{
|
||||
}
|
||||
|
||||
Vector3(Type x, Type y, Type z) : Vector<Type, 3>()
|
||||
{
|
||||
Vector3 &v(*this);
|
||||
v(0) = x;
|
||||
v(1) = y;
|
||||
v(2) = z;
|
||||
}
|
||||
Vector3(Type x, Type y, Type z) : Vector<Type, 3>()
|
||||
{
|
||||
Vector3 &v(*this);
|
||||
v(0) = x;
|
||||
v(1) = y;
|
||||
v(2) = z;
|
||||
}
|
||||
|
||||
Vector3 cross(const Vector3 & b) {
|
||||
// TODO
|
||||
Vector3 &a(*this);
|
||||
Vector3 c;
|
||||
c(0) = 0;
|
||||
c(1) = 0;
|
||||
c(2) = 0;
|
||||
return c;
|
||||
}
|
||||
Vector3 cross(const Vector3 & b) {
|
||||
// TODO
|
||||
Vector3 &a(*this);
|
||||
(void)a;
|
||||
Vector3 c;
|
||||
c(0) = 0;
|
||||
c(1) = 0;
|
||||
c(2) = 0;
|
||||
return c;
|
||||
}
|
||||
};
|
||||
|
||||
typedef Vector3<float> Vector3f;
|
||||
|
||||
+4
-4
@@ -6,8 +6,8 @@ using namespace matrix;
|
||||
|
||||
int main()
|
||||
{
|
||||
Dcmf dcm;
|
||||
Quatf q = Quatf(dcm);
|
||||
Eulerf e = Eulerf(dcm);
|
||||
return 0;
|
||||
Dcmf dcm;
|
||||
Quatf q = Quatf(dcm);
|
||||
Eulerf e = Eulerf(dcm);
|
||||
return 0;
|
||||
}
|
||||
|
||||
+9
-6
@@ -1,4 +1,5 @@
|
||||
#include "Euler.hpp"
|
||||
#include "Scalar.hpp"
|
||||
#include <assert.h>
|
||||
#include <stdio.h>
|
||||
|
||||
@@ -6,10 +7,12 @@ using namespace matrix;
|
||||
|
||||
int main()
|
||||
{
|
||||
Eulerf e;
|
||||
float dp = e.T()*e;
|
||||
Dcmf dcm = Dcmf(e);
|
||||
Quatf q = Quatf(e);
|
||||
float n = e.norm();
|
||||
return 0;
|
||||
Eulerf e;
|
||||
float dp = Scalarf(e.T()*e);
|
||||
(void)dp;
|
||||
Dcmf dcm = Dcmf(e);
|
||||
Quatf q = Quatf(e);
|
||||
float n = e.norm();
|
||||
(void)n;
|
||||
return 0;
|
||||
}
|
||||
|
||||
+18
-18
@@ -6,25 +6,25 @@ using namespace matrix;
|
||||
|
||||
int main()
|
||||
{
|
||||
float data[9] = {1, 0, 0, 0, 1, 0, 1, 0, 1};
|
||||
Matrix3f A(data);
|
||||
Matrix3f A_I = A.inverse();
|
||||
float data_check[9] = {1, 0, 0, 0, 1, 0, -1, 0, 1};
|
||||
Matrix3f A_I_check(data_check);
|
||||
(void)A_I;
|
||||
assert(A_I == A_I_check);
|
||||
float data[9] = {1, 0, 0, 0, 1, 0, 1, 0, 1};
|
||||
Matrix3f A(data);
|
||||
Matrix3f A_I = A.inverse();
|
||||
float data_check[9] = {1, 0, 0, 0, 1, 0, -1, 0, 1};
|
||||
Matrix3f A_I_check(data_check);
|
||||
(void)A_I;
|
||||
assert(A_I == A_I_check);
|
||||
|
||||
// stess test
|
||||
static const size_t n = 100;
|
||||
Matrix<float, n, n> A_large;
|
||||
A_large.setIdentity();
|
||||
Matrix<float, n, n> A_large_I;
|
||||
A_large_I.setZero();
|
||||
// stess test
|
||||
static const size_t n = 100;
|
||||
Matrix<float, n, n> A_large;
|
||||
A_large.setIdentity();
|
||||
Matrix<float, n, n> A_large_I;
|
||||
A_large_I.setZero();
|
||||
|
||||
for (size_t i = 0; i < 100; i++) {
|
||||
A_large_I = A_large.inverse();
|
||||
assert(A_large == A_large_I);
|
||||
}
|
||||
for (size_t i = 0; i < 100; i++) {
|
||||
A_large_I = A_large.inverse();
|
||||
assert(A_large == A_large_I);
|
||||
}
|
||||
|
||||
return 0;
|
||||
return 0;
|
||||
}
|
||||
|
||||
+17
-17
@@ -5,25 +5,25 @@ using namespace matrix;
|
||||
|
||||
int main()
|
||||
{
|
||||
Matrix3f m;
|
||||
m.setZero();
|
||||
m(0, 0) = 1;
|
||||
m(0, 1) = 2;
|
||||
m(0, 2) = 3;
|
||||
m(1, 0) = 4;
|
||||
m(1, 1) = 5;
|
||||
m(1, 2) = 6;
|
||||
m(2, 0) = 7;
|
||||
m(2, 1) = 8;
|
||||
m(2, 2) = 9;
|
||||
Matrix3f m;
|
||||
m.setZero();
|
||||
m(0, 0) = 1;
|
||||
m(0, 1) = 2;
|
||||
m(0, 2) = 3;
|
||||
m(1, 0) = 4;
|
||||
m(1, 1) = 5;
|
||||
m(1, 2) = 6;
|
||||
m(2, 0) = 7;
|
||||
m(2, 1) = 8;
|
||||
m(2, 2) = 9;
|
||||
|
||||
m.print();
|
||||
m.print();
|
||||
|
||||
float data[9] = {1, 2, 3, 4, 5, 6, 7, 8, 9};
|
||||
Matrix3f m2(data);
|
||||
m2.print();
|
||||
float data[9] = {1, 2, 3, 4, 5, 6, 7, 8, 9};
|
||||
Matrix3f m2(data);
|
||||
m2.print();
|
||||
|
||||
assert(m == m2);
|
||||
assert(m == m2);
|
||||
|
||||
return 0;
|
||||
return 0;
|
||||
}
|
||||
|
||||
+16
-16
@@ -6,21 +6,21 @@ using namespace matrix;
|
||||
|
||||
int main()
|
||||
{
|
||||
float data[9] = {1, 0, 0, 0, 1, 0, 1, 0, 1};
|
||||
Matrix3f A(data);
|
||||
float data_check[9] = {1, 0, 0, 0, 1, 0, -1, 0, 1};
|
||||
Matrix3f A_I(data_check);
|
||||
Matrix3f I;
|
||||
I.setIdentity();
|
||||
A.print();
|
||||
A_I.print();
|
||||
Matrix3f R = A * A_I;
|
||||
R.print();
|
||||
assert(R == I);
|
||||
float data[9] = {1, 0, 0, 0, 1, 0, 1, 0, 1};
|
||||
Matrix3f A(data);
|
||||
float data_check[9] = {1, 0, 0, 0, 1, 0, -1, 0, 1};
|
||||
Matrix3f A_I(data_check);
|
||||
Matrix3f I;
|
||||
I.setIdentity();
|
||||
A.print();
|
||||
A_I.print();
|
||||
Matrix3f R = A * A_I;
|
||||
R.print();
|
||||
assert(R == I);
|
||||
|
||||
Matrix3f R2 = A;
|
||||
R2 *= A_I;
|
||||
R2.print();
|
||||
assert(R2 == I);
|
||||
return 0;
|
||||
Matrix3f R2 = A;
|
||||
R2 *= A_I;
|
||||
R2.print();
|
||||
assert(R2 == I);
|
||||
return 0;
|
||||
}
|
||||
|
||||
@@ -6,13 +6,13 @@ using namespace matrix;
|
||||
|
||||
int main()
|
||||
{
|
||||
float data[9] = {1, 2, 3, 4, 5, 6, 7, 8, 9};
|
||||
Matrix3f A(data);
|
||||
A = A * 2;
|
||||
float data_check[9] = {2, 4, 6, 8, 10, 12, 14, 16, 18};
|
||||
Matrix3f A_check(data_check);
|
||||
A.print();
|
||||
A_check.print();
|
||||
assert(A == A_check);
|
||||
return 0;
|
||||
float data[9] = {1, 2, 3, 4, 5, 6, 7, 8, 9};
|
||||
Matrix3f A(data);
|
||||
A = A * 2;
|
||||
//float data_check[9] = {2, 4, 6, 8, 10, 12, 14, 16, 18};
|
||||
//Matrix3f A_check(data_check);
|
||||
//A.print();
|
||||
//A_check.print();
|
||||
//assert(A == A_check);
|
||||
return 0;
|
||||
}
|
||||
|
||||
+6
-6
@@ -6,10 +6,10 @@ using namespace matrix;
|
||||
|
||||
int main()
|
||||
{
|
||||
Quatf p(1, 0, 0, 0);
|
||||
Quatf q(0, 1, 0, 0);
|
||||
Quatf r = p*q;
|
||||
Dcmf dcm = Dcmf(p);
|
||||
Eulerf e = Eulerf(p);
|
||||
return 0;
|
||||
Quatf p(1, 0, 0, 0);
|
||||
Quatf q(0, 1, 0, 0);
|
||||
Quatf r = p*q;
|
||||
Dcmf dcm = Dcmf(p);
|
||||
Eulerf e = Eulerf(p);
|
||||
return 0;
|
||||
}
|
||||
|
||||
+12
-14
@@ -5,21 +5,19 @@ using namespace matrix;
|
||||
|
||||
int main()
|
||||
{
|
||||
Matrix3f A;
|
||||
A.setIdentity();
|
||||
assert(A.rows() == 3);
|
||||
assert(A.cols() == 3);
|
||||
Matrix3f A;
|
||||
A.setIdentity();
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
for (int j = 0; j < 3; j++) {
|
||||
if (i == j) {
|
||||
assert(A(i, j) == 1);
|
||||
for (int i = 0; i < 3; i++) {
|
||||
for (int j = 0; j < 3; j++) {
|
||||
if (i == j) {
|
||||
assert(A(i, j) == 1);
|
||||
|
||||
} else {
|
||||
assert(A(i, j) == 0);
|
||||
}
|
||||
}
|
||||
}
|
||||
} else {
|
||||
assert(A(i, j) == 0);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
return 0;
|
||||
}
|
||||
|
||||
+9
-9
@@ -6,13 +6,13 @@ using namespace matrix;
|
||||
|
||||
int main()
|
||||
{
|
||||
float data[9] = {1, 2, 3, 4, 5, 6};
|
||||
Matrix<float, 2, 3> A(data);
|
||||
Matrix<float, 3, 2> A_T = A.transpose();
|
||||
float data_check[9] = {1, 4, 2, 5, 3, 6};
|
||||
Matrix<float, 3, 2> A_T_check(data_check);
|
||||
A_T.print();
|
||||
A_T_check.print();
|
||||
assert(A_T == A_T_check);
|
||||
return 0;
|
||||
float data[9] = {1, 2, 3, 4, 5, 6};
|
||||
Matrix<float, 2, 3> A(data);
|
||||
Matrix<float, 3, 2> A_T = A.transpose();
|
||||
float data_check[9] = {1, 4, 2, 5, 3, 6};
|
||||
Matrix<float, 3, 2> A_T_check(data_check);
|
||||
A_T.print();
|
||||
A_T_check.print();
|
||||
assert(A_T == A_T_check);
|
||||
return 0;
|
||||
}
|
||||
|
||||
+6
-4
@@ -6,8 +6,10 @@ using namespace matrix;
|
||||
|
||||
int main()
|
||||
{
|
||||
Vector<float, 5> v;
|
||||
float n = v.norm();
|
||||
float r = v.dot(v);
|
||||
return 0;
|
||||
Vector<float, 5> v;
|
||||
float n = v.norm();
|
||||
(void)n;
|
||||
float r = v.dot(v);
|
||||
(void)r;
|
||||
return 0;
|
||||
}
|
||||
|
||||
+4
-4
@@ -6,8 +6,8 @@ using namespace matrix;
|
||||
|
||||
int main()
|
||||
{
|
||||
Vector3f a(1, 0, 0);
|
||||
Vector3f b(0, 1, 0);
|
||||
Vector3f c = a.cross(b);
|
||||
return 0;
|
||||
Vector3f a(1, 0, 0);
|
||||
Vector3f b(0, 1, 0);
|
||||
Vector3f c = a.cross(b);
|
||||
return 0;
|
||||
}
|
||||
|
||||
+14
-14
@@ -5,24 +5,24 @@ using namespace matrix;
|
||||
|
||||
int main()
|
||||
{
|
||||
Vector3f v;
|
||||
v(0) = 1;
|
||||
v(1) = 2;
|
||||
v(2) = 3;
|
||||
Vector3f v;
|
||||
v(0) = 1;
|
||||
v(1) = 2;
|
||||
v(2) = 3;
|
||||
|
||||
v.print();
|
||||
v.print();
|
||||
|
||||
assert(v(0) == 1);
|
||||
assert(v(1) == 2);
|
||||
assert(v(2) == 3);
|
||||
assert(v(0) == 1);
|
||||
assert(v(1) == 2);
|
||||
assert(v(2) == 3);
|
||||
|
||||
Vector3f v2(4, 5, 6);
|
||||
Vector3f v2(4, 5, 6);
|
||||
|
||||
v2.print();
|
||||
v2.print();
|
||||
|
||||
assert(v2(0) == 4);
|
||||
assert(v2(1) == 5);
|
||||
assert(v2(2) == 6);
|
||||
assert(v2(0) == 4);
|
||||
assert(v2(1) == 5);
|
||||
assert(v2(2) == 6);
|
||||
|
||||
return 0;
|
||||
return 0;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user