diff --git a/dune/gfe/rotation.hh b/dune/gfe/rotation.hh index d92bc4cc1e2c58ca3a5d19b681a2b706d3b38981..1d9bb430fc1dfdc7df2662a53c1453134059bdf8 100644 --- a/dune/gfe/rotation.hh +++ b/dune/gfe/rotation.hh @@ -606,7 +606,7 @@ public: // Make sure we do the right thing if a and b are not in the same sheet // of the double covering of the unit quaternions over SO(3) if (dist>=M_PI) { - dist -= M_PI; + dist = 2*M_PI - dist; diff *= -1; } diff --git a/test/rotationtest.cc b/test/rotationtest.cc index 432a6bfc6c965b88c44ea98adea18c722188a57a..a5f73cfdc368e25f58c3fdca3d16d5a8563aeeb4 100644 --- a/test/rotationtest.cc +++ b/test/rotationtest.cc @@ -347,6 +347,26 @@ void testRotation(Rotation<double,3> q) } } +//! 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; @@ -358,6 +378,11 @@ int main (int argc, char *argv[]) try 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 // ////////////////////////////////////////////// @@ -368,6 +393,8 @@ int main (int argc, char *argv[]) try // ////////////////////////////////////////////// testDerivativeOfInterpolatedPosition(); + return not passed; + } catch (Exception e) { std::cout << e << std::endl; diff --git a/test/valuefactory.hh b/test/valuefactory.hh index 63618eec2f355911bba1869c72f75e6fe359234e..cb3e53c3f619943f316306d7824f05a2abee5a30 100644 --- a/test/valuefactory.hh +++ b/test/valuefactory.hh @@ -158,8 +158,8 @@ public: std::vector<std::array<double,4> > testPoints = {{1,0,0,0}, {0,1,0,0}, {-0.838114,0.356751,-0.412667,0.5}, {-0.490946,-0.306456,0.81551,0.23},{-0.944506,0.123687,-0.304319,-0.7}, {-0.6,0.1,-0.2,0.8},{0.45,0.12,0.517,0}, - {-0.1,0.3,-0.1,0.73},{-0.444506,0.123687,0.104319,-0.23},{-0.7,-0.123687,-0.304319,0.72}}; - + {-0.1,0.3,-0.1,0.73},{-0.444506,0.123687,0.104319,-0.23},{-0.7,-0.123687,-0.304319,0.72}, + {-0.035669, -0.463824, -0.333265, 0.820079}, {0.0178678, 0.916836, 0.367358, 0.155374}}; values.resize(testPoints.size());