mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-17 17:33:33 +08:00
Initial commit.
This commit is contained in:
@@ -0,0 +1,20 @@
|
||||
set(tests
|
||||
setIdentity
|
||||
inverse
|
||||
matrixMult
|
||||
vectorAssignment
|
||||
matrixAssignment
|
||||
matrixScalarMult
|
||||
transpose
|
||||
quaternion
|
||||
dcm
|
||||
vector
|
||||
vector3
|
||||
euler
|
||||
)
|
||||
|
||||
foreach(test ${tests})
|
||||
add_executable(${test}
|
||||
${test}.cpp)
|
||||
add_test(${test} ${test})
|
||||
endforeach()
|
||||
@@ -0,0 +1,13 @@
|
||||
#include "Dcm.hpp"
|
||||
#include <assert.h>
|
||||
#include <stdio.h>
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
int main()
|
||||
{
|
||||
Dcmf dcm;
|
||||
Quatf q = Quatf(dcm);
|
||||
Eulerf e = Eulerf(dcm);
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,15 @@
|
||||
#include "Euler.hpp"
|
||||
#include <assert.h>
|
||||
#include <stdio.h>
|
||||
|
||||
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;
|
||||
}
|
||||
@@ -0,0 +1,30 @@
|
||||
#include "Matrix.hpp"
|
||||
#include <assert.h>
|
||||
#include <stdio.h>
|
||||
|
||||
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);
|
||||
|
||||
// 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);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,29 @@
|
||||
#include "Matrix.hpp"
|
||||
#include <assert.h>
|
||||
|
||||
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;
|
||||
|
||||
m.print();
|
||||
|
||||
float data[9] = {1, 2, 3, 4, 5, 6, 7, 8, 9};
|
||||
Matrix3f m2(data);
|
||||
m2.print();
|
||||
|
||||
assert(m == m2);
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,26 @@
|
||||
#include "Matrix.hpp"
|
||||
#include <assert.h>
|
||||
#include <stdio.h>
|
||||
|
||||
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);
|
||||
|
||||
Matrix3f R2 = A;
|
||||
R2 *= A_I;
|
||||
R2.print();
|
||||
assert(R2 == I);
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,18 @@
|
||||
#include "Matrix.hpp"
|
||||
#include <assert.h>
|
||||
#include <stdio.h>
|
||||
|
||||
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;
|
||||
}
|
||||
@@ -0,0 +1,15 @@
|
||||
#include "Quaternion.hpp"
|
||||
#include <assert.h>
|
||||
#include <stdio.h>
|
||||
|
||||
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;
|
||||
}
|
||||
@@ -0,0 +1,25 @@
|
||||
#include "Matrix.hpp"
|
||||
#include <assert.h>
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
int main()
|
||||
{
|
||||
Matrix3f A;
|
||||
A.setIdentity();
|
||||
assert(A.rows() == 3);
|
||||
assert(A.cols() == 3);
|
||||
|
||||
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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,18 @@
|
||||
#include "Matrix.hpp"
|
||||
#include <assert.h>
|
||||
#include <stdio.h>
|
||||
|
||||
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;
|
||||
}
|
||||
@@ -0,0 +1,13 @@
|
||||
#include "Vector.hpp"
|
||||
#include <assert.h>
|
||||
#include <stdio.h>
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
int main()
|
||||
{
|
||||
Vector<float, 5> v;
|
||||
float n = v.norm();
|
||||
float r = v.dot(v);
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,13 @@
|
||||
#include "Vector3.hpp"
|
||||
#include <assert.h>
|
||||
#include <stdio.h>
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
int main()
|
||||
{
|
||||
Vector3f a(1, 0, 0);
|
||||
Vector3f b(0, 1, 0);
|
||||
Vector3f c = a.cross(b);
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,28 @@
|
||||
#include "Vector3.hpp"
|
||||
#include <assert.h>
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
int main()
|
||||
{
|
||||
Vector3f v;
|
||||
v(0) = 1;
|
||||
v(1) = 2;
|
||||
v(2) = 3;
|
||||
|
||||
v.print();
|
||||
|
||||
assert(v(0) == 1);
|
||||
assert(v(1) == 2);
|
||||
assert(v(2) == 3);
|
||||
|
||||
Vector3f v2(4, 5, 6);
|
||||
|
||||
v2.print();
|
||||
|
||||
assert(v2(0) == 4);
|
||||
assert(v2(1) == 5);
|
||||
assert(v2(2) == 6);
|
||||
|
||||
return 0;
|
||||
}
|
||||
Reference in New Issue
Block a user