TEST(RotMatDerivs, SmallTheta2_Works)
{
VecN<3> p;
for (size_t i = 0; i < p.size(); ++i) p[i] = i+1;
p /= norm<3>(p) * 1e7;
MatrixNM<3,3> mx(1.);
MatrixNM<3,3> drm1(0.);
MatrixNM<3,3> drm2(0.);
MatrixNM<3,3> drm3(0.);
MatrixNM<3,3> mx_2(1.);
MatrixNM<3,3> drm1_2(0.);
MatrixNM<3,3> drm2_2(0.);
MatrixNM<3,3> drm3_2(0.);
pele::rot_mat_derivatives(p, mx, drm1, drm2, drm3);
pele::rot_mat_derivatives_small_theta(p, mx_2, drm1_2, drm2_2, drm3_2, true);
for (size_t i=0; i<9; ++i) {
ASSERT_DOUBLE_EQ(mx.data()[i], mx_2.data()[i]);
ASSERT_DOUBLE_EQ(drm1.data()[i], drm1_2.data()[i]);
ASSERT_DOUBLE_EQ(drm2.data()[i], drm2_2.data()[i]);
ASSERT_DOUBLE_EQ(drm3.data()[i], drm3_2.data()[i]);
}
}
TEST(Rotations, AA2QAndBack_Works)
{
VecN<3> p;
for (size_t i = 0; i < p.size(); ++i) p[i] = i+1;
p /= norm<3>(p);
VecN<3> pnew = pele::quaternion_to_aa(pele::aa_to_quaternion(p));
ASSERT_NEAR(p[0], pnew[0], 1e-5);
ASSERT_NEAR(p[1], pnew[1], 1e-5);
ASSERT_NEAR(p[2], pnew[2], 1e-5);
}
TEST(Rotations, AA2Q_Works)
{
VecN<3> p;
for (size_t i = 0; i < p.size(); ++i) p[i] = i+1;
p /= norm<3>(p);
VecN<4> q = pele::aa_to_quaternion(p);
ASSERT_NEAR(q[0], 0.87758256, 1e-5);
ASSERT_NEAR(q[1], 0.12813186, 1e-5);
ASSERT_NEAR(q[2], 0.25626373, 1e-5);
ASSERT_NEAR(q[3], 0.38439559, 1e-5);
}
TEST(Rotations, Quaternion2AA_Works){
VecN<3> v;
for (size_t i = 0; i < v.size(); ++i) v[i] = i+1;
v /= norm<3>(v);
VecN<4> q(4);
for (size_t i = 0; i < 3; ++i) q[i+1] = v[i];
// std::cout << q << std::endl;
VecN<3> aa = pele::quaternion_to_aa(q);
ASSERT_NEAR(aa[0], 0.1309466, 1e-5);
ASSERT_NEAR(aa[1], 0.26189321, 1e-5);
ASSERT_NEAR(aa[2], 0.39283981, 1e-5);
}
TEST(Rotations, AA2RotMat_Works)
{
VecN<3> p;
for (size_t i = 0; i < p.size(); ++i) p[i] = i+1;
p /= norm<3>(p);
auto mx = pele::aa_to_rot_mat(p);
ASSERT_NEAR(mx(0,0), 0.57313786, 1e-5);
ASSERT_NEAR(mx(1,0), 0.74034884, 1e-5);
ASSERT_NEAR(mx(2,0), -0.35127851, 1e-4);
ASSERT_NEAR(mx(0,1), -0.60900664, 1e-5);
ASSERT_NEAR(mx(1,1), 0.6716445, 1e-5);
ASSERT_NEAR(mx(2,1), 0.42190588, 1e-5);
ASSERT_NEAR(mx(0,2), 0.54829181, 1e-5);
ASSERT_NEAR(mx(1,2), -0.02787928, 1e-5);
ASSERT_NEAR(mx(2,2), 0.83582225, 1e-5);
}
TEST(RotMat, SmallTheta_Works)
{
VecN<3> p;
for (size_t i = 0; i < p.size(); ++i) p[i] = i+1;
p /= norm<3>(p) * 1e7;
auto mx = pele::aa_to_rot_mat(p);
MatrixNM<3,3> mx_2(1.);
MatrixNM<3,3> drm1_2(0.);
MatrixNM<3,3> drm2_2(0.);
MatrixNM<3,3> drm3_2(0.);
pele::rot_mat_derivatives_small_theta(p, mx_2, drm1_2, drm2_2, drm3_2, false);
for (size_t i=0; i<9; ++i) {
ASSERT_DOUBLE_EQ(mx.data()[i], mx_2.data()[i]);
}
}
TEST(RotMatDerivs, SmallTheta_Works)
{
VecN<3> p;
for (size_t i = 0; i < p.size(); ++i) p[i] = i+1;
p /= norm<3>(p) * 1e7;
MatrixNM<3,3> mx(1.);
MatrixNM<3,3> drm1(0.);
MatrixNM<3,3> drm2(0.);
MatrixNM<3,3> drm3(0.);
pele::rot_mat_derivatives_small_theta(p, mx, drm1, drm2, drm3, true);
// mx
EXPECT_NEAR_RELATIVE(mx(0,0), 1., 1e-5);
EXPECT_NEAR_RELATIVE(mx(1,0), 8.01783726e-08, 1e-5);
EXPECT_NEAR_RELATIVE(mx(2,0), -5.34522484e-08, 1e-4);
EXPECT_NEAR_RELATIVE(mx(0,1), -8.01783726e-08, 1e-5);
EXPECT_NEAR_RELATIVE(mx(1,1), 1, 1e-5);
EXPECT_NEAR_RELATIVE(mx(2,1), 2.67261242e-08, 1e-5);
EXPECT_NEAR_RELATIVE(mx(0,2), 5.34522484e-08, 1e-5);
EXPECT_NEAR_RELATIVE(mx(1,2), -2.67261242e-08, 1e-5);
EXPECT_NEAR_RELATIVE(mx(2,2), 1, 1e-5);
// drm1
EXPECT_NEAR_RELATIVE(drm1(0,0), 0, 1e-5);
EXPECT_NEAR_RELATIVE(drm1(0,2), 4.00891863e-08, 1e-5);
EXPECT_NEAR_RELATIVE(drm1(1,1), -2.67261242e-08, 1e-5);
EXPECT_NEAR_RELATIVE(drm1(2,0), 4.00891863e-08, 1e-5);
EXPECT_NEAR_RELATIVE(drm1(2,2), -2.67261242e-08, 1e-5);
// drm2
EXPECT_NEAR_RELATIVE(drm2(0,0), -5.34522484e-08, 1e-5);
ASSERT_NEAR(drm2(0,2), 1, 1e-5);
ASSERT_NEAR(drm2(1,1), 0., 1e-5);
ASSERT_NEAR(drm2(2,0), -1, 1e-5);
EXPECT_NEAR_RELATIVE(drm2(2,2), -5.34522484e-08, 1e-5);
// drm3
EXPECT_NEAR_RELATIVE(drm3(0,0), -8.01783726e-08, 1e-5);
EXPECT_NEAR_RELATIVE(drm3(0,2), 1.33630621e-08, 1e-5);
EXPECT_NEAR_RELATIVE(drm3(1,1), -8.01783726e-08, 1e-5);
EXPECT_NEAR_RELATIVE(drm3(2,0), 1.33630621e-08, 1e-5);
ASSERT_NEAR(drm3(2,2), 0., 1e-5);
}
TEST(RotMatDerivs, Works)
{
VecN<3> p;
for (size_t i = 0; i < p.size(); ++i) p[i] = i+1;
p /= norm<3>(p);
MatrixNM<3,3> mx(1.);
MatrixNM<3,3> drm1(0.);
MatrixNM<3,3> drm2(0.);
MatrixNM<3,3> drm3(0.);
pele::rot_mat_derivatives(p, mx, drm1, drm2, drm3);
// mx
ASSERT_NEAR(mx(0,0), 0.57313786, 1e-5);
ASSERT_NEAR(mx(1,0), 0.74034884, 1e-5);
ASSERT_NEAR(mx(2,0), -0.35127851, 1e-4);
ASSERT_NEAR(mx(0,1), -0.60900664, 1e-5);
ASSERT_NEAR(mx(1,1), 0.6716445, 1e-5);
ASSERT_NEAR(mx(2,1), 0.42190588, 1e-5);
ASSERT_NEAR(mx(0,2), 0.54829181, 1e-5);
ASSERT_NEAR(mx(1,2), -0.02787928, 1e-5);
ASSERT_NEAR(mx(2,2), 0.83582225, 1e-5);
// drm1
ASSERT_NEAR(drm1(0,0), 0.01933859, 1e-5);
ASSERT_NEAR(drm1(0,2), 0.32109128, 1e-5);
ASSERT_NEAR(drm1(1,1), -0.23084292, 1e-5);
ASSERT_NEAR(drm1(2,0), 0.40713948, 1e-5);
ASSERT_NEAR(drm1(2,2), -0.23828083, 1e-5);
// drm2
ASSERT_NEAR(drm2(0,0), -0.45276033, 1e-5);
ASSERT_NEAR(drm2(0,2), 0.74649729, 1e-5);
ASSERT_NEAR(drm2(1,1), 0.02975168, 1e-5);
ASSERT_NEAR(drm2(2,0), -0.76434829, 1e-5);
ASSERT_NEAR(drm2(2,2), -0.47656167, 1e-5);
// drm3
ASSERT_NEAR(drm3(0,0), -0.67914049, 1e-5);
ASSERT_NEAR(drm3(0,2), -0.01960117, 1e-5);
ASSERT_NEAR(drm3(1,1), -0.69252875, 1e-5);
ASSERT_NEAR(drm3(2,0), 0.23854341, 1e-5);
ASSERT_NEAR(drm3(2,2), 0.02231376, 1e-5);
}
TEST_F(AATopologyTest, TransformRotate_Works)
{
auto x = x0.copy();
pele::TransformAACluster transform(rbtopology.get());
VecN<3> p;
for (size_t i = 0; i < p.size(); ++i) p[i] = i+1;
p /= norm<3>(p);
transform.rotate(x, pele::aa_to_rot_mat(p));
// std::cout << p << std::endl;
// std::cout << pele::aa_to_rot_mat(p) << std::endl;
// std::cout << x0 << std::endl;
// std::cout << x << std::endl;
ASSERT_NEAR(x[0], 0.48757698, 1e-5);
ASSERT_NEAR(x[4], 4.76822812, 1e-5);
ASSERT_NEAR(x[8], 7.53224809, 1e-5);
ASSERT_NEAR(x[12], 0.72426504, 1e-5);
ASSERT_NEAR(x[16], 1.25159032, 1e-5);
}
/*!\brief TODO
//
// \param A TODO
// \param b TODO
// \param x TODO
// \return TODO
// \exception std::invalid_argument Invalid matrix size.
// \exception std::invalid_argument Invalid right-hand side vector size.
//
// TODO: description
// TODO: problem formulation \f$ A \cdot x + b = 0 \f$ !!
*/
bool GaussianElimination::solve( const CMatMxN& A, const VecN& b, VecN& x )
{
if( A.rows() != A.columns() )
throw std::invalid_argument( "Invalid matrix size" );
if( A.rows() != b.size() )
throw std::invalid_argument( "Invalid right-hand side vector size" );
const size_t n( b.size() );
// Allocating helper data
A_ = A;
b_ = -b;
x.resize( n, false );
size_t pi, pj;
lastPrecision_ = real(0);
// Initializing the pivot vector
DynamicVector<size_t> p( n );
for( size_t j=0; j<n; ++j ) {
p[j] = j;
}
// Performing the Gaussian elimination
for( size_t j=0; j<n; ++j )
{
size_t max( j );
real max_val( std::fabs( A_(p[max],j) ) );
// Partial search for pivot
for( size_t i=j+1; i<n; ++i ) {
if( std::fabs( A_(p[i],j) ) > max_val ) {
max = i;
max_val = std::fabs( A_(p[max],j) );
}
}
// Swapping rows such the pivot lies on the diagonal
std::swap( p[max], p[j] );
pj = p[j];
if( !isDefault( A_(pj,j) ) )
{
// Eliminating the column below the diagonal
for( size_t i=j+1; i<n; ++i )
{
pi = p[i];
const real f = A_(pi,j) / A_(pj,j);
reset( A_(pi,j) );
for( size_t k=j+1; k<n; ++k ) {
A_(pi,k) -= A_(pj,k) * f;
}
b_[pi] -= b_[pj] * f;
}
}
else {
// Asserting that the column is zero below the diagonal
for( size_t i=j+1; i<n; ++i ) {
BLAZE_INTERNAL_ASSERT( isDefault( A_(p[i],j) ), "Fatal error in Gaussian elimination" );
}
}
}
// Performing the backward substitution
for( size_t i=n-1; i<n; --i )
{
pi = p[i];
real rhs = b_[pi];
for( size_t j=i+1; j<n; ++j ) {
rhs -= x[j] * A_(pi,j);
}
if( std::fabs( A_(pi,i) ) > accuracy ) {
x[i] = rhs / A_(pi,i);
}
else {
// This will introduce errors in the solution
reset( x[i] );
lastPrecision_ = max( lastPrecision_, std::fabs( rhs ) );
}
}
BLAZE_LOG_DEBUG_SECTION( log ) {
if( lastPrecision_ < threshold_ )
log << " Solved the linear system using Gaussian elimination.";
else
log << BLAZE_YELLOW << " WARNING: Did not solve the linear system within accuracy. (" << lastPrecision_ << ")" << BLAZE_OLDCOLOR;
}
lastIterations_ = 1;
return lastPrecision_ < threshold_;
}
