From ce6f99990430a16a6fae6ea85f572a72e03f38eb Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Vladim=C3=ADr=20Vondru=C5=A1?= <mosra@centrum.cz>
Date: Sun, 17 Mar 2013 11:42:01 +0100
Subject: [PATCH] Math: better isNormalized() implementation.

Comparing squared length to 1 is not sufficient to compare within range
[1 - epsilon, 1 + epsilon], as e.g. Quaternion with dot() = 1 + 1e-7
when converted to matrix has column vectors with dot() = 1 + 1e-6, which
is just above 1 + epsilon. Thus it's needed to compare sqrt(dot()) in
range [1 - epsilon, 1 + epsilon] or dot() in range [1 - 2*epsilon +
epsilon^2, 1 + 2*epsilon + epsilon^2]. Because epsilon^2 is way off
machine precision, it's omitted, thus dot() in all isNormalized()
implementations is now compared this way:

    abs(dot() - 1) < 2*epsilon;
---
 src/Math/Complex.h        |  2 +-
 src/Math/DualComplex.h    |  2 +-
 src/Math/DualQuaternion.h |  6 +++++-
 src/Math/Quaternion.h     |  2 +-
 src/Math/TypeTraits.h     | 13 +++++++++++++
 src/Math/Vector.h         |  2 +-
 6 files changed, 22 insertions(+), 5 deletions(-)

diff --git a/src/Math/Complex.h b/src/Math/Complex.h
index 766d577cb..2a2cb2349 100644
--- a/src/Math/Complex.h
+++ b/src/Math/Complex.h
@@ -156,7 +156,7 @@ template<class T> class Complex {
          * @see dot(), normalized()
          */
         inline bool isNormalized() const {
-            return TypeTraits<T>::equals(dot(), T(1));
+            return Implementation::isNormalizedSquared(dot());
         }
 
         /** @brief Real part */
diff --git a/src/Math/DualComplex.h b/src/Math/DualComplex.h
index d008b78e0..648f9f6bc 100644
--- a/src/Math/DualComplex.h
+++ b/src/Math/DualComplex.h
@@ -135,7 +135,7 @@ template<class T> class DualComplex: public Dual<Complex<T>> {
          * @see Complex::dot(), normalized()
          */
         inline bool isNormalized() const {
-            return TypeTraits<T>::equals(this->real().dot(), T(1));
+            return Implementation::isNormalizedSquared(lengthSquared());
         }
 
         /**
diff --git a/src/Math/DualQuaternion.h b/src/Math/DualQuaternion.h
index c3e8002cc..8d4837aac 100644
--- a/src/Math/DualQuaternion.h
+++ b/src/Math/DualQuaternion.h
@@ -141,7 +141,11 @@ template<class T> class DualQuaternion: public Dual<Quaternion<T>> {
          * @see lengthSquared(), normalized()
          */
         inline bool isNormalized() const {
-            return lengthSquared() == Dual<T>(1);
+            /* Comparing dual part classically, as comparing sqrt() of it would
+               lead to overly strict precision */
+            Dual<T> a = lengthSquared();
+            return Implementation::isNormalizedSquared(a.real()) &&
+                   TypeTraits<T>::equals(a.dual(), T(0));
         }
 
         /**
diff --git a/src/Math/Quaternion.h b/src/Math/Quaternion.h
index 0e3f77503..0cc1ac8ad 100644
--- a/src/Math/Quaternion.h
+++ b/src/Math/Quaternion.h
@@ -232,7 +232,7 @@ template<class T> class Quaternion {
          * @see dot(), normalized()
          */
         inline bool isNormalized() const {
-            return TypeTraits<T>::equals(dot(), T(1));
+            return Implementation::isNormalizedSquared(dot());
         }
 
         /** @brief %Vector part */
diff --git a/src/Math/TypeTraits.h b/src/Math/TypeTraits.h
index 8897f33c3..addcb4206 100644
--- a/src/Math/TypeTraits.h
+++ b/src/Math/TypeTraits.h
@@ -174,6 +174,19 @@ template<> struct TypeTraits<long double>: Implementation::TypeTraitsFloatingPoi
 
     inline constexpr static long double epsilon() { return LONG_DOUBLE_EQUALITY_PRECISION; }
 };
+
+/* Comparing squared length to 1 is not sufficient to compare within range
+   [1 - epsilon, 1 + epsilon], as e.g. Quaternion with dot() = 1 + 1e-7 when
+   converted to matrix has column vectors with dot() = 1 + 1e-6, which is just
+   above 1 + epsilon. Thus it's needed to compare sqrt(dot()) in range
+   [1 - epsilon, 1 + epsilon] or dot() in range [1 - 2*epsilon + epsilon^2,
+   1 + 2*epsilon + epsilon^2]. Because epsilon^2 is way off machine precision,
+   it's omitted. */
+namespace Implementation {
+    template<class T> inline bool isNormalizedSquared(T lengthSquared) {
+        return std::abs(lengthSquared - T(1)) < T(2)*TypeTraits<T>::epsilon();
+    }
+}
 #endif
 
 }}
diff --git a/src/Math/Vector.h b/src/Math/Vector.h
index 1d4c197a9..5ef5b508a 100644
--- a/src/Math/Vector.h
+++ b/src/Math/Vector.h
@@ -254,7 +254,7 @@ template<std::size_t size, class T> class Vector {
          * @see dot(), normalized()
          */
         inline bool isNormalized() const {
-            return TypeTraits<T>::equals(dot(), T(1));
+            return Implementation::isNormalizedSquared(dot());
         }
 
         /**