From 7723f10e957039019eb5cd27d4a8a5dc60e79c66 Mon Sep 17 00:00:00 2001
From: Oliver Sander <sander@igpm.rwth-aachen.de>
Date: Wed, 21 Jun 2006 15:17:09 +0000
Subject: [PATCH] stable exponential mapping due to grassia
[[Imported from SVN: r853]]
---
src/quaternion.hh | 7 ++++---
1 file changed, 4 insertions(+), 3 deletions(-)
diff --git a/src/quaternion.hh b/src/quaternion.hh
index aaf0edf9..879fd520 100644
--- a/src/quaternion.hh
+++ b/src/quaternion.hh
@@ -28,11 +28,12 @@ public:
Quaternion<T> q;
T normV = std::sqrt(v0*v0 + v1*v1 + v2*v2);
- T sin = std::sin(normV/2)/normV;
+
+ // Stabilization for small |v| due Grassia
+ T sin = (normV < 1e-4) ? 0.5 * (normV*normV/48) : std::sin(normV/2)/normV;
// if normV == 0 then q = (0,0,0,1)
- if (isnan(sin))
- sin = 0;
+ assert(!isnan(sin));
q[0] = sin * v0;
q[1] = sin * v1;
--
GitLab