Merge pull request #437 from ey6es/master

Sphere/box and capsule/box collisions for voxels; allows us to slide along and stand on voxels.  Also a couple build fixes.

interface/src/Application.cpp

@@ -901,7 +901,7 @@ void Application::idle() {
         for(AgentList::iterator agent = agentList->begin(); agent != agentList->end(); agent++) {
             if (agent->getLinkedData() != NULL) {
                 Avatar *avatar = (Avatar *)agent->getLinkedData();
-                avatar->simulate(deltaTime, false);
+                avatar->simulate(deltaTime, NULL);
                 avatar->setMouseRay(mouseRayOrigin, mouseRayDirection);
             }
         }
@@ -1669,7 +1669,6 @@ void Application::displaySide(Camera& whichCamera) {
     glPopMatrix();
 
     //draw a grid ground plane....
-    const float EDGE_SIZE_GROUND_PLANE = 20.f;
     drawGroundPlaneGrid(EDGE_SIZE_GROUND_PLANE);
     
     //  Draw voxels

interface/src/Avatar.cpp

@@ -640,11 +640,14 @@ void Avatar::updateCollisionWithVoxels() {
 }
 
 void Avatar::applyCollisionWithScene(const glm::vec3& penetration) {
-    _position += penetration;
+    _position -= penetration;
         
     // reflect the velocity component in the direction of penetration
-    glm::vec3 direction = glm::normalize(penetration);
-    _velocity -= 2.0f * glm::dot(_velocity, direction) * direction * BOUNCE;
+    float penetrationLength = glm::length(penetration);
+    if (penetrationLength > EPSILON) {
+        glm::vec3 direction = penetration / penetrationLength;
+        _velocity -= 2.0f * glm::dot(_velocity, direction) * direction * BOUNCE;
+    }
 }
 
 void Avatar::updateAvatarCollisions(float deltaTime) {

interface/src/Environment.cpp

@@ -60,8 +60,8 @@ void Environment::renderAtmospheres(Camera& camera) {
 glm::vec3 Environment::getGravity (const glm::vec3& position) {
     // the "original gravity"
     glm::vec3 gravity;
-    if (position.x > 0.0f && position.x < 10.0f && position.y > 0.0f &&
-            position.y < 3.0f && position.z > 0.0f && position.z < 10.0f) {
+    if (position.x > 0.0f && position.x < EDGE_SIZE_GROUND_PLANE && position.y > 0.0f &&
+            position.y < 3.0f && position.z > 0.0f && position.z < EDGE_SIZE_GROUND_PLANE) {
         gravity = glm::vec3(0.0f, -1.0f, 0.0f);
     }
     
@@ -101,13 +101,7 @@ const EnvironmentData Environment::getClosestData(const glm::vec3& position) {
 bool Environment::findCapsulePenetration(const glm::vec3& start, const glm::vec3& end,
                                          float radius, glm::vec3& penetration) {
     // collide with the "floor"
-    bool found = false;
-    penetration = glm::vec3(0.0f, 0.0f, 0.0f);
-    float floorDist = qMin(start.y, end.y) - radius;
-    if (floorDist < 0.0f) {
-        penetration.y = -floorDist;
-        found = true;
-    }
+    bool found = findCapsulePlanePenetration(start, end, radius, glm::vec4(0.0f, 1.0f, 0.0f, 0.0f), penetration);
     
     // get the lock for the duration of the call
     QMutexLocker locker(&_mutex);
@@ -117,11 +111,10 @@ bool Environment::findCapsulePenetration(const glm::vec3& start, const glm::vec3
             if (environmentData.getGravity() == 0.0f) {
                 continue; // don't bother colliding with gravity-less environments
             }
-            glm::vec3 vector = computeVectorFromPointToSegment(environmentData.getAtmosphereCenter(), start, end);
-            float vectorLength = glm::length(vector);
-            float distance = vectorLength - environmentData.getAtmosphereInnerRadius() - radius;
-            if (distance < 0.0f) {
-                penetration += vector * (-distance / vectorLength);
+            glm::vec3 environmentPenetration;
+            if (findCapsuleSpherePenetration(start, end, radius, environmentData.getAtmosphereCenter(),
+                    environmentData.getAtmosphereInnerRadius(), environmentPenetration)) {
+                penetration = addPenetrations(penetration, environmentPenetration);
                 found = true;
             }
         }

interface/src/OculusManager.cpp

@@ -38,9 +38,11 @@ void OculusManager::connect() {
 }
 
 void OculusManager::updateYawOffset() {
+#ifdef __APPLE__
     float yaw, pitch, roll;
    _sensorFusion.GetOrientation().GetEulerAngles<Axis_Y, Axis_X, Axis_Z, Rotate_CCW, Handed_R>(&yaw, &pitch, &roll);
     _yawOffset = yaw;
+#endif
 }
 
 void OculusManager::getEulerAngles(float& yaw, float& pitch, float& roll) {

interface/src/world.h

@@ -17,4 +17,6 @@ const float WORLD_SIZE = 10.0;
 #define PIf 3.14159265f
 #define GRAVITY_EARTH 9.80665f;
 
+const float EDGE_SIZE_GROUND_PLANE = 20.f;
+
 #endif

libraries/voxels/src/AABox.cpp

@@ -11,6 +11,7 @@
 #include "SharedUtil.h"
 
 #include "AABox.h"
+#include "GeometryUtil.h"
 
 
 void AABox::scale(float scale) {
@@ -82,6 +83,17 @@ bool AABox::contains(const glm::vec3& point) const {
         isWithin(point.z, _corner.z, _size.z);
 }
 
+// determines whether a value is within the expanded extents
+static bool isWithinExpanded(float value, float corner, float size, float expansion) {
+    return value >= corner - expansion && value <= corner + size + expansion;
+}
+
+bool AABox::expandedContains(const glm::vec3& point, float expansion) const {
+    return isWithinExpanded(point.x, _corner.x, _size.x, expansion) &&
+        isWithinExpanded(point.y, _corner.y, _size.y, expansion) &&
+        isWithinExpanded(point.z, _corner.z, _size.z, expansion);
+}
+
 // finds the intersection between a ray and the facing plane on one axis
 static bool findIntersection(float origin, float direction, float corner, float size, float& distance) {
     if (direction > EPSILON) {
@@ -95,6 +107,30 @@ static bool findIntersection(float origin, float direction, float corner, float
     return false;
 }
 
+bool AABox::expandedIntersectsSegment(const glm::vec3& start, const glm::vec3& end, float expansion) const {
+    // handle the trivial cases where the expanded box contains the start or end
+    if (expandedContains(start, expansion) || expandedContains(end, expansion)) {
+        return true;
+    }
+    // check each axis
+    glm::vec3 expandedCorner = _corner - glm::vec3(expansion, expansion, expansion);
+    glm::vec3 expandedSize = _size + glm::vec3(expansion, expansion, expansion) * 2.0f;
+    glm::vec3 direction = end - start;
+    float axisDistance;
+    return (findIntersection(start.x, direction.x, expandedCorner.x, expandedSize.x, axisDistance) &&
+                axisDistance >= 0.0f && axisDistance <= 1.0f &&
+                isWithin(start.y + axisDistance*direction.y, expandedCorner.y, expandedSize.y) &&
+                isWithin(start.z + axisDistance*direction.z, expandedCorner.z, expandedSize.z)) ||
+           (findIntersection(start.y, direction.y, expandedCorner.y, expandedSize.y, axisDistance) &&
+                axisDistance >= 0.0f && axisDistance <= 1.0f &&
+                isWithin(start.x + axisDistance*direction.x, expandedCorner.x, expandedSize.x) &&
+                isWithin(start.z + axisDistance*direction.z, expandedCorner.z, expandedSize.z)) ||
+           (findIntersection(start.z, direction.z, expandedCorner.z, expandedSize.z, axisDistance) &&
+                axisDistance >= 0.0f && axisDistance <= 1.0f &&
+                isWithin(start.y + axisDistance*direction.y, expandedCorner.y, expandedSize.y) &&
+                isWithin(start.x + axisDistance*direction.x, expandedCorner.x, expandedSize.x));
+}
+
 bool AABox::findRayIntersection(const glm::vec3& origin, const glm::vec3& direction, float& distance, BoxFace& face) const {
     // handle the trivial case where the box contains the origin
     if (contains(origin)) {
@@ -126,3 +162,168 @@ bool AABox::findRayIntersection(const glm::vec3& origin, const glm::vec3& direct
     }
     return false;
 }
+
+bool AABox::findSpherePenetration(const glm::vec3& center, float radius, glm::vec3& penetration) const {
+    glm::vec4 center4 = glm::vec4(center, 1.0f);
+    
+    float minPenetrationLength = FLT_MAX;
+    for (int i = 0; i < FACE_COUNT; i++) {
+        glm::vec4 facePlane = getPlane((BoxFace)i);
+        glm::vec3 vector = getClosestPointOnFace(center, (BoxFace)i) - center;
+        if (glm::dot(center4, getPlane((BoxFace)i)) >= 0.0f) {
+            // outside this face, so use vector to closest point to determine penetration
+            return ::findSpherePenetration(vector, glm::vec3(-facePlane), radius, penetration);
+        }
+        float vectorLength = glm::length(vector);
+        if (vectorLength < minPenetrationLength) {
+            // remember the smallest penetration vector; if we're inside all faces, we'll use that
+            penetration = (vectorLength < EPSILON) ? glm::vec3(-facePlane) * radius :
+                vector * ((vectorLength + radius) / -vectorLength);
+            minPenetrationLength = vectorLength;
+        }
+    }
+    
+    return true;
+}
+
+bool AABox::findCapsulePenetration(const glm::vec3& start, const glm::vec3& end, float radius, glm::vec3& penetration) const {
+    glm::vec4 start4 = glm::vec4(start, 1.0f);
+    glm::vec4 end4 = glm::vec4(end, 1.0f);
+    glm::vec4 startToEnd = glm::vec4(end - start, 0.0f);
+    
+    float minPenetrationLength = FLT_MAX;
+    for (int i = 0; i < FACE_COUNT; i++) {
+        // find the vector from the segment to the closest point on the face (starting from deeper end)
+        glm::vec4 facePlane = getPlane((BoxFace)i);
+        glm::vec3 closest = (glm::dot(start4, facePlane) <= glm::dot(end4, facePlane)) ?
+            getClosestPointOnFace(start4, startToEnd, (BoxFace)i) : getClosestPointOnFace(end4, -startToEnd, (BoxFace)i);
+        glm::vec3 vector = -computeVectorFromPointToSegment(closest, start, end);
+        if (glm::dot(vector, glm::vec3(facePlane)) < 0.0f) {
+            // outside this face, so use vector to closest point to determine penetration
+            return ::findSpherePenetration(vector, glm::vec3(-facePlane), radius, penetration);
+        }
+        float vectorLength = glm::length(vector);
+        if (vectorLength < minPenetrationLength) {
+            // remember the smallest penetration vector; if we're inside all faces, we'll use that
+            penetration = (vectorLength < EPSILON) ? glm::vec3(-facePlane) * radius :
+                vector * ((vectorLength + radius) / -vectorLength);
+            minPenetrationLength = vectorLength;
+        }
+    } 
+    
+    return true;
+}
+
+glm::vec3 AABox::getClosestPointOnFace(const glm::vec3& point, BoxFace face) const {
+    switch (face) {
+        case MIN_X_FACE:
+            return glm::clamp(point, glm::vec3(_corner.x, _corner.y, _corner.z),
+                glm::vec3(_corner.x, _corner.y + _size.y, _corner.z + _size.z));
+        
+        case MAX_X_FACE:
+            return glm::clamp(point, glm::vec3(_corner.x + _size.x, _corner.y, _corner.z),
+                glm::vec3(_corner.x + _size.x, _corner.y + _size.y, _corner.z + _size.z));
+        
+        case MIN_Y_FACE:
+            return glm::clamp(point, glm::vec3(_corner.x, _corner.y, _corner.z),
+                glm::vec3(_corner.x + _size.x, _corner.y, _corner.z + _size.z));
+    
+        case MAX_Y_FACE:
+            return glm::clamp(point, glm::vec3(_corner.x, _corner.y + _size.y, _corner.z),
+                glm::vec3(_corner.x + _size.x, _corner.y + _size.y, _corner.z + _size.z));
+    
+        case MIN_Z_FACE:
+            return glm::clamp(point, glm::vec3(_corner.x, _corner.y, _corner.z),
+                glm::vec3(_corner.x + _size.x, _corner.y + _size.y, _corner.z));
+    
+        case MAX_Z_FACE:
+            return glm::clamp(point, glm::vec3(_corner.x, _corner.y, _corner.z + _size.z),
+                glm::vec3(_corner.x + _size.x, _corner.y + _size.y, _corner.z + _size.z));
+    }
+}
+
+glm::vec3 AABox::getClosestPointOnFace(const glm::vec4& origin, const glm::vec4& direction, BoxFace face) const {
+    // check against the four planes that border the face
+    BoxFace oppositeFace = getOppositeFace(face);
+    bool anyOutside = false;
+    for (int i = 0; i < FACE_COUNT; i++) {
+        if (i == face || i == oppositeFace) {
+            continue;
+        }
+        glm::vec4 iPlane = getPlane((BoxFace)i);
+        float originDistance = glm::dot(origin, iPlane);
+        if (originDistance < 0.0f) {
+            continue; // inside the border
+        }
+        anyOutside = true;
+        float divisor = glm::dot(direction, iPlane);
+        if (fabs(divisor) < EPSILON) {
+            continue; // segment is parallel to plane
+        }
+        // find intersection and see if it lies within face bounds
+        float directionalDistance = -originDistance / divisor;
+        glm::vec4 intersection = origin + direction * directionalDistance;
+        BoxFace iOppositeFace = getOppositeFace((BoxFace)i);
+        for (int j = 0; j < FACE_COUNT; j++) {
+            if (j == face || j == oppositeFace || j == i || j == iOppositeFace) {
+                continue;
+            }
+            if (glm::dot(intersection, getPlane((BoxFace)j)) > 0.0f) {
+                goto outerContinue; // intersection is out of bounds
+            }
+        }
+        return getClosestPointOnFace(glm::vec3(intersection), face);
+        
+        outerContinue: ;
+    }
+    
+    // if we were outside any of the sides, we must check against the diagonals
+    if (anyOutside) {
+        int faceAxis = face / 2;
+        int secondAxis = (faceAxis + 1) % 3;
+        int thirdAxis = (faceAxis + 2) % 3;
+        
+        glm::vec4 secondAxisMinPlane = getPlane((BoxFace)(secondAxis * 2));
+        glm::vec4 secondAxisMaxPlane = getPlane((BoxFace)(secondAxis * 2 + 1));
+        glm::vec4 thirdAxisMaxPlane = getPlane((BoxFace)(thirdAxis * 2 + 1));
+        
+        glm::vec4 offset = glm::vec4(0.0f, 0.0f, 0.0f,
+            glm::dot(glm::vec3(secondAxisMaxPlane + thirdAxisMaxPlane), _size) * 0.5f);
+        glm::vec4 diagonals[] = { secondAxisMinPlane + thirdAxisMaxPlane + offset,
+            secondAxisMaxPlane + thirdAxisMaxPlane + offset };
+        
+        for (int i = 0; i < sizeof(diagonals) / sizeof(diagonals[0]); i++) {
+            float divisor = glm::dot(direction, diagonals[i]);
+            if (fabs(divisor) < EPSILON) {
+                continue; // segment is parallel to diagonal plane
+            }
+            float directionalDistance = -glm::dot(origin, diagonals[i]) / divisor;
+            return getClosestPointOnFace(glm::vec3(origin + direction * directionalDistance), face);
+        }
+    }
+    
+    // last resort or all inside: clamp origin to face
+    return getClosestPointOnFace(glm::vec3(origin), face);
+}
+
+glm::vec4 AABox::getPlane(BoxFace face) const {
+    switch (face) {
+        case MIN_X_FACE: return glm::vec4(-1.0f, 0.0f, 0.0f, _corner.x);
+        case MAX_X_FACE: return glm::vec4(1.0f, 0.0f, 0.0f, -_corner.x - _size.x);
+        case MIN_Y_FACE: return glm::vec4(0.0f, -1.0f, 0.0f, _corner.y);
+        case MAX_Y_FACE: return glm::vec4(0.0f, 1.0f, 0.0f, -_corner.y - _size.y);
+        case MIN_Z_FACE: return glm::vec4(0.0f, 0.0f, -1.0f, _corner.z);
+        case MAX_Z_FACE: return glm::vec4(0.0f, 0.0f, 1.0f, -_corner.z - _size.z);
+    }
+}
+
+BoxFace AABox::getOppositeFace(BoxFace face) {
+    switch (face) {
+        case MIN_X_FACE: return MAX_X_FACE;
+        case MAX_X_FACE: return MIN_X_FACE;
+        case MIN_Y_FACE: return MAX_Y_FACE;
+        case MAX_Y_FACE: return MIN_Y_FACE;
+        case MIN_Z_FACE: return MAX_Z_FACE;
+        case MAX_Z_FACE: return MIN_Z_FACE;
+    }
+}

libraries/voxels/src/AABox.h

@@ -19,7 +19,8 @@ enum BoxFace {
     MIN_Y_FACE,
     MAX_Y_FACE,
     MIN_Z_FACE,
-    MAX_Z_FACE
+    MAX_Z_FACE,
+    FACE_COUNT
 };
 
 class AABox 
@@ -46,9 +47,20 @@ public:
 	const glm::vec3& getCenter() const { return _center; };
 
     bool contains(const glm::vec3& point) const;
+    bool expandedContains(const glm::vec3& point, float expansion) const;
+    bool expandedIntersectsSegment(const glm::vec3& start, const glm::vec3& end, float expansion) const;
     bool findRayIntersection(const glm::vec3& origin, const glm::vec3& direction, float& distance, BoxFace& face) const;
+    bool findSpherePenetration(const glm::vec3& center, float radius, glm::vec3& penetration) const;
+    bool findCapsulePenetration(const glm::vec3& start, const glm::vec3& end, float radius, glm::vec3& penetration) const;
 
 private:
+    
+    glm::vec3 getClosestPointOnFace(const glm::vec3& point, BoxFace face) const;
+    glm::vec3 getClosestPointOnFace(const glm::vec4& origin, const glm::vec4& direction, BoxFace face) const;
+    glm::vec4 getPlane(BoxFace face) const;
+    
+    static BoxFace getOppositeFace(BoxFace face);
+    
 	glm::vec3 _corner;
 	glm::vec3 _center;
 	glm::vec3 _size;

libraries/voxels/src/GeometryUtil.cpp

@@ -5,12 +5,18 @@
 //  Created by Andrzej Kapolka on 5/21/13.
 //  Copyright (c) 2013 High Fidelity, Inc. All rights reserved.
 
+#include <SharedUtil.h>
+
 #include "GeometryUtil.h"
 
 glm::vec3 computeVectorFromPointToSegment(const glm::vec3& point, const glm::vec3& start, const glm::vec3& end) {
     // compute the projection of the point vector onto the segment vector
     glm::vec3 segmentVector = end - start;
-    float proj = glm::dot(point - start, segmentVector) / glm::dot(segmentVector, segmentVector);
+    float lengthSquared = glm::dot(segmentVector, segmentVector);
+    if (lengthSquared < EPSILON) {
+        return start - point; // start and end the same
+    }
+    float proj = glm::dot(point - start, segmentVector) / lengthSquared;
     if (proj <= 0.0f) { // closest to the start
         return start - point;
         
@@ -21,3 +27,91 @@ glm::vec3 computeVectorFromPointToSegment(const glm::vec3& point, const glm::vec
         return start + segmentVector*proj - point;
     }
 }
+
+bool findSpherePenetration(const glm::vec3& penetratorToPenetratee, const glm::vec3& direction,
+                           float combinedRadius, glm::vec3& penetration) {
+    float vectorLength = glm::length(penetratorToPenetratee);
+    if (vectorLength < EPSILON) {
+        penetration = direction * combinedRadius;
+        return true;
+    }
+    float distance = vectorLength - combinedRadius;
+    if (distance < 0.0f) {
+        penetration = penetratorToPenetratee * (-distance / vectorLength);
+        return true;
+    }
+    return false;
+}
+
+bool findSpherePointPenetration(const glm::vec3& penetratorCenter, float penetratorRadius,
+                                const glm::vec3& penetrateeLocation, glm::vec3& penetration) {
+    return findSpherePenetration(penetrateeLocation - penetratorCenter, glm::vec3(0.0f, -1.0f, 0.0f),
+        penetratorRadius, penetration);
+}
+
+bool findSphereSpherePenetration(const glm::vec3& penetratorCenter, float penetratorRadius,
+                                 const glm::vec3& penetrateeCenter, float penetrateeRadius, glm::vec3& penetration) {
+    return findSpherePointPenetration(penetratorCenter, penetratorRadius + penetrateeRadius, penetrateeCenter, penetration);
+}
+
+bool findSphereSegmentPenetration(const glm::vec3& penetratorCenter, float penetratorRadius,
+                                  const glm::vec3& penetrateeStart, const glm::vec3& penetrateeEnd, glm::vec3& penetration) {
+    return findSpherePenetration(computeVectorFromPointToSegment(penetratorCenter, penetrateeStart, penetrateeEnd),
+        glm::vec3(0.0f, -1.0f, 0.0f), penetratorRadius, penetration);
+}
+
+bool findSphereCapsulePenetration(const glm::vec3& penetratorCenter, float penetratorRadius, const glm::vec3& penetrateeStart,
+                                  const glm::vec3& penetrateeEnd, float penetrateeRadius, glm::vec3& penetration) {
+    return findSphereSegmentPenetration(penetratorCenter, penetratorRadius + penetrateeRadius,
+        penetrateeStart, penetrateeEnd, penetration);
+}
+
+bool findSpherePlanePenetration(const glm::vec3& penetratorCenter, float penetratorRadius, 
+                                const glm::vec4& penetrateePlane, glm::vec3& penetration) {
+    float distance = glm::dot(penetrateePlane, glm::vec4(penetratorCenter, 1.0f)) - penetratorRadius;
+    if (distance < 0.0f) {
+        penetration = glm::vec3(penetrateePlane) * distance;
+        return true;
+    }
+    return false;
+}
+
+bool findCapsuleSpherePenetration(const glm::vec3& penetratorStart, const glm::vec3& penetratorEnd, float penetratorRadius,
+                                  const glm::vec3& penetrateeCenter, float penetrateeRadius, glm::vec3& penetration) {
+    if (findSphereCapsulePenetration(penetrateeCenter, penetrateeRadius,
+            penetratorStart, penetratorEnd, penetratorRadius, penetration)) {
+        penetration = -penetration;
+        return true;
+    }
+    return false;
+}
+
+bool findCapsulePlanePenetration(const glm::vec3& penetratorStart, const glm::vec3& penetratorEnd, float penetratorRadius,
+                                 const glm::vec4& penetrateePlane, glm::vec3& penetration) {
+    float distance = glm::min(glm::dot(penetrateePlane, glm::vec4(penetratorStart, 1.0f)),
+        glm::dot(penetrateePlane, glm::vec4(penetratorEnd, 1.0f))) - penetratorRadius;
+    if (distance < 0.0f) {
+        penetration = glm::vec3(penetrateePlane) * distance;
+        return true;
+    }
+    return false;
+}
+
+glm::vec3 addPenetrations(const glm::vec3& currentPenetration, const glm::vec3& newPenetration) {
+    // find the component of the new penetration in the direction of the current
+    float currentLength = glm::length(currentPenetration);
+    if (currentLength == 0.0f) {
+        return newPenetration;
+    }
+    glm::vec3 currentDirection = currentPenetration / currentLength;
+    float directionalComponent = glm::dot(newPenetration, currentDirection);
+    
+    // if orthogonal or in the opposite direction, we can simply add
+    if (directionalComponent <= 0.0f) {
+        return currentPenetration + newPenetration;
+    }
+    
+    // otherwise, we need to take the maximum component of current and new
+    return currentDirection * glm::max(directionalComponent, currentLength) +
+        newPenetration - (currentDirection * directionalComponent);
+}

libraries/voxels/src/GeometryUtil.h

@@ -13,4 +13,30 @@
 
 glm::vec3 computeVectorFromPointToSegment(const glm::vec3& point, const glm::vec3& start, const glm::vec3& end);
 
+bool findSpherePenetration(const glm::vec3& penetratorToPenetratee, const glm::vec3& direction,
+                           float combinedRadius, glm::vec3& penetration);
+
+bool findSpherePointPenetration(const glm::vec3& penetratorCenter, float penetratorRadius,
+                                const glm::vec3& penetrateeLocation, glm::vec3& penetration);
+
+bool findSphereSpherePenetration(const glm::vec3& penetratorCenter, float penetratorRadius,
+                                 const glm::vec3& penetrateeCenter, float penetrateeRadius, glm::vec3& penetration);
+                     
+bool findSphereSegmentPenetration(const glm::vec3& penetratorCenter, float penetratorRadius,
+                                  const glm::vec3& penetrateeStart, const glm::vec3& penetrateeEnd, glm::vec3& penetration);
+
+bool findSphereCapsulePenetration(const glm::vec3& penetratorCenter, float penetratorRadius, const glm::vec3& penetrateeStart,
+                                  const glm::vec3& penetrateeEnd, float penetrateeRadius, glm::vec3& penetration);
+                                  
+bool findSpherePlanePenetration(const glm::vec3& penetratorCenter, float penetratorRadius, 
+                                const glm::vec4& penetrateePlane, glm::vec3& penetration);
+                                  
+bool findCapsuleSpherePenetration(const glm::vec3& penetratorStart, const glm::vec3& penetratorEnd, float penetratorRadius,
+                                  const glm::vec3& penetrateeCenter, float penetrateeRadius, glm::vec3& penetration);
+
+bool findCapsulePlanePenetration(const glm::vec3& penetratorStart, const glm::vec3& penetratorEnd, float penetratorRadius,
+                                 const glm::vec4& penetrateePlane, glm::vec3& penetration);
+
+glm::vec3 addPenetrations(const glm::vec3& currentPenetration, const glm::vec3& newPenetration);
+
 #endif /* defined(__interface__GeometryUtil__) */

libraries/voxels/src/VoxelTree.cpp

@@ -716,20 +716,20 @@ public:
 bool findSpherePenetrationOp(VoxelNode* node, void* extraData) {
     SphereArgs* args = static_cast<SphereArgs*>(extraData);
     
-    // currently, we treat each node as a sphere enveloping the box
-    const glm::vec3& nodeCenter = node->getCenter();
-    glm::vec3 vector = args->center - nodeCenter;
-    float vectorLength = glm::length(vector);
-    float distance = vectorLength - node->getEnclosingRadius() - args->radius;
-    if (distance >= 0.0f) {
+    // coarse check against bounds
+    const AABox& box = node->getAABox();
+    if (!box.expandedContains(args->center, args->radius)) {
         return false;
     }
     if (!node->isLeaf()) {
         return true; // recurse on children
     }
     if (node->isColored()) {
-        args->penetration += vector * (-distance * TREE_SCALE / vectorLength);
-        args->found = true;
+        glm::vec3 nodePenetration;
+        if (box.findSpherePenetration(args->center, args->radius, nodePenetration)) {
+            args->penetration = addPenetrations(args->penetration, nodePenetration * (float)TREE_SCALE);
+            args->found = true;
+        }        
     }
     return false;
 }
@@ -753,20 +753,20 @@ public:
 bool findCapsulePenetrationOp(VoxelNode* node, void* extraData) {
     CapsuleArgs* args = static_cast<CapsuleArgs*>(extraData);
     
-    // currently, we treat each node as a sphere enveloping the box
-    const glm::vec3& nodeCenter = node->getCenter();
-    glm::vec3 vector = computeVectorFromPointToSegment(nodeCenter, args->start, args->end);
-    float vectorLength = glm::length(vector);
-    float distance = vectorLength - node->getEnclosingRadius() - args->radius;
-    if (distance >= 0.0f) {
+    // coarse check against bounds
+    const AABox& box = node->getAABox();
+    if (!box.expandedIntersectsSegment(args->start, args->end, args->radius)) {
         return false;
     }
     if (!node->isLeaf()) {
         return true; // recurse on children
     }
     if (node->isColored()) {
-        args->penetration += vector * (-distance * TREE_SCALE / vectorLength);
-        args->found = true;
+        glm::vec3 nodePenetration;
+        if (box.findCapsulePenetration(args->start, args->end, args->radius, nodePenetration)) {
+            args->penetration = addPenetrations(args->penetration, nodePenetration * (float)TREE_SCALE);
+            args->found = true;
+        }        
     }
     return false;
 }