diff --git a/src/geodesicdifference.hh b/src/geodesicdifference.hh index eac08f727828611852bbeb50ca75ec981d182b3d..c3bbf8e0880d568a65847de4e5c37237c2efd61f 100644 --- a/src/geodesicdifference.hh +++ b/src/geodesicdifference.hh @@ -3,8 +3,8 @@ #include "rigidbodymotion.hh" -Dune::BlockVector<Dune::FieldVector<double,6> > computeRodDifference(const std::vector<RigidBodyMotion<3> >& a, - const std::vector<RigidBodyMotion<3> >& b) +Dune::BlockVector<Dune::FieldVector<double,6> > computeGeodesicDifference(const std::vector<RigidBodyMotion<3> >& a, + const std::vector<RigidBodyMotion<3> >& b) { if (a.size() != b.size()) DUNE_THROW(Dune::Exception, "a and b have to have the same length!"); @@ -29,4 +29,22 @@ Dune::BlockVector<Dune::FieldVector<double,6> > computeRodDifference(const std:: return result; } +Dune::BlockVector<Dune::FieldVector<double,3> > computeGeodesicDifference(const std::vector<Rotation<3,double> >& a, + const std::vector<Rotation<3,double> >& b) +{ + if (a.size() != b.size()) + DUNE_THROW(Dune::Exception, "a and b have to have the same length!"); + + Dune::BlockVector<Dune::FieldVector<double,3> > result(a.size()); + + for (size_t i=0; i<result.size(); i++) { + + // Subtract orientations on the tangent space of 'a' + result[i] = Rotation<3,double>::difference(a[i], b[i]); + + } + + return result; +} + #endif