/* -*- mia-c++  -*-
 *
 * This file is part of MIA - a toolbox for medical image analysis
 * Copyright (c) Leipzig, Madrid 1999-2017 Gert Wollny
 *
 * MIA is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with MIA; if not, see <http://www.gnu.org/licenses/>.
 *
 */

#include <mia/internal/autotest.hh>
#include <mia/3d/affine_matrix.hh>

using namespace mia;

BOOST_AUTO_TEST_CASE( test_identity )
{
       CAffinTransformMatrix identity;
       C3DFVector x(1.0f, 2.0f, 3.0f);
       const C3DFVector y = identity * x;
       BOOST_CHECK_EQUAL(y, x);
}


BOOST_AUTO_TEST_CASE( test_identity_reset )
{
       CAffinTransformMatrix identity;
       // fill matrix with some values
       identity.rotate_z(2, C3DFVector(1, 2, 3));
       identity.rotate_y(1, C3DFVector(1, 23, 3));
       identity.translate(C3DFVector(1, 2, 3));
       identity.identity();
       const auto& data = identity.data();
       BOOST_CHECK_EQUAL(data[0], 1.0f);
       BOOST_CHECK_EQUAL(data[1], 0.0f);
       BOOST_CHECK_EQUAL(data[2], 0.0f);
       BOOST_CHECK_EQUAL(data[3], 0.0f);
       BOOST_CHECK_EQUAL(data[4], 0.0f);
       BOOST_CHECK_EQUAL(data[5], 1.0f);
       BOOST_CHECK_EQUAL(data[6], 0.0f);
       BOOST_CHECK_EQUAL(data[7], 0.0f);
       BOOST_CHECK_EQUAL(data[ 8], 0.0f);
       BOOST_CHECK_EQUAL(data[ 9], 0.0f);
       BOOST_CHECK_EQUAL(data[10], 1.0f);
       BOOST_CHECK_EQUAL(data[11], 0.0f);
       BOOST_CHECK_EQUAL(data[12], 0.0f);
       BOOST_CHECK_EQUAL(data[13], 0.0f);
       BOOST_CHECK_EQUAL(data[14], 0.0f);
       BOOST_CHECK_EQUAL(data[15], 1.0f);
}


BOOST_AUTO_TEST_CASE( test_simple_inverse )
{
       CAffinTransformMatrix simple(2.0f, 0.0f, 0.0f, 0.0f,
                                    0.0f, 0.5f, 0.0f, 0.0f,
                                    0.0f, 0.0f, 0.2f, 0.0f);
       auto inv = simple.inverse();
       const auto& inv_data = inv.data();
       BOOST_CHECK_CLOSE(inv_data[0], 0.5f, 0.01);
       BOOST_CHECK_SMALL(inv_data[4], 1e-5f);
       BOOST_CHECK_SMALL(inv_data[8], 1e-5f);
       BOOST_CHECK_SMALL(inv_data[12], 1e-5f);
       BOOST_CHECK_SMALL(inv_data[1], 1e-5f);
       BOOST_CHECK_CLOSE(inv_data[5], 2.0f, 0.01);
       BOOST_CHECK_SMALL(inv_data[9], 1e-5f);
       BOOST_CHECK_SMALL(inv_data[13], 1e-5f);
       BOOST_CHECK_SMALL(inv_data[2], 1e-5f);
       BOOST_CHECK_SMALL(inv_data[6], 1e-5f);
       BOOST_CHECK_CLOSE(inv_data[10], 5.0f, 0.01);
       BOOST_CHECK_SMALL(inv_data[14], 1e-5f);
       BOOST_CHECK_SMALL(inv_data[3], 1e-5f);
       BOOST_CHECK_SMALL(inv_data[7], 1e-5f);
       BOOST_CHECK_SMALL(inv_data[11], 1e-5f);
       BOOST_CHECK_CLOSE(inv_data[15], 1.0f, 0.01);
}

BOOST_AUTO_TEST_CASE( test_inverse )
{
       CAffinTransformMatrix simple(2.0f, 3.0f, 2.0f, 1.0f,
                                    3.0f, 1.5f, 3.0f, 5.0f,
                                    4.0f, 2.0f, 1.2f, 1.8f);
       auto inv = simple.inverse();
       const auto& inv_data = inv.data();
       BOOST_CHECK_CLOSE(inv_data[0], -0.25f, 0.01);
       BOOST_CHECK_CLOSE(inv_data[4], 1.0f / 42.0f, 0.01);
       BOOST_CHECK_CLOSE(inv_data[8], 5.0f / 14.0f, 0.01);
       BOOST_CHECK_CLOSE(inv_data[12], -43.0f / 84.0f, 0.01);
       BOOST_CHECK_CLOSE(inv_data[1], 1.0f / 2.0f, 0.01);
       BOOST_CHECK_CLOSE(inv_data[5], -1.0f / 3.0f, 0.01);
       BOOST_CHECK_SMALL(inv_data[9], 1e-5f);
       BOOST_CHECK_CLOSE(inv_data[13], 7.0f / 6.0f, 0.01);
       BOOST_CHECK_SMALL(inv_data[2], 1e-5f);
       BOOST_CHECK_CLOSE(inv_data[6], 10.0f / 21.0f, 0.01);
       BOOST_CHECK_CLOSE(inv_data[10], -5.0f / 14.0f, 0.01);
       BOOST_CHECK_CLOSE(inv_data[14], -73.0f / 42.0f, 0.01);
       BOOST_CHECK_SMALL(inv_data[3], 1e-5f);
       BOOST_CHECK_SMALL(inv_data[7], 1e-5f);
       BOOST_CHECK_SMALL(inv_data[11], 1e-5f);
       BOOST_CHECK_CLOSE(inv_data[15], 1.0f, 0.01);
}

BOOST_AUTO_TEST_CASE( test_rot_x_from_identity )
{
       CAffinTransformMatrix m;
       m.rotate_x(M_PI / 3.0f, C3DFVector(20.0f, 12.0f, 3.0f));
       const float sin_pi_3 = sqrt(3.0f) / 2.0f;
       const float cos_pi_3 = 1.0f / 2.0f;
       const auto& data = m.data();
       BOOST_CHECK_CLOSE(data[0], 1.0, 0.01);
       BOOST_CHECK_SMALL(data[4], 1e-5f);
       BOOST_CHECK_SMALL(data[8], 1e-5f);
       BOOST_CHECK_SMALL(data[12], 1e-5f);
       BOOST_CHECK_SMALL(data[1], 1e-5f);
       BOOST_CHECK_CLOSE(data[5], cos_pi_3, 0.01f);
       BOOST_CHECK_CLOSE(data[9], -sin_pi_3, 0.01f);
       BOOST_CHECK_CLOSE(data[13], +12.0f * 0.5f + sin_pi_3 * 3.0f, 0.01f);
       BOOST_CHECK_SMALL(data[2], 1e-5f);
       BOOST_CHECK_CLOSE(data[6], sin_pi_3, 0.01);
       BOOST_CHECK_CLOSE(data[10], cos_pi_3, 0.01);
       BOOST_CHECK_CLOSE(data[14], -12.0f * sin_pi_3 + 1.5f, 0.01);
       BOOST_CHECK_SMALL(data[3], 1e-5f);
       BOOST_CHECK_SMALL(data[7], 1e-5f);
       BOOST_CHECK_SMALL(data[11], 1e-5f);
       BOOST_CHECK_CLOSE(data[15], 1.0f, 0.01);
}

BOOST_AUTO_TEST_CASE( test_rot_y_from_identity )
{
       CAffinTransformMatrix m;
       m.rotate_y(M_PI / 3.0f, C3DFVector(20.0f, 12.0f, 3.0f));
       const float sin_pi_3 = sqrt(3.0f) / 2.0f;
       const float cos_pi_3 = 1.0f / 2.0f;
       const auto& data = m.data();
       BOOST_CHECK_CLOSE(data[0], cos_pi_3, 0.01f);
       BOOST_CHECK_SMALL(data[4], 1e-5f);
       BOOST_CHECK_CLOSE(data[8], -sin_pi_3, 0.01f);
       BOOST_CHECK_CLOSE(data[12], 20.0f * 0.5f + sin_pi_3 * 3.0f, 0.01f);
       BOOST_CHECK_SMALL(data[1], 1e-5f);
       BOOST_CHECK_CLOSE(data[5], 1.0f, 0.01);
       BOOST_CHECK_SMALL(data[9], 1e-5f);
       BOOST_CHECK_SMALL(data[13], 1e-5f);
       BOOST_CHECK_CLOSE(data[2], sin_pi_3, 0.01);
       BOOST_CHECK_SMALL(data[6], 1e-5f);
       BOOST_CHECK_CLOSE(data[10], cos_pi_3, 0.01);
       BOOST_CHECK_CLOSE(data[14], -20.0f * sin_pi_3 + 1.5f, 0.01);
       BOOST_CHECK_SMALL(data[3], 1e-5f);
       BOOST_CHECK_SMALL(data[7], 1e-5f);
       BOOST_CHECK_SMALL(data[11], 1e-5f);
       BOOST_CHECK_CLOSE(data[15], 1.0f, 0.01);
}

BOOST_AUTO_TEST_CASE( test_rot_z_from_identity )
{
       CAffinTransformMatrix m;
       m.rotate_z(M_PI / 3.0f, C3DFVector(20.0f, 12.0f, 3.0f));
       const float sin_pi_3 = sqrt(3.0f) / 2.0f;
       const float cos_pi_3 = 1.0f / 2.0f;
       const auto& data = m.data();
       BOOST_CHECK_CLOSE(data[0], cos_pi_3, 0.01f);
       BOOST_CHECK_CLOSE(data[4], -sin_pi_3, 0.01f);
       BOOST_CHECK_SMALL(data[8], 1e-5f);
       BOOST_CHECK_CLOSE(data[12], 20.0f * 0.5f + sin_pi_3 * 12.0f, 0.01f);
       BOOST_CHECK_CLOSE(data[1], sin_pi_3, 0.01);
       BOOST_CHECK_CLOSE(data[5], cos_pi_3, 0.01);
       BOOST_CHECK_SMALL(data[9], 1e-5f);
       BOOST_CHECK_CLOSE(data[13], -20.0f * sin_pi_3 + 6.0f, 0.01);
       BOOST_CHECK_SMALL(data[2], 1e-5f);
       BOOST_CHECK_SMALL(data[6], 1e-5f);
       BOOST_CHECK_CLOSE(data[10], 1.0f, 0.01);
       BOOST_CHECK_SMALL(data[14], 1e-5f);
       BOOST_CHECK_SMALL(data[3], 1e-5f);
       BOOST_CHECK_SMALL(data[7], 1e-5f);
       BOOST_CHECK_SMALL(data[11], 1e-5f);
       BOOST_CHECK_CLOSE(data[15], 1.0f, 0.01);
}

BOOST_AUTO_TEST_CASE( test_translate_from_identity )
{
       CAffinTransformMatrix m;
       m.translate(C3DFVector(2.0f, 3.0f, 4.0f));
       const auto& data = m.data();
       BOOST_CHECK_CLOSE(data[0], 1.0f, 0.01f);
       BOOST_CHECK_SMALL(data[4], 1e-5f);
       BOOST_CHECK_SMALL(data[8], 1e-5f);
       BOOST_CHECK_CLOSE(data[12], 2.0f, 0.01f);
       BOOST_CHECK_SMALL(data[1], 1e-5f);
       BOOST_CHECK_CLOSE(data[5], 1.0f, 0.01f);
       BOOST_CHECK_SMALL(data[9], 1e-5f);
       BOOST_CHECK_CLOSE(data[13], 3.0f, 0.01f);
       BOOST_CHECK_SMALL(data[2], 1e-5f);
       BOOST_CHECK_SMALL(data[6], 1e-5f);
       BOOST_CHECK_CLOSE(data[10], 1.0f, 0.01f);
       BOOST_CHECK_CLOSE(data[14], 4.0f, 0.01);
       BOOST_CHECK_SMALL(data[3], 1e-5f);
       BOOST_CHECK_SMALL(data[7], 1e-5f);
       BOOST_CHECK_SMALL(data[11], 1e-5f);
       BOOST_CHECK_CLOSE(data[15], 1.0f, 0.01);
       const auto inv = m.inverse();
       const auto& inv_data = inv.data();
       BOOST_CHECK_CLOSE(inv_data[0], 1.0f, 0.01f);
       BOOST_CHECK_SMALL(inv_data[4], 1e-5f);
       BOOST_CHECK_SMALL(inv_data[8], 1e-5f);
       BOOST_CHECK_CLOSE(inv_data[12], -2.0f, 0.01f);
       BOOST_CHECK_SMALL(inv_data[1], 1e-5f);
       BOOST_CHECK_CLOSE(inv_data[5], 1.0f, 0.01f);
       BOOST_CHECK_SMALL(inv_data[9], 1e-5f);
       BOOST_CHECK_CLOSE(inv_data[13], -3.0f, 0.01f);
       BOOST_CHECK_SMALL(inv_data[2], 1e-5f);
       BOOST_CHECK_SMALL(inv_data[6], 1e-5f);
       BOOST_CHECK_CLOSE(inv_data[10], 1.0f, 0.01f);
       BOOST_CHECK_CLOSE(inv_data[14], -4.0f, 0.01);
       BOOST_CHECK_SMALL(inv_data[3], 1e-5f);
       BOOST_CHECK_SMALL(inv_data[7], 1e-5f);
       BOOST_CHECK_SMALL(inv_data[11], 1e-5f);
       BOOST_CHECK_CLOSE(inv_data[15], 1.0f, 0.01);
}


struct ConcatationFixture {

       ConcatationFixture();
       void run_check(const CAffinTransformMatrix& m);

       CAffinTransformMatrix start;
       C3DFVector x;
       C3DFVector y1;
};

ConcatationFixture::ConcatationFixture():
       start(2.0f, 1.0f, 3.0f, 1.0f,
             2.0f, 0.5f, 4.0f, 2.0f,
             3.0f, 4.0f, 0.2f, 1.0f),
       x(2.0, 4.0, 9.0),
       y1(start * x)

{
}

void ConcatationFixture::run_check(const CAffinTransformMatrix& m)
{
       C3DFVector y2_direct = start * x;
       C3DFVector y2_step = m * y1;
       BOOST_CHECK_CLOSE(y2_direct.x, y2_step.x, 0.01);
       BOOST_CHECK_CLOSE(y2_direct.y, y2_step.y, 0.01);
       BOOST_CHECK_CLOSE(y2_direct.z, y2_step.z, 0.01);
}

BOOST_FIXTURE_TEST_CASE( test_translate, ConcatationFixture)
{
       C3DFVector t(1.0, 2.0, 3.0);
       start.translate(t);
       CAffinTransformMatrix m;
       m.translate(t);
       run_check(m);
}

BOOST_AUTO_TEST_CASE( test_scale_from_identity )
{
       CAffinTransformMatrix m;
       C3DFVector center(1, 2, 3);
       m.scale(C3DFVector(2.0f, 3.0f, 4.0f), center);
       const auto& data = m.data();
       BOOST_CHECK_CLOSE(data[0], 2.0f, 0.01f);
       BOOST_CHECK_SMALL(data[4], 1e-5f);
       BOOST_CHECK_SMALL(data[8], 1e-5f);
       BOOST_CHECK_CLOSE(data[12], -1.0f, 0.01f);
       BOOST_CHECK_SMALL(data[1], 1e-5f);
       BOOST_CHECK_CLOSE(data[5], 3.0f, 0.01f);
       BOOST_CHECK_SMALL(data[9], 1e-5f);
       BOOST_CHECK_CLOSE(data[13], -4.0f, 0.01f);
       BOOST_CHECK_SMALL(data[2], 1e-5f);
       BOOST_CHECK_SMALL(data[6], 1e-5f);
       BOOST_CHECK_CLOSE(data[10], 4.0f, 0.01f);
       BOOST_CHECK_CLOSE(data[14], -9.0f, 0.01);
       BOOST_CHECK_SMALL(data[3], 1e-5f);
       BOOST_CHECK_SMALL(data[7], 1e-5f);
       BOOST_CHECK_SMALL(data[11], 1e-5f);
       BOOST_CHECK_CLOSE(data[15], 1.0f, 0.01);
}


BOOST_AUTO_TEST_CASE( test_rot_from_quaternion_and_identity )
{
       CAffinTransformMatrix m;
       C3DFVector center(20.0f, 12.0f, 3.0f);
       Quaternion q(sqrt(10.0f) / 4.0f, 0.25f, -0.25f, 0.5f);
       m.rotate(q, center);
       const auto& data = m.data();
       BOOST_CHECK_CLOSE(data[0], 0.375f, 0.01f);
       BOOST_CHECK_CLOSE(data[4], 2 * ( -1 / 16.0 - sqrt(10.0) / 8.0), 0.01f);
       BOOST_CHECK_CLOSE(data[8], 2 * (0.125 - sqrt(10.0) / 16.0 ), 0.01f );
       BOOST_CHECK_CLOSE(data[12], 20.0f - (data[0] * 20.0f + data[4] * 12.0f + data[8] * 3.0f  ), 0.01f);
       BOOST_CHECK_CLOSE(data[1], 2 * ( sqrt(10.0) / 8.0 - 1 / 16.0), 0.01);
       BOOST_CHECK_CLOSE(data[5], 0.375f, 0.01);
       BOOST_CHECK_CLOSE(data[9], 2 * (-.125 - sqrt(10.0) / 16.0), 0.01f);
       BOOST_CHECK_CLOSE(data[13], 12.0f - (data[1] * 20 + data[5] * 12.0f + data[9] * 3.0f  ), 0.01);
       BOOST_CHECK_CLOSE(data[2], 2 * (1 / 8.0 + sqrt(10.0) / 16.0), 0.01f);
       BOOST_CHECK_CLOSE(data[6], 2 * (-1 / 8.0 + sqrt(10.0) / 16.0), 0.01f);
       BOOST_CHECK_CLOSE(data[10], 3.0f / 4.0f, 0.01);
       BOOST_CHECK_CLOSE(data[14], 3.0f - (data[2] * 20 + data[6] * 12.0f + data[10] * 3.0f  ), 0.01f);
       BOOST_CHECK_SMALL(data[3], 1e-5f);
       BOOST_CHECK_SMALL(data[7], 1e-5f);
       BOOST_CHECK_SMALL(data[11], 1e-5f);
       BOOST_CHECK_CLOSE(data[15], 1.0f, 0.01);
}


BOOST_FIXTURE_TEST_CASE( test_rot_from_quaternion_and_pseudorando_matrix, ConcatationFixture)
{
       C3DFVector center(20.0f, 12.0f, 3.0f);
       Quaternion q(sqrt(10.0f) / 4.0f, 0.25f, -0.25f, 0.5f);
       CAffinTransformMatrix m;
       m.rotate(q, center);
       start.rotate(q, center);
       run_check(m);
}


BOOST_FIXTURE_TEST_CASE( test_rot_x_pseudorando_matrix, ConcatationFixture)
{
       C3DFVector center(20.0f, 12.0f, 3.0f);
       CAffinTransformMatrix m;
       m.rotate_x(1.0, center);
       start.rotate_x(1.0, center);
       run_check(m);
}

BOOST_FIXTURE_TEST_CASE( test_rot_y_pseudorando_matrix, ConcatationFixture)
{
       C3DFVector center(20.0f, 12.0f, 3.0f);
       CAffinTransformMatrix m;
       m.rotate_y(1.0, center);
       start.rotate_y(1.0, center);
       run_check(m);
}

BOOST_FIXTURE_TEST_CASE( test_rot_z_pseudorando_matrix, ConcatationFixture)
{
       C3DFVector center(20.0f, 12.0f, 3.0f);
       CAffinTransformMatrix m;
       m.rotate_z(1.0, center);
       start.rotate_z(1.0, center);
       run_check(m);
}

BOOST_FIXTURE_TEST_CASE( test_scale_pseudorando_matrix, ConcatationFixture)
{
       C3DFVector center(20.0f, 12.0f, 3.0f);
       C3DFVector scales(2.0f, 12.0f, 0.5f);
       CAffinTransformMatrix m;
       m.scale(scales, center);
       start.scale(scales, center);
       run_check(m);
}


BOOST_AUTO_TEST_CASE( test_multiply )
{
       CAffinTransformMatrix lhs(2.0f, 1.0f, 3.0f, 1.0f,
                                 2.0f, 0.5f, 4.0f, 2.0f,
                                 3.0f, 4.0f, 0.2f, 1.0f);
       CAffinTransformMatrix rhs(3.0f, 4.0f, 1.0f, 7.0f,
                                 2.0f, 1.5f, 2.0f, 3.0f,
                                 3.5f, 2.0f, 2.2f, 2.0f);
       lhs *= rhs;
       const auto& data = lhs.data();
       BOOST_CHECK_CLOSE(data[0], 18.5, 0.01);
       BOOST_CHECK_CLOSE(data[4], 15.5, 0.01);
       BOOST_CHECK_CLOSE(data[8], 10.6, 0.01);
       BOOST_CHECK_CLOSE(data[12], 24.0, 0.01);
       BOOST_CHECK_CLOSE(data[1], 21.0, 0.01);
       BOOST_CHECK_CLOSE(data[5], 16.75, 0.01);
       BOOST_CHECK_CLOSE(data[9], 11.8, 0.01);
       BOOST_CHECK_CLOSE(data[13], 25.5, 0.01);
       BOOST_CHECK_CLOSE(data[2], 17.7, 0.01);
       BOOST_CHECK_CLOSE(data[6], 18.4, 0.01);
       BOOST_CHECK_CLOSE(data[10], 11.44, 0.01);
       BOOST_CHECK_CLOSE(data[14], 34.4, 0.01);
       BOOST_CHECK_SMALL(data[3], 1e-5f);
       BOOST_CHECK_SMALL(data[7], 1e-5f);
       BOOST_CHECK_SMALL(data[11], 1e-5f);
       BOOST_CHECK_EQUAL(data[15], 1.0f);
}

#if 0
// not yet supported, will be added if needed
BOOST_AUTO_TEST_CASE( test_shear )
{
       CAffinTransformMatrix m;
       m.shear(1.1, 1.5, 0.5);
       C3DFVector x(1, 2, 3);
       auto y = m * x;
       BOOST_CHECK_CLOSE(y.x, 5.6f, 0.01f);
       BOOST_CHECK_CLOSE(y.y, 4.5f, 0.01f);
       BOOST_CHECK_CLOSE(y.z, 3.0f, 0.01f);
}
#endif

BOOST_AUTO_TEST_CASE( test_multiply_const_input )
{
       const CAffinTransformMatrix lhs(2.0f, 1.0f, 3.0f, 1.0f,
                                       2.0f, 0.5f, 4.0f, 2.0f,
                                       3.0f, 4.0f, 0.2f, 1.0f);
       const CAffinTransformMatrix rhs(3.0f, 4.0f, 1.0f, 7.0f,
                                       2.0f, 1.5f, 2.0f, 3.0f,
                                       3.5f, 2.0f, 2.2f, 2.0f);
       auto r = lhs * rhs;
       const auto& data = r.data();
       BOOST_CHECK_CLOSE(data[0], 18.5, 0.01);
       BOOST_CHECK_CLOSE(data[4], 15.5, 0.01);
       BOOST_CHECK_CLOSE(data[8], 10.6, 0.01);
       BOOST_CHECK_CLOSE(data[12], 24.0, 0.01);
       BOOST_CHECK_CLOSE(data[1], 21.0, 0.01);
       BOOST_CHECK_CLOSE(data[5], 16.75, 0.01);
       BOOST_CHECK_CLOSE(data[9], 11.8, 0.01);
       BOOST_CHECK_CLOSE(data[13], 25.5, 0.01);
       BOOST_CHECK_CLOSE(data[2], 17.7, 0.01);
       BOOST_CHECK_CLOSE(data[6], 18.4, 0.01);
       BOOST_CHECK_CLOSE(data[10], 11.44, 0.01);
       BOOST_CHECK_CLOSE(data[14], 34.4, 0.01);
       BOOST_CHECK_SMALL(data[3], 1e-5f);
       BOOST_CHECK_SMALL(data[7], 1e-5f);
       BOOST_CHECK_SMALL(data[11], 1e-5f);
       BOOST_CHECK_EQUAL(data[15], 1.0f);
}

BOOST_AUTO_TEST_CASE( test_shear_at_zero )
{
       CAffinTransformMatrix m;
       m.shear(C3DFVector(2, 3, 4));
       const auto& data = m.data();
       BOOST_CHECK_CLOSE(data[0], 1.0f, 0.01);
       BOOST_CHECK_CLOSE(data[4], 2.0f, 0.01);
       BOOST_CHECK_SMALL(data[8], 1e-5f);
       BOOST_CHECK_SMALL(data[12], 1e-5f);
       BOOST_CHECK_SMALL(data[1], 1e-5f);
       BOOST_CHECK_CLOSE(data[5], 1.0f, 0.01);
       BOOST_CHECK_CLOSE(data[9], 3.0f, 0.01);
       BOOST_CHECK_SMALL(data[13], 1e-5f);
       BOOST_CHECK_CLOSE(data[2], 4.0f, 0.01);
       BOOST_CHECK_SMALL(data[6], 1e-5f);
       BOOST_CHECK_CLOSE(data[10], 1.0f, 0.01);
       BOOST_CHECK_SMALL(data[14], 1e-5f);
       BOOST_CHECK_SMALL(data[3], 1e-5f);
       BOOST_CHECK_SMALL(data[7], 1e-5f);
       BOOST_CHECK_SMALL(data[11], 1e-5f);
       BOOST_CHECK_EQUAL(data[15], 1.0f);
}

BOOST_AUTO_TEST_CASE( test_zero_shear_centered )
{
       CAffinTransformMatrix m;
       C3DFVector center(2, 3, 4);
       m.shear(C3DFVector::_0, center);
       const auto& data = m.data();
       BOOST_CHECK_CLOSE(data[0], 1.0f, 0.01);
       BOOST_CHECK_SMALL(data[4], 1e-5f);
       BOOST_CHECK_SMALL(data[8], 1e-5f);
       BOOST_CHECK_SMALL(data[12], 1e-5f);
       BOOST_CHECK_SMALL(data[1], 1e-5f);
       BOOST_CHECK_CLOSE(data[5], 1.0f, 0.01);
       BOOST_CHECK_SMALL(data[9], 1e-5f);
       BOOST_CHECK_SMALL(data[13], 1e-5f);
       BOOST_CHECK_SMALL(data[2], 1e-5f);
       BOOST_CHECK_SMALL(data[6], 1e-5f);
       BOOST_CHECK_CLOSE(data[10], 1.0f, 0.01);
       BOOST_CHECK_SMALL(data[14], 1e-5f);
       BOOST_CHECK_SMALL(data[3], 1e-5f);
       BOOST_CHECK_SMALL(data[7], 1e-5f);
       BOOST_CHECK_SMALL(data[11], 1e-5f);
       BOOST_CHECK_EQUAL(data[15], 1.0f);
       auto vt = m * center;
       BOOST_CHECK_CLOSE(vt.x, center.x, 0.01);
       BOOST_CHECK_CLOSE(vt.y, center.y, 0.01);
       BOOST_CHECK_CLOSE(vt.z, center.z, 0.01);
}


BOOST_AUTO_TEST_CASE( test_shear_centered )
{
       CAffinTransformMatrix m;
       C3DFVector center(2, 3, 4);
       m.shear(C3DFVector(0.2, 0.3, 0.4), center);
       const auto& data = m.data();
       BOOST_CHECK_CLOSE(data[0], 1.0f, 0.01);
       BOOST_CHECK_SMALL(data[1], 1e-5f);
       BOOST_CHECK_CLOSE(data[2], 0.4f, 1e-5f);
       BOOST_CHECK_SMALL(data[3], 1e-5f);
       BOOST_CHECK_CLOSE(data[4], 0.2f, 1e-5f);
       BOOST_CHECK_CLOSE(data[5], 1.0f, 0.01);
       BOOST_CHECK_SMALL(data[6], 1e-5f);
       BOOST_CHECK_SMALL(data[7], 1e-5f);
       BOOST_CHECK_SMALL(data[8], 1e-5f);
       BOOST_CHECK_CLOSE(data[9], 0.3f, 1e-5f);
       BOOST_CHECK_CLOSE(data[10], 1.0f, 0.01);
       BOOST_CHECK_SMALL(data[11], 1e-5f);
       BOOST_CHECK_CLOSE(data[12], -0.6, 1e-4f);
       BOOST_CHECK_CLOSE(data[13], -1.2, 1e-4f);
       BOOST_CHECK_CLOSE(data[14], -0.8, 1e-4f);
       BOOST_CHECK_EQUAL(data[15], 1.0f);
       auto cc = m * center;
       BOOST_CHECK_CLOSE(cc.x, center.x, 1e-4f);
       BOOST_CHECK_CLOSE(cc.y, center.y, 1e-4f);
       BOOST_CHECK_CLOSE(cc.z, center.z, 1e-4f);
}

BOOST_AUTO_TEST_CASE( test_transform_centered )
{
       CAffinTransformMatrix lhs(2.0f, 1.0f, 3.0f, 1.0f,
                                 2.0f, 8.0f, 4.0f, 2.0f,
                                 3.0f, 4.0f, 6.0f, 1.0f);
       C3DFMatrix m(C3DFVector(2.0, 3.0, 5.0),
                    C3DFVector(4.0, 1.0, 8.0),
                    C3DFVector(6.0, 7.0, 9.0));
       C3DFVector c(10, 11, 12);
       lhs.transform_centered(m, c);
       const auto& data = lhs.data();
       BOOST_CHECK_CLOSE(data[0], 25, 0.01);
       BOOST_CHECK_CLOSE(data[1], 34, 0.01);
       BOOST_CHECK_CLOSE(data[2], 53, 0.01);
       BOOST_CHECK_SMALL(data[3], 1e-5f);
       BOOST_CHECK_CLOSE(data[4], 46, 0.01);
       BOOST_CHECK_CLOSE(data[5], 44, 0.01);
       BOOST_CHECK_CLOSE(data[6], 98, 0.01);
       BOOST_CHECK_SMALL(data[7], 1e-5f);
       BOOST_CHECK_CLOSE(data[8], 48, 0.01);
       BOOST_CHECK_CLOSE(data[9], 64, 0.01);
       BOOST_CHECK_CLOSE(data[10], 100, 0.01);
       BOOST_CHECK_SMALL(data[11], 1e-5f);
       BOOST_CHECK_CLOSE(data[12], -90, 0.01);
       BOOST_CHECK_CLOSE(data[13], -122, 0.01);
       BOOST_CHECK_CLOSE(data[14], -204, 0.01);
       BOOST_CHECK_EQUAL(data[15], 1.0f);
}
