implement interpolation of the gradient for members of SO(3), some of this is...

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

@@ -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]);
+
+    }
+
 }