From 00c8673cebeb654d210c3291a2c43a10b81c4eec Mon Sep 17 00:00:00 2001
From: Philip Rosedale <philip@coffeeandpower.com>
Date: Mon, 6 May 2013 18:45:06 -0700
Subject: [PATCH] Added Utility function angleBetween() which returns the
 positive angle between any two 3D vectors.

---
 interface/src/Avatar.cpp | 8 ++++++++
 interface/src/Util.cpp   | 5 +++++
 interface/src/Util.h     | 2 ++
 3 files changed, 15 insertions(+)

diff --git a/interface/src/Avatar.cpp b/interface/src/Avatar.cpp
index a1a93fbfe3..70e3907b4a 100644
--- a/interface/src/Avatar.cpp
+++ b/interface/src/Avatar.cpp
@@ -469,6 +469,14 @@ void Avatar::updateHandMovementAndTouching(float deltaTime) {
                 glm::vec3 v(_joint[ AVATAR_JOINT_RIGHT_SHOULDER ].position);
                 v -= otherAvatar->getJointPosition(AVATAR_JOINT_RIGHT_SHOULDER);
                 
+                /*
+                //  Test:  Show angle between your fwd vector and nearest avatar
+                glm::vec3 vectorBetweenUs = otherAvatar->getJointPosition(AVATAR_JOINT_PELVIS) -
+                                getJointPosition(AVATAR_JOINT_PELVIS);
+                glm::vec3 myForwardVector = _orientation.getFront();
+                printLog("Angle between: %f\n", angleBetween(&vectorBetweenUs, &myForwardVector));
+                */
+                
                 float distance = glm::length(v);
                 if (distance < _distanceToNearestAvatar) {_distanceToNearestAvatar = distance;}
                 
diff --git a/interface/src/Util.cpp b/interface/src/Util.cpp
index 355dec185c..430d200af1 100644
--- a/interface/src/Util.cpp
+++ b/interface/src/Util.cpp
@@ -65,6 +65,11 @@ float angle_to(glm::vec3 head_pos, glm::vec3 source_pos, float render_yaw, float
     return atan2(head_pos.x - source_pos.x, head_pos.z - source_pos.z) * 180.0f / PIf + render_yaw + head_yaw;
 }
 
+//  Helper function computes the angle between two 3D vectors 
+float angleBetween(glm::vec3 * v1, glm::vec3 * v2) {
+    return acos((glm::dot(*v1,*v2))/(glm::length(*v1) * glm::length(*v2))) * 180.f/PI;
+}
+
 //  Draw a 3D vector floating in space
 void drawVector(glm::vec3 * vector)
 {
diff --git a/interface/src/Util.h b/interface/src/Util.h
index 7ea65bef4c..add347fb3b 100644
--- a/interface/src/Util.h
+++ b/interface/src/Util.h
@@ -42,6 +42,8 @@ void drawvec3(int x, int y, float scale, float rotate, float thick, int mono, gl
 
 void drawVector(glm::vec3* vector);
 
+float angleBetween(glm::vec3 * v1, glm::vec3 * v2); 
+
 double diffclock(timeval *clock1,timeval *clock2);
 
 void drawGroundPlaneGrid(float size);