From 9053dd974dd5aed2bf1cc4221fff4a4f179eae38 Mon Sep 17 00:00:00 2001
From: Oliver Sander <sander@igpm.rwth-aachen.de>
Date: Mon, 22 Mar 2010 10:46:51 +0000
Subject: [PATCH] add the distance function and its derivatives to Rotation<2>
[[Imported from SVN: r5784]]
---
src/rotation.hh | 24 ++++++++++++++++++++++++
1 file changed, 24 insertions(+)
diff --git a/src/rotation.hh b/src/rotation.hh
index cf7e4829..28a0b155 100644
--- a/src/rotation.hh
+++ b/src/rotation.hh
@@ -33,6 +33,10 @@ public:
: angle_(0)
{}
+ Rotation(const T& angle)
+ : angle_(angle)
+ {}
+
/** \brief Return the identity element */
static Rotation<2,T> identity() {
// Default constructor creates an identity
@@ -40,6 +44,16 @@ public:
return id;
}
+ static T distance(const Rotation<2,T>& a, const Rotation<2,T>& b) {
+ T dist = a.angle_ - b.angle_;
+ while (dist < 0)
+ dist += 2*M_PI;
+ while (dist > 2*M_PI)
+ dist -= 2*M_PI;
+
+ return (dist <= M_PI) ? dist : 2*M_PI - dist;
+ }
+
/** \brief The exponential map from a given point $p \in SO(3)$. */
static Rotation<2,T> exp(const Rotation<2,T>& p, const TangentVector& v) {
Rotation<2,T> result = p;
@@ -55,6 +69,16 @@ public:
return result;
}
+ static TangentVector derivativeOfDistanceSquaredWRTSecondArgument(const Rotation<2,T>& a,
+ const Rotation<2,T>& b) {
+ return -2 * distance(a,b);
+ }
+
+ static TangentVector secondDerivativeOfDistanceSquaredWRTSecondArgument(const Rotation<2,T>& a,
+ const Rotation<2,T>& b) {
+ return 2;
+ }
+
/** \brief Right multiplication */
Rotation<2,T> mult(const Rotation<2,T>& other) const {
Rotation<2,T> q = *this;
--
GitLab