From 246e46b69a8e6556aea29cdcaf4b678962f481a7 Mon Sep 17 00:00:00 2001
From: Brad Hefta-Gaub <brad@highfidelity.io>
Date: Wed, 21 Oct 2015 10:46:50 -0700
Subject: [PATCH] implement improvement in sphere test

---
 .../src/EntityItemPropertiesDefaults.h        |  4 +-
 libraries/entities/src/EntityTreeElement.cpp  | 44 ++++++++++++-------
 2 files changed, 31 insertions(+), 17 deletions(-)

diff --git a/libraries/entities/src/EntityItemPropertiesDefaults.h b/libraries/entities/src/EntityItemPropertiesDefaults.h
index 538d7dd890..06c6565d1b 100644
--- a/libraries/entities/src/EntityItemPropertiesDefaults.h
+++ b/libraries/entities/src/EntityItemPropertiesDefaults.h
@@ -21,8 +21,8 @@
 // There is a minor performance gain when comparing/copying an existing glm::vec3 rather than 
 // creating a new one on the stack so we declare the ZERO_VEC3 constant as an optimization.
 const glm::vec3 ENTITY_ITEM_ZERO_VEC3 = glm::vec3(0.0f);
-const glm::vec3 ENTITY_ITEM_ONE_VEC3 = glm::vec3(1.0f, 1.0f, 1.0f);
-const glm::vec3 ENTITY_ITEM_HALF_VEC3 = ENTITY_ITEM_ONE_VEC3 / 2.0f;
+const glm::vec3 ENTITY_ITEM_ONE_VEC3 = glm::vec3(1.0f);
+const glm::vec3 ENTITY_ITEM_HALF_VEC3 = glm::vec3(0.5f);
 
 const bool ENTITY_ITEM_DEFAULT_LOCKED = false;
 const QString ENTITY_ITEM_DEFAULT_USER_DATA = QString("");
diff --git a/libraries/entities/src/EntityTreeElement.cpp b/libraries/entities/src/EntityTreeElement.cpp
index 3ea6cfdbe8..57f49f2354 100644
--- a/libraries/entities/src/EntityTreeElement.cpp
+++ b/libraries/entities/src/EntityTreeElement.cpp
@@ -648,7 +648,6 @@ EntityItemPointer EntityTreeElement::getClosestEntity(glm::vec3 position) const
 
 // TODO: change this to use better bounding shape for entity than sphere
 void EntityTreeElement::getEntities(const glm::vec3& searchPosition, float searchRadius, QVector<EntityItemPointer>& foundEntities) const {
-    float compareRadius = searchRadius * searchRadius;
     forEachEntity([&](EntityItemPointer entity) {
 
         AABox entityBox = entity->getAABox();
@@ -657,26 +656,41 @@ void EntityTreeElement::getEntities(const glm::vec3& searchPosition, float searc
         glm::vec3 penetration;
         if (entityBox.findSpherePenetration(searchPosition, searchRadius, penetration)) {
 
-            // FIXME - handle entity->getShapeType() == SHAPE_TYPE_SPHERE case better
+            glm::vec3 dimensions = entity->getDimensions();
+
             // FIXME - consider allowing the entity to determine penetration so that
             //         entities could presumably dull actuall hull testing if they wanted to
+            // FIXME - handle entity->getShapeType() == SHAPE_TYPE_SPHERE case better in particular
+            //         can we handle the ellipsoid case better? We only currently handle perfect spheres
+            //         with centered registration points
+            if (entity->getShapeType() == SHAPE_TYPE_SPHERE &&
+                (dimensions.x == dimensions.y && dimensions.y == dimensions.z)) {
 
-            // determine the worldToEntityMatrix that doesn't include scale because
-            // we're going to use the registration aware aa box in the entity frame
-            glm::mat4 rotation = glm::mat4_cast(entity->getRotation());
-            glm::mat4 translation = glm::translate(entity->getPosition());
-            glm::mat4 entityToWorldMatrix = translation * rotation;
-            glm::mat4 worldToEntityMatrix = glm::inverse(entityToWorldMatrix);
+                // NOTE: entity->getRadius() doesn't return the true radius, it returns the radius of the
+                //       maximum bounding sphere, which is actually larger than our actual radius
+                float entityTrueRadius = dimensions.x / 2.0f;
 
-            glm::vec3 dimensions = entity->getDimensions();
-            glm::vec3 registrationPoint = entity->getRegistrationPoint();
-            glm::vec3 corner = -(dimensions * registrationPoint);
+                if (findSphereSpherePenetration(searchPosition, searchRadius, 
+                        entity->getCenterPosition(), entityTrueRadius, penetration)) {
+                    foundEntities.push_back(entity);
+                }
+            } else {
+                // determine the worldToEntityMatrix that doesn't include scale because
+                // we're going to use the registration aware aa box in the entity frame
+                glm::mat4 rotation = glm::mat4_cast(entity->getRotation());
+                glm::mat4 translation = glm::translate(entity->getPosition());
+                glm::mat4 entityToWorldMatrix = translation * rotation;
+                glm::mat4 worldToEntityMatrix = glm::inverse(entityToWorldMatrix);
 
-            AABox entityFrameBox(corner, dimensions);
+                glm::vec3 registrationPoint = entity->getRegistrationPoint();
+                glm::vec3 corner = -(dimensions * registrationPoint);
 
-            glm::vec3 entityFrameSearchPosition = glm::vec3(worldToEntityMatrix * glm::vec4(searchPosition, 1.0f));
-            if (entityFrameBox.findSpherePenetration(entityFrameSearchPosition, searchRadius, penetration)) {
-                foundEntities.push_back(entity);
+                AABox entityFrameBox(corner, dimensions);
+
+                glm::vec3 entityFrameSearchPosition = glm::vec3(worldToEntityMatrix * glm::vec4(searchPosition, 1.0f));
+                if (entityFrameBox.findSpherePenetration(entityFrameSearchPosition, searchRadius, penetration)) {
+                    foundEntities.push_back(entity);
+                }
             }
         }
     });