diff --git a/dune/gfe/realtuple.hh b/dune/gfe/realtuple.hh
index 476f2b50398f228a202a494a1069a14400115124..200fe06098ac0ced6a7770447dc80b896e8bd8f9 100644
--- a/dune/gfe/realtuple.hh
+++ b/dune/gfe/realtuple.hh
@@ -20,7 +20,7 @@ public:
/** \brief The type used for global coordinates */
typedef Dune::FieldVector<T,N> CoordinateType;
-
+
/** \brief Dimension of the manifold formed by unit vectors */
static const int dim = N;
@@ -33,7 +33,7 @@ public:
/** \brief The global convexity radius of the Euclidean space */
static constexpr T convexityRadius = std::numeric_limits<T>::infinity();
-
+
/** \brief Default constructor */
RealTuple()
{}
@@ -64,7 +64,7 @@ public:
return RealTuple(p.data_+v);
}
- /** \brief Geodesic distance between two points
+ /** \brief Geodesic distance between two points
Simply the Euclidean distance */
static T distance(const RealTuple& a, const RealTuple& b) {
@@ -101,7 +101,7 @@ public:
Unlike the distance itself the squared distance is differentiable at zero
*/
static Dune::FieldMatrix<T,N,N> secondDerivativeOfDistanceSquaredWRTFirstAndSecondArgument(const RealTuple& a, const RealTuple& b) {
-
+
Dune::FieldMatrix<T,N,N> result;
for (int i=0; i<N; i++)
for (int j=0; j<N; j++)
@@ -109,7 +109,7 @@ public:
return result;
}
-
+
/** \brief Compute the mixed third derivative \partial d^3 / \partial db^3
The result is the constant zero-tensor.
@@ -117,7 +117,7 @@ public:
static Tensor3<T,N,N,N> thirdDerivativeOfDistanceSquaredWRTSecondArgument(const RealTuple& a, const RealTuple& b) {
return Tensor3<T,N,N,N>(0);
}
-
+
/** \brief Compute the mixed third derivative \partial d^3 / \partial da db^2
The result is the constant zero-tensor.
@@ -125,7 +125,7 @@ public:
static Tensor3<T,N,N,N> thirdDerivativeOfDistanceSquaredWRTFirst1AndSecond2Argument(const RealTuple& a, const RealTuple& b) {
return Tensor3<T,N,N,N>(0);
}
-
+
/** \brief Project tangent vector of R^n onto the tangent space */
EmbeddedTangentVector projectOntoTangentSpace(const EmbeddedTangentVector& v) const {
return v;
@@ -143,13 +143,13 @@ public:
Dune::FieldMatrix<T,N,N> orthonormalFrame() const {
Dune::FieldMatrix<T,N,N> result;
-
+
for (int i=0; i<N; i++)
for (int j=0; j<N; j++)
result[i][j] = (i==j);
return result;
}
-
+
/** \brief Write LocalKey object to output stream */
friend std::ostream& operator<< (std::ostream& s, const RealTuple& realTuple)
{
@@ -157,7 +157,7 @@ public:
}
private:
-
+
Dune::FieldVector<T,N> data_;
};
diff --git a/dune/gfe/rigidbodymotion.hh b/dune/gfe/rigidbodymotion.hh
index 5653059e126f6b790a5e54dc6dcd29ce3b04590f..85fbb7ba64313efe35467938737b3de37a5558f2 100644
--- a/dune/gfe/rigidbodymotion.hh
+++ b/dune/gfe/rigidbodymotion.hh
@@ -11,13 +11,13 @@ template <class T, int N>
struct RigidBodyMotion
{
public:
-
+
/** \brief Dimension of manifold */
static const int dim = N + Rotation<T,N>::dim;
-
+
/** \brief Dimension of the embedding space */
static const int embeddedDim = N + Rotation<T,N>::embeddedDim;
-
+
/** \brief Type of an infinitesimal rigid body motion */
typedef Dune::FieldVector<T, dim> TangentVector;
@@ -26,17 +26,17 @@ public:
/** \brief The type used for coordinates */
typedef T ctype;
-
+
/** \brief The type used for global coordinates */
typedef Dune::FieldVector<T,embeddedDim> CoordinateType;
/** \brief The global convexity radius of the rigid body motions */
static constexpr T convexityRadius = Rotation<T,N>::convexityRadius;
-
+
/** \brief Default constructor */
RigidBodyMotion()
{}
-
+
/** \brief Constructor from a translation and a rotation */
RigidBodyMotion(const Dune::FieldVector<ctype, N>& translation,
const Rotation<ctype,N>& rotation)
@@ -47,16 +47,16 @@ public:
{
for (int i=0; i<N; i++)
r[i] = globalCoordinates[i];
-
+
for (int i=N; i<embeddedDim; i++)
q[i-N] = globalCoordinates[i];
-
+
// Turn this into a unit quaternion if it isn't already
q.normalize();
}
-
- /** \brief The exponential map from a given point $p \in SE(d)$.
-
+
+ /** \brief The exponential map from a given point $p \in SE(d)$.
+
Why the template parameter? Well, it should work with both TangentVector and EmbeddedTangentVector.
In general these differ and we could just have two exp methods. However in 2d they do _not_ differ,
and then the compiler complains about having two methods with the same signature.
@@ -84,14 +84,14 @@ public:
/** \brief Compute geodesic distance from a to b */
static T distance(const RigidBodyMotion<ctype,N>& a, const RigidBodyMotion<ctype,N>& b) {
-
+
T euclideanDistanceSquared = (a.r - b.r).two_norm2();
-
+
T rotationDistance = Rotation<ctype,N>::distance(a.q, b.q);
-
+
return std::sqrt(euclideanDistanceSquared + rotationDistance*rotationDistance);
}
-
+
/** \brief Compute difference vector from a to b on the tangent space of a */
static TangentVector difference(const RigidBodyMotion<ctype,N>& a,
const RigidBodyMotion<ctype,N>& b) {
@@ -111,7 +111,7 @@ public:
return result;
}
-
+
static EmbeddedTangentVector derivativeOfDistanceSquaredWRTSecondArgument(const RigidBodyMotion<ctype,N>& a,
const RigidBodyMotion<ctype,N>& b) {
@@ -121,17 +121,17 @@ public:
linearDerivative *= -2;
// rotation part
- typename Rotation<ctype,N>::EmbeddedTangentVector rotationDerivative
+ typename Rotation<ctype,N>::EmbeddedTangentVector rotationDerivative
= Rotation<ctype,N>::derivativeOfDistanceSquaredWRTSecondArgument(a.q, b.q);
-
+
return concat(linearDerivative, rotationDerivative);
}
-
+
/** \brief Compute the Hessian of the squared distance function keeping the first argument fixed */
static Dune::FieldMatrix<T,embeddedDim,embeddedDim> secondDerivativeOfDistanceSquaredWRTSecondArgument(const RigidBodyMotion<ctype,N> & p, const RigidBodyMotion<ctype,N> & q)
{
Dune::FieldMatrix<T,embeddedDim,embeddedDim> result(0);
-
+
// The linear part
Dune::FieldMatrix<T,N,N> linearPart = RealTuple<T,N>::secondDerivativeOfDistanceSquaredWRTSecondArgument(p.r,q.r);
for (int i=0; i<N; i++)
@@ -139,7 +139,7 @@ public:
result[i][j] = linearPart[i][j];
// The rotation part
- Dune::FieldMatrix<T,Rotation<T,N>::embeddedDim,Rotation<T,N>::embeddedDim> rotationPart
+ Dune::FieldMatrix<T,Rotation<T,N>::embeddedDim,Rotation<T,N>::embeddedDim> rotationPart
= Rotation<ctype,N>::secondDerivativeOfDistanceSquaredWRTSecondArgument(p.q,q.q);
for (int i=0; i<Rotation<T,N>::embeddedDim; i++)
for (int j=0; j<Rotation<T,N>::embeddedDim; j++)
@@ -147,7 +147,7 @@ public:
return result;
}
-
+
/** \brief Compute the mixed second derivate \partial d^2 / \partial da db
Unlike the distance itself the squared distance is differentiable at zero
@@ -155,7 +155,7 @@ public:
static Dune::FieldMatrix<T,embeddedDim,embeddedDim> secondDerivativeOfDistanceSquaredWRTFirstAndSecondArgument(const RigidBodyMotion<ctype,N> & p, const RigidBodyMotion<ctype,N> & q)
{
Dune::FieldMatrix<T,embeddedDim,embeddedDim> result(0);
-
+
// The linear part
Dune::FieldMatrix<T,N,N> linearPart = RealTuple<T,N>::secondDerivativeOfDistanceSquaredWRTFirstAndSecondArgument(p.r,q.r);
for (int i=0; i<N; i++)
@@ -163,7 +163,7 @@ public:
result[i][j] = linearPart[i][j];
// The rotation part
- Dune::FieldMatrix<T,Rotation<T,N>::embeddedDim,Rotation<T,N>::embeddedDim> rotationPart
+ Dune::FieldMatrix<T,Rotation<T,N>::embeddedDim,Rotation<T,N>::embeddedDim> rotationPart
= Rotation<ctype,N>::secondDerivativeOfDistanceSquaredWRTFirstAndSecondArgument(p.q,q.q);
for (int i=0; i<Rotation<T,N>::embeddedDim; i++)
for (int j=0; j<Rotation<T,N>::embeddedDim; j++)
@@ -171,7 +171,7 @@ public:
return result;
}
-
+
/** \brief Compute the third derivative \partial d^3 / \partial dq^3
Unlike the distance itself the squared distance is differentiable at zero
@@ -179,7 +179,7 @@ public:
static Tensor3<T,embeddedDim,embeddedDim,embeddedDim> thirdDerivativeOfDistanceSquaredWRTSecondArgument(const RigidBodyMotion<ctype,N> & p, const RigidBodyMotion<ctype,N> & q)
{
Tensor3<T,embeddedDim,embeddedDim,embeddedDim> result(0);
-
+
// The linear part
Tensor3<T,N,N,N> linearPart = RealTuple<T,N>::thirdDerivativeOfDistanceSquaredWRTSecondArgument(p.r,q.r);
for (int i=0; i<N; i++)
@@ -188,9 +188,9 @@ public:
result[i][j][k] = linearPart[i][j][k];
// The rotation part
- Tensor3<T,Rotation<T,N>::embeddedDim,Rotation<T,N>::embeddedDim,Rotation<T,N>::embeddedDim> rotationPart
+ Tensor3<T,Rotation<T,N>::embeddedDim,Rotation<T,N>::embeddedDim,Rotation<T,N>::embeddedDim> rotationPart
= Rotation<ctype,N>::thirdDerivativeOfDistanceSquaredWRTSecondArgument(p.q,q.q);
-
+
for (int i=0; i<Rotation<T,N>::embeddedDim; i++)
for (int j=0; j<Rotation<T,N>::embeddedDim; j++)
for (int k=0; k<Rotation<T,N>::embeddedDim; k++)
@@ -198,7 +198,7 @@ public:
return result;
}
-
+
/** \brief Compute the mixed third derivative \partial d^3 / \partial da db^2
Unlike the distance itself the squared distance is differentiable at zero
@@ -206,7 +206,7 @@ public:
static Tensor3<T,embeddedDim,embeddedDim,embeddedDim> thirdDerivativeOfDistanceSquaredWRTFirst1AndSecond2Argument(const RigidBodyMotion<ctype,N> & p, const RigidBodyMotion<ctype,N> & q)
{
Tensor3<T,embeddedDim,embeddedDim,embeddedDim> result(0);
-
+
// The linear part
Tensor3<T,N,N,N> linearPart = RealTuple<T,N>::thirdDerivativeOfDistanceSquaredWRTFirst1AndSecond2Argument(p.r,q.r);
for (int i=0; i<N; i++)
@@ -224,14 +224,14 @@ public:
return result;
}
-
-
+
+
/** \brief Project tangent vector of R^n onto the tangent space */
EmbeddedTangentVector projectOntoTangentSpace(const EmbeddedTangentVector& v) const {
DUNE_THROW(Dune::NotImplemented, "!");
}
-
+
/** \brief Compute an orthonormal basis of the tangent space of SE(3).
This basis may not be globally continuous.
@@ -242,7 +242,7 @@ public:
// Get the R^d part
for (int i=0; i<N; i++)
result[i][i] = 1;
-
+
Dune::FieldMatrix<T,Rotation<T,N>::dim,Rotation<T,N>::embeddedDim> SO3Part = q.orthonormalFrame();
for (int i=0; i<Rotation<T,N>::dim; i++)
@@ -251,7 +251,7 @@ public:
return result;
}
-
+
/** \brief The global coordinates, if you really want them */
CoordinateType globalCoordinates() const {
return concat(r, q.globalCoordinates());
@@ -264,9 +264,9 @@ public:
// Rotational part
Rotation<ctype,N> q;
-
+
private:
-
+
/** \brief Concatenate two FieldVectors */
template <int NN, int M>
static Dune::FieldVector<ctype,NN+M> concat(const Dune::FieldVector<ctype,NN>& a,
diff --git a/dune/gfe/unitvector.hh b/dune/gfe/unitvector.hh
index 33ec628d62116cdc4d724702fe3b53b4033c3bf7..838c92bd50231f3756493b8136152406a543525a 100644
--- a/dune/gfe/unitvector.hh
+++ b/dune/gfe/unitvector.hh
@@ -17,9 +17,9 @@ class Rotation;
template <class T, int N>
class UnitVector
{
- // Rotation<T,3> is friend, because it needs the various derivatives of the arccos
+ // Rotation<T,3> is friend, because it needs the various derivatives of the arccos
friend class Rotation<T,3>;
-
+
/** \brief Computes sin(x) / x without getting unstable for small x */
static T sinc(const T& x) {
return (x < 1e-4) ? 1 - (x*x/6) : std::sin(x)/x;
@@ -67,7 +67,7 @@ public:
/** \brief The type used for global coordinates */
typedef Dune::FieldVector<T,N> CoordinateType;
-
+
/** \brief Dimension of the manifold formed by unit vectors */
static const int dim = N-1;
@@ -77,21 +77,21 @@ public:
typedef Dune::FieldVector<T,N-1> TangentVector;
typedef Dune::FieldVector<T,N> EmbeddedTangentVector;
-
+
/** \brief The global convexity radius of the unit sphere */
static constexpr T convexityRadius = 0.5*M_PI;
-
+
/** \brief Default constructor */
UnitVector()
{}
-
+
/** \brief Constructor from a vector. The vector gets normalized */
UnitVector(const Dune::FieldVector<T,N>& vector)
: data_(vector)
{
data_ /= data_.two_norm();
}
-
+
/** \brief Constructor from an array. The array gets normalized */
UnitVector(const Dune::array<T,N>& vector)
{
@@ -114,7 +114,7 @@ public:
EmbeddedTangentVector ev;
frame.mtv(v,ev);
-
+
return exp(p,ev);
}
@@ -137,10 +137,10 @@ public:
// supposed to handle perturbations of unit vectors as well. Therefore
// we normalize here.
T x = a.data_ * b.data_/a.data_.two_norm()/b.data_.two_norm();
-
+
// paranoia: if the argument is just eps larger than 1 acos returns NaN
x = std::min(x,1.0);
-
+
return std::acos(x);
}
@@ -171,14 +171,14 @@ public:
static Dune::FieldMatrix<T,N,N> secondDerivativeOfDistanceSquaredWRTSecondArgument(const UnitVector& p, const UnitVector& q) {
T sp = p.data_ * q.data_;
-
+
Dune::FieldVector<T,N> pProjected = q.projectOntoTangentSpace(p.globalCoordinates());
Dune::FieldMatrix<T,N,N> A;
for (int i=0; i<N; i++)
for (int j=0; j<N; j++)
A[i][j] = pProjected[i]*pProjected[j];
-
+
A *= secondDerivativeOfArcCosSquared(sp);
// Compute matrix B (see notes)
@@ -222,7 +222,7 @@ public:
Pp[i][j] = (i==j) - a.data_[i]*a.data_[j];
Pq[i][j] = (i==j) - b.data_[i]*b.data_[j];
}
-
+
Dune::FieldMatrix<T,N,N> B;
Dune::FMatrixHelp::multMatrix(Pp,Pq,B);
@@ -231,8 +231,8 @@ public:
return A;
}
-
-
+
+
/** \brief Compute the third derivative \partial d^3 / \partial dq^3
Unlike the distance itself the squared distance is differentiable at zero
@@ -242,13 +242,13 @@ public:
Tensor3<T,N,N,N> result;
T sp = p.data_ * q.data_;
-
+
// The projection matrix onto the tangent space at p and q
Dune::FieldMatrix<T,N,N> Pq;
for (int i=0; i<N; i++)
for (int j=0; j<N; j++)
Pq[i][j] = (i==j) - q.globalCoordinates()[i]*q.globalCoordinates()[j];
-
+
Dune::FieldVector<T,N> pProjected = q.projectOntoTangentSpace(p.globalCoordinates());
for (int i=0; i<N; i++)
@@ -262,12 +262,12 @@ public:
+ derivativeOfArcCosSquared(sp) * ((i==j)*q.globalCoordinates()[k] + (i==k)*q.globalCoordinates()[j]) * sp
- derivativeOfArcCosSquared(sp) * p.globalCoordinates()[i] * Pq[j][k];
}
-
+
result = Pq * result;
-
+
return result;
- }
-
+ }
+
/** \brief Compute the mixed third derivative \partial d^3 / \partial da db^2
Unlike the distance itself the squared distance is differentiable at zero
@@ -277,7 +277,7 @@ public:
Tensor3<T,N,N,N> result;
T sp = p.data_ * q.data_;
-
+
// The projection matrix onto the tangent space at p and q
Dune::FieldMatrix<T,N,N> Pp, Pq;
for (int i=0; i<N; i++)
@@ -285,10 +285,10 @@ public:
Pp[i][j] = (i==j) - p.globalCoordinates()[i]*p.globalCoordinates()[j];
Pq[i][j] = (i==j) - q.globalCoordinates()[i]*q.globalCoordinates()[j];
}
-
+
Dune::FieldVector<T,N> pProjected = q.projectOntoTangentSpace(p.globalCoordinates());
Dune::FieldVector<T,N> qProjected = p.projectOntoTangentSpace(q.globalCoordinates());
-
+
Tensor3<T,N,N,N> derivativeOfPqOTimesPq;
for (int i=0; i<N; i++)
for (int j=0; j<N; j++)
@@ -297,16 +297,16 @@ public:
for (int l=0; l<N; l++)
derivativeOfPqOTimesPq[i][j][k] += Pp[i][l] * (Pq[j][l]*pProjected[k] + pProjected[j]*Pq[k][l]);
}
-
+
result = thirdDerivativeOfArcCosSquared(sp) * Tensor3<T,N,N,N>::product(qProjected,pProjected,pProjected)
+ secondDerivativeOfArcCosSquared(sp) * derivativeOfPqOTimesPq
- secondDerivativeOfArcCosSquared(sp) * sp * Tensor3<T,N,N,N>::product(qProjected,Pq)
- derivativeOfArcCosSquared(sp) * Tensor3<T,N,N,N>::product(qProjected,Pq);
-
+
return result;
}
-
-
+
+
/** \brief Project tangent vector of R^n onto the tangent space */
EmbeddedTangentVector projectOntoTangentSpace(const EmbeddedTangentVector& v) const {
EmbeddedTangentVector result = v;
@@ -329,40 +329,40 @@ public:
// Coordinates of the stereographic projection
Dune::FieldVector<T,N-1> X;
-
+
if (data_[N-1] <= 0) {
-
+
// Stereographic projection from the north pole onto R^{N-1}
for (size_t i=0; i<N-1; i++)
X[i] = data_[i] / (1-data_[N-1]);
-
+
} else {
-
+
// Stereographic projection from the south pole onto R^{N-1}
for (size_t i=0; i<N-1; i++)
X[i] = data_[i] / (1+data_[N-1]);
-
+
}
-
+
T RSquared = X.two_norm2();
-
+
for (size_t i=0; i<N-1; i++)
for (size_t j=0; j<N-1; j++)
// Note: the matrix is the transpose of the one in the paper
result[j][i] = 2*(i==j)*(1+RSquared) - 4*X[i]*X[j];
-
+
for (size_t j=0; j<N-1; j++)
result[j][N-1] = 4*X[j];
-
+
// Upper hemisphere: adapt formulas so it is the stereographic projection from the south pole
- if (data_[N-1] > 0)
+ if (data_[N-1] > 0)
for (size_t j=0; j<N-1; j++)
result[j][N-1] *= -1;
-
+
// normalize the rows to make the orthogonal basis orthonormal
for (size_t i=0; i<N-1; i++)
result[i] /= result[i].two_norm();
-
+
return result;
}