From 9e6d3de54aeb5717a7ab3ac85519d4bc85cd2142 Mon Sep 17 00:00:00 2001
From: Oliver Sander <sander@igpm.rwth-aachen.de>
Date: Mon, 2 Feb 2009 10:08:28 +0000
Subject: [PATCH] A new class which embodies rotations in Euclidean spaces
[[Imported from SVN: r3487]]
---
src/rotation.hh | 98 +++++++++++++++++++++++++++++++++++++++++++++++++
1 file changed, 98 insertions(+)
create mode 100644 src/rotation.hh
diff --git a/src/rotation.hh b/src/rotation.hh
new file mode 100644
index 00000000..b9293cd9
--- /dev/null
+++ b/src/rotation.hh
@@ -0,0 +1,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
--
GitLab