From 70b25fd6c4192b0de102f9c813019b7053e19505 Mon Sep 17 00:00:00 2001 From: Andrzej Kapolka Date: Sat, 25 May 2013 19:59:07 -0700 Subject: [PATCH 1/4] Sphere/box collisions for voxels, various utility functions. --- interface/src/Avatar.cpp | 12 ++- interface/src/Environment.cpp | 17 ++--- libraries/voxels/src/AABox.cpp | 70 ++++++++++++++++++ libraries/voxels/src/AABox.h | 9 ++- libraries/voxels/src/GeometryUtil.cpp | 102 +++++++++++++++++++++++++- libraries/voxels/src/GeometryUtil.h | 31 +++++++- libraries/voxels/src/VoxelTree.cpp | 26 +++---- 7 files changed, 234 insertions(+), 33 deletions(-) diff --git a/interface/src/Avatar.cpp b/interface/src/Avatar.cpp index 7568e45116..5bc7a07bf1 100644 --- a/interface/src/Avatar.cpp +++ b/interface/src/Avatar.cpp @@ -615,15 +615,19 @@ void Avatar::updateCollisionWithEnvironment() { void Avatar::updateCollisionWithVoxels() { float radius = _height * 0.125f; glm::vec3 penetration; - if (Application::getInstance()->getVoxels()->findCapsulePenetration( - _position - glm::vec3(0.0f, _pelvisFloatingHeight - radius, 0.0f), - _position + glm::vec3(0.0f, _height - _pelvisFloatingHeight - radius, 0.0f), radius, penetration)) { + //if (Application::getInstance()->getVoxels()->findCapsulePenetration( + // _position - glm::vec3(0.0f, _pelvisFloatingHeight - radius, 0.0f), + // _position + glm::vec3(0.0f, _height - _pelvisFloatingHeight - radius, 0.0f), radius, penetration)) { + // applyCollisionWithScene(penetration); + //} + + if (Application::getInstance()->getVoxels()->findSpherePenetration(_position, _height/2, penetration)) { applyCollisionWithScene(penetration); } } 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); diff --git a/interface/src/Environment.cpp b/interface/src/Environment.cpp index 49ef26bb82..5063c990db 100644 --- a/interface/src/Environment.cpp +++ b/interface/src/Environment.cpp @@ -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; } } diff --git a/libraries/voxels/src/AABox.cpp b/libraries/voxels/src/AABox.cpp index c26e73e12a..d49c04136b 100644 --- a/libraries/voxels/src/AABox.cpp +++ b/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) { @@ -126,3 +138,61 @@ 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::vec3 vector = getClosestPointOnFace(center, (BoxFace)i) - center; + if (glm::dot(center4, getPlane((BoxFace)i)) >= 0.0f) { + return ::findSpherePenetration(vector, radius, penetration); + } + float vectorLength = glm::length(vector); + if (vectorLength < minPenetrationLength) { + penetration = 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::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); + } +} diff --git a/libraries/voxels/src/AABox.h b/libraries/voxels/src/AABox.h index 63d586de22..5f4f84d417 100755 --- a/libraries/voxels/src/AABox.h +++ b/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,15 @@ 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 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; private: + + glm::vec3 getClosestPointOnFace(const glm::vec3& point, BoxFace face) const; + glm::vec4 getPlane(BoxFace face) const; + glm::vec3 _corner; glm::vec3 _center; glm::vec3 _size; diff --git a/libraries/voxels/src/GeometryUtil.cpp b/libraries/voxels/src/GeometryUtil.cpp index 46e83018cd..b059645553 100644 --- a/libraries/voxels/src/GeometryUtil.cpp +++ b/libraries/voxels/src/GeometryUtil.cpp @@ -7,7 +7,36 @@ #include "GeometryUtil.h" -glm::vec3 computeVectorFromPointToSegment(const glm::vec3& point, const glm::vec3& start, const glm::vec3& end) { +bool findSpherePenetration(const glm::vec3& penetratorToPenetratee, float combinedRadius, glm::vec3& penetration) { + float vectorLength = glm::length(penetratorToPenetratee); + 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, 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 findSphereLinePenetration(const glm::vec3& penetratorCenter, float penetratorRadius, + const glm::vec3& penetrateeOrigin, const glm::vec3& penetrateeDirection, + glm::vec3& penetration) { + // compute the projection of the penetrator vector onto the line + float proj = glm::dot(penetratorCenter - penetrateeOrigin, penetrateeDirection); + return findSpherePenetration((penetrateeOrigin + penetrateeDirection*proj) - penetratorCenter, + penetratorRadius, penetration); +} + +static 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); @@ -21,3 +50,74 @@ glm::vec3 computeVectorFromPointToSegment(const glm::vec3& point, const glm::vec return start + segmentVector*proj - point; } } + +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), + 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 findCapsulePointPenetration(const glm::vec3& penetratorStart, const glm::vec3& penetratorEnd, float penetratorRadius, + const glm::vec3& penetrateeLocation, glm::vec3& penetration) { + if (findSphereSegmentPenetration(penetrateeLocation, penetratorRadius, penetratorStart, penetratorEnd, penetration)) { + penetration = -penetration; + 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); +} diff --git a/libraries/voxels/src/GeometryUtil.h b/libraries/voxels/src/GeometryUtil.h index f0c63e5a86..0d80626507 100644 --- a/libraries/voxels/src/GeometryUtil.h +++ b/libraries/voxels/src/GeometryUtil.h @@ -11,6 +11,35 @@ #include -glm::vec3 computeVectorFromPointToSegment(const glm::vec3& point, const glm::vec3& start, const glm::vec3& end); +bool findSpherePenetration(const glm::vec3& penetratorToPenetratee, 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 findSphereLinePenetration(const glm::vec3& penetratorCenter, float penetratorRadius, + const glm::vec3& penetrateeOrigin, const glm::vec3& penetrateeDirection, 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 findCapsulePointPenetration(const glm::vec3& penetratorStart, const glm::vec3& penetratorEnd, float penetratorRadius, + const glm::vec3& penetrateeLocation, 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__) */ diff --git a/libraries/voxels/src/VoxelTree.cpp b/libraries/voxels/src/VoxelTree.cpp index 798b447d6c..c97765a3fb 100644 --- a/libraries/voxels/src/VoxelTree.cpp +++ b/libraries/voxels/src/VoxelTree.cpp @@ -716,20 +716,20 @@ public: bool findSpherePenetrationOp(VoxelNode* node, void* extraData) { SphereArgs* args = static_cast(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; } @@ -754,18 +754,16 @@ bool findCapsulePenetrationOp(VoxelNode* node, void* extraData) { CapsuleArgs* args = static_cast(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) { + glm::vec3 nodePenetration; + if (!findCapsuleSpherePenetration(args->start, args->end, args->radius, node->getCenter(), + node->getEnclosingRadius(), nodePenetration)) { return false; } if (!node->isLeaf()) { return true; // recurse on children } if (node->isColored()) { - args->penetration += vector * (-distance * TREE_SCALE / vectorLength); + args->penetration = addPenetrations(args->penetration, nodePenetration * (float)TREE_SCALE); args->found = true; } return false; From cfb66bee955a5e2948f6b940bf176f0b1823345f Mon Sep 17 00:00:00 2001 From: Andrzej Kapolka Date: Mon, 27 May 2013 14:05:44 -0700 Subject: [PATCH 2/4] Capsule/box collisions. --- interface/src/Avatar.cpp | 10 +- libraries/voxels/src/AABox.cpp | 131 +++++++++++++++++++++++++- libraries/voxels/src/AABox.h | 5 + libraries/voxels/src/GeometryUtil.cpp | 76 +++++++-------- libraries/voxels/src/GeometryUtil.h | 13 +-- libraries/voxels/src/VoxelTree.cpp | 14 +-- 6 files changed, 186 insertions(+), 63 deletions(-) diff --git a/interface/src/Avatar.cpp b/interface/src/Avatar.cpp index 5bc7a07bf1..17958a4fa2 100644 --- a/interface/src/Avatar.cpp +++ b/interface/src/Avatar.cpp @@ -615,13 +615,9 @@ void Avatar::updateCollisionWithEnvironment() { void Avatar::updateCollisionWithVoxels() { float radius = _height * 0.125f; glm::vec3 penetration; - //if (Application::getInstance()->getVoxels()->findCapsulePenetration( - // _position - glm::vec3(0.0f, _pelvisFloatingHeight - radius, 0.0f), - // _position + glm::vec3(0.0f, _height - _pelvisFloatingHeight - radius, 0.0f), radius, penetration)) { - // applyCollisionWithScene(penetration); - //} - - if (Application::getInstance()->getVoxels()->findSpherePenetration(_position, _height/2, penetration)) { + if (Application::getInstance()->getVoxels()->findCapsulePenetration( + _position - glm::vec3(0.0f, _pelvisFloatingHeight - radius, 0.0f), + _position + glm::vec3(0.0f, _height - _pelvisFloatingHeight - radius, 0.0f), radius, penetration)) { applyCollisionWithScene(penetration); } } diff --git a/libraries/voxels/src/AABox.cpp b/libraries/voxels/src/AABox.cpp index d49c04136b..ce6d653eb0 100644 --- a/libraries/voxels/src/AABox.cpp +++ b/libraries/voxels/src/AABox.cpp @@ -107,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)) { @@ -144,12 +168,15 @@ bool AABox::findSpherePenetration(const glm::vec3& center, float radius, glm::ve 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) { - return ::findSpherePenetration(vector, radius, penetration); + // 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 = vector * ((vectorLength + radius) / -vectorLength); minPenetrationLength = vectorLength; } @@ -158,6 +185,33 @@ bool AABox::findSpherePenetration(const glm::vec3& center, float radius, glm::ve 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 = 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: @@ -186,6 +240,70 @@ glm::vec3 AABox::getClosestPointOnFace(const glm::vec3& point, BoxFace face) con } } +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); @@ -196,3 +314,14 @@ glm::vec4 AABox::getPlane(BoxFace face) const { 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; + } +} diff --git a/libraries/voxels/src/AABox.h b/libraries/voxels/src/AABox.h index 5f4f84d417..591704d4a3 100755 --- a/libraries/voxels/src/AABox.h +++ b/libraries/voxels/src/AABox.h @@ -48,14 +48,19 @@ public: 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; diff --git a/libraries/voxels/src/GeometryUtil.cpp b/libraries/voxels/src/GeometryUtil.cpp index b059645553..aceaa32b37 100644 --- a/libraries/voxels/src/GeometryUtil.cpp +++ b/libraries/voxels/src/GeometryUtil.cpp @@ -5,41 +5,18 @@ // Created by Andrzej Kapolka on 5/21/13. // Copyright (c) 2013 High Fidelity, Inc. All rights reserved. +#include + #include "GeometryUtil.h" -bool findSpherePenetration(const glm::vec3& penetratorToPenetratee, float combinedRadius, glm::vec3& penetration) { - float vectorLength = glm::length(penetratorToPenetratee); - 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, 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 findSphereLinePenetration(const glm::vec3& penetratorCenter, float penetratorRadius, - const glm::vec3& penetrateeOrigin, const glm::vec3& penetrateeDirection, - glm::vec3& penetration) { - // compute the projection of the penetrator vector onto the line - float proj = glm::dot(penetratorCenter - penetrateeOrigin, penetrateeDirection); - return findSpherePenetration((penetrateeOrigin + penetrateeDirection*proj) - penetratorCenter, - penetratorRadius, penetration); -} - -static glm::vec3 computeVectorFromPointToSegment(const glm::vec3& point, const glm::vec3& start, const glm::vec3& end) { +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; @@ -51,10 +28,36 @@ static glm::vec3 computeVectorFromPointToSegment(const glm::vec3& point, const g } } +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), - penetratorRadius, penetration); + glm::vec3(0.0f, -1.0f, 0.0f), penetratorRadius, penetration); } bool findSphereCapsulePenetration(const glm::vec3& penetratorCenter, float penetratorRadius, const glm::vec3& penetrateeStart, @@ -73,15 +76,6 @@ bool findSpherePlanePenetration(const glm::vec3& penetratorCenter, float penetra return false; } -bool findCapsulePointPenetration(const glm::vec3& penetratorStart, const glm::vec3& penetratorEnd, float penetratorRadius, - const glm::vec3& penetrateeLocation, glm::vec3& penetration) { - if (findSphereSegmentPenetration(penetrateeLocation, penetratorRadius, penetratorStart, penetratorEnd, penetration)) { - penetration = -penetration; - 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, diff --git a/libraries/voxels/src/GeometryUtil.h b/libraries/voxels/src/GeometryUtil.h index 0d80626507..01cd87a4f3 100644 --- a/libraries/voxels/src/GeometryUtil.h +++ b/libraries/voxels/src/GeometryUtil.h @@ -11,17 +11,17 @@ #include -bool findSpherePenetration(const glm::vec3& penetratorToPenetratee, float combinedRadius, glm::vec3& penetration); +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 findSphereLinePenetration(const glm::vec3& penetratorCenter, float penetratorRadius, - const glm::vec3& penetrateeOrigin, const glm::vec3& penetrateeDirection, glm::vec3& penetration); - + bool findSphereSegmentPenetration(const glm::vec3& penetratorCenter, float penetratorRadius, const glm::vec3& penetrateeStart, const glm::vec3& penetrateeEnd, glm::vec3& penetration); @@ -30,9 +30,6 @@ bool findSphereCapsulePenetration(const glm::vec3& penetratorCenter, float penet bool findSpherePlanePenetration(const glm::vec3& penetratorCenter, float penetratorRadius, const glm::vec4& penetrateePlane, glm::vec3& penetration); - -bool findCapsulePointPenetration(const glm::vec3& penetratorStart, const glm::vec3& penetratorEnd, float penetratorRadius, - const glm::vec3& penetrateeLocation, glm::vec3& penetration); bool findCapsuleSpherePenetration(const glm::vec3& penetratorStart, const glm::vec3& penetratorEnd, float penetratorRadius, const glm::vec3& penetrateeCenter, float penetrateeRadius, glm::vec3& penetration); diff --git a/libraries/voxels/src/VoxelTree.cpp b/libraries/voxels/src/VoxelTree.cpp index c97765a3fb..39a7fceff6 100644 --- a/libraries/voxels/src/VoxelTree.cpp +++ b/libraries/voxels/src/VoxelTree.cpp @@ -753,18 +753,20 @@ public: bool findCapsulePenetrationOp(VoxelNode* node, void* extraData) { CapsuleArgs* args = static_cast(extraData); - // currently, we treat each node as a sphere enveloping the box - glm::vec3 nodePenetration; - if (!findCapsuleSpherePenetration(args->start, args->end, args->radius, node->getCenter(), - node->getEnclosingRadius(), nodePenetration)) { + // 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 = addPenetrations(args->penetration, nodePenetration * (float)TREE_SCALE); - 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; } From 949befee09fae741220540001d3f1080793fdec0 Mon Sep 17 00:00:00 2001 From: Andrzej Kapolka Date: Mon, 27 May 2013 14:37:07 -0700 Subject: [PATCH 3/4] Fix for NaN. --- interface/src/Avatar.cpp | 7 +++++-- libraries/voxels/src/AABox.cpp | 6 ++++-- 2 files changed, 9 insertions(+), 4 deletions(-) diff --git a/interface/src/Avatar.cpp b/interface/src/Avatar.cpp index 17958a4fa2..4713d09a21 100644 --- a/interface/src/Avatar.cpp +++ b/interface/src/Avatar.cpp @@ -626,8 +626,11 @@ void Avatar::applyCollisionWithScene(const glm::vec3& 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) { diff --git a/libraries/voxels/src/AABox.cpp b/libraries/voxels/src/AABox.cpp index ce6d653eb0..b7580b243e 100644 --- a/libraries/voxels/src/AABox.cpp +++ b/libraries/voxels/src/AABox.cpp @@ -177,7 +177,8 @@ bool AABox::findSpherePenetration(const glm::vec3& center, float radius, glm::ve float vectorLength = glm::length(vector); if (vectorLength < minPenetrationLength) { // remember the smallest penetration vector; if we're inside all faces, we'll use that - penetration = vector * ((vectorLength + radius) / -vectorLength); + penetration = (vectorLength < EPSILON) ? glm::vec3(-facePlane) * radius : + vector * ((vectorLength + radius) / -vectorLength); minPenetrationLength = vectorLength; } } @@ -204,7 +205,8 @@ bool AABox::findCapsulePenetration(const glm::vec3& start, const glm::vec3& end, float vectorLength = glm::length(vector); if (vectorLength < minPenetrationLength) { // remember the smallest penetration vector; if we're inside all faces, we'll use that - penetration = vector * ((vectorLength + radius) / -vectorLength); + penetration = (vectorLength < EPSILON) ? glm::vec3(-facePlane) * radius : + vector * ((vectorLength + radius) / -vectorLength); minPenetrationLength = vectorLength; } } From 278ff009260d81cb756e2df04890725096f3e68f Mon Sep 17 00:00:00 2001 From: Andrzej Kapolka Date: Mon, 27 May 2013 14:46:16 -0700 Subject: [PATCH 4/4] Fixed Linux build error and a warning on call to Avatar.simulate, moved grid size constant to world.h and made gravity bounds match. --- interface/src/Application.cpp | 3 +-- interface/src/Environment.cpp | 4 ++-- interface/src/OculusManager.cpp | 2 ++ interface/src/world.h | 2 ++ 4 files changed, 7 insertions(+), 4 deletions(-) diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp index a0979eb4b9..b94103c576 100644 --- a/interface/src/Application.cpp +++ b/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 diff --git a/interface/src/Environment.cpp b/interface/src/Environment.cpp index 5063c990db..fec4ba0529 100644 --- a/interface/src/Environment.cpp +++ b/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); } diff --git a/interface/src/OculusManager.cpp b/interface/src/OculusManager.cpp index 168616ec83..17775b9f9c 100644 --- a/interface/src/OculusManager.cpp +++ b/interface/src/OculusManager.cpp @@ -38,9 +38,11 @@ void OculusManager::connect() { } void OculusManager::updateYawOffset() { +#ifdef __APPLE__ float yaw, pitch, roll; _sensorFusion.GetOrientation().GetEulerAngles(&yaw, &pitch, &roll); _yawOffset = yaw; +#endif } void OculusManager::getEulerAngles(float& yaw, float& pitch, float& roll) { diff --git a/interface/src/world.h b/interface/src/world.h index 7d22aab6a2..c0c514c26d 100644 --- a/interface/src/world.h +++ b/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