void TestTranslation3DWithUblas()
    {
        TrianglesMeshReader<3,3> mesh_reader("mesh/test/data/cube_136_elements");
        TetrahedralMesh<3,3> mesh;
        mesh.ConstructFromMeshReader(mesh_reader);

        double volume = mesh.GetVolume();
        double surface_area = mesh.GetSurfaceArea();
        Node<3>* p_node1 = mesh.GetNode(36);
        ChastePoint<3> point1 = p_node1->GetPoint();
        Node<3>* p_node2 = mesh.GetNode(23);
        ChastePoint<3> point2 = p_node2->GetPoint();

        c_vector<double, 3> old_location1 = point1.rGetLocation();
        c_vector<double, 3> old_location2 = point2.rGetLocation();

        // Set translation Vector
        c_vector<double, 3> trans_vec;
        trans_vec(0) = 2.0;
        trans_vec(1) = 2.0;
        trans_vec(2) = 2.0;

        // Translate
        mesh.Translate(trans_vec);
        c_vector<double, 3> new_location1 = point1.rGetLocation();
        c_vector<double, 3> new_location2 = point2.rGetLocation();

        // Check Volume and Surface Area are invariant
        TS_ASSERT_DELTA(mesh.GetVolume(), volume, 1e-6);
        TS_ASSERT_DELTA(mesh.GetSurfaceArea(), surface_area, 1e-6);

        // Spot check a couple of nodes
        TS_ASSERT_DELTA(inner_prod(new_location1-old_location1, trans_vec), 0, 1e-6);
        TS_ASSERT_DELTA(inner_prod(new_location2-old_location2, trans_vec), 0, 1e-6);
    }
    void TestRefreshMeshByScaling()
    {
        TrianglesMeshReader<3,3> mesh_reader("mesh/test/data/cube_136_elements");
        TetrahedralMesh<3,3> mesh;
        mesh.ConstructFromMeshReader(mesh_reader);

        TS_ASSERT_DELTA(mesh.GetVolume(), 1.0, 1e-6);
        TS_ASSERT_DELTA(mesh.GetSurfaceArea(), 6.0, 1e-6);

        // Change coordinates

        for (unsigned i=0; i<mesh.GetNumNodes(); i++)
        {
            Node<3>* p_node = mesh.GetNode(i);
            ChastePoint<3> point = p_node->GetPoint();
            point.SetCoordinate(0, point[0]*2.0);
            point.SetCoordinate(1, point[1]*2.0);
            point.SetCoordinate(2, point[2]*2.0);
            p_node->SetPoint(point);
        }

        mesh.RefreshMesh();

        TS_ASSERT_DELTA(mesh.GetVolume(), 8.0, 1e-6);
        TS_ASSERT_DELTA(mesh.GetSurfaceArea(), 24.0, 1e-6);
    }
    void TestXaxisRotation3DWithHomogeneousUblas()
    {
        TrianglesMeshReader<3,3> mesh_reader("mesh/test/data/cube_136_elements");
        TetrahedralMesh<3,3> mesh;
        mesh.ConstructFromMeshReader(mesh_reader);

        TS_ASSERT_DELTA(mesh.GetVolume(), 1.0, 1e-6);
        TS_ASSERT_DELTA(mesh.GetSurfaceArea(), 6.0, 1e-6);

        // Change coordinates

        c_matrix<double, 4, 4> x_rotation_matrix = identity_matrix<double>(4);

        double theta = M_PI/2;

        x_rotation_matrix(1,1) = cos(theta);
        x_rotation_matrix(1,2) = sin(theta);
        x_rotation_matrix(2,1) = -sin(theta);
        x_rotation_matrix(2,2) = cos(theta);

        ChastePoint<3> corner_before = mesh.GetNode(6)->GetPoint();
        TS_ASSERT_EQUALS(corner_before[0], 1.0);
        TS_ASSERT_EQUALS(corner_before[1], 1.0);
        TS_ASSERT_EQUALS(corner_before[2], 1.0);

        for (unsigned i=0; i<mesh.GetNumNodes(); i++)
        {
            Node<3>* p_node = mesh.GetNode(i);
            ChastePoint<3> point = p_node->GetPoint();

            c_vector<double, 4> point_location;

            point_location[0] = point[0];
            point_location[1] = point[1];
            point_location[2] = point[2];
            point_location[3] = 1.0;

            c_vector<double, 4> new_point_location = prod(x_rotation_matrix, point_location);

            TS_ASSERT_EQUALS(new_point_location[3], 1.0);

            point.SetCoordinate(0, new_point_location[0]);
            point.SetCoordinate(1, new_point_location[1]);
            point.SetCoordinate(2, new_point_location[2]);
            p_node->SetPoint(point);
        }

        ChastePoint<3> corner_after = mesh.GetNode(6)->GetPoint();
        TS_ASSERT_EQUALS(corner_after[0], 1.0);
        TS_ASSERT_EQUALS(corner_after[1], 1.0);
        TS_ASSERT_DELTA(corner_after[2], -1.0, 1e-7);

        mesh.RefreshMesh();

        TS_ASSERT_DELTA(mesh.GetVolume(), 1.0, 1e-6);
        TS_ASSERT_DELTA(mesh.GetSurfaceArea(), 6.0, 1e-6);
    }
    void TestGeneralConvolution3DWithHomogeneousUblas()
    {
        TrianglesMeshReader<3,3> mesh_reader("mesh/test/data/cube_136_elements");
        TetrahedralMesh<3,3> mesh;
        mesh.ConstructFromMeshReader(mesh_reader);

        TS_ASSERT_DELTA(mesh.GetVolume(), 1.0, 1e-6);
        TS_ASSERT_DELTA(mesh.GetSurfaceArea(), 6.0, 1e-6);

        // Change coordinates

        c_matrix<double, 4, 4> x_rotation_matrix = identity_matrix<double>(4);
        c_matrix<double, 4, 4> y_rotation_matrix = identity_matrix<double>(4);
        c_matrix<double, 4, 4> z_rotation_matrix = identity_matrix<double>(4);
        c_matrix<double, 4, 4> translation_matrix = identity_matrix<double>(4);

        double theta = 0.7;
        double phi = 0.3;
        double psi = 1.4;

        x_rotation_matrix(1,1) = cos(theta);
        x_rotation_matrix(1,2) = sin(theta);
        x_rotation_matrix(2,1) = -sin(theta);
        x_rotation_matrix(2,2) = cos(theta);

        y_rotation_matrix(0,0) = cos(phi);
        y_rotation_matrix(0,2) = -sin(phi);
        y_rotation_matrix(2,0) = sin(phi);
        y_rotation_matrix(2,2) = cos(phi);

        z_rotation_matrix(0,0) = cos(psi);
        z_rotation_matrix(0,1) = sin(psi);
        z_rotation_matrix(1,0) = -sin(psi);
        z_rotation_matrix(1,1) = cos(psi);

        translation_matrix(0,3) = 2.3;
        translation_matrix(1,3) = 3.1;
        translation_matrix(2,3) = 1.7;

        /*
        Note: because we are using column-major vectors this tranformation:
        RotX(theta) . RotY(phi) . RotZ(psi) . Trans(...)
        is actually being applied right-to-left
        See test below.
        */
        c_matrix<double, 4, 4> transformation_matrix = prod (x_rotation_matrix, y_rotation_matrix);
        transformation_matrix = prod (transformation_matrix, z_rotation_matrix);
        transformation_matrix = prod (transformation_matrix, translation_matrix);

        for (unsigned i=0; i<mesh.GetNumNodes(); i++)
        {
            Node<3>* p_node = mesh.GetNode(i);
            ChastePoint<3> point = p_node->GetPoint();

            c_vector<double, 4> point_location;

            point_location[0] = point[0];
            point_location[1] = point[1];
            point_location[2] = point[2];
            point_location[3] = 1.0;

            c_vector<double, 4> new_point_location = prod(transformation_matrix, point_location);

            TS_ASSERT_EQUALS(new_point_location[3], 1.0);

            point.SetCoordinate(0,new_point_location[0]);
            point.SetCoordinate(1,new_point_location[1]);
            point.SetCoordinate(2,new_point_location[2]);
            p_node->SetPoint(point);
        }
        mesh.RefreshMesh();

        TS_ASSERT_DELTA(mesh.GetVolume(), 1.0, 1e-6);
        TS_ASSERT_DELTA(mesh.GetSurfaceArea(), 6.0, 1e-6);

        ChastePoint<3> corner_after = mesh.GetNode(6)->GetPoint();
        TS_ASSERT_DELTA(corner_after[0], 3.59782,  5e-5);
        TS_ASSERT_DELTA(corner_after[1], 0.583418, 5e-5);
        TS_ASSERT_DELTA(corner_after[2], 4.65889,  5e-5);

        // Write to file
        TrianglesMeshWriter<3,3> mesh_writer("","TransformedMesh");
        mesh_writer.WriteFilesUsingMesh(mesh);

        /*
        * Now try
        tetview /tmp/chaste/testoutput/TransformedMesh
        */
    }