From b08d5b87ca7e954a16c115c8d31fff1efb78aa67 Mon Sep 17 00:00:00 2001
From: ZappoMan <bradh@konamoxt.com>
Date: Sun, 7 Dec 2014 01:28:06 -0800
Subject: [PATCH] implement proper ray picking against ellipsoids

Conflicts:
	libraries/entities/src/SphereEntityItem.cpp
---
 libraries/entities/src/SphereEntityItem.cpp | 28 +++++++++++++--------
 libraries/entities/src/SphereEntityItem.h   |  2 ++
 2 files changed, 19 insertions(+), 11 deletions(-)

diff --git a/libraries/entities/src/SphereEntityItem.cpp b/libraries/entities/src/SphereEntityItem.cpp
index 12fdd7a8c4..651d4db4ef 100644
--- a/libraries/entities/src/SphereEntityItem.cpp
+++ b/libraries/entities/src/SphereEntityItem.cpp
@@ -10,9 +10,12 @@
 //
 
 
+#include <glm/gtx/transform.hpp>
+
 #include <QDebug>
 
 #include <ByteCountCoding.h>
+#include <GeometryUtil.h>
 
 #include "EntityTree.h"
 #include "EntityTreeElement.h"
@@ -97,18 +100,21 @@ void SphereEntityItem::recalculateCollisionShape() {
 bool SphereEntityItem::findDetailedRayIntersection(const glm::vec3& origin, const glm::vec3& direction,
                      bool& keepSearching, OctreeElement*& element, float& distance, BoxFace& face, 
                      void** intersectedObject) const {
-                     
-    // NOTE: origin and direction are in tree units. But our _sphereShape is in meters, so we need to
-    // do a little math to make these match each other.
-    RayIntersectionInfo rayInfo;
-    rayInfo._rayStart = origin * (float)TREE_SCALE;
-    rayInfo._rayDirection = direction;
+    // determine the ray in the frame of the entity transformed from a unit sphere
+    glm::mat4 translation = glm::translate(getPosition());
+    glm::mat4 rotation = glm::mat4_cast(getRotation());
+    glm::mat4 scale = glm::scale(getDimensions());
+    glm::mat4 registration = glm::translate(glm::vec3(0.5f,0.5f,0.5f) - getRegistrationPoint());
+    glm::mat4 entityToWorldMatrix = translation * rotation * scale * registration;
+    glm::mat4 worldToEntityMatrix = glm::inverse(entityToWorldMatrix);
+    glm::vec3 entityFrameOrigin = glm::vec3(worldToEntityMatrix * glm::vec4(origin, 1.0f));
+    glm::vec3 entityFrameDirection = glm::normalize(glm::vec3(worldToEntityMatrix * glm::vec4(direction, 1.0f)));
 
-    // TODO: Note this is really doing ray intersections against a sphere, which is fine except in cases
-    // where our dimensions actually make us an ellipsoid. But we'll live with this for now until we
-    // get a more full fledged physics library
-    if (_sphereShape.findRayIntersection(rayInfo)) {
-        distance = rayInfo._hitDistance / (float)TREE_SCALE;
+    float localDistance;
+    if (findRaySphereIntersection(entityFrameOrigin, entityFrameDirection, glm::vec3(0.0f), 0.5f, localDistance)) {
+        glm::vec3 entityFrameHitAt = entityFrameOrigin + (entityFrameDirection * localDistance);
+        glm::vec3 hitAt = glm::vec3(entityToWorldMatrix * glm::vec4(entityFrameHitAt, 1.0f));
+        distance = glm::distance(origin,hitAt);
         return true;
     }
     return false;                
diff --git a/libraries/entities/src/SphereEntityItem.h b/libraries/entities/src/SphereEntityItem.h
index bb4f41726c..80b026e31d 100644
--- a/libraries/entities/src/SphereEntityItem.h
+++ b/libraries/entities/src/SphereEntityItem.h
@@ -61,6 +61,8 @@ public:
                          bool& keepSearching, OctreeElement*& element, float& distance, BoxFace& face, 
                          void** intersectedObject) const;
     
+    bool isCircle() const { return false && _dimensions.x == _dimensions.y && _dimensions.y == _dimensions.z; }
+    
 protected:
     virtual void recalculateCollisionShape();