rotationtest.cc

#include <config.h>

#include <iostream>
#include <array>

#include <dune/common/fmatrix.hh>

#include <dune/gfe/rotation.hh>
#include "valuefactory.hh"

using namespace Dune;

void testDDExp()
{
    std::array<FieldVector<double,3>, 125> v;
    int ct = 0;
    double eps = 1e-4;

    for (int i=-2; i<3; i++)
        for (int j=-2; j<3; j++)
            for (int k=-2; k<3; k++) {
                v[ct][0] = i;
                v[ct][1] = j;
                v[ct][2] = k;
                ct++;
            }

    for (size_t i=0; i<v.size(); i++) {

        // Compute FD approximation of second derivative of exp
        std::array<Dune::FieldMatrix<double,3,3>, 4> fdDDExp;

        for (int j=0; j<3; j++) {

            for (int k=0; k<3; k++) {

                if (j==k) {

                    SkewMatrix<double,3> forward(v[i]);
                    forward.axial()[j] += eps;
                    Rotation<double,3> forwardQ  = Rotation<double,3>::exp(forward);

                    SkewMatrix<double,3> center(v[i]);
                    Rotation<double,3> centerQ   = Rotation<double,3>::exp(center);

                    SkewMatrix<double,3> backward(v[i]);
                    backward.axial()[j] -= eps;
                    Rotation<double,3> backwardQ = Rotation<double,3>::exp(backward);

                    for (int l=0; l<4; l++)
                        fdDDExp[l][j][j] = (forwardQ[l] - 2*centerQ[l] + backwardQ[l]) / (eps*eps);


                } else {

                    SkewMatrix<double,3> ffV(v[i]);      ffV.axial()[j] += eps;     ffV.axial()[k] += eps;
                    SkewMatrix<double,3> fbV(v[i]);      fbV.axial()[j] += eps;     fbV.axial()[k] -= eps;
                    SkewMatrix<double,3> bfV(v[i]);      bfV.axial()[j] -= eps;     bfV.axial()[k] += eps;
                    SkewMatrix<double,3> bbV(v[i]);      bbV.axial()[j] -= eps;     bbV.axial()[k] -= eps;

                    Rotation<double,3> forwardForwardQ = Rotation<double,3>::exp(ffV);
                    Rotation<double,3> forwardBackwardQ = Rotation<double,3>::exp(fbV);
                    Rotation<double,3> backwardForwardQ = Rotation<double,3>::exp(bfV);
                    Rotation<double,3> backwardBackwardQ = Rotation<double,3>::exp(bbV);

                    for (int l=0; l<4; l++)
                        fdDDExp[l][j][k] = (forwardForwardQ[l] + backwardBackwardQ[l]
                                            - forwardBackwardQ[l] - backwardForwardQ[l]) / (4*eps*eps);

                }

            }

        }

        // Compute analytical second derivative of exp
        std::array<Dune::FieldMatrix<double,3,3>, 4> ddExp;
        Rotation<double,3>::DDexp(v[i], ddExp);

        for (int m=0; m<4; m++)
            for (int j=0; j<3; j++)
                for (int k=0; k<3; k++)
                    if ( std::abs(fdDDExp[m][j][k] - ddExp[m][j][k]) > eps) {
                        std::cout << "Error at v = " << v[i]
                                  << "[" << m << ", " << j << ", " << k << "] "
                                  << "    fd: " << fdDDExp[m][j][k]
                                  << "    analytical: " << ddExp[m][j][k] << std::endl;
                    }
    }
}

void testRotation(Rotation<double,3> q)
{
    // Make sure it really is a unit quaternion
    q.normalize();

    assert(std::abs(1-q.two_norm()) < 1e-12);

    // Turn it into a matrix
    FieldMatrix<double,3,3> matrix;
    q.matrix(matrix);

    // make sure it is an orthogonal matrix
    if (std::abs(1-matrix.determinant()) > 1e-12 )
        DUNE_THROW(Exception, "Expected determinant 1, but the computed value is " << matrix.determinant());

    assert( std::abs( matrix[0]*matrix[1] ) < 1e-12 );
    assert( std::abs( matrix[0]*matrix[2] ) < 1e-12 );
    assert( std::abs( matrix[1]*matrix[2] ) < 1e-12 );

    // Turn the matrix back into a quaternion, and check whether it is the same one
    // Since the quaternions form a double covering of SO(3), we may either get q back
    // or -q.  We have to check both.
    Rotation<double,3> newQ;
    newQ.set(matrix);

    Rotation<double,3> diff = newQ;
    diff -= q;

    Rotation<double,3> sum  = newQ;
    sum += q;

    if (diff.infinity_norm() > 1e-12 && sum.infinity_norm() > 1e-12)
        DUNE_THROW(Exception, "Backtransformation failed for " << q << ". ");

    // //////////////////////////////////////////////////////
    //   Check the director vectors
    // //////////////////////////////////////////////////////

    for (int i=0; i<3; i++)
        for (int j=0; j<3; j++)
            assert( std::abs(matrix[i][j] - q.director(j)[i]) < 1e-12 );

    // //////////////////////////////////////////////////////
    //   Check multiplication with another unit quaternion
    // //////////////////////////////////////////////////////

    for (int i=-2; i<2; i++)
        for (int j=-2; j<2; j++)
            for (int k=-2; k<2; k++)
                for (int l=-2; l<2; l++)
                    if (i!=0 || j!=0 || k!=0 || l!=0) {

                        Rotation<double,3> q2(Quaternion<double>(i,j,k,l));
                        q2.normalize();

                        // set up corresponding rotation matrix
                        FieldMatrix<double,3,3> q2Matrix;
                        q2.matrix(q2Matrix);

                        // q2 = q2 * q   Quaternion multiplication
                        q2 = q2.mult(q);

                        // q2 = q2 * q   Matrix multiplication
                        q2Matrix.rightmultiply(matrix);

                        FieldMatrix<double,3,3> productMatrix;
                        q2.matrix(productMatrix);

                        // Make sure we got identical results
                        productMatrix -= q2Matrix;
                        assert(productMatrix.infinity_norm() < 1e-10);

                    }

    // ////////////////////////////////////////////////////////////////
    //   Check the operators 'B' that create an orthonormal basis of H
    // ////////////////////////////////////////////////////////////////

    Quaternion<double> Bq[4];
    Bq[0] = q;
    Bq[1] = q.B(0);
    Bq[2] = q.B(1);
    Bq[3] = q.B(2);

    for (int i=0; i<4; i++) {

        for (int j=0; j<4; j++) {

            double prod = Bq[i]*Bq[j];
            assert( std::abs( prod - (i==j) ) < 1e-6 );

        }

    }

    //////////////////////////////////////////////////////////////////////
    //  Check whether the derivativeOfMatrixToQuaternion methods works
    //////////////////////////////////////////////////////////////////////

    Tensor3<double,4,3,3> derivative = Rotation<double,3>::derivativeOfMatrixToQuaternion(matrix);

    const double eps = 1e-8;
    Tensor3<double,4,3,3> derivativeFD;

    for (size_t i=0; i<3; i++)
    {
      for (size_t j=0; j<3; j++)
      {
        auto forwardMatrix = matrix;
        forwardMatrix[i][j] += eps;
        auto backwardMatrix = matrix;
        backwardMatrix[i][j] -= eps;

        Rotation<double,3> forwardRotation, backwardRotation;
        forwardRotation.set(forwardMatrix);
        backwardRotation.set(backwardMatrix);

        for (size_t k=0; k<4; k++)
          derivativeFD[k][i][j] = (forwardRotation.globalCoordinates()[k] - backwardRotation.globalCoordinates()[k]) / (2*eps);

      }
    }

    if ((derivative - derivativeFD).infinity_norm() > 1e-6)
    {
      std::cout << "At matrix:\n" << matrix << std::endl;

      std::cout << "Derivative of matrix to quaternion map does not match its FD approximation" << std::endl;
      std::cout << "Analytical derivative:" << std::endl;
      std::cout << derivative << std::endl;
      std::cout << "Finite difference approximation" << std::endl;
      std::cout << derivativeFD << std::endl;
      abort();
    }
}

//! test interpolation between two rotations
bool testInterpolation(const Rotation<double, 3>& a, const Rotation<double, 3>& b) {

    // Compute difference on T_a SO(3)
    Rotation<double, 3> newB = Rotation<double, 3>::interpolate(a, b, 1.0);

    // Compare matrix representation
    FieldMatrix<double, 3, 3> matB;
    b.matrix(matB);

    FieldMatrix<double, 3, 3> matNewB;
    newB.matrix(matNewB);

    matNewB -= matB;
    if (matNewB.infinity_norm() > 1e-14)
        std::cout << " Interpolation failed with difference " << matNewB.infinity_norm()  << std::endl;

    return (matNewB.infinity_norm() < 1e-14);
}

int main (int argc, char *argv[]) try
{
    std::vector<Rotation<double,3> > testPoints;
    ValueFactory<Rotation<double,3> >::get(testPoints);

    int nTestPoints = testPoints.size();

    // Test each element in the list
    for (int i=0; i<nTestPoints; i++)
        testRotation(testPoints[i]);

    bool passed(true);
    // Test interpolating between pairs of rotations
    for (int i=0; i<nTestPoints-1; i++)
        passed = passed and testInterpolation(testPoints[i], testPoints[i+1]);

    // //////////////////////////////////////////////
    //   Test second derivative of exp
    // //////////////////////////////////////////////
    testDDExp();

    return not passed;

 } catch (Exception& e) {

    std::cout << e.what() << std::endl;

 }