110 lines
2.6 KiB
C++
110 lines
2.6 KiB
C++
|
#include <UnitTest++.h>
|
||
|
|
|
||
|
|
#include "rbdl/Logging.h"
|
||
|
|
#include "rbdl/rbdl_math.h"
|
||
|
|
#include "rbdl/rbdl_mathutils.h"
|
||
|
|
#include <iostream>
|
||
|
|
|
||
|
|
const double TEST_PREC = 1.0e-14;
|
||
|
|
|
||
|
|
using namespace std;
|
||
|
|
using namespace RigidBodyDynamics::Math;
|
||
|
|
|
||
|
|
struct MathFixture {
|
||
|
|
};
|
||
|
|
|
||
|
|
TEST (GaussElimPivot) {
|
||
|
|
ClearLogOutput();
|
||
|
|
|
||
|
|
MatrixNd A;
|
||
|
|
A.resize(3,3);
|
||
|
|
VectorNd b(3);
|
||
|
|
VectorNd x(3);
|
||
|
|
|
||
|
|
A(0,0) = 0; A(0,1) = 2; A(0,2) = 1;
|
||
|
|
A(1,0) = 1; A(1,1) = 1; A(1,2) = 5;
|
||
|
|
A(2,0) = 0; A(2,1) = 0; A(2,2) = 1;
|
||
|
|
|
||
|
|
b[0] = 1;
|
||
|
|
b[1] = 2;
|
||
|
|
b[2] = 3;
|
||
|
|
|
||
|
|
VectorNd test_result (3);
|
||
|
|
|
||
|
|
test_result[0] = -12;
|
||
|
|
test_result[1] = -1;
|
||
|
|
test_result[2] = 3;
|
||
|
|
|
||
|
|
LinSolveGaussElimPivot (A, b, x);
|
||
|
|
|
||
|
|
CHECK_ARRAY_CLOSE (test_result.data(), x.data(), 3, TEST_PREC);
|
||
|
|
|
||
|
|
A(0,0) = 0; A(0,1) = -2; A(0,2) = 1;
|
||
|
|
A(1,0) = 1; A(1,1) = 1; A(1,2) = 5;
|
||
|
|
A(2,0) = 0; A(2,1) = 0; A(2,2) = 1;
|
||
|
|
|
||
|
|
LinSolveGaussElimPivot (A, b, x);
|
||
|
|
test_result[0] = -14;
|
||
|
|
test_result[1] = 1;
|
||
|
|
test_result[2] = 3;
|
||
|
|
|
||
|
|
CHECK_ARRAY_CLOSE (test_result.data(), x.data(), 3, TEST_PREC);
|
||
|
|
}
|
||
|
|
|
||
|
|
TEST (Dynamic_1D_initialize_value) {
|
||
|
|
VectorNd myvector_10 = VectorNd::Constant ((size_t) 10, 12.);
|
||
|
|
|
||
|
|
double *test_values = new double[10];
|
||
|
|
for (unsigned int i = 0; i < 10; i++)
|
||
|
|
test_values[i] = 12.;
|
||
|
|
|
||
|
|
CHECK_ARRAY_EQUAL (test_values, myvector_10.data(), 10);
|
||
|
|
delete[] test_values;
|
||
|
|
}
|
||
|
|
|
||
|
|
TEST (Dynamic_2D_initialize_value) {
|
||
|
|
MatrixNd mymatrix_10x10 = MatrixNd::Constant (10, 10, 12.);
|
||
|
|
|
||
|
|
double *test_values = new double[10 * 10];
|
||
|
|
for (unsigned int i = 0; i < 10; i++)
|
||
|
|
for (unsigned int j = 0; j < 10; j++)
|
||
|
|
test_values[i*10 + j] = 12.;
|
||
|
|
|
||
|
|
CHECK_ARRAY_EQUAL (test_values, mymatrix_10x10.data(), 10*10);
|
||
|
|
delete[] test_values;
|
||
|
|
}
|
||
|
|
|
||
|
|
TEST (SpatialMatrix_Multiplication) {
|
||
|
|
SpatialMatrix X_1 (
|
||
|
|
1., 2., 3., 4., 5., 6.,
|
||
|
|
11., 12., 13., 14., 15., 16.,
|
||
|
|
21., 22., 23., 24., 25., 26.,
|
||
|
|
31., 32., 33., 34., 35., 36.,
|
||
|
|
41., 42., 43., 44., 45., 46.,
|
||
|
|
51., 52., 53., 54., 55., 56.
|
||
|
|
);
|
||
|
|
|
||
|
|
SpatialMatrix X_2 (X_1);
|
||
|
|
|
||
|
|
X_2 *= 2.;
|
||
|
|
|
||
|
|
SpatialMatrix correct_result (
|
||
|
|
1442, 1484, 1526, 1568, 1610, 1652,
|
||
|
|
4562, 4724, 4886, 5048, 5210, 5372,
|
||
|
|
7682, 7964, 8246, 8528, 8810, 9092,
|
||
|
|
10802, 11204, 11606, 12008, 12410, 12812,
|
||
|
|
13922, 14444, 14966, 15488, 16010, 16532,
|
||
|
|
17042, 17684, 18326, 18968, 19610, 20252
|
||
|
|
);
|
||
|
|
|
||
|
|
SpatialMatrix test_result = X_1 * X_2;
|
||
|
|
|
||
|
|
CHECK_ARRAY_CLOSE (correct_result.data(), test_result.data(), 6 * 6, TEST_PREC);
|
||
|
|
|
||
|
|
// check the *= operator:
|
||
|
|
test_result = X_1;
|
||
|
|
test_result *= X_2;
|
||
|
|
|
||
|
|
CHECK_ARRAY_CLOSE (correct_result.data(), test_result.data(), 6 * 6, TEST_PREC);
|
||
|
|
}
|