diff --git a/dune/gfe/rigidbodymotion.hh b/dune/gfe/rigidbodymotion.hh
index 92801bad514611948a06d8ab18b00c98c4dcd918..d96b5ad6e5d6f44a79e751f3fdf5e0d9af39199a 100644
--- a/dune/gfe/rigidbodymotion.hh
+++ b/dune/gfe/rigidbodymotion.hh
@@ -6,39 +6,52 @@
#include <dune/gfe/realtuple.hh>
#include "rotation.hh"
-/** \brief A rigid-body motion in R^d, i.e., a member of SE(d) */
-template <int dim, class T=double>
+/** \brief A rigid-body motion in R^N, i.e., a member of SE(N) */
+template <int N, class T=double>
struct RigidBodyMotion
{
-private:
+public:
/** \brief Dimension of manifold */
- static const int dimension = dim + Rotation<dim,T>::TangentVector::dimension;
+ static const int dim = N + Rotation<N,T>::dim;
/** \brief Dimension of the embedding space */
- static const int embeddedDimension = dim + Rotation<dim,T>::EmbeddedTangentVector::dimension;
-
-public:
+ static const int embeddedDim = N + Rotation<N,T>::embeddedDim;
/** \brief Type of an infinitesimal rigid body motion */
- typedef Dune::FieldVector<T, dimension> TangentVector;
+ typedef Dune::FieldVector<T, dim> TangentVector;
/** \brief Type of an infinitesimal rigid body motion */
- typedef Dune::FieldVector<T, embeddedDimension> EmbeddedTangentVector;
+ typedef Dune::FieldVector<T, embeddedDim> EmbeddedTangentVector;
/** \brief The type used for coordinates */
typedef T ctype;
+ /** \brief The type used for global coordinates */
+ typedef Dune::FieldVector<T,embeddedDim> CoordinateType;
+
/** \brief Default constructor */
RigidBodyMotion()
{}
/** \brief Constructor from a translation and a rotation */
- RigidBodyMotion(const Dune::FieldVector<ctype, dim>& translation,
- const Rotation<dim,ctype>& rotation)
+ RigidBodyMotion(const Dune::FieldVector<ctype, N>& translation,
+ const Rotation<N,ctype>& rotation)
: r(translation), q(rotation)
{}
+ RigidBodyMotion(const CoordinateType& globalCoordinates)
+ {
+ 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)$.
Why the template parameter? Well, it should work with both TangentVector and EmbeddedTangentVector.
@@ -46,87 +59,88 @@ public:
and then the compiler complains about having two methods with the same signature.
*/
template <class TVector>
- static RigidBodyMotion<dim,ctype> exp(const RigidBodyMotion<dim,ctype>& p, const TVector& v) {
+ static RigidBodyMotion<N,ctype> exp(const RigidBodyMotion<N,ctype>& p, const TVector& v) {
- RigidBodyMotion<dim,ctype> result;
+ RigidBodyMotion<N,ctype> result;
// Add translational correction
- for (int i=0; i<dim; i++)
+ for (int i=0; i<N; i++)
result.r[i] = p.r[i] + v[i];
// Add rotational correction
typedef typename Dune::SelectType<Dune::is_same<TVector,TangentVector>::value,
- typename Rotation<dim,ctype>::TangentVector,
- typename Rotation<dim,ctype>::EmbeddedTangentVector>::Type RotationTangentVector;
+ typename Rotation<N,ctype>::TangentVector,
+ typename Rotation<N,ctype>::EmbeddedTangentVector>::Type RotationTangentVector;
RotationTangentVector qCorr;
for (int i=0; i<RotationTangentVector::dimension; i++)
- qCorr[i] = v[dim+i];
+ qCorr[i] = v[N+i];
- result.q = Rotation<dim,ctype>::exp(p.q, qCorr);
+ result.q = Rotation<N,ctype>::exp(p.q, qCorr);
return result;
}
/** \brief Compute geodesic distance from a to b */
- static T distance(const RigidBodyMotion<dim,ctype>& a, const RigidBodyMotion<dim,ctype>& b) {
+ static T distance(const RigidBodyMotion<N,ctype>& a, const RigidBodyMotion<N,ctype>& b) {
T euclideanDistanceSquared = (a.r - b.r).two_norm2();
- T rotationDistance = Rotation<dim,ctype>::distance(a.q, b.q);
+ T rotationDistance = Rotation<N,ctype>::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<dim,ctype>& a,
- const RigidBodyMotion<dim,ctype>& b) {
+ static TangentVector difference(const RigidBodyMotion<N,ctype>& a,
+ const RigidBodyMotion<N,ctype>& b) {
TangentVector result;
// Usual linear difference
- for (int i=0; i<dim; i++)
+ for (int i=0; i<N; i++)
result[i] = a.r[i] - b.r[i];
// Subtract orientations on the tangent space of 'a'
- typename Rotation<dim,ctype>::TangentVector v = Rotation<dim,ctype>::difference(a.q, b.q).axial();
+ typename Rotation<N,ctype>::TangentVector v = Rotation<N,ctype>::difference(a.q, b.q).axial();
// Compute difference on T_a SO(3)
- for (int i=0; i<Rotation<dim,ctype>::TangentVector::dimension; i++)
- result[i+dim] = v[i];
+ for (int i=0; i<Rotation<N,ctype>::TangentVector::dimension; i++)
+ result[i+N] = v[i];
return result;
}
- static EmbeddedTangentVector derivativeOfDistanceSquaredWRTSecondArgument(const RigidBodyMotion<dim,ctype>& a,
- const RigidBodyMotion<dim,ctype>& b) {
+ static EmbeddedTangentVector derivativeOfDistanceSquaredWRTSecondArgument(const RigidBodyMotion<N,ctype>& a,
+ const RigidBodyMotion<N,ctype>& b) {
// linear part
- Dune::FieldVector<ctype,dim> linearDerivative = a.r;
+ Dune::FieldVector<ctype,N> linearDerivative = a.r;
linearDerivative -= b.r;
linearDerivative *= -2;
// rotation part
- typename Rotation<dim,ctype>::EmbeddedTangentVector rotationDerivative
- = Rotation<dim,ctype>::derivativeOfDistanceSquaredWRTSecondArgument(a.q, b.q);
+ typename Rotation<N,ctype>::EmbeddedTangentVector rotationDerivative
+ = Rotation<N,ctype>::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<double,embeddedDimension,embeddedDimension> secondDerivativeOfDistanceSquaredWRTSecondArgument(const RigidBodyMotion<dim,ctype> & p, const RigidBodyMotion<dim,ctype> & q)
+ static Dune::FieldMatrix<double,embeddedDim,embeddedDim> secondDerivativeOfDistanceSquaredWRTSecondArgument(const RigidBodyMotion<N,ctype> & p, const RigidBodyMotion<N,ctype> & q)
{
- Dune::FieldMatrix<double,embeddedDimension,embeddedDimension> result(0);
+ Dune::FieldMatrix<double,embeddedDim,embeddedDim> result(0);
// The linear part
- Dune::FieldMatrix<double,dim,dim> linearPart = RealTuple<dim>::secondDerivativeOfDistanceSquaredWRTSecondArgument(p.r,q.r);
- for (int i=0; i<dim; i++)
- for (int j=0; j<dim; j++)
+ Dune::FieldMatrix<double,N,N> linearPart = RealTuple<N>::secondDerivativeOfDistanceSquaredWRTSecondArgument(p.r,q.r);
+ for (int i=0; i<N; i++)
+ for (int j=0; j<N; j++)
result[i][j] = linearPart[i][j];
// The rotation part
- Dune::FieldMatrix<double,Rotation<dim,T>::EmbeddedTangentVector::dimension,Rotation<dim,T>::EmbeddedTangentVector::dimension> rotationPart = Rotation<dim,ctype>::secondDerivativeOfDistanceSquaredWRTSecondArgument(p.q,q.q);
- for (int i=0; i<Rotation<dim,T>::EmbeddedTangentVector::dimension; i++)
- for (int j=0; j<Rotation<dim,T>::EmbeddedTangentVector::dimension; j++)
- result[dim+i][dim+j] = rotationPart[i][j];
+ Dune::FieldMatrix<double,Rotation<N,T>::embeddedDim,Rotation<N,T>::embeddedDim> rotationPart
+ = Rotation<N,ctype>::secondDerivativeOfDistanceSquaredWRTSecondArgument(p.q,q.q);
+ for (int i=0; i<Rotation<N,T>::embeddedDim; i++)
+ for (int j=0; j<Rotation<N,T>::embeddedDim; j++)
+ result[N+i][N+j] = rotationPart[i][j];
return result;
}
@@ -135,21 +149,22 @@ public:
Unlike the distance itself the squared distance is differentiable at zero
*/
- static Dune::FieldMatrix<double,embeddedDimension,embeddedDimension> secondDerivativeOfDistanceSquaredWRTFirstAndSecondArgument(const RigidBodyMotion<dim,ctype> & p, const RigidBodyMotion<dim,ctype> & q)
+ static Dune::FieldMatrix<double,embeddedDim,embeddedDim> secondDerivativeOfDistanceSquaredWRTFirstAndSecondArgument(const RigidBodyMotion<N,ctype> & p, const RigidBodyMotion<N,ctype> & q)
{
- Dune::FieldMatrix<double,embeddedDimension,embeddedDimension> result(0);
+ Dune::FieldMatrix<double,embeddedDim,embeddedDim> result(0);
// The linear part
- Dune::FieldMatrix<double,dim,dim> linearPart = RealTuple<dim>::secondDerivativeOfDistanceSquaredWRTFirstAndSecondArgument(p.r,q.r);
- for (int i=0; i<dim; i++)
- for (int j=0; j<dim; j++)
+ Dune::FieldMatrix<double,N,N> linearPart = RealTuple<N>::secondDerivativeOfDistanceSquaredWRTFirstAndSecondArgument(p.r,q.r);
+ for (int i=0; i<N; i++)
+ for (int j=0; j<N; j++)
result[i][j] = linearPart[i][j];
// The rotation part
- Dune::FieldMatrix<double,Rotation<dim,T>::EmbeddedTangentVector::dimension,Rotation<dim,T>::EmbeddedTangentVector::dimension> rotationPart = Rotation<dim,ctype>::secondDerivativeOfDistanceSquaredWRTFirstAndSecondArgument(p.q,q.q);
- for (int i=0; i<Rotation<dim,T>::EmbeddedTangentVector::dimension; i++)
- for (int j=0; j<Rotation<dim,T>::EmbeddedTangentVector::dimension; j++)
- result[dim+i][dim+j] = rotationPart[i][j];
+ Dune::FieldMatrix<double,Rotation<N,T>::embeddedDim,Rotation<N,T>::embeddedDim> rotationPart
+ = Rotation<N,ctype>::secondDerivativeOfDistanceSquaredWRTFirstAndSecondArgument(p.q,q.q);
+ for (int i=0; i<Rotation<N,T>::embeddedDim; i++)
+ for (int j=0; j<Rotation<N,T>::embeddedDim; j++)
+ result[N+i][N+j] = rotationPart[i][j];
return result;
}
@@ -158,23 +173,25 @@ public:
Unlike the distance itself the squared distance is differentiable at zero
*/
- static Tensor3<double,embeddedDimension,embeddedDimension,embeddedDimension> thirdDerivativeOfDistanceSquaredWRTSecondArgument(const RigidBodyMotion<dim,ctype> & p, const RigidBodyMotion<dim,ctype> & q)
+ static Tensor3<double,embeddedDim,embeddedDim,embeddedDim> thirdDerivativeOfDistanceSquaredWRTSecondArgument(const RigidBodyMotion<N,ctype> & p, const RigidBodyMotion<N,ctype> & q)
{
- Tensor3<double,embeddedDimension,embeddedDimension,embeddedDimension> result(0);
+ Tensor3<double,embeddedDim,embeddedDim,embeddedDim> result(0);
// The linear part
- Tensor3<double,dim,dim,dim> linearPart = RealTuple<dim>::thirdDerivativeOfDistanceSquaredWRTSecondArgument(p.r,q.r);
- for (int i=0; i<dim; i++)
- for (int j=0; j<dim; j++)
- for (int k=0; k<dim; k++)
+ Tensor3<double,N,N,N> linearPart = RealTuple<N>::thirdDerivativeOfDistanceSquaredWRTSecondArgument(p.r,q.r);
+ for (int i=0; i<N; i++)
+ for (int j=0; j<N; j++)
+ for (int k=0; k<N; k++)
result[i][j][k] = linearPart[i][j][k];
// The rotation part
- Tensor3<double,Rotation<dim,T>::EmbeddedTangentVector::dimension,Rotation<dim,T>::EmbeddedTangentVector::dimension,Rotation<dim,T>::EmbeddedTangentVector::dimension> rotationPart = Rotation<dim,ctype>::thirdDerivativeOfDistanceSquaredWRTSecondArgument(p.q,q.q);
- for (int i=0; i<Rotation<dim,T>::EmbeddedTangentVector::dimension; i++)
- for (int j=0; j<Rotation<dim,T>::EmbeddedTangentVector::dimension; j++)
- for (int k=0; k<Rotation<dim,T>::EmbeddedTangentVector::dimension; k++)
- result[dim+i][dim+j][dim+k] = rotationPart[i][j][k];
+ Tensor3<double,Rotation<N,T>::embeddedDim,Rotation<N,T>::embeddedDim,Rotation<N,T>::embeddedDim> rotationPart
+ = Rotation<N,ctype>::thirdDerivativeOfDistanceSquaredWRTSecondArgument(p.q,q.q);
+
+ for (int i=0; i<Rotation<N,T>::embeddedDim; i++)
+ for (int j=0; j<Rotation<N,T>::embeddedDim; j++)
+ for (int k=0; k<Rotation<N,T>::embeddedDim; k++)
+ result[N+i][N+j][N+k] = rotationPart[i][j][k];
return result;
}
@@ -183,23 +200,23 @@ public:
Unlike the distance itself the squared distance is differentiable at zero
*/
- static Tensor3<double,embeddedDimension,embeddedDimension,embeddedDimension> thirdDerivativeOfDistanceSquaredWRTFirst1AndSecond2Argument(const RigidBodyMotion<dim,ctype> & p, const RigidBodyMotion<dim,ctype> & q)
+ static Tensor3<double,embeddedDim,embeddedDim,embeddedDim> thirdDerivativeOfDistanceSquaredWRTFirst1AndSecond2Argument(const RigidBodyMotion<N,ctype> & p, const RigidBodyMotion<N,ctype> & q)
{
- Tensor3<double,embeddedDimension,embeddedDimension,embeddedDimension> result(0);
+ Tensor3<double,embeddedDim,embeddedDim,embeddedDim> result(0);
// The linear part
- Tensor3<double,dim,dim,dim> linearPart = RealTuple<dim>::thirdDerivativeOfDistanceSquaredWRTFirst1AndSecond2Argument(p.r,q.r);
- for (int i=0; i<dim; i++)
- for (int j=0; j<dim; j++)
- for (int k=0; k<dim; k++)
+ Tensor3<double,N,N,N> linearPart = RealTuple<N>::thirdDerivativeOfDistanceSquaredWRTFirst1AndSecond2Argument(p.r,q.r);
+ for (int i=0; i<N; i++)
+ for (int j=0; j<N; j++)
+ for (int k=0; k<N; k++)
result[i][j][k] = linearPart[i][j][k];
// The rotation part
- Tensor3<double,Rotation<dim,T>::EmbeddedTangentVector::dimension,Rotation<dim,T>::EmbeddedTangentVector::dimension,Rotation<dim,T>::EmbeddedTangentVector::dimension> rotationPart = Rotation<dim,ctype>::thirdDerivativeOfDistanceSquaredWRTFirst1AndSecond2Argument(p.q,q.q);
- for (int i=0; i<Rotation<dim,T>::EmbeddedTangentVector::dimension; i++)
- for (int j=0; j<Rotation<dim,T>::EmbeddedTangentVector::dimension; j++)
- for (int k=0; k<Rotation<dim,T>::EmbeddedTangentVector::dimension; k++)
- result[dim+i][dim+j][dim+k] = rotationPart[i][j][k];
+ Tensor3<double,Rotation<N,T>::embeddedDim,Rotation<N,T>::embeddedDim,Rotation<N,T>::embeddedDim> rotationPart = Rotation<N,ctype>::thirdDerivativeOfDistanceSquaredWRTFirst1AndSecond2Argument(p.q,q.q);
+ for (int i=0; i<Rotation<N,T>::embeddedDim; i++)
+ for (int j=0; j<Rotation<N,T>::embeddedDim; j++)
+ for (int k=0; k<Rotation<N,T>::embeddedDim; k++)
+ result[N+i][N+j][N+k] = rotationPart[i][j][k];
return result;
}
@@ -216,50 +233,55 @@ public:
This basis may not be globally continuous.
*/
- Dune::FieldMatrix<double,dimension,embeddedDimension> orthonormalFrame() const {
- Dune::FieldMatrix<double,dimension,embeddedDimension> result(0);
+ Dune::FieldMatrix<double,dim,embeddedDim> orthonormalFrame() const {
+ Dune::FieldMatrix<double,dim,embeddedDim> result(0);
// Get the R^d part
- for (int i=0; i<dim; i++)
+ for (int i=0; i<N; i++)
result[i][i] = 1;
- Dune::FieldMatrix<double,Rotation<dim>::TangentVector::dimension,Rotation<dim>::EmbeddedTangentVector::dimension> SO3Part = q.orthonormalFrame();
+ Dune::FieldMatrix<double,Rotation<N>::dim,Rotation<N>::embeddedDim> SO3Part = q.orthonormalFrame();
- for (int i=0; i<Rotation<dim>::TangentVector::dimension; i++)
- for (int j=0; j<Rotation<dim>::EmbeddedTangentVector::dimension; j++)
- result[dim+i][dim+j] = SO3Part[i][j];
+ for (int i=0; i<Rotation<N>::dim; i++)
+ for (int j=0; j<Rotation<N>::embeddedDim; j++)
+ result[N+i][N+j] = SO3Part[i][j];
return result;
}
+ /** \brief The global coordinates, if you really want them */
+ CoordinateType globalCoordinates() const {
+ return concat(r, q.globalCoordinates());
+ }
+
// Translational part
- Dune::FieldVector<ctype, dim> r;
+ Dune::FieldVector<ctype, N> r;
// Rotational part
- Rotation<dim,ctype> q;
+ Rotation<N,ctype> q;
private:
/** \brief Concatenate two FieldVectors */
- template <int N, int M>
- static Dune::FieldVector<ctype,N+M> concat(const Dune::FieldVector<ctype,N>& a,
+ template <int NN, int M>
+ static Dune::FieldVector<ctype,NN+M> concat(const Dune::FieldVector<ctype,NN>& a,
const Dune::FieldVector<ctype,M>& b)
{
- Dune::FieldVector<ctype,N+M> result;
- for (int i=0; i<N; i++)
+ Dune::FieldVector<ctype,NN+M> result;
+ for (int i=0; i<NN; i++)
result[i] = a[i];
for (int i=0; i<M; i++)
- result[i+N] = b[i];
+ result[i+NN] = b[i];
return result;
}
};
//! Send configuration to output stream
-template <int dim, class ctype>
-std::ostream& operator<< (std::ostream& s, const RigidBodyMotion<dim,ctype>& c)
+template <int N, class ctype>
+std::ostream& operator<< (std::ostream& s, const RigidBodyMotion<N,ctype>& c)
{
s << "(" << c.r << ") (" << c.q << ")";
return s;