move AABox back to octree lib

Fleshing out ShapeCollider::collideShapeWithAACube()

libraries/octree/src/Octree.cpp

@@ -710,16 +710,16 @@ bool findShapeCollisionsOp(OctreeElement* node, void* extraData) {
     const ShapeArgs* args = static_cast<ShapeArgs*>(extraData);
 
     // coarse check against bounds
-    AABox box = node->getAABox();
-    box.scale(TREE_SCALE);
-    if (!box.expandedContains(args->shape->getPosition(), args->shape->getBoundingRadius())) {
+    AABox cube = node->getAABox();
+    cube.scale(TREE_SCALE);
+    if (!cube.expandedContains(args->shape->getPosition(), args->shape->getBoundingRadius())) {
         return false;
     }
     if (!node->isLeaf()) {
         return true; // recurse on children
     }
     if (node->hasContent()) {
-        return ShapeCollider::collideShapeWithBox(args->shape, box, args->collisions);
+        return ShapeCollider::collideShapeWithAACube(args->shape, cube.calcCenter(), cube.getScale(), args->collisions);
     }
     return false;
 }

libraries/octree/src/OctreeElement.cpp

@@ -15,16 +15,16 @@
 
 #include <QtCore/QDebug>
 
-#include <AABox.h>
 #include <NodeList.h>
 #include <PerfStat.h>
 #include <assert.h>
 
+#include "AABox.h"
 #include "OctalCode.h"
-#include "SharedUtil.h"
 #include "OctreeConstants.h"
 #include "OctreeElement.h"
 #include "Octree.h"
+#include "SharedUtil.h"
 
 quint64 OctreeElement::_voxelMemoryUsage = 0;
 quint64 OctreeElement::_octcodeMemoryUsage = 0;

libraries/octree/src/OctreeElement.h

@@ -18,9 +18,9 @@
 
 #include <QReadWriteLock>
 
-#include <AABox.h>
 #include <SharedUtil.h>
 
+#include "AABox.h"
 #include "ViewFrustum.h"
 #include "OctreeConstants.h"
 //#include "Octree.h"

libraries/octree/src/ViewFrustum.h

@@ -17,9 +17,8 @@
 #include <glm/glm.hpp>
 #include <glm/gtc/quaternion.hpp>
 
-#include <AABox.h>
+#include "AABox.h"
 #include "Plane.h"
-
 #include "OctreeConstants.h"
 #include "OctreeProjectedPolygon.h"
 

libraries/shared/src/ShapeCollider.cpp

@@ -92,8 +92,26 @@ bool collideShapesCoarse(const QVector<const Shape*>& shapesA, const QVector<con
     return false;
 }
 
-bool collideShapeWithBox(const Shape* shapeA, const AABox& boxB, CollisionList& collisions) {
-    // TODO: Andrew to implement this
+bool collideShapeWithAACube(const Shape* shapeA, const glm::vec3& cubeCenter, float cubeSide, CollisionList& collisions) {
+    int typeA = shapeA->getType();
+    if (typeA == Shape::SPHERE_SHAPE) {
+        return sphereAACube(static_cast<const SphereShape*>(shapeA), cubeCenter, cubeSide, collisions);
+    } else if (typeA == Shape::CAPSULE_SHAPE) {
+        return capsuleAACube(static_cast<const CapsuleShape*>(shapeA), cubeCenter, cubeSide, collisions);
+    } else if (typeA == Shape::LIST_SHAPE) {
+        const ListShape* listA = static_cast<const ListShape*>(shapeA);
+        bool touching = false;
+        for (int i = 0; i < listA->size() && !collisions.isFull(); ++i) {
+            const Shape* subShape = listA->getSubShape(i);
+            int subType = subShape->getType();
+            if (subType == Shape::SPHERE_SHAPE) {
+                touching = sphereAACube(static_cast<const SphereShape*>(subShape), cubeCenter, cubeSide, collisions) || touching;
+            } else if (subType == Shape::CAPSULE_SHAPE) {
+                touching = capsuleAACube(static_cast<const CapsuleShape*>(subShape), cubeCenter, cubeSide, collisions) || touching;
+            }
+        }
+        return touching;
+    }
     return false;
 }
 
@@ -572,4 +590,62 @@ bool listList(const ListShape* listA, const ListShape* listB, CollisionList& col
     return touching;
 }
 
+// helper function
+bool sphereAACube(const glm::vec3& sphereCenter, float sphereRadius, const glm::vec3& cubeCenter, float cubeSide, CollisionList& collisions) {
+    glm::vec3 BA = cubeCenter - sphereCenter;
+    float distance = glm::length(BA);
+    if (distance > EPSILON) {
+        BA /= distance; // BA is now normalized
+        // compute the nearest point on sphere
+        glm::vec3 surfaceA = sphereCenter + sphereRadius * BA;
+        // compute the nearest point on cube
+        float maxBA = glm::max(glm::max(fabs(BA.x), fabs(BA.y)), fabs(BA.z));
+        glm::vec3 surfaceB = cubeCenter - (0.5f * cubeSide / maxBA) * BA;
+        // collision happens when "vector to surfaceB from surfaceA" dots with BA to produce a positive value
+        glm::vec3 surfaceBA = surfaceB - surfaceA;
+        if (glm::dot(surfaceBA, BA) > 0.f) {
+            CollisionInfo* collision = collisions.getNewCollision();
+            if (collision) {
+                collision->_penetration = surfaceBA;
+                // contactPoint is on surface of A
+                collision->_contactPoint = surfaceA;
+                return true;
+            }
+        }
+    } else if (sphereRadius + 0.5f * cubeSide > distance) {
+        // NOTE: for cocentric approximation we collide sphere and cube as two spheres which means 
+        // this algorithm will probably be wrong when both sphere and cube are very small (both ~EPSILON)
+        CollisionInfo* collision = collisions.getNewCollision();
+        if (collision) {
+            // the penetration and contactPoint are undefined, so we pick a penetration direction (-yAxis)
+            collision->_penetration = (sphereRadius + 0.5f * cubeSide) * glm::vec3(0.0f, -1.0f, 0.0f);
+            // contactPoint is on surface of A
+            collision->_contactPoint = sphereCenter + collision->_penetration;
+            return true;
+        }
+    }
+    return false;
+}
+
+bool sphereAACube(const SphereShape* sphereA, const glm::vec3& cubeCenter, float cubeSide, CollisionList& collisions) {
+    return sphereAACube(sphereA->getPosition(), sphereA->getRadius(), cubeCenter, cubeSide, collisions);
+}
+
+bool capsuleAACube(const CapsuleShape* capsuleA, const glm::vec3& cubeCenter, float cubeSide, CollisionList& collisions) {
+    // find nerest approach of capsule line segment to cube
+    glm::vec3 capsuleAxis;
+    capsuleA->computeNormalizedAxis(capsuleAxis);
+    float offset = glm::dot(cubeCenter - capsuleA->getPosition(), capsuleAxis);
+    float halfHeight = capsuleA->getHalfHeight();
+    if (offset > halfHeight) {
+        offset = halfHeight;
+    } else if (offset < -halfHeight) {
+        offset = -halfHeight;
+    }
+    glm::vec3 nearestApproach = capsuleA->getPosition() + offset * capsuleAxis;
+    // collide nearest approach like a sphere at that point
+    return sphereAACube(nearestApproach, capsuleA->getRadius(), cubeCenter, cubeSide, collisions);
+}
+
+
 }   // namespace ShapeCollider

libraries/shared/src/ShapeCollider.h

@@ -12,7 +12,6 @@
 #ifndef hifi_ShapeCollider_h
 #define hifi_ShapeCollider_h
 
-#include "AABox.h"
 #include "CapsuleShape.h"
 #include "CollisionInfo.h"
 #include "ListShape.h"
@@ -35,10 +34,11 @@ namespace ShapeCollider {
     bool collideShapesCoarse(const QVector<const Shape*>& shapesA, const QVector<const Shape*>& shapesB, CollisionInfo& collision);
 
     /// \param shapeA a pointer to a shape
-    /// \param boxB an axis aligned box
+    /// \param cubeCenter center of cube
+    /// \param cubeSide lenght of side of cube
     /// \param collisions[out] average collision details
-    /// \return true if shapeA collides with boxB
-    bool collideShapeWithBox(const Shape* shapeA, const AABox& boxB, CollisionList& collisions);
+    /// \return true if shapeA collides with axis aligned cube
+    bool collideShapeWithAACube(const Shape* shapeA, const glm::vec3& cubeCenter, float cubeSide, CollisionList& collisions);
 
     /// \param sphereA pointer to first shape
     /// \param sphereB pointer to second shape
@@ -136,6 +136,20 @@ namespace ShapeCollider {
     /// \return true if shapes collide
     bool listList(const ListShape* listA, const ListShape* listB, CollisionList& collisions);
 
+    /// \param sphereA pointer to sphere
+    /// \param cubeCenter center of cube
+    /// \param cubeSide lenght of side of cube
+    /// \param[out] collisions where to append collision details
+    /// \return true if sphereA collides with axis aligned cube
+    bool sphereAACube(const SphereShape* sphereA, const glm::vec3& cubeCenter, float cubeSide, CollisionList& collisions);
+
+    /// \param capsuleA pointer to capsule
+    /// \param cubeCenter center of cube
+    /// \param cubeSide lenght of side of cube
+    /// \param[out] collisions where to append collision details
+    /// \return true if capsuleA collides with axis aligned cube
+    bool capsuleAACube(const CapsuleShape* capsuleA, const glm::vec3& cubeCenter, float cubeSide, CollisionList& collisions);
+
 }   // namespace ShapeCollider
 
 #endif // hifi_ShapeCollider_h