Source

rbdl / tests / MathTests.cc

Full commit
#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);
}