Newer
Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
#ifndef ROTATION_HH
#define ROTATION_HH
/** \file
\brief Define rotations in Euclidean spaces
*/
#include <dune/common/fvector.hh>
#include <dune/common/fixedarray.hh>
#include <dune/common/fmatrix.hh>
#include <dune/common/exceptions.hh>
#include "quaternion.hh"
template <int dim, class T>
class Rotation
{
};
/** \brief Specialization for dim==3
Uses unit quaternion coordinates.
*/
template <class T>
class Rotation<3,T> : public Quaternion<T>
{
/** \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 creates the identity element */
Rotation()
: Quaternion<T>(0,0,0,1)
{}
Rotation<3,T>(Dune::FieldVector<T,3> axis, T angle)
: Quaternion<T>(axis, angle)
{}
/** \brief Assignment from a quaternion
\deprecated Using this is bad design.
*/
Rotation& operator=(const Quaternion<T>& other) {
(*this)[0] = other[0];
(*this)[1] = other[1];
(*this)[2] = other[2];
(*this)[3] = other[3];
return *this;
}
/** \brief Return the identity element */
static Rotation<3,T> identity() {
// Default constructor creates an identity
Rotation<3,T> id;
return id;
}
/** \brief Right multiplication */
Rotation<3,T> mult(const Rotation<3,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 Rotation<3,T> exp(const T& v0, const T& v1, const T& v2) {
Rotation<3,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;
}
};
#endif