From a37921c1d5304194d8a41836965d0072b0786260 Mon Sep 17 00:00:00 2001
From: ZappoMan <bradh@konamoxt.com>
Date: Sun, 13 Apr 2014 19:09:07 -0700
Subject: [PATCH] move isNaN() to shared utils

---
 libraries/octree/src/ViewFrustum.cpp |  6 +-----
 libraries/shared/src/SharedUtil.cpp  | 19 +++++++++++++++++++
 libraries/shared/src/SharedUtil.h    |  9 +++++++++
 3 files changed, 29 insertions(+), 5 deletions(-)

diff --git a/libraries/octree/src/ViewFrustum.cpp b/libraries/octree/src/ViewFrustum.cpp
index fa6873b093..bc617c9bc7 100644
--- a/libraries/octree/src/ViewFrustum.cpp
+++ b/libraries/octree/src/ViewFrustum.cpp
@@ -424,10 +424,6 @@ bool ViewFrustum::matches(const ViewFrustum& compareTo, bool debug) const {
     return result;
 }
 
-bool isNaN(float f) {
-    return f != f;
-}
-
 bool ViewFrustum::isVerySimilar(const ViewFrustum& compareTo, bool debug) const {
 
     //  Compute distance between the two positions
@@ -449,7 +445,7 @@ bool ViewFrustum::isVerySimilar(const ViewFrustum& compareTo, bool debug) const
     float angleEyeOffsetOrientation = compareTo._eyeOffsetOrientation == _eyeOffsetOrientation
                                             ? 0.0f : glm::degrees(glm::angle(dQEyeOffsetOrientation));
     if (isNaN(angleEyeOffsetOrientation)) {
-        angleOrientation = 0.0f;
+        angleEyeOffsetOrientation = 0.0f;
     }
 
     bool result =
diff --git a/libraries/shared/src/SharedUtil.cpp b/libraries/shared/src/SharedUtil.cpp
index efd5180d03..786302ae2a 100644
--- a/libraries/shared/src/SharedUtil.cpp
+++ b/libraries/shared/src/SharedUtil.cpp
@@ -661,3 +661,22 @@ glm::vec3 safeEulerAngles(const glm::quat& q) {
     }
 }
 
+bool isNaN(float f) { 
+    return f != f; 
+}
+
+
+bool isSimilarOrientation(const glm::quat& orientionA, const glm::quat& orientionB, float similarEnough) {
+    // Compute the angular distance between the two orientations
+    float angleOrientation = orientionA == orientionB ? 0.0f : glm::degrees(glm::angle(orientionA * glm::inverse(orientionB)));
+    if (isNaN(angleOrientation)) {
+        angleOrientation = 0.0f;
+    }
+    return (angleOrientation <= similarEnough);
+}
+
+bool isSimilarPosition(const glm::vec3& positionA, const glm::vec3& positionB, float similarEnough) {
+    // Compute the distance between the two points
+    float positionDistance = glm::distance(positionA, positionB);
+    return (positionDistance <= similarEnough);
+}
diff --git a/libraries/shared/src/SharedUtil.h b/libraries/shared/src/SharedUtil.h
index d8d686c63b..91b0054794 100644
--- a/libraries/shared/src/SharedUtil.h
+++ b/libraries/shared/src/SharedUtil.h
@@ -168,4 +168,13 @@ int unpackFloatVec3FromSignedTwoByteFixed(const unsigned char* sourceBuffer, glm
 /// \return vec3 with euler angles in radians
 glm::vec3 safeEulerAngles(const glm::quat& q);
 
+/// \return bool are two orientations similar to each other
+const float ORIENTATION_SIMILAR_ENOUGH = 5.0f; // 10 degrees in any direction
+bool isSimilarOrientation(const glm::quat& orientionA, const glm::quat& orientionB, 
+                        float similarEnough = ORIENTATION_SIMILAR_ENOUGH);
+const float POSITION_SIMILAR_ENOUGH = 0.1f; // 0.1 meter
+bool isSimilarPosition(const glm::vec3& positionA, const glm::vec3& positionB, float similarEnough = POSITION_SIMILAR_ENOUGH);
+                        
+bool isNaN(float f);
+
 #endif /* defined(__hifi__SharedUtil__) */