From 9136f4c44e4b9e8dc16d965e60cad990422336cf Mon Sep 17 00:00:00 2001
From: Oliver Sander <sander@igpm.rwth-aachen.de>
Date: Fri, 14 Jan 2011 09:35:11 +0000
Subject: [PATCH] implement derivativeOfDistanceSquaredWRTSecondArgument
[[Imported from SVN: r6738]]
---
dune/gfe/rotation.hh | 28 +++++++++++++++++++++++++---
1 file changed, 25 insertions(+), 3 deletions(-)
diff --git a/dune/gfe/rotation.hh b/dune/gfe/rotation.hh
index b141a84d..ebc0b1d5 100644
--- a/dune/gfe/rotation.hh
+++ b/dune/gfe/rotation.hh
@@ -128,6 +128,11 @@ class Rotation<3,T> : public Quaternion<T>
return (x < 1e-4) ? 0.5 + (x*x/48) : std::sin(x/2)/x;
}
+ /** \brief Derivative of the inverse cosine */
+ static T arccosDer(const T& x) {
+ return -1 / std::sqrt(1-x*x);
+ }
+
public:
/** \brief The type used for coordinates */
@@ -392,9 +397,26 @@ public:
return v;
}
- static EmbeddedTangentVector derivativeOfDistanceSquaredWRTSecondArgument(const Rotation<3,T>& a,
- const Rotation<3,T>& b) {
- DUNE_THROW(Dune::NotImplemented, "derivativeOfDistanceSquaredWRTSecondArgument");
+ static EmbeddedTangentVector derivativeOfDistanceSquaredWRTSecondArgument(const Rotation<3,T>& p,
+ const Rotation<3,T>& q) {
+
+ T dist = distance(p,q);
+
+ // unefficient: parts of the following have already been computed while computing the distance
+ Rotation<3,T> pInv = p;
+ pInv.invert();
+
+ // the forth component of pInv times q
+ double pInvq_4 = - pInv[0]*q[0] - pInv[1]*q[1] - pInv[2]*q[2] + pInv[3]*q[3];
+
+ double arccosDer_pInvq_4 = arccosDer(pInvq_4);
+
+ EmbeddedTangentVector result;
+ result[0] = 2 * dist * (-2 * arccosDer_pInvq_4 * pInv[0]);
+ result[1] = 2 * dist * (-2 * arccosDer_pInvq_4 * pInv[1]);
+ result[2] = 2 * dist * (-2 * arccosDer_pInvq_4 * pInv[2]);
+ result[3] = 2 * dist * ( 2 * arccosDer_pInvq_4 * pInv[3]);
+
}
/** \brief Interpolate between two rotations */
--
GitLab