80 lines
1.7 KiB
C++
80 lines
1.7 KiB
C++
#include "rbdl_tests.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;
|
|
|
|
using namespace Catch::Matchers::Floating;
|
|
|
|
struct MathFixture {
|
|
};
|
|
|
|
TEST_CASE(__FILE__"_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);
|
|
|
|
REQUIRE_THAT (test_result, AllCloseVector(x));
|
|
|
|
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;
|
|
|
|
x[0] += 1.0e-13;
|
|
|
|
REQUIRE_THAT (test_result, AllCloseVector(x));
|
|
}
|
|
|
|
TEST_CASE(__FILE__"_QuaternionSlerpNegativeCosHalfTheta", "") {
|
|
ClearLogOutput();
|
|
|
|
Quaternion q1 (-0.518934,0.561432,-0.074923,0.640225);
|
|
Quaternion q2 (0.54702,-0.564195,0.078871,-0.613379);
|
|
|
|
Quaternion s = q1.slerp (0.2021, q2);
|
|
Quaternion q_ref (0.0068865, 0.406762, -0.000610507, 0.913655);
|
|
|
|
REQUIRE_THAT (s, AllCloseVector(q_ref));
|
|
}
|
|
|
|
TEST_CASE(__FILE__"_QuaternionFromMatrixSingularity", "") {
|
|
ClearLogOutput();
|
|
Matrix3d m;
|
|
m << -1., 0, 0, 0, 1, 0, 0, 0, -1;
|
|
|
|
Quaternion q = Quaternion::fromMatrix(m);
|
|
Quaternion q_ref (0., 1., 0., 0.);
|
|
|
|
REQUIRE_THAT (q, AllCloseVector(q_ref));
|
|
}
|