diff --git a/src/quaternion.hh b/src/quaternion.hh index dec08c50e5c9f8e3df2aff58d5f8903837a43be6..211acbc688cfdcf3c9a95b31c62a1d32c8f84399 100644 --- a/src/quaternion.hh +++ b/src/quaternion.hh @@ -121,9 +121,20 @@ public: return result; } + /** \brief Invert the quaternion */ + void invert() { + + (*this)[0] *= -1; + (*this)[1] *= -1; + (*this)[2] *= -1; + + (*this) /= this->two_norm2(); + } + /** \brief Interpolate between two rotations */ static Quaternion<T> interpolate(const Quaternion<T>& a, const Quaternion<T>& b, double omega) { +#if 0 // Interpolation in H plus normalization Quaternion<T> result; for (int i=0; i<4; i++) @@ -131,6 +142,39 @@ public: result.normalize(); + return result; +#else // Interpolation on the geodesic in SO(3) from a to b + + Quaternion<T> diff = a; + diff.invert(); + diff.mult(b); + + T dist = 2*std::acos(diff[3]); + + T invSinc = (dist < 1e-4) ? 1/(0.5+(dist*dist/48)) : dist / std::sin(dist/2); + + // Compute difference on T_a SO(3) + Dune::FieldVector<double,3> v; + v[0] = diff[0] * invSinc; + v[1] = diff[1] * invSinc; + v[2] = diff[2] * invSinc; + + v *= omega; + + return a.mult(exp(v[0], v[1], v[2])); +#endif + + } + + /** \brief Interpolate between two rotations */ + static Quaternion<T> interpolateDerivative(const Quaternion<T>& a, const Quaternion<T>& b, + double omega, double intervallLength) { + + Quaternion<T> result; + + for (int i=0; i<4; i++) + result[i] = a[i] / (-intervallLength) + b[i] / intervallLength; + return result; } diff --git a/src/rodassembler.cc b/src/rodassembler.cc index 8f15337ef13e411d9069673dbb0079d8bb62c18c..1cd3c0830b5c4abb4147739168173f87824e33a0 100644 --- a/src/rodassembler.cc +++ b/src/rodassembler.cc @@ -303,9 +303,8 @@ getLocalMatrix( EntityPointer &entity, } // Get the derivative of the rotation at the quadrature point by interpolating in $H$ - Quaternion<double> hatq_s; - for (int i=0; i<4; i++) - hatq_s[i] = localSolution[0].q[i]*shapeGrad[0] + localSolution[1].q[i]*shapeGrad[1]; + Quaternion<double> hatq_s = Quaternion<double>::interpolateDerivative(localSolution[0].q, localSolution[1].q, + quadPos, 1/shapeGrad[1]); // The current strain FieldVector<double,blocksize> strain = getStrain(globalSolution, entity, quadPos); @@ -402,7 +401,6 @@ getLocalMatrix( EntityPointer &entity, // \partial W^2 \partial v^i_j \partial v^k_l // All other derivatives are zero - //double sum = duLocal_dvij[k][l][m] * (duCan_dvij[i][j] * hatq.director(m) + darbouxCan*dd_dvj[m][j]*shapeFunction[i]); double sum = du_dvij[k][l][m] * du_dvij[i][j][m]; sum += (strain[m+3] - referenceStrain[m+3]) * du_dvij_dvkl[i][j][k][l][m]; @@ -501,9 +499,8 @@ assembleGradient(const std::vector<Configuration>& sol, Quaternion<double> hatq = Quaternion<double>::interpolate(localSolution[0].q, localSolution[1].q,quadPos[0]); // Get the derivative of the rotation at the quadrature point by interpolating in $H$ - Quaternion<double> hatq_s; - for (int i=0; i<4; i++) - hatq_s[i] = localSolution[0].q[i]*shapeGrad[0] + localSolution[1].q[i]*shapeGrad[1]; + Quaternion<double> hatq_s = Quaternion<double>::interpolateDerivative(localSolution[0].q, localSolution[1].q, + quadPos, 1/shapeGrad[1]); // The current strain FieldVector<double,blocksize> strain = getStrain(sol, it, quadPos); @@ -811,11 +808,8 @@ Dune::FieldVector<double, 6> Dune::RodAssembler<GridType>::getStrain(const std:: Quaternion<double> q = Quaternion<double>::interpolate(localSolution[0].q, localSolution[1].q, pos); // Get the derivative of the rotation at the quadrature point by interpolating in $H$ - Quaternion<double> q_s; - q_s[0] = localSolution[0].q[0]*shapeGrad[0][0] + localSolution[1].q[0]*shapeGrad[1][0]; - q_s[1] = localSolution[0].q[1]*shapeGrad[0][0] + localSolution[1].q[1]*shapeGrad[1][0]; - q_s[2] = localSolution[0].q[2]*shapeGrad[0][0] + localSolution[1].q[2]*shapeGrad[1][0]; - q_s[3] = localSolution[0].q[3]*shapeGrad[0][0] + localSolution[1].q[3]*shapeGrad[1][0]; + Quaternion<double> q_s = Quaternion<double>::interpolateDerivative(localSolution[0].q, localSolution[1].q, + pos, 1/shapeGrad[1]); // ///////////////////////////////////////////// // Sum it all up