From bc319072673f9dab57641693c41c580f07c10a57 Mon Sep 17 00:00:00 2001
From: Oliver Sander <sander@igpm.rwth-aachen.de>
Date: Sat, 18 Apr 2009 09:49:05 +0000
Subject: [PATCH] add new method 'difference'
[[Imported from SVN: r4045]]
---
src/rigidbodymotion.hh | 20 ++++++++++++++++++++
1 file changed, 20 insertions(+)
diff --git a/src/rigidbodymotion.hh b/src/rigidbodymotion.hh
index 3247433a..40e8ff6e 100644
--- a/src/rigidbodymotion.hh
+++ b/src/rigidbodymotion.hh
@@ -11,6 +11,26 @@ struct RigidBodyMotion
/** \brief Type of an infinitesimal rigid body motion */
typedef Dune::FieldVector<ctype, (dim==3) ? 6 : 3> TangentVector;
+ /** \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) {
+
+ TangentVector result;
+
+ // Usual linear difference
+ for (int i=0; i<dim; 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);
+
+ // Compute difference on T_a SO(3)
+ for (int i=0; i<Rotation<dim,ctype>::TangentVector::size; i++)
+ result[i+dim] = v[i];
+
+ return result;
+ }
+
// Translational part
Dune::FieldVector<ctype, dim> r;
--
GitLab