diff --git a/test/rotationtest.cc b/test/rotationtest.cc index 5208d3487fe0bcae473f5a0c50320e84d8197a4d..78a6b210193a5bd91042b084e64c24b52d7a392f 100644 --- a/test/rotationtest.cc +++ b/test/rotationtest.cc @@ -344,6 +344,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; @@ -355,6 +375,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 // ////////////////////////////////////////////// @@ -365,6 +390,8 @@ int main (int argc, char *argv[]) try // ////////////////////////////////////////////// testDerivativeOfInterpolatedPosition(); + return not passed; + } catch (Exception e) { std::cout << e << std::endl;