diff --git a/src/quaternion.hh b/src/quaternion.hh index f9a6ed49ee39064659c53111d1cee2b4368193ca..048b6e063cfc3e166348f81b9abd846be29bd1e6 100644 --- a/src/quaternion.hh +++ b/src/quaternion.hh @@ -133,14 +133,14 @@ public: for (int m=0; m<3; m++) { - result[m][i][j] = -0.25*v[i]*v[j]*v[m]*norm*norm*norm*std::sin(norm/2) - + 0.5 / (norm*norm) * ((i==j)*v[m] + (j==m)*v[i] + (i==m)*v[j] - 3*v[i]*v[j]*v[m]/(norm*norm)) - * (std::cos(norm/2) - sincHalf(norm)); + result[m][i][j] = -0.25*std::sin(norm/2)*v[i]*v[j]*v[m]/(norm*norm*norm) + + ((i==j)*v[m] + (j==m)*v[i] + (i==m)*v[j] - 3*v[i]*v[j]*v[m]/(norm*norm)) + * (0.5*std::cos(norm/2) - sincHalf(norm)) / (norm*norm); } - result[3][i][j] = -0.25/(norm*norm) + result[3][i][j] = -0.5/(norm*norm) * ( 0.5*std::cos(norm/2)*v[i]*v[j] + std::sin(norm/2) * ((i==j)*norm - v[i]*v[j]/norm)); } @@ -402,21 +402,21 @@ public: Quaternion<T> result(0); // Compute difference on T_a SO(3) - Dune::FieldVector<double,3> v = difference(a,b); + Dune::FieldVector<double,3> xi = difference(a,b); - Dune::FieldVector<double,3> der = v; - der /= intervallLength; + xi /= intervallLength; + + 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. // ///////////////////////////////////////////////////////////// - v *= omega; - Dune::FieldMatrix<double,4,3> diffExp = Quaternion<double>::Dexp(v); - diffExp.umv(der,result); + diffExp.umv(xi,result); return a.mult(result); } diff --git a/src/rodassembler.cc b/src/rodassembler.cc index 2dc8e421a33113f5e8e7567a8f7176f6109e76cc..90af7f7abdab32aba0ec98421c4e44dc19ded1e4 100644 --- a/src/rodassembler.cc +++ b/src/rodassembler.cc @@ -200,9 +200,13 @@ interpolationVelocityDerivative(const Quaternion<RT>& q0, const Quaternion<RT>& addend0 /= intervalLength; // \parder{\xi}{w^1_j} = ... - /** \todo Dieser Teil kommt unten nochmal! */ + Quaternion<RT> dwConj = dw; + dwConj.conjugate(); + dwConj[3] -= 2 * dExp_v_0[3][i]; + dwConj = dwConj.mult(q0Inv.mult(q1)); + Dune::FieldVector<RT,3> dxi(0); - Quaternion<RT>::DexpInv(q0Inv.mult(q1)).umv(q0Inv.mult(q1.mult(dw)), dxi); + Quaternion<RT>::DexpInv(q0Inv.mult(q1)).umv(dwConj, dxi); Quaternion<RT> vHv; for (int j=0; j<4; j++) { @@ -216,13 +220,7 @@ interpolationVelocityDerivative(const Quaternion<RT>& q0, const Quaternion<RT>& vHv *= s/intervalLength/intervalLength; // Third addend - dw.conjugate(); - - dw[3] -= 2 * dExp_v_0[3][i]; - - dw = dw.mult(q0Inv.mult(q1)); - - mat.umv(dw,grad[i]); + mat.umv(dwConj,grad[i]); // add up grad[i] += addend0; @@ -247,7 +245,7 @@ interpolationVelocityDerivative(const Quaternion<RT>& q0, const Quaternion<RT>& // \parder{\xi}{w^1_j} = ... Dune::FieldVector<RT,3> dxi(0); - Quaternion<RT>::DexpInv(q0Inv.mult(q1)).umv(q0Inv.mult(q1.mult(dw)), dxi); + dExpInv.umv(q0Inv.mult(q1.mult(dw)), dxi); Quaternion<RT> vHv; for (int j=0; j<4; j++) {