From b7debaba553b243de971fe0ed12904f649456bfe Mon Sep 17 00:00:00 2001
From: Oliver Sander <sander@igpm.rwth-aachen.de>
Date: Tue, 4 Mar 2008 17:20:39 +0000
Subject: [PATCH] add method interpolateDerivate with explicit intervalLength
argument
[[Imported from SVN: r2017]]
---
src/quaternion.hh | 29 +++++++++++++++++++++++++++++
1 file changed, 29 insertions(+)
diff --git a/src/quaternion.hh b/src/quaternion.hh
index c2e233c3..a09525b7 100644
--- a/src/quaternion.hh
+++ b/src/quaternion.hh
@@ -338,6 +338,8 @@ public:
/** \brief Interpolate between two rotations
\param omega must be between 0 and 1
+ \todo I'd say this method is incorrect and is other one is correct.
+ The solver works much better with this one, though. I don't get it.
*/
static Quaternion<T> interpolateDerivative(const Quaternion<T>& a, const Quaternion<T>& b,
double omega) {
@@ -361,6 +363,33 @@ public:
return a.mult(result);
}
+ /** \brief Interpolate between two rotations
+ \param omega must be between 0 and 1
+ */
+ static Quaternion<T> interpolateDerivative(const Quaternion<T>& a, const Quaternion<T>& b,
+ double omega, double intervalLength) {
+ Quaternion<T> result(0);
+
+ // Compute difference on T_a SO(3)
+ Dune::FieldVector<double,3> xi = difference(a,b);
+
+ xi /= intervalLength;
+
+ Dune::FieldVector<double,3> v = xi;
+ v *= omega;
+
+ // //////////////////////////////////////////////////////////////
+ // v now contains the derivative at 'a'. The derivative at
+ // the requested site is v pushed forward by Dexp.
+ // /////////////////////////////////////////////////////////////
+
+ Dune::FieldMatrix<double,4,3> diffExp = Quaternion<double>::Dexp(v);
+
+ diffExp.umv(xi,result);
+
+ return a.mult(result);
+ }
+
/** \brief Return the corresponding orthogonal matrix */
void matrix(Dune::FieldMatrix<T,3,3>& m) const {
--
GitLab