mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-06-26 15:50:34 +08:00
Reformat of math library with astyle.
This commit is contained in:
jgoppert
+113
-100
@@ -45,136 +45,149 @@
|
||||
namespace math
|
||||
{
|
||||
|
||||
static const float data_testA[] =
|
||||
{1,2,3,
|
||||
4,5,6};
|
||||
static Matrix testA(2,3,data_testA);
|
||||
static const float data_testA[] = {
|
||||
1, 2, 3,
|
||||
4, 5, 6
|
||||
};
|
||||
static Matrix testA(2, 3, data_testA);
|
||||
|
||||
static const float data_testB[] =
|
||||
{0,1,3,
|
||||
7,-1,2};
|
||||
static Matrix testB(2,3,data_testB);
|
||||
static const float data_testB[] = {
|
||||
0, 1, 3,
|
||||
7, -1, 2
|
||||
};
|
||||
static Matrix testB(2, 3, data_testB);
|
||||
|
||||
static const float data_testC[] =
|
||||
{0,1,
|
||||
2,1,
|
||||
3,2};
|
||||
static Matrix testC(3,2,data_testC);
|
||||
static const float data_testC[] = {
|
||||
0, 1,
|
||||
2, 1,
|
||||
3, 2
|
||||
};
|
||||
static Matrix testC(3, 2, data_testC);
|
||||
|
||||
static const float data_testD[] =
|
||||
{0,1,2,
|
||||
2,1,4,
|
||||
5,2,0};
|
||||
static Matrix testD(3,3,data_testD);
|
||||
static const float data_testD[] = {
|
||||
0, 1, 2,
|
||||
2, 1, 4,
|
||||
5, 2, 0
|
||||
};
|
||||
static Matrix testD(3, 3, data_testD);
|
||||
|
||||
static const float data_testE[] =
|
||||
{1,-1,2,
|
||||
0,2,3,
|
||||
2,-1,1};
|
||||
static Matrix testE(3,3,data_testE);
|
||||
static const float data_testE[] = {
|
||||
1, -1, 2,
|
||||
0, 2, 3,
|
||||
2, -1, 1
|
||||
};
|
||||
static Matrix testE(3, 3, data_testE);
|
||||
|
||||
static const float data_testF[] =
|
||||
{3.777e006f, 2.915e007f, 0.000e000f,
|
||||
2.938e007f, 2.267e008f, 0.000e000f,
|
||||
0.000e000f, 0.000e000f, 6.033e008f};
|
||||
static Matrix testF(3,3,data_testF);
|
||||
static const float data_testF[] = {
|
||||
3.777e006f, 2.915e007f, 0.000e000f,
|
||||
2.938e007f, 2.267e008f, 0.000e000f,
|
||||
0.000e000f, 0.000e000f, 6.033e008f
|
||||
};
|
||||
static Matrix testF(3, 3, data_testF);
|
||||
|
||||
int __EXPORT matrixTest()
|
||||
{
|
||||
matrixAddTest();
|
||||
matrixSubTest();
|
||||
matrixMultTest();
|
||||
matrixInvTest();
|
||||
matrixDivTest();
|
||||
return 0;
|
||||
matrixAddTest();
|
||||
matrixSubTest();
|
||||
matrixMultTest();
|
||||
matrixInvTest();
|
||||
matrixDivTest();
|
||||
return 0;
|
||||
}
|
||||
|
||||
int matrixAddTest()
|
||||
{
|
||||
printf("Test Matrix Add\t\t: ");
|
||||
Matrix r = testA + testB;
|
||||
float data_test[] =
|
||||
{ 1.0f, 3.0f, 6.0f,
|
||||
11.0f, 4.0f, 8.0f};
|
||||
ASSERT(matrixEqual(Matrix(2,3,data_test),r));
|
||||
printf("PASS\n");
|
||||
return 0;
|
||||
printf("Test Matrix Add\t\t: ");
|
||||
Matrix r = testA + testB;
|
||||
float data_test[] = {
|
||||
1.0f, 3.0f, 6.0f,
|
||||
11.0f, 4.0f, 8.0f
|
||||
};
|
||||
ASSERT(matrixEqual(Matrix(2, 3, data_test), r));
|
||||
printf("PASS\n");
|
||||
return 0;
|
||||
}
|
||||
|
||||
int matrixSubTest()
|
||||
{
|
||||
printf("Test Matrix Sub\t\t: ");
|
||||
Matrix r = testA - testB;
|
||||
float data_test[] =
|
||||
{ 1.0f, 1.0f, 0.0f,
|
||||
-3.0f, 6.0f, 4.0f};
|
||||
ASSERT(matrixEqual(Matrix(2,3,data_test),r));
|
||||
printf("PASS\n");
|
||||
return 0;
|
||||
printf("Test Matrix Sub\t\t: ");
|
||||
Matrix r = testA - testB;
|
||||
float data_test[] = {
|
||||
1.0f, 1.0f, 0.0f,
|
||||
-3.0f, 6.0f, 4.0f
|
||||
};
|
||||
ASSERT(matrixEqual(Matrix(2, 3, data_test), r));
|
||||
printf("PASS\n");
|
||||
return 0;
|
||||
}
|
||||
|
||||
int matrixMultTest()
|
||||
{
|
||||
printf("Test Matrix Mult\t: ");
|
||||
Matrix r = testC * testB;
|
||||
float data_test[] =
|
||||
{ 7.0f, -1.0f, 2.0f,
|
||||
7.0f, 1.0f, 8.0f,
|
||||
14.0f, 1.0f, 13.0f};
|
||||
ASSERT(matrixEqual(Matrix(3,3,data_test),r));
|
||||
printf("PASS\n");
|
||||
return 0;
|
||||
printf("Test Matrix Mult\t: ");
|
||||
Matrix r = testC * testB;
|
||||
float data_test[] = {
|
||||
7.0f, -1.0f, 2.0f,
|
||||
7.0f, 1.0f, 8.0f,
|
||||
14.0f, 1.0f, 13.0f
|
||||
};
|
||||
ASSERT(matrixEqual(Matrix(3, 3, data_test), r));
|
||||
printf("PASS\n");
|
||||
return 0;
|
||||
}
|
||||
|
||||
int matrixInvTest()
|
||||
{
|
||||
printf("Test Matrix Inv\t\t: ");
|
||||
Matrix origF = testF;
|
||||
Matrix r = testF.inverse();
|
||||
float data_test[] =
|
||||
{ -0.0012518f, 0.0001610f, 0.0000000f,
|
||||
0.0001622f, -0.0000209f, 0.0000000f,
|
||||
0.0000000f, 0.0000000f, 1.6580e-9f};
|
||||
ASSERT(matrixEqual(Matrix(3,3,data_test),r));
|
||||
// make sure F in unchanged
|
||||
ASSERT(matrixEqual(origF,testF));
|
||||
printf("PASS\n");
|
||||
return 0;
|
||||
printf("Test Matrix Inv\t\t: ");
|
||||
Matrix origF = testF;
|
||||
Matrix r = testF.inverse();
|
||||
float data_test[] = {
|
||||
-0.0012518f, 0.0001610f, 0.0000000f,
|
||||
0.0001622f, -0.0000209f, 0.0000000f,
|
||||
0.0000000f, 0.0000000f, 1.6580e-9f
|
||||
};
|
||||
ASSERT(matrixEqual(Matrix(3, 3, data_test), r));
|
||||
// make sure F in unchanged
|
||||
ASSERT(matrixEqual(origF, testF));
|
||||
printf("PASS\n");
|
||||
return 0;
|
||||
}
|
||||
|
||||
int matrixDivTest()
|
||||
{
|
||||
printf("Test Matrix Div\t\t: ");
|
||||
Matrix r = testD / testE;
|
||||
float data_test[] = {
|
||||
0.2222222f, 0.5555556f, -0.1111111f,
|
||||
0.0f, 1.0f, 1.0,
|
||||
-4.1111111f, 1.2222222f, 4.5555556f};
|
||||
ASSERT(matrixEqual(Matrix(3,3,data_test),r));
|
||||
printf("PASS\n");
|
||||
return 0;
|
||||
printf("Test Matrix Div\t\t: ");
|
||||
Matrix r = testD / testE;
|
||||
float data_test[] = {
|
||||
0.2222222f, 0.5555556f, -0.1111111f,
|
||||
0.0f, 1.0f, 1.0,
|
||||
-4.1111111f, 1.2222222f, 4.5555556f
|
||||
};
|
||||
ASSERT(matrixEqual(Matrix(3, 3, data_test), r));
|
||||
printf("PASS\n");
|
||||
return 0;
|
||||
}
|
||||
|
||||
bool matrixEqual(const Matrix & a, const Matrix & b, float eps)
|
||||
bool matrixEqual(const Matrix &a, const Matrix &b, float eps)
|
||||
{
|
||||
if (a.getRows() != b.getRows()) {
|
||||
printf("row number not equal a: %d, b:%d\n", a.getRows(), b.getRows());
|
||||
return false;
|
||||
} else if (a.getCols() != b.getCols()) {
|
||||
printf("column number not equal a: %d, b:%d\n", a.getCols(), b.getCols());
|
||||
return false;
|
||||
}
|
||||
bool ret = true;
|
||||
for (size_t i=0;i<a.getRows();i++)
|
||||
for (size_t j =0;j<a.getCols();j++)
|
||||
{
|
||||
if (!equal(a(i,j),b(i,j),eps))
|
||||
{
|
||||
printf("element mismatch (%d, %d)\n", i, j);
|
||||
ret = false;
|
||||
}
|
||||
}
|
||||
return ret;
|
||||
if (a.getRows() != b.getRows()) {
|
||||
printf("row number not equal a: %d, b:%d\n", a.getRows(), b.getRows());
|
||||
return false;
|
||||
|
||||
} else if (a.getCols() != b.getCols()) {
|
||||
printf("column number not equal a: %d, b:%d\n", a.getCols(), b.getCols());
|
||||
return false;
|
||||
}
|
||||
|
||||
bool ret = true;
|
||||
|
||||
for (size_t i = 0; i < a.getRows(); i++)
|
||||
for (size_t j = 0; j < a.getCols(); j++) {
|
||||
if (!equal(a(i, j), b(i, j), eps)) {
|
||||
printf("element mismatch (%d, %d)\n", i, j);
|
||||
ret = false;
|
||||
}
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
||||
} // namespace math
|
||||
|
||||
Reference in New Issue
Block a user