diff --git a/src/quaternion.hh b/src/quaternion.hh
index a09525b711409b88e849d1beccbdac2594832034..1a2294fd625c585190264c15e15f32e6513fc0d4 100644
--- a/src/quaternion.hh
+++ b/src/quaternion.hh
@@ -9,12 +9,6 @@
template <class T>
class Quaternion : public Dune::FieldVector<T,4>
{
-
- /** \brief Computes sin(x/2) / x without getting unstable for small x */
- static T sincHalf(const T& x) {
- return (x < 1e-4) ? 0.5 + (x*x/48) : std::sin(x/2)/x;
- }
-
public:
/** \brief Default constructor */
@@ -63,148 +57,6 @@ public:
return id;
}
- /** \brief The exponential map from \f$ \mathfrak{so}(3) \f$ to \f$ SO(3) \f$
- */
- static Quaternion<T> exp(const Dune::FieldVector<T,3>& v) {
- return exp(v[0], v[1], v[2]);
- }
-
- /** \brief The exponential map from \f$ \mathfrak{so}(3) \f$ to \f$ SO(3) \f$
- */
- static Quaternion<T> exp(const T& v0, const T& v1, const T& v2) {
- Quaternion<T> q;
-
- T normV = std::sqrt(v0*v0 + v1*v1 + v2*v2);
-
- // Stabilization for small |v| due to Grassia
- T sin = sincHalf(normV);
-
- // if normV == 0 then q = (0,0,0,1)
- assert(!isnan(sin));
-
- q[0] = sin * v0;
- q[1] = sin * v1;
- q[2] = sin * v2;
- q[3] = std::cos(normV/2);
-
- return q;
- }
-
- static Dune::FieldMatrix<T,4,3> Dexp(const Dune::FieldVector<T,3>& v) {
-
- Dune::FieldMatrix<T,4,3> result(0);
- T norm = v.two_norm();
-
- for (int i=0; i<3; i++) {
-
- for (int m=0; m<3; m++) {
-
- result[m][i] = (norm==0)
- /** \todo Isn't there a better way to implement this stably? */
- ? 0.5 * (i==m)
- : 0.5 * std::cos(norm/2) * v[i] * v[m] / (norm*norm) + sincHalf(norm) * ( (i==m) - v[i]*v[m]/(norm*norm));
-
- }
-
- result[3][i] = - 0.5 * sincHalf(norm) * v[i];
-
- }
- return result;
- }
-
- static void DDexp(const Dune::FieldVector<T,3>& v,
- Dune::array<Dune::FieldMatrix<T,3,3>, 4>& result) {
-
- T norm = v.two_norm();
- if (norm==0) {
-
- for (int m=0; m<4; m++)
- result[m] = 0;
-
- for (int i=0; i<3; i++)
- result[3][i][i] = -0.25;
-
-
- } else {
-
- for (int i=0; i<3; i++) {
-
- for (int j=0; j<3; j++) {
-
- for (int m=0; m<3; m++) {
-
- 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.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));
-
- }
-
- }
-
- }
-
- }
-
- /** \brief The inverse of the exponential map */
- static Dune::FieldVector<T,3> expInv(const Quaternion<T>& q) {
- // Compute v = exp^{-1} q
- // Due to numerical dirt, q[3] may be larger than 1.
- // In that case, use 1 instead of q[3].
- Dune::FieldVector<T,3> v;
- if (q[3] > 1.0) {
-
- v = 0;
-
- } else {
-
- T invSinc = 1/sincHalf(2*std::acos(q[3]));
-
- v[0] = q[0] * invSinc;
- v[1] = q[1] * invSinc;
- v[2] = q[2] * invSinc;
-
- }
- return v;
- }
-
- /** \brief The derivative of the inverse of the exponential map, evaluated at q */
- static Dune::FieldMatrix<T,3,4> DexpInv(const Quaternion<T>& q) {
-
- // Compute v = exp^{-1} q
- Dune::FieldVector<T,3> v = expInv(q);
-
- // The derivative of exp at v
- Dune::FieldMatrix<T,4,3> A = Dexp(v);
-
- // Compute the Moore-Penrose pseudo inverse A^+ = (A^T A)^{-1} A^T
- Dune::FieldMatrix<T,3,3> ATA;
-
- for (int i=0; i<3; i++)
- for (int j=0; j<3; j++) {
- ATA[i][j] = 0;
- for (int k=0; k<4; k++)
- ATA[i][j] += A[k][i] * A[k][j];
- }
-
- ATA.invert();
-
- Dune::FieldMatrix<T,3,4> APseudoInv;
- for (int i=0; i<3; i++)
- for (int j=0; j<4; j++) {
- APseudoInv[i][j] = 0;
- for (int k=0; k<3; k++)
- APseudoInv[i][j] += ATA[i][k] * A[j][k];
- }
-
- return APseudoInv;
- }
-
/** \brief Right quaternion multiplication */
Quaternion<T> mult(const Quaternion<T>& other) const {
Quaternion<T> q;
@@ -295,191 +147,6 @@ public:
}
- static Dune::FieldVector<T,3> difference(const Quaternion<T>& a, const Quaternion<T>& b) {
-
- Quaternion<T> diff = a;
- diff.invert();
- diff = diff.mult(b);
-
- // Compute the geodesical distance between a and b on SO(3)
- // Due to numerical dirt, diff[3] may be larger than 1.
- // In that case, use 1 instead of diff[3].
- Dune::FieldVector<T,3> v;
- if (diff[3] > 1.0) {
-
- v = 0;
-
- } else {
-
- T dist = 2*std::acos( std::min(diff[3],1.0) );
-
- T invSinc = 1/sincHalf(dist);
-
- // Compute difference on T_a SO(3)
- v[0] = diff[0] * invSinc;
- v[1] = diff[1] * invSinc;
- v[2] = diff[2] * invSinc;
-
- }
-
- return v;
- }
-
- /** \brief Interpolate between two rotations */
- static Quaternion<T> interpolate(const Quaternion<T>& a, const Quaternion<T>& b, double omega) {
-
- // Compute difference on T_a SO(3)
- Dune::FieldVector<T,3> v = difference(a,b);
-
- v *= omega;
-
- return a.mult(exp(v[0], v[1], v[2]));
- }
-
- /** \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) {
- Quaternion<T> result(0);
-
- // Compute difference on T_a SO(3)
- Dune::FieldVector<double,3> xi = difference(a,b);
-
- 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 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 {
-
- m[0][0] = (*this)[0]*(*this)[0] - (*this)[1]*(*this)[1] - (*this)[2]*(*this)[2] + (*this)[3]*(*this)[3];
- m[0][1] = 2 * ( (*this)[0]*(*this)[1] - (*this)[2]*(*this)[3] );
- m[0][2] = 2 * ( (*this)[0]*(*this)[2] + (*this)[1]*(*this)[3] );
-
- m[1][0] = 2 * ( (*this)[0]*(*this)[1] + (*this)[2]*(*this)[3] );
- m[1][1] = - (*this)[0]*(*this)[0] + (*this)[1]*(*this)[1] - (*this)[2]*(*this)[2] + (*this)[3]*(*this)[3];
- m[1][2] = 2 * ( -(*this)[0]*(*this)[3] + (*this)[1]*(*this)[2] );
-
- m[2][0] = 2 * ( (*this)[0]*(*this)[2] - (*this)[1]*(*this)[3] );
- m[2][1] = 2 * ( (*this)[0]*(*this)[3] + (*this)[1]*(*this)[2] );
- m[2][2] = - (*this)[0]*(*this)[0] - (*this)[1]*(*this)[1] + (*this)[2]*(*this)[2] + (*this)[3]*(*this)[3];
-
- }
-
- /** \brief Set unit quaternion from orthogonal matrix
-
- We tacitly assume that the matrix really is orthogonal */
- void set(const Dune::FieldMatrix<T,3,3>& m) {
-
- // Easier writing
- Dune::FieldVector<T,4>& p = (*this);
- // The following equations for the derivation of a unit quaternion from a rotation
- // matrix comes from 'E. Salamin, Application of Quaternions to Computation with
- // Rotations, Technical Report, Stanford, 1974'
-
- p[0] = (1 + m[0][0] - m[1][1] - m[2][2]) / 4;
- p[1] = (1 - m[0][0] + m[1][1] - m[2][2]) / 4;
- p[2] = (1 - m[0][0] - m[1][1] + m[2][2]) / 4;
- p[3] = (1 + m[0][0] + m[1][1] + m[2][2]) / 4;
-
- // avoid rounding problems
- if (p[0] >= p[1] && p[0] >= p[2] && p[0] >= p[3]) {
-
- p[0] = std::sqrt(p[0]);
-
- // r_x r_y = (R_12 + R_21) / 4
- p[1] = (m[0][1] + m[1][0]) / 4 / p[0];
-
- // r_x r_z = (R_13 + R_31) / 4
- p[2] = (m[0][2] + m[2][0]) / 4 / p[0];
-
- // r_0 r_x = (R_32 - R_23) / 4
- p[3] = (m[2][1] - m[1][2]) / 4 / p[0];
-
- } else if (p[1] >= p[0] && p[1] >= p[2] && p[1] >= p[3]) {
-
- p[1] = std::sqrt(p[1]);
-
- // r_x r_y = (R_12 + R_21) / 4
- p[0] = (m[0][1] + m[1][0]) / 4 / p[1];
-
- // r_y r_z = (R_23 + R_32) / 4
- p[2] = (m[1][2] + m[2][1]) / 4 / p[1];
-
- // r_0 r_y = (R_13 - R_31) / 4
- p[3] = (m[0][2] - m[2][0]) / 4 / p[1];
-
- } else if (p[2] >= p[0] && p[2] >= p[1] && p[2] >= p[3]) {
-
- p[2] = std::sqrt(p[2]);
-
- // r_x r_z = (R_13 + R_31) / 4
- p[0] = (m[0][2] + m[2][0]) / 4 / p[2];
-
- // r_y r_z = (R_23 + R_32) / 4
- p[1] = (m[1][2] + m[2][1]) / 4 / p[2];
-
- // r_0 r_z = (R_21 - R_12) / 4
- p[3] = (m[1][0] - m[0][1]) / 4 / p[2];
-
- } else {
-
- p[3] = std::sqrt(p[3]);
-
- // r_0 r_x = (R_32 - R_23) / 4
- p[0] = (m[2][1] - m[1][2]) / 4 / p[3];
-
- // r_0 r_y = (R_13 - R_31) / 4
- p[1] = (m[0][2] - m[2][0]) / 4 / p[3];
-
- // r_0 r_z = (R_21 - R_12) / 4
- p[2] = (m[1][0] - m[0][1]) / 4 / p[3];
-
- }
-
- }
-
/** \brief Create three vectors which form an orthonormal basis of \mathbb{H} together
with this one.
@@ -509,6 +176,8 @@ public:
return r;
}
+
+
};
#endif
diff --git a/src/rodassembler.cc b/src/rodassembler.cc
index 491141c2f441d60ead7f409443a0bbebfddabc34..cfd9d5fda026db13c83bd710bf92fd44392d6e76 100644
--- a/src/rodassembler.cc
+++ b/src/rodassembler.cc
@@ -73,7 +73,7 @@ energy(const Entity& element,
template <class GridType, class RT>
void RodLocalStiffness<GridType, RT>::
-interpolationDerivative(const Quaternion<RT>& q0, const Quaternion<RT>& q1, double s,
+interpolationDerivative(const Rotation<3,RT>& q0, const Rotation<3,RT>& q1, double s,
Dune::array<Quaternion<double>,6>& grad)
{
// Clear output array
@@ -83,18 +83,18 @@ interpolationDerivative(const Quaternion<RT>& q0, const Quaternion<RT>& q1, doub
// The derivatives with respect to w^1
// Compute q_1^{-1}q_0
- Quaternion<RT> q1InvQ0 = q1;
+ Rotation<3,RT> q1InvQ0 = q1;
q1InvQ0.invert();
q1InvQ0 = q1InvQ0.mult(q0);
{
// Compute v = (1-s) \exp^{-1} ( q_1^{-1} q_0)
- Dune::FieldVector<RT,3> v = Quaternion<RT>::expInv(q1InvQ0);
+ Dune::FieldVector<RT,3> v = Rotation<3,RT>::expInv(q1InvQ0);
v *= (1-s);
- Dune::FieldMatrix<RT,4,3> dExp_v = Quaternion<RT>::Dexp(v);
+ Dune::FieldMatrix<RT,4,3> dExp_v = Rotation<3,RT>::Dexp(v);
- Dune::FieldMatrix<RT,3,4> dExpInv = Quaternion<RT>::DexpInv(q1InvQ0);
+ Dune::FieldMatrix<RT,3,4> dExpInv = Rotation<3,RT>::DexpInv(q1InvQ0);
Dune::FieldMatrix<RT,4,4> mat(0);
for (int i=0; i<4; i++)
@@ -120,18 +120,18 @@ interpolationDerivative(const Quaternion<RT>& q0, const Quaternion<RT>& q1, doub
// The derivatives with respect to w^1
// Compute q_0^{-1}
- Quaternion<RT> q0InvQ1 = q0;
+ Rotation<3,RT> q0InvQ1 = q0;
q0InvQ1.invert();
q0InvQ1 = q0InvQ1.mult(q1);
{
// Compute v = s \exp^{-1} ( q_0^{-1} q_1)
- Dune::FieldVector<RT,3> v = Quaternion<RT>::expInv(q0InvQ1);
+ Dune::FieldVector<RT,3> v = Rotation<3,RT>::expInv(q0InvQ1);
v *= s;
- Dune::FieldMatrix<RT,4,3> dExp_v = Quaternion<RT>::Dexp(v);
+ Dune::FieldMatrix<RT,4,3> dExp_v = Rotation<3,RT>::Dexp(v);
- Dune::FieldMatrix<RT,3,4> dExpInv = Quaternion<RT>::DexpInv(q0InvQ1);
+ Dune::FieldMatrix<RT,3,4> dExpInv = Rotation<3,RT>::DexpInv(q0InvQ1);
Dune::FieldMatrix<RT,4,4> mat(0);
for (int i=0; i<4; i++)
@@ -158,7 +158,7 @@ interpolationDerivative(const Quaternion<RT>& q0, const Quaternion<RT>& q1, doub
template <class GridType, class RT>
void RodLocalStiffness<GridType, RT>::
-interpolationVelocityDerivative(const Quaternion<RT>& q0, const Quaternion<RT>& q1, double s,
+interpolationVelocityDerivative(const Rotation<3,RT>& q0, const Rotation<3,RT>& q1, double s,
double intervalLength, Dune::array<Quaternion<double>,6>& grad)
{
// Clear output array
@@ -166,20 +166,20 @@ interpolationVelocityDerivative(const Quaternion<RT>& q0, const Quaternion<RT>&
grad[i] = 0;
// Compute q_0^{-1}
- Quaternion<RT> q0Inv = q0;
+ Rotation<3,RT> q0Inv = q0;
q0Inv.invert();
// Compute v = s \exp^{-1} ( q_0^{-1} q_1)
- Dune::FieldVector<RT,3> v = Quaternion<RT>::expInv(q0Inv.mult(q1));
+ Dune::FieldVector<RT,3> v = Rotation<3,RT>::expInv(q0Inv.mult(q1));
v *= s/intervalLength;
- Dune::FieldMatrix<RT,4,3> dExp_v = Quaternion<RT>::Dexp(v);
+ Dune::FieldMatrix<RT,4,3> dExp_v = Rotation<3,RT>::Dexp(v);
Dune::array<Dune::FieldMatrix<RT,3,3>, 4> ddExp;
- Quaternion<RT>::DDexp(v, ddExp);
+ Rotation<3,RT>::DDexp(v, ddExp);
- Dune::FieldMatrix<RT,3,4> dExpInv = Quaternion<RT>::DexpInv(q0Inv.mult(q1));
+ Dune::FieldMatrix<RT,3,4> dExpInv = Rotation<3,RT>::DexpInv(q0Inv.mult(q1));
Dune::FieldMatrix<RT,4,4> mat(0);
for (int i=0; i<4; i++)
@@ -199,7 +199,7 @@ interpolationVelocityDerivative(const Quaternion<RT>& q0, const Quaternion<RT>&
dw[j] = 0.5*(i==j); // dExp_v_0[j][i];
// \xi = \exp^{-1} q_0^{-1} q_1
- Dune::FieldVector<RT,3> xi = Quaternion<RT>::expInv(q0Inv.mult(q1));
+ Dune::FieldVector<RT,3> xi = Rotation<3,RT>::expInv(q0Inv.mult(q1));
Quaternion<RT> addend0;
addend0 = 0;
@@ -214,7 +214,7 @@ interpolationVelocityDerivative(const Quaternion<RT>& q0, const Quaternion<RT>&
dwConj = dwConj.mult(q0Inv.mult(q1));
Dune::FieldVector<RT,3> dxi(0);
- Quaternion<RT>::DexpInv(q0Inv.mult(q1)).umv(dwConj, dxi);
+ Rotation<3,RT>::DexpInv(q0Inv.mult(q1)).umv(dwConj, dxi);
Quaternion<RT> vHv;
for (int j=0; j<4; j++) {
@@ -249,7 +249,7 @@ interpolationVelocityDerivative(const Quaternion<RT>& q0, const Quaternion<RT>&
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<RT,3> xi = Quaternion<RT>::expInv(q0Inv.mult(q1));
+ Dune::FieldVector<RT,3> xi = Rotation<3,RT>::expInv(q0Inv.mult(q1));
// \parder{\xi}{w^1_j} = ...
Dune::FieldVector<RT,3> dxi(0);
@@ -329,10 +329,10 @@ getStrain(const Dune::array<Configuration,2>& localSolution,
r_s[i] = localSolution[0].r[i]*shapeGrad[0][0] + localSolution[1].r[i]*shapeGrad[1][0];
// Interpolate the rotation at the quadrature point
- Quaternion<double> q = Quaternion<double>::interpolate(localSolution[0].q, localSolution[1].q, pos);
+ Rotation<3,double> q = Rotation<3,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 = Quaternion<double>::interpolateDerivative(localSolution[0].q, localSolution[1].q,
+ Quaternion<double> q_s = Rotation<3,double>::interpolateDerivative(localSolution[0].q, localSolution[1].q,
pos);
// Transformation from the reference element
q_s *= inv[0][0];
@@ -421,7 +421,7 @@ assembleGradient(const Entity& element,
r_s[i] = solution[0].r[i]*shapeGrad[0] + solution[1].r[i]*shapeGrad[1];
// Interpolate current rotation at this quadrature point
- Quaternion<double> q = Quaternion<double>::interpolate(solution[0].q, solution[1].q,quadPos[0]);
+ Rotation<3,double> q = Rotation<3,double>::interpolate(solution[0].q, solution[1].q,quadPos[0]);
// The current strain
FieldVector<double,blocksize> strain = getStrain(solution, element, quadPos);
@@ -490,10 +490,10 @@ assembleGradient(const Entity& element,
double weight = bendingQuad[pt].weight() * integrationElement;
// Interpolate current rotation at this quadrature point
- Quaternion<double> q = Quaternion<double>::interpolate(solution[0].q, solution[1].q,quadPos[0]);
+ Rotation<3,double> q = Rotation<3,double>::interpolate(solution[0].q, solution[1].q,quadPos[0]);
// Get the derivative of the rotation at the quadrature point by interpolating in $H$
- Quaternion<double> q_s = Quaternion<double>::interpolateDerivative(solution[0].q, solution[1].q,
+ Quaternion<double> q_s = Rotation<3,double>::interpolateDerivative(solution[0].q, solution[1].q,
quadPos);
// Transformation from the reference element
q_s *= inv[0][0];
@@ -529,6 +529,8 @@ assembleGradient(const Entity& element,
for (int m=0; m<3; m++) {
// Compute derivative of the strain
+ /** \todo Is this formula correct? It seems strange to call
+ B(m) for a _derivative_ of a rotation */
double du_dvij_m = 2 * (dq_dwij[i*3+j].B(m) * q_s)
+ 2* ( q.B(m) * dq_ds_dwij[i*3+j]);
diff --git a/src/rodassembler.hh b/src/rodassembler.hh
index 403000616b62c5fca72a3f17c31f4f044ccdb7b8..04f47bb56cade4ee96d54a758ec11ecdb7b92734 100644
--- a/src/rodassembler.hh
+++ b/src/rodassembler.hh
@@ -98,10 +98,10 @@ public:
const Dune::array<Configuration,2>& localReferenceConfiguration,
int k=1);
- static void interpolationDerivative(const Quaternion<RT>& q0, const Quaternion<RT>& q1, double s,
+ static void interpolationDerivative(const Rotation<3,RT>& q0, const Rotation<3,RT>& q1, double s,
Dune::array<Quaternion<double>,6>& grad);
- static void interpolationVelocityDerivative(const Quaternion<RT>& q0, const Quaternion<RT>& q1, double s,
+ static void interpolationVelocityDerivative(const Rotation<3,RT>& q0, const Rotation<3,RT>& q1, double s,
double intervalLength, Dune::array<Quaternion<double>,6>& grad);
Dune::FieldVector<double, 6> getStrain(const Dune::array<Configuration,2>& localSolution,
@@ -115,7 +115,7 @@ public:
Dune::array<Dune::FieldVector<double,6>, 2>& gradient) const;
template <class T>
- static Dune::FieldVector<T,3> darboux(const Quaternion<T>& q, const Dune::FieldVector<T,4>& q_s)
+ static Dune::FieldVector<T,3> darboux(const Rotation<3,T>& q, const Dune::FieldVector<T,4>& q_s)
{
Dune::FieldVector<double,3> u; // The Darboux vector
diff --git a/src/roddifference.hh b/src/roddifference.hh
index ab32cb6facdf63c39c5de2804df64e4108e8f4c6..5cd16ebdb3f0864d71cbbed5883c2001d3e0e358 100644
--- a/src/roddifference.hh
+++ b/src/roddifference.hh
@@ -16,7 +16,7 @@ Dune::BlockVector<Dune::FieldVector<double,6> > computeRodDifference(const std::
result[i][j] = a[i].r[j] - b[i].r[j];
// Subtract orientations on the tangent space of 'a'
- Dune::FieldVector<double,3> v = Quaternion<double>::difference(a[i].q, b[i].q);
+ Dune::FieldVector<double,3> v = Rotation<3,double>::difference(a[i].q, b[i].q);
// Compute difference on T_a SO(3)
for (int j=0; j<3; j++)
diff --git a/src/rodrefine.hh b/src/rodrefine.hh
index fd78d913200e26bbeb01d23191477027c2056957..fa8b8fa04cc4bbcbdb304a2f5b468f07e686c6f8 100644
--- a/src/rodrefine.hh
+++ b/src/rodrefine.hh
@@ -61,7 +61,7 @@ void globalRodRefine(GridType& grid, std::vector<Configuration>& x)
x[indexSet.template subIndex<1>(*eIt,i)].r = (p0.r + p1.r);
x[indexSet.template subIndex<1>(*eIt,i)].r *= 0.5;
x[indexSet.template subIndex<1>(*eIt,i)].q
- = Quaternion<double>::interpolate(p0.q, p1.q, 0.5);
+ = Rotation<3,double>::interpolate(p0.q, p1.q, 0.5);
}
diff --git a/src/rotation.hh b/src/rotation.hh
index b9293cd9902f8574a1638ba8f86fe97b57ba8baf..c632da95d2f4439a3a6e25562a120b62afa91333 100644
--- a/src/rotation.hh
+++ b/src/rotation.hh
@@ -72,6 +72,24 @@ public:
return q;
}
+ /** \brief Right multiplication with a quaternion
+ \todo do we really need this?*/
+ Rotation<3,T> mult(const Quaternion<T>& other) const {
+ Rotation<3,T> q;
+ q[0] = (*this)[3]*other[0] - (*this)[2]*other[1] + (*this)[1]*other[2] + (*this)[0]*other[3];
+ q[1] = (*this)[2]*other[0] + (*this)[3]*other[1] - (*this)[0]*other[2] + (*this)[1]*other[3];
+ q[2] = - (*this)[1]*other[0] + (*this)[0]*other[1] + (*this)[3]*other[2] + (*this)[2]*other[3];
+ q[3] = - (*this)[0]*other[0] - (*this)[1]*other[1] - (*this)[2]*other[2] + (*this)[3]*other[3];
+
+ return q;
+ }
+
+ /** \brief The exponential map from \f$ \mathfrak{so}(3) \f$ to \f$ SO(3) \f$
+ */
+ static Quaternion<T> exp(const Dune::FieldVector<T,3>& v) {
+ return exp(v[0], v[1], v[2]);
+ }
+
/** \brief The exponential map from \f$ \mathfrak{so}(3) \f$ to \f$ SO(3) \f$
*/
static Rotation<3,T> exp(const T& v0, const T& v1, const T& v2) {
@@ -92,6 +110,340 @@ public:
return q;
}
+
+ static Dune::FieldMatrix<T,4,3> Dexp(const Dune::FieldVector<T,3>& v) {
+
+ Dune::FieldMatrix<T,4,3> result(0);
+ T norm = v.two_norm();
+
+ for (int i=0; i<3; i++) {
+
+ for (int m=0; m<3; m++) {
+
+ result[m][i] = (norm==0)
+ /** \todo Isn't there a better way to implement this stably? */
+ ? 0.5 * (i==m)
+ : 0.5 * std::cos(norm/2) * v[i] * v[m] / (norm*norm) + sincHalf(norm) * ( (i==m) - v[i]*v[m]/(norm*norm));
+
+ }
+
+ result[3][i] = - 0.5 * sincHalf(norm) * v[i];
+
+ }
+ return result;
+ }
+
+ static void DDexp(const Dune::FieldVector<T,3>& v,
+ Dune::array<Dune::FieldMatrix<T,3,3>, 4>& result) {
+
+ T norm = v.two_norm();
+ if (norm==0) {
+
+ for (int m=0; m<4; m++)
+ result[m] = 0;
+
+ for (int i=0; i<3; i++)
+ result[3][i][i] = -0.25;
+
+
+ } else {
+
+ for (int i=0; i<3; i++) {
+
+ for (int j=0; j<3; j++) {
+
+ for (int m=0; m<3; m++) {
+
+ 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.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));
+
+ }
+
+ }
+
+ }
+
+ }
+
+ /** \brief The inverse of the exponential map */
+ static Dune::FieldVector<T,3> expInv(const Rotation<3,T>& q) {
+ // Compute v = exp^{-1} q
+ // Due to numerical dirt, q[3] may be larger than 1.
+ // In that case, use 1 instead of q[3].
+ Dune::FieldVector<T,3> v;
+ if (q[3] > 1.0) {
+
+ v = 0;
+
+ } else {
+
+ T invSinc = 1/sincHalf(2*std::acos(q[3]));
+
+ v[0] = q[0] * invSinc;
+ v[1] = q[1] * invSinc;
+ v[2] = q[2] * invSinc;
+
+ }
+ return v;
+ }
+
+ /** \brief The derivative of the inverse of the exponential map, evaluated at q */
+ static Dune::FieldMatrix<T,3,4> DexpInv(const Rotation<3,T>& q) {
+
+ // Compute v = exp^{-1} q
+ Dune::FieldVector<T,3> v = expInv(q);
+
+ // The derivative of exp at v
+ Dune::FieldMatrix<T,4,3> A = Dexp(v);
+
+ // Compute the Moore-Penrose pseudo inverse A^+ = (A^T A)^{-1} A^T
+ Dune::FieldMatrix<T,3,3> ATA;
+
+ for (int i=0; i<3; i++)
+ for (int j=0; j<3; j++) {
+ ATA[i][j] = 0;
+ for (int k=0; k<4; k++)
+ ATA[i][j] += A[k][i] * A[k][j];
+ }
+
+ ATA.invert();
+
+ Dune::FieldMatrix<T,3,4> APseudoInv;
+ for (int i=0; i<3; i++)
+ for (int j=0; j<4; j++) {
+ APseudoInv[i][j] = 0;
+ for (int k=0; k<3; k++)
+ APseudoInv[i][j] += ATA[i][k] * A[j][k];
+ }
+
+ return APseudoInv;
+ }
+
+
+ /** \brief Compute the vector in T_aSO(3) that is mapped by the exponential map
+ to the geodesic from a to b
+ */
+ static Dune::FieldVector<T,3> difference(const Rotation<3,T>& a, const Rotation<3,T>& b) {
+
+ Quaternion<T> diff = a;
+ diff.invert();
+ diff = diff.mult(b);
+
+ // Compute the geodesical distance between a and b on SO(3)
+ // Due to numerical dirt, diff[3] may be larger than 1.
+ // In that case, use 1 instead of diff[3].
+ Dune::FieldVector<T,3> v;
+ if (diff[3] > 1.0) {
+
+ v = 0;
+
+ } else {
+
+ T dist = 2*std::acos( std::min(diff[3],1.0) );
+
+ T invSinc = 1/sincHalf(dist);
+
+ // Compute difference on T_a SO(3)
+ v[0] = diff[0] * invSinc;
+ v[1] = diff[1] * invSinc;
+ v[2] = diff[2] * invSinc;
+
+ }
+
+ return v;
+ }
+
+ /** \brief Interpolate between two rotations */
+ static Rotation<3,T> interpolate(const Rotation<3,T>& a, const Rotation<3,T>& b, double omega) {
+
+ // Compute difference on T_a SO(3)
+ Dune::FieldVector<T,3> v = difference(a,b);
+
+ v *= omega;
+
+ return a.mult(exp(v[0], v[1], v[2]));
+ }
+
+ /** \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 Rotation<3,T>& a, const Rotation<3,T>& b,
+ double omega) {
+ Quaternion<T> result(0);
+
+ // Compute difference on T_a SO(3)
+ Dune::FieldVector<double,3> xi = difference(a,b);
+
+ 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 = Dexp(v);
+
+ diffExp.umv(xi,result);
+
+ return a.Quaternion<T>::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 = 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 {
+
+ m[0][0] = (*this)[0]*(*this)[0] - (*this)[1]*(*this)[1] - (*this)[2]*(*this)[2] + (*this)[3]*(*this)[3];
+ m[0][1] = 2 * ( (*this)[0]*(*this)[1] - (*this)[2]*(*this)[3] );
+ m[0][2] = 2 * ( (*this)[0]*(*this)[2] + (*this)[1]*(*this)[3] );
+
+ m[1][0] = 2 * ( (*this)[0]*(*this)[1] + (*this)[2]*(*this)[3] );
+ m[1][1] = - (*this)[0]*(*this)[0] + (*this)[1]*(*this)[1] - (*this)[2]*(*this)[2] + (*this)[3]*(*this)[3];
+ m[1][2] = 2 * ( -(*this)[0]*(*this)[3] + (*this)[1]*(*this)[2] );
+
+ m[2][0] = 2 * ( (*this)[0]*(*this)[2] - (*this)[1]*(*this)[3] );
+ m[2][1] = 2 * ( (*this)[0]*(*this)[3] + (*this)[1]*(*this)[2] );
+ m[2][2] = - (*this)[0]*(*this)[0] - (*this)[1]*(*this)[1] + (*this)[2]*(*this)[2] + (*this)[3]*(*this)[3];
+
+ }
+
+ /** \brief Set rotation from orthogonal matrix
+
+ We tacitly assume that the matrix really is orthogonal */
+ void set(const Dune::FieldMatrix<T,3,3>& m) {
+
+ // Easier writing
+ Dune::FieldVector<T,4>& p = (*this);
+ // The following equations for the derivation of a unit quaternion from a rotation
+ // matrix comes from 'E. Salamin, Application of Quaternions to Computation with
+ // Rotations, Technical Report, Stanford, 1974'
+
+ p[0] = (1 + m[0][0] - m[1][1] - m[2][2]) / 4;
+ p[1] = (1 - m[0][0] + m[1][1] - m[2][2]) / 4;
+ p[2] = (1 - m[0][0] - m[1][1] + m[2][2]) / 4;
+ p[3] = (1 + m[0][0] + m[1][1] + m[2][2]) / 4;
+
+ // avoid rounding problems
+ if (p[0] >= p[1] && p[0] >= p[2] && p[0] >= p[3]) {
+
+ p[0] = std::sqrt(p[0]);
+
+ // r_x r_y = (R_12 + R_21) / 4
+ p[1] = (m[0][1] + m[1][0]) / 4 / p[0];
+
+ // r_x r_z = (R_13 + R_31) / 4
+ p[2] = (m[0][2] + m[2][0]) / 4 / p[0];
+
+ // r_0 r_x = (R_32 - R_23) / 4
+ p[3] = (m[2][1] - m[1][2]) / 4 / p[0];
+
+ } else if (p[1] >= p[0] && p[1] >= p[2] && p[1] >= p[3]) {
+
+ p[1] = std::sqrt(p[1]);
+
+ // r_x r_y = (R_12 + R_21) / 4
+ p[0] = (m[0][1] + m[1][0]) / 4 / p[1];
+
+ // r_y r_z = (R_23 + R_32) / 4
+ p[2] = (m[1][2] + m[2][1]) / 4 / p[1];
+
+ // r_0 r_y = (R_13 - R_31) / 4
+ p[3] = (m[0][2] - m[2][0]) / 4 / p[1];
+
+ } else if (p[2] >= p[0] && p[2] >= p[1] && p[2] >= p[3]) {
+
+ p[2] = std::sqrt(p[2]);
+
+ // r_x r_z = (R_13 + R_31) / 4
+ p[0] = (m[0][2] + m[2][0]) / 4 / p[2];
+
+ // r_y r_z = (R_23 + R_32) / 4
+ p[1] = (m[1][2] + m[2][1]) / 4 / p[2];
+
+ // r_0 r_z = (R_21 - R_12) / 4
+ p[3] = (m[1][0] - m[0][1]) / 4 / p[2];
+
+ } else {
+
+ p[3] = std::sqrt(p[3]);
+
+ // r_0 r_x = (R_32 - R_23) / 4
+ p[0] = (m[2][1] - m[1][2]) / 4 / p[3];
+
+ // r_0 r_y = (R_13 - R_31) / 4
+ p[1] = (m[0][2] - m[2][0]) / 4 / p[3];
+
+ // r_0 r_z = (R_21 - R_12) / 4
+ p[2] = (m[1][0] - m[0][1]) / 4 / p[3];
+
+ }
+
+ }
+
+ /** \brief Create three vectors which form an orthonormal basis of \mathbb{H} together
+ with this one.
+
+ This is used to compute the strain in rod problems.
+ See: Dichmann, Li, Maddocks, 'Hamiltonian Formulations and Symmetries in
+ Rod Mechanics', page 83
+ */
+ Quaternion<T> B(int m) const {
+ assert(m>=0 && m<3);
+ Quaternion<T> r;
+ if (m==0) {
+ r[0] = (*this)[3];
+ r[1] = (*this)[2];
+ r[2] = -(*this)[1];
+ r[3] = -(*this)[0];
+ } else if (m==1) {
+ r[0] = -(*this)[2];
+ r[1] = (*this)[3];
+ r[2] = (*this)[0];
+ r[3] = -(*this)[1];
+ } else {
+ r[0] = (*this)[1];
+ r[1] = -(*this)[0];
+ r[2] = (*this)[3];
+ r[3] = -(*this)[2];
+ }
+
+ return r;
+ }
};