From 427595d9cb05673b9c1b5b83e0c86259a36c81f5 Mon Sep 17 00:00:00 2001
From: Oliver Sander <sander@igpm.rwth-aachen.de>
Date: Sun, 9 Jan 2011 16:35:10 +0000
Subject: [PATCH] add method 'distance'
[[Imported from SVN: r6705]]
---
dune/gfe/rigidbodymotion.hh | 10 ++++++++++
1 file changed, 10 insertions(+)
diff --git a/dune/gfe/rigidbodymotion.hh b/dune/gfe/rigidbodymotion.hh
index 124347ed..f7ce7f78 100644
--- a/dune/gfe/rigidbodymotion.hh
+++ b/dune/gfe/rigidbodymotion.hh
@@ -38,6 +38,16 @@ struct RigidBodyMotion
return result;
}
+ /** \brief Compute geodesic distance from a to b */
+ static T distance(const RigidBodyMotion<dim,ctype>& a, const RigidBodyMotion<dim,ctype>& b) {
+
+ T euclideanDistanceSquared = (a.r - b.r).two_norm2();
+
+ T rotationDistance = Rotation<dim,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) {
--
GitLab