From d340e8ebb47ea260bfb0a71a6f0befd79bef1acd Mon Sep 17 00:00:00 2001
From: Oliver Sander <sander@igpm.rwth-aachen.de>
Date: Mon, 20 Apr 2009 12:32:47 +0000
Subject: [PATCH] implement interpolation of the gradient for members of SO(3),
some of this is still educated guesswork, though
[[Imported from SVN: r4060]]
---
src/localgeodesicfefunction.hh | 167 ++++++++++++++++++++++++++++++++-
1 file changed, 165 insertions(+), 2 deletions(-)
diff --git a/src/localgeodesicfefunction.hh b/src/localgeodesicfefunction.hh
index 5ae292a1..55e6baa4 100644
--- a/src/localgeodesicfefunction.hh
+++ b/src/localgeodesicfefunction.hh
@@ -39,13 +39,21 @@ public:
private:
/** \brief Compute the derivate of geodesic interpolation wrt to the initial point
- and return it as an _embedded_ tangent vector
+ and return it as a linear map on quaternion space
\todo This is group-specific and should not really be here
*/
static Dune::FieldMatrix<ctype, EmbeddedTangentVector::size, EmbeddedTangentVector::size>
derivativeWRTFirstPoint(const Rotation<3,ctype>& a, const Rotation<3,ctype>& b, double s);
+ /** \brief Compute the derivate of geodesic interpolation wrt to the initial point
+
+ \todo This is group-specific and should not really be here
+ */
+ static void interpolationVelocityDerivative(const Rotation<3,ctype>& q0, const Rotation<3,ctype>& q1, double s,
+ double intervalLength, Dune::array<Quaternion<double>,6>& grad);
+
+
/** \brief The type of the reference element */
Dune::GeometryType type_;
@@ -59,7 +67,162 @@ Dune::FieldMatrix<ctype, TargetSpace::EmbeddedTangentVector::size, TargetSpace::
LocalGeodesicFEFunction<dim,ctype,TargetSpace>::
derivativeWRTFirstPoint(const Rotation<3,ctype>& a, const Rotation<3,ctype>& b, double s)
{
- DUNE_THROW(Dune::NotImplemented, "derivativeWRTFirstPoint");
+ // Get the derivative of [a,b](s) wrt 'a'
+ /** \brief The method actually compute the derivatives wrt to 'a' and 'b'. This is a waste! */
+ Dune::array<Quaternion<double>,6> grad;
+ interpolationVelocityDerivative(a,b,s,1,grad);
+
+ // We are really only interested in the first three entries
+
+ Quaternion<ctype> aInv = a;
+ aInv.invert();
+
+ Dune::array<Quaternion<double>,3> derAlpha;
+ derAlpha[0] = aInv.mult(grad[0]);
+ derAlpha[1] = aInv.mult(grad[1]);
+ derAlpha[2] = aInv.mult(grad[2]);
+
+ // Copy the thing into a matrix
+ Dune::FieldMatrix<ctype,3,4> derAlphaMatrix;
+ for (int i=0; i<3; i++)
+ derAlphaMatrix[i] = derAlpha[i];
+
+ // Get derivative of the exponential map at the identity. Incidentally, the implementation
+ // maps skew-symmetric matrices (== three-vectors) to quaternions
+
+ Dune::FieldMatrix<ctype,4,3> Dexp = Rotation<3,ctype>::Dexp(Dune::FieldVector<ctype,3>(0));
+
+ Dune::FieldMatrix<ctype, TargetSpace::EmbeddedTangentVector::size, TargetSpace::EmbeddedTangentVector::size> result;
+
+ Dune::FMatrixHelp::multMatrix(Dexp, derAlphaMatrix, result);
+
+ return result;
+}
+
+template <int dim, class ctype, class TargetSpace>
+void LocalGeodesicFEFunction<dim,ctype,TargetSpace>::
+interpolationVelocityDerivative(const Rotation<3,ctype>& q0, const Rotation<3,ctype>& q1, double s,
+ double intervalLength, Dune::array<Quaternion<double>,6>& grad)
+{
+ // Clear output array
+ for (int i=0; i<6; i++)
+ grad[i] = 0;
+
+ // Compute q_0^{-1}
+ Rotation<3,ctype> q0Inv = q0;
+ q0Inv.invert();
+
+
+ // Compute v = s \exp^{-1} ( q_0^{-1} q_1)
+ Dune::FieldVector<ctype,3> v = Rotation<3,ctype>::expInv(q0Inv.mult(q1));
+ v *= s/intervalLength;
+
+ Dune::FieldMatrix<ctype,4,3> dExp_v = Rotation<3,ctype>::Dexp(v);
+
+ Dune::array<Dune::FieldMatrix<ctype,3,3>, 4> ddExp;
+ Rotation<3,ctype>::DDexp(v, ddExp);
+
+ Dune::FieldMatrix<ctype,3,4> dExpInv = Rotation<3,ctype>::DexpInv(q0Inv.mult(q1));
+
+ Dune::FieldMatrix<ctype,4,4> mat(0);
+ for (int i=0; i<4; i++)
+ for (int j=0; j<4; j++)
+ for (int k=0; k<3; k++)
+ mat[i][j] += 1/intervalLength * dExp_v[i][k] * dExpInv[k][j];
+
+
+ // /////////////////////////////////////////////////
+ // The derivatives with respect to w^0
+ // /////////////////////////////////////////////////
+ for (int i=0; i<3; i++) {
+
+ // \partial exp \partial w^1_j at 0
+ Quaternion<ctype> dw;
+ for (int j=0; j<4; j++)
+ dw[j] = 0.5*(i==j); // dExp_v_0[j][i];
+
+ // \xi = \exp^{-1} q_0^{-1} q_1
+ Dune::FieldVector<ctype,3> xi = Rotation<3,ctype>::expInv(q0Inv.mult(q1));
+
+ Quaternion<ctype> addend0;
+ addend0 = 0;
+ dExp_v.umv(xi,addend0);
+ addend0 = dw.mult(addend0);
+ addend0 /= intervalLength;
+
+ // \parder{\xi}{w^1_j} = ...
+ Quaternion<ctype> dwConj = dw;
+ dwConj.conjugate();
+ //dwConj[3] -= 2 * dExp_v_0[3][i]; the last row of dExp_v_0 is zero
+ dwConj = dwConj.mult(q0Inv.mult(q1));
+
+ Dune::FieldVector<ctype,3> dxi(0);
+ Rotation<3,ctype>::DexpInv(q0Inv.mult(q1)).umv(dwConj, dxi);
+
+ Quaternion<ctype> vHv;
+ for (int j=0; j<4; j++) {
+ vHv[j] = 0;
+ // vHv[j] = dxi * DDexp * xi
+ for (int k=0; k<3; k++)
+ for (int l=0; l<3; l++)
+ vHv[j] += ddExp[j][k][l]*dxi[k]*xi[l];
+ }
+
+ vHv *= s/intervalLength/intervalLength;
+
+ // Third addend
+ mat.umv(dwConj,grad[i]);
+
+ // add up
+ grad[i] += addend0;
+ grad[i] += vHv;
+
+ grad[i] = q0.mult(grad[i]);
+ }
+
+
+ // /////////////////////////////////////////////////
+ // The derivatives with respect to w^1
+ // /////////////////////////////////////////////////
+ for (int i=3; i<6; i++) {
+
+ // \partial exp \partial w^1_j at 0
+ Quaternion<ctype> dw;
+ for (int j=0; j<4; j++)
+ dw[j] = 0.5 * ((i-3)==j); // dw[j] = dExp_v_0[j][i-3];
+
+ // \xi = \exp^{-1} q_0^{-1} q_1
+ Dune::FieldVector<ctype,3> xi = Rotation<3,ctype>::expInv(q0Inv.mult(q1));
+
+ // \parder{\xi}{w^1_j} = ...
+ Dune::FieldVector<ctype,3> dxi(0);
+ dExpInv.umv(q0Inv.mult(q1.mult(dw)), dxi);
+
+ Quaternion<ctype> vHv;
+ for (int j=0; j<4; j++) {
+ // vHv[j] = dxi * DDexp * xi
+ vHv[j] = 0;
+ for (int k=0; k<3; k++)
+ for (int l=0; l<3; l++)
+ vHv[j] += ddExp[j][k][l]*dxi[k]*xi[l];
+ }
+
+ vHv *= s/intervalLength/intervalLength;
+
+ // ///////////////////////////////////
+ // second addend
+ // ///////////////////////////////////
+
+
+ dw = q0Inv.mult(q1.mult(dw));
+
+ mat.umv(dw,grad[i]);
+ grad[i] += vHv;
+
+ grad[i] = q0.mult(grad[i]);
+
+ }
+
}
--
GitLab