From 5c0b12abf6a7d1983dc5a080cf31422c41ee36d3 Mon Sep 17 00:00:00 2001 From: SamGondelman Date: Wed, 22 Aug 2018 12:07:32 -0700 Subject: [PATCH] ordered ray/parabola intersection code --- interface/src/avatar/AvatarManager.cpp | 8 +- interface/src/raypick/LaserPointer.cpp | 3 + interface/src/raypick/ParabolaPointer.cpp | 3 + .../src/RenderablePolyVoxEntityItem.cpp | 13 +- libraries/entities/src/EntityTree.cpp | 54 +- libraries/entities/src/EntityTreeElement.cpp | 33 +- libraries/entities/src/EntityTreeElement.h | 9 +- libraries/entities/src/ShapeEntityItem.cpp | 28 +- libraries/octree/src/Octree.cpp | 127 ++--- libraries/octree/src/Octree.h | 22 +- libraries/render-utils/src/Model.cpp | 177 ++++-- libraries/render-utils/src/Model.h | 10 + libraries/shared/src/GeometryUtil.cpp | 7 +- libraries/shared/src/TriangleSet.cpp | 518 +++++++++++------- libraries/shared/src/TriangleSet.h | 25 +- 15 files changed, 589 insertions(+), 448 deletions(-) diff --git a/interface/src/avatar/AvatarManager.cpp b/interface/src/avatar/AvatarManager.cpp index 0d180bc40d..139f44d58b 100644 --- a/interface/src/avatar/AvatarManager.cpp +++ b/interface/src/avatar/AvatarManager.cpp @@ -553,8 +553,6 @@ RayToAvatarIntersectionResult AvatarManager::findRayIntersectionVector(const Pic return result; } - glm::vec3 normDirection = glm::normalize(ray.direction); - auto avatarHashCopy = getHashCopy(); for (auto avatarData : avatarHashCopy) { auto avatar = std::static_pointer_cast(avatarData); @@ -587,14 +585,14 @@ RayToAvatarIntersectionResult AvatarManager::findRayIntersectionVector(const Pic glm::vec3 end; float radius; avatar->getCapsule(start, end, radius); - bool intersects = findRayCapsuleIntersection(ray.origin, normDirection, start, end, radius, distance); + bool intersects = findRayCapsuleIntersection(ray.origin, ray.direction, start, end, radius, distance); if (!intersects) { // ray doesn't intersect avatar's capsule continue; } QVariantMap extraInfo; - intersects = avatarModel->findRayIntersectionAgainstSubMeshes(ray.origin, normDirection, + intersects = avatarModel->findRayIntersectionAgainstSubMeshes(ray.origin, ray.direction, distance, face, surfaceNormal, extraInfo, true); if (intersects && (!result.intersects || distance < result.distance)) { @@ -608,7 +606,7 @@ RayToAvatarIntersectionResult AvatarManager::findRayIntersectionVector(const Pic } if (result.intersects) { - result.intersection = ray.origin + normDirection * result.distance; + result.intersection = ray.origin + ray.direction * result.distance; } return result; diff --git a/interface/src/raypick/LaserPointer.cpp b/interface/src/raypick/LaserPointer.cpp index 2382a95105..40a5e5cb69 100644 --- a/interface/src/raypick/LaserPointer.cpp +++ b/interface/src/raypick/LaserPointer.cpp @@ -42,6 +42,9 @@ glm::vec3 LaserPointer::getPickOrigin(const PickResultPointer& pickResult) const glm::vec3 LaserPointer::getPickEnd(const PickResultPointer& pickResult, float distance) const { auto rayPickResult = std::static_pointer_cast(pickResult); + if (!rayPickResult) { + return glm::vec3(0.0f); + } if (distance > 0.0f) { PickRay pick = PickRay(rayPickResult->pickVariant); return pick.origin + distance * pick.direction; diff --git a/interface/src/raypick/ParabolaPointer.cpp b/interface/src/raypick/ParabolaPointer.cpp index 097c98340c..57d57e11c4 100644 --- a/interface/src/raypick/ParabolaPointer.cpp +++ b/interface/src/raypick/ParabolaPointer.cpp @@ -67,6 +67,9 @@ glm::vec3 ParabolaPointer::getPickOrigin(const PickResultPointer& pickResult) co glm::vec3 ParabolaPointer::getPickEnd(const PickResultPointer& pickResult, float distance) const { auto parabolaPickResult = std::static_pointer_cast(pickResult); + if (!parabolaPickResult) { + return glm::vec3(0.0f); + } if (distance > 0.0f) { PickParabola pick = PickParabola(parabolaPickResult->pickVariant); return pick.origin + pick.velocity * distance + 0.5f * pick.acceleration * distance * distance; diff --git a/libraries/entities-renderer/src/RenderablePolyVoxEntityItem.cpp b/libraries/entities-renderer/src/RenderablePolyVoxEntityItem.cpp index c11ccb70a0..63f27dd170 100644 --- a/libraries/entities-renderer/src/RenderablePolyVoxEntityItem.cpp +++ b/libraries/entities-renderer/src/RenderablePolyVoxEntityItem.cpp @@ -584,8 +584,6 @@ bool RenderablePolyVoxEntityItem::findDetailedRayIntersection(const glm::vec3& o glm::vec4 originInVoxel = wtvMatrix * glm::vec4(origin, 1.0f); glm::vec4 farInVoxel = wtvMatrix * glm::vec4(farPoint, 1.0f); - glm::vec4 directionInVoxel = glm::normalize(farInVoxel - originInVoxel); - glm::vec4 result = glm::vec4(0.0f, 0.0f, 0.0f, 0.0f); PolyVox::RaycastResult raycastResult = doRayCast(originInVoxel, farInVoxel, result); if (raycastResult == PolyVox::RaycastResults::Completed) { @@ -599,14 +597,9 @@ bool RenderablePolyVoxEntityItem::findDetailedRayIntersection(const glm::vec3& o voxelBox += result3 - Vectors::HALF; voxelBox += result3 + Vectors::HALF; - float voxelDistance; - bool hit = voxelBox.findRayIntersection(glm::vec3(originInVoxel), glm::vec3(directionInVoxel), - voxelDistance, face, surfaceNormal); - - glm::vec4 voxelIntersectionPoint = glm::vec4(glm::vec3(originInVoxel) + glm::vec3(directionInVoxel) * voxelDistance, 1.0); - glm::vec4 intersectionPoint = vtwMatrix * voxelIntersectionPoint; - distance = glm::distance(origin, glm::vec3(intersectionPoint)); - return hit; + glm::vec4 directionInVoxel = wtvMatrix * glm::vec4(direction, 0.0f); + return voxelBox.findRayIntersection(glm::vec3(originInVoxel), glm::vec3(directionInVoxel), + distance, face, surfaceNormal); } bool RenderablePolyVoxEntityItem::findDetailedParabolaIntersection(const glm::vec3& origin, const glm::vec3& velocity, diff --git a/libraries/entities/src/EntityTree.cpp b/libraries/entities/src/EntityTree.cpp index 377e192bb1..1cf6e45d5b 100644 --- a/libraries/entities/src/EntityTree.cpp +++ b/libraries/entities/src/EntityTree.cpp @@ -825,15 +825,38 @@ bool findRayIntersectionOp(const OctreeElementPointer& element, void* extraData) RayArgs* args = static_cast(extraData); bool keepSearching = true; EntityTreeElementPointer entityTreeElementPointer = std::static_pointer_cast(element); - EntityItemID entityID = entityTreeElementPointer->findRayIntersection(args->origin, args->direction, keepSearching, + EntityItemID entityID = entityTreeElementPointer->findRayIntersection(args->origin, args->direction, args->element, args->distance, args->face, args->surfaceNormal, args->entityIdsToInclude, args->entityIdsToDiscard, args->visibleOnly, args->collidableOnly, args->extraInfo, args->precisionPicking); if (!entityID.isNull()) { args->entityID = entityID; + // We recurse OctreeElements in order, so if we hit something, we can stop immediately + keepSearching = false; } return keepSearching; } +float findRayIntersectionSortingOp(const OctreeElementPointer& element, void* extraData) { + RayArgs* args = static_cast(extraData); + EntityTreeElementPointer entityTreeElementPointer = std::static_pointer_cast(element); + float distance = FLT_MAX; + // If origin is inside the cube, always check this element first + if (entityTreeElementPointer->getAACube().contains(args->origin)) { + distance = 0.0f; + } else { + float boundDistance = FLT_MAX; + BoxFace face; + glm::vec3 surfaceNormal; + if (entityTreeElementPointer->getAACube().findRayIntersection(args->origin, args->direction, boundDistance, face, surfaceNormal)) { + // Don't add this cell if it's already farther than our best distance so far + if (boundDistance < args->distance) { + distance = boundDistance; + } + } + } + return distance; +} + EntityItemID EntityTree::findRayIntersection(const glm::vec3& origin, const glm::vec3& direction, QVector entityIdsToInclude, QVector entityIdsToDiscard, bool visibleOnly, bool collidableOnly, bool precisionPicking, @@ -846,7 +869,7 @@ EntityItemID EntityTree::findRayIntersection(const glm::vec3& origin, const glm: bool requireLock = lockType == Octree::Lock; bool lockResult = withReadLock([&]{ - recurseTreeWithOperation(findRayIntersectionOp, &args); + recurseTreeWithOperationSorted(findRayIntersectionOp, findRayIntersectionSortingOp, &args); }, requireLock); if (accurateResult) { @@ -860,15 +883,38 @@ bool findParabolaIntersectionOp(const OctreeElementPointer& element, void* extra ParabolaArgs* args = static_cast(extraData); bool keepSearching = true; EntityTreeElementPointer entityTreeElementPointer = std::static_pointer_cast(element); - EntityItemID entityID = entityTreeElementPointer->findParabolaIntersection(args->origin, args->velocity, args->acceleration, keepSearching, + EntityItemID entityID = entityTreeElementPointer->findParabolaIntersection(args->origin, args->velocity, args->acceleration, args->element, args->parabolicDistance, args->face, args->surfaceNormal, args->entityIdsToInclude, args->entityIdsToDiscard, args->visibleOnly, args->collidableOnly, args->extraInfo, args->precisionPicking); if (!entityID.isNull()) { args->entityID = entityID; + // We recurse OctreeElements in order, so if we hit something, we can stop immediately + keepSearching = false; } return keepSearching; } +float findParabolaIntersectionSortingOp(const OctreeElementPointer& element, void* extraData) { + ParabolaArgs* args = static_cast(extraData); + EntityTreeElementPointer entityTreeElementPointer = std::static_pointer_cast(element); + float distance = FLT_MAX; + // If origin is inside the cube, always check this element first + if (entityTreeElementPointer->getAACube().contains(args->origin)) { + distance = 0.0f; + } else { + float boundDistance = FLT_MAX; + BoxFace face; + glm::vec3 surfaceNormal; + if (entityTreeElementPointer->getAACube().findParabolaIntersection(args->origin, args->velocity, args->acceleration, boundDistance, face, surfaceNormal)) { + // Don't add this cell if it's already farther than our best distance so far + if (boundDistance < args->parabolicDistance) { + distance = boundDistance; + } + } + } + return distance; +} + EntityItemID EntityTree::findParabolaIntersection(const PickParabola& parabola, QVector entityIdsToInclude, QVector entityIdsToDiscard, bool visibleOnly, bool collidableOnly, bool precisionPicking, @@ -882,7 +928,7 @@ EntityItemID EntityTree::findParabolaIntersection(const PickParabola& parabola, bool requireLock = lockType == Octree::Lock; bool lockResult = withReadLock([&] { - recurseTreeWithOperation(findParabolaIntersectionOp, &args); + recurseTreeWithOperationSorted(findParabolaIntersectionOp, findParabolaIntersectionSortingOp, &args); }, requireLock); if (accurateResult) { diff --git a/libraries/entities/src/EntityTreeElement.cpp b/libraries/entities/src/EntityTreeElement.cpp index 5974fce6c5..e8e11c0ee1 100644 --- a/libraries/entities/src/EntityTreeElement.cpp +++ b/libraries/entities/src/EntityTreeElement.cpp @@ -140,27 +140,17 @@ bool EntityTreeElement::bestFitBounds(const glm::vec3& minPoint, const glm::vec3 } EntityItemID EntityTreeElement::findRayIntersection(const glm::vec3& origin, const glm::vec3& direction, - bool& keepSearching, OctreeElementPointer& element, float& distance, - BoxFace& face, glm::vec3& surfaceNormal, const QVector& entityIdsToInclude, - const QVector& entityIdsToDiscard, bool visibleOnly, bool collidableOnly, - QVariantMap& extraInfo, bool precisionPicking) { + OctreeElementPointer& element, float& distance, BoxFace& face, glm::vec3& surfaceNormal, + const QVector& entityIdsToInclude, const QVector& entityIdsToDiscard, + bool visibleOnly, bool collidableOnly, QVariantMap& extraInfo, bool precisionPicking) { EntityItemID result; - float distanceToElementCube = std::numeric_limits::max(); + float distanceToElementCube = FLT_MAX; BoxFace localFace; glm::vec3 localSurfaceNormal; - // if the ray doesn't intersect with our cube OR the distance to element is less than current best distance - // we can stop searching! - bool hit = _cube.findRayIntersection(origin, direction, distanceToElementCube, localFace, localSurfaceNormal); - if (!hit || (!_cube.contains(origin) && distanceToElementCube > distance)) { - keepSearching = false; // no point in continuing to search - return result; // we did not intersect - } - - // by default, we only allow intersections with leaves with content if (!canPickIntersect()) { - return result; // we don't intersect with non-leaves, and we keep searching + return result; } // if the distance to the element cube is not less than the current best distance, then it's not possible @@ -289,7 +279,7 @@ bool EntityTreeElement::findSpherePenetration(const glm::vec3& center, float rad } EntityItemID EntityTreeElement::findParabolaIntersection(const glm::vec3& origin, const glm::vec3& velocity, - const glm::vec3& acceleration, bool& keepSearching, OctreeElementPointer& element, float& parabolicDistance, + const glm::vec3& acceleration, OctreeElementPointer& element, float& parabolicDistance, BoxFace& face, glm::vec3& surfaceNormal, const QVector& entityIdsToInclude, const QVector& entityIdsToDiscard, bool visibleOnly, bool collidableOnly, QVariantMap& extraInfo, bool precisionPicking) { @@ -299,17 +289,8 @@ EntityItemID EntityTreeElement::findParabolaIntersection(const glm::vec3& origin BoxFace localFace; glm::vec3 localSurfaceNormal; - // if the parabola doesn't intersect with our cube OR the distance to element is less than current best distance - // we can stop searching! - bool hit = _cube.findParabolaIntersection(origin, velocity, acceleration, distanceToElementCube, localFace, localSurfaceNormal); - if (!hit || (!_cube.contains(origin) && distanceToElementCube > parabolicDistance)) { - keepSearching = false; // no point in continuing to search - return result; // we did not intersect - } - - // by default, we only allow intersections with leaves with content if (!canPickIntersect()) { - return result; // we don't intersect with non-leaves, and we keep searching + return result; } // if the distance to the element cube is not less than the current best distance, then it's not possible diff --git a/libraries/entities/src/EntityTreeElement.h b/libraries/entities/src/EntityTreeElement.h index d6f9db08d6..793340c9a4 100644 --- a/libraries/entities/src/EntityTreeElement.h +++ b/libraries/entities/src/EntityTreeElement.h @@ -136,10 +136,9 @@ public: virtual bool canPickIntersect() const override { return hasEntities(); } virtual EntityItemID findRayIntersection(const glm::vec3& origin, const glm::vec3& direction, - bool& keepSearching, OctreeElementPointer& element, float& distance, - BoxFace& face, glm::vec3& surfaceNormal, const QVector& entityIdsToInclude, - const QVector& entityIdsToDiscard, bool visibleOnly, bool collidableOnly, - QVariantMap& extraInfo, bool precisionPicking = false); + OctreeElementPointer& element, float& distance, BoxFace& face, glm::vec3& surfaceNormal, + const QVector& entityIdsToInclude, const QVector& entityIdsToDiscard, + bool visibleOnly, bool collidableOnly, QVariantMap& extraInfo, bool precisionPicking = false); virtual EntityItemID findDetailedRayIntersection(const glm::vec3& origin, const glm::vec3& direction, OctreeElementPointer& element, float& distance, BoxFace& face, glm::vec3& surfaceNormal, const QVector& entityIdsToInclude, @@ -149,7 +148,7 @@ public: glm::vec3& penetration, void** penetratedObject) const override; virtual EntityItemID findParabolaIntersection(const glm::vec3& origin, const glm::vec3& velocity, - const glm::vec3& acceleration, bool& keepSearching, OctreeElementPointer& element, float& parabolicDistance, + const glm::vec3& acceleration, OctreeElementPointer& element, float& parabolicDistance, BoxFace& face, glm::vec3& surfaceNormal, const QVector& entityIdsToInclude, const QVector& entityIdsToDiscard, bool visibleOnly, bool collidableOnly, QVariantMap& extraInfo, bool precisionPicking = false); diff --git a/libraries/entities/src/ShapeEntityItem.cpp b/libraries/entities/src/ShapeEntityItem.cpp index e4ea1470c1..773a7059dc 100644 --- a/libraries/entities/src/ShapeEntityItem.cpp +++ b/libraries/entities/src/ShapeEntityItem.cpp @@ -262,20 +262,18 @@ bool ShapeEntityItem::findDetailedRayIntersection(const glm::vec3& origin, const glm::mat4 entityToWorldMatrix = getEntityToWorldMatrix(); 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, 0.0f))); + glm::vec3 entityFrameDirection = glm::vec3(worldToEntityMatrix * glm::vec4(direction, 0.0f)); - float localDistance; // NOTE: unit sphere has center of 0,0,0 and radius of 0.5 - if (findRaySphereIntersection(entityFrameOrigin, entityFrameDirection, glm::vec3(0.0f), 0.5f, localDistance)) { - // determine where on the unit sphere the hit point occured - glm::vec3 entityFrameHitAt = entityFrameOrigin + (entityFrameDirection * localDistance); - // then translate back to work coordinates - glm::vec3 hitAt = glm::vec3(entityToWorldMatrix * glm::vec4(entityFrameHitAt, 1.0f)); - distance = glm::distance(origin, hitAt); + if (findRaySphereIntersection(entityFrameOrigin, entityFrameDirection, glm::vec3(0.0f), 0.5f, distance)) { bool success; - // FIXME: this is only correct for uniformly scaled spheres - surfaceNormal = glm::normalize(hitAt - getCenterPosition(success)); - if (!success) { + glm::vec3 center = getCenterPosition(success); + if (success) { + // FIXME: this is only correct for uniformly scaled spheres + // determine where on the unit sphere the hit point occured + glm::vec3 hitAt = origin + (direction * distance); + surfaceNormal = glm::normalize(hitAt - center); + } else { return false; } return true; @@ -297,9 +295,11 @@ bool ShapeEntityItem::findDetailedParabolaIntersection(const glm::vec3& origin, // NOTE: unit sphere has center of 0,0,0 and radius of 0.5 if (findParabolaSphereIntersection(entityFrameOrigin, entityFrameVelocity, entityFrameAcceleration, glm::vec3(0.0f), 0.5f, parabolicDistance)) { bool success; - // FIXME: this is only correct for uniformly scaled spheres - surfaceNormal = glm::normalize((origin + velocity * parabolicDistance + 0.5f * acceleration * parabolicDistance * parabolicDistance) - getCenterPosition(success)); - if (!success) { + glm::vec3 center = getCenterPosition(success); + if (success) { + // FIXME: this is only correct for uniformly scaled spheres + surfaceNormal = glm::normalize((origin + velocity * parabolicDistance + 0.5f * acceleration * parabolicDistance * parabolicDistance) - center); + } else { return false; } return true; diff --git a/libraries/octree/src/Octree.cpp b/libraries/octree/src/Octree.cpp index 9bb0e25982..c6474c1fe4 100644 --- a/libraries/octree/src/Octree.cpp +++ b/libraries/octree/src/Octree.cpp @@ -68,55 +68,12 @@ Octree::~Octree() { eraseAllOctreeElements(false); } - -// Inserts the value and key into three arrays sorted by the key array, the first array is the value, -// the second array is a sorted key for the value, the third array is the index for the value in it original -// non-sorted array -// returns -1 if size exceeded -// originalIndexArray is optional -int insertOctreeElementIntoSortedArrays(const OctreeElementPointer& value, float key, int originalIndex, - OctreeElementPointer* valueArray, float* keyArray, int* originalIndexArray, - int currentCount, int maxCount) { - - if (currentCount < maxCount) { - int i = 0; - if (currentCount > 0) { - while (i < currentCount && key > keyArray[i]) { - i++; - } - // i is our desired location - // shift array elements to the right - if (i < currentCount && i+1 < maxCount) { - for (int j = currentCount - 1; j > i; j--) { - valueArray[j] = valueArray[j - 1]; - keyArray[j] = keyArray[j - 1]; - } - } - } - // place new element at i - valueArray[i] = value; - keyArray[i] = key; - if (originalIndexArray) { - originalIndexArray[i] = originalIndex; - } - return currentCount + 1; - } - return -1; // error case -} - - - // Recurses voxel tree calling the RecurseOctreeOperation function for each element. // stops recursion if operation function returns false. void Octree::recurseTreeWithOperation(const RecurseOctreeOperation& operation, void* extraData) { recurseElementWithOperation(_rootElement, operation, extraData); } -// Recurses voxel tree calling the RecurseOctreePostFixOperation function for each element in post-fix order. -void Octree::recurseTreeWithPostOperation(const RecurseOctreeOperation& operation, void* extraData) { - recurseElementWithPostOperation(_rootElement, operation, extraData); -} - // Recurses voxel element with an operation function void Octree::recurseElementWithOperation(const OctreeElementPointer& element, const RecurseOctreeOperation& operation, void* extraData, int recursionCount) { @@ -129,71 +86,53 @@ void Octree::recurseElementWithOperation(const OctreeElementPointer& element, co for (int i = 0; i < NUMBER_OF_CHILDREN; i++) { OctreeElementPointer child = element->getChildAtIndex(i); if (child) { - recurseElementWithOperation(child, operation, extraData, recursionCount+1); + recurseElementWithOperation(child, operation, extraData, recursionCount + 1); } } } } -// Recurses voxel element with an operation function -void Octree::recurseElementWithPostOperation(const OctreeElementPointer& element, const RecurseOctreeOperation& operation, - void* extraData, int recursionCount) { +void Octree::recurseTreeWithOperationSorted(const RecurseOctreeOperation& operation, const RecurseOctreeSortingOperation& sortingOperation, void* extraData) { + recurseElementWithOperationSorted(_rootElement, operation, sortingOperation, extraData); +} + +// Recurses voxel element with an operation function, calling operation on its children in a specific order +bool Octree::recurseElementWithOperationSorted(const OctreeElementPointer& element, const RecurseOctreeOperation& operation, + const RecurseOctreeSortingOperation& sortingOperation, void* extraData, int recursionCount) { if (recursionCount > DANGEROUSLY_DEEP_RECURSION) { - HIFI_FCDEBUG(octree(), "Octree::recurseElementWithPostOperation() reached DANGEROUSLY_DEEP_RECURSION, bailing!"); - return; + HIFI_FCDEBUG(octree(), "Octree::recurseElementWithOperationSorted() reached DANGEROUSLY_DEEP_RECURSION, bailing!"); + // If we go too deep, we want to keep searching other paths + return true; } + bool keepSearching = operation(element, extraData); + + std::vector sortedChildren; for (int i = 0; i < NUMBER_OF_CHILDREN; i++) { OctreeElementPointer child = element->getChildAtIndex(i); if (child) { - recurseElementWithPostOperation(child, operation, extraData, recursionCount+1); - } - } - operation(element, extraData); -} - -// Recurses voxel tree calling the RecurseOctreeOperation function for each element. -// stops recursion if operation function returns false. -void Octree::recurseTreeWithOperationDistanceSorted(const RecurseOctreeOperation& operation, - const glm::vec3& point, void* extraData) { - - recurseElementWithOperationDistanceSorted(_rootElement, operation, point, extraData); -} - -// Recurses voxel element with an operation function -void Octree::recurseElementWithOperationDistanceSorted(const OctreeElementPointer& element, const RecurseOctreeOperation& operation, - const glm::vec3& point, void* extraData, int recursionCount) { - - if (recursionCount > DANGEROUSLY_DEEP_RECURSION) { - HIFI_FCDEBUG(octree(), "Octree::recurseElementWithOperationDistanceSorted() reached DANGEROUSLY_DEEP_RECURSION, bailing!"); - return; - } - - if (operation(element, extraData)) { - // determine the distance sorted order of our children - OctreeElementPointer sortedChildren[NUMBER_OF_CHILDREN] = { NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL }; - float distancesToChildren[NUMBER_OF_CHILDREN] = { 0, 0, 0, 0, 0, 0, 0, 0 }; - int indexOfChildren[NUMBER_OF_CHILDREN] = { 0, 0, 0, 0, 0, 0, 0, 0 }; - int currentCount = 0; - - for (int i = 0; i < NUMBER_OF_CHILDREN; i++) { - OctreeElementPointer childElement = element->getChildAtIndex(i); - if (childElement) { - // chance to optimize, doesn't need to be actual distance!! Could be distance squared - float distanceSquared = childElement->distanceSquareToPoint(point); - currentCount = insertOctreeElementIntoSortedArrays(childElement, distanceSquared, i, - sortedChildren, (float*)&distancesToChildren, - (int*)&indexOfChildren, currentCount, NUMBER_OF_CHILDREN); - } - } - - for (int i = 0; i < currentCount; i++) { - OctreeElementPointer childElement = sortedChildren[i]; - if (childElement) { - recurseElementWithOperationDistanceSorted(childElement, operation, point, extraData); + float priority = sortingOperation(child, extraData); + if (priority < FLT_MAX) { + sortedChildren.emplace_back(priority, child); } } } + + if (sortedChildren.size() > 1) { + static auto comparator = [](const SortedChild& left, const SortedChild& right) { return left.first > right.first; }; + std::sort(sortedChildren.begin(), sortedChildren.end(), comparator); + } + + for (auto it = sortedChildren.begin(); it != sortedChildren.end(); ++it) { + const SortedChild& sortedChild = *it; + // Our children were sorted, so if one hits something, we don't need to check the others + if (!recurseElementWithOperationSorted(sortedChild.second, operation, sortingOperation, extraData, recursionCount + 1)) { + return false; + } + } + // We checked all our children and didn't find anything. + // Stop if we hit something in this element. Continue if we didn't. + return keepSearching; } void Octree::recurseTreeWithOperator(RecurseOctreeOperator* operatorObject) { diff --git a/libraries/octree/src/Octree.h b/libraries/octree/src/Octree.h index b827ed7cd0..a2b2f227cb 100644 --- a/libraries/octree/src/Octree.h +++ b/libraries/octree/src/Octree.h @@ -49,6 +49,9 @@ public: // Callback function, for recuseTreeWithOperation using RecurseOctreeOperation = std::function; +// Function for sorting octree children during recursion. If return value == FLT_MAX, child is discarded +using RecurseOctreeSortingOperation = std::function; +using SortedChild = std::pair; typedef QHash CubeList; const bool NO_EXISTS_BITS = false; @@ -163,17 +166,10 @@ public: OctreeElementPointer getOrCreateChildElementContaining(const AACube& box); void recurseTreeWithOperation(const RecurseOctreeOperation& operation, void* extraData = NULL); - void recurseTreeWithPostOperation(const RecurseOctreeOperation& operation, void* extraData = NULL); - - /// \param operation type of operation - /// \param point point in world-frame (meters) - /// \param extraData hook for user data to be interpreted by special context - void recurseTreeWithOperationDistanceSorted(const RecurseOctreeOperation& operation, - const glm::vec3& point, void* extraData = NULL); + void recurseTreeWithOperationSorted(const RecurseOctreeOperation& operation, const RecurseOctreeSortingOperation& sortingOperation, void* extraData = NULL); void recurseTreeWithOperator(RecurseOctreeOperator* operatorObject); - bool isDirty() const { return _isDirty; } void clearDirtyBit() { _isDirty = false; } void setDirtyBit() { _isDirty = true; } @@ -227,14 +223,8 @@ public: void recurseElementWithOperation(const OctreeElementPointer& element, const RecurseOctreeOperation& operation, void* extraData, int recursionCount = 0); - - /// Traverse child nodes of node applying operation in post-fix order - /// - void recurseElementWithPostOperation(const OctreeElementPointer& element, const RecurseOctreeOperation& operation, - void* extraData, int recursionCount = 0); - - void recurseElementWithOperationDistanceSorted(const OctreeElementPointer& element, const RecurseOctreeOperation& operation, - const glm::vec3& point, void* extraData, int recursionCount = 0); + bool recurseElementWithOperationSorted(const OctreeElementPointer& element, const RecurseOctreeOperation& operation, + const RecurseOctreeSortingOperation& sortingOperation, void* extraData, int recursionCount = 0); bool recurseElementWithOperator(const OctreeElementPointer& element, RecurseOctreeOperator* operatorObject, int recursionCount = 0); diff --git a/libraries/render-utils/src/Model.cpp b/libraries/render-utils/src/Model.cpp index 8f4eb57f8e..22bc6b6ca7 100644 --- a/libraries/render-utils/src/Model.cpp +++ b/libraries/render-utils/src/Model.cpp @@ -379,14 +379,17 @@ bool Model::findRayIntersectionAgainstSubMeshes(const glm::vec3& origin, const g if (modelFrameBox.findRayIntersection(modelFrameOrigin, modelFrameDirection, distance, face, surfaceNormal)) { QMutexLocker locker(&_mutex); - float bestDistance = std::numeric_limits::max(); + float bestDistance = FLT_MAX; + BoxFace bestFace; Triangle bestModelTriangle; Triangle bestWorldTriangle; - int bestSubMeshIndex = 0; + glm::vec3 bestWorldIntersectionPoint; + glm::vec3 bestMeshIntersectionPoint; + int bestPartIndex; + int bestShapeID; + int bestSubMeshIndex; - int subMeshIndex = 0; const FBXGeometry& geometry = getFBXGeometry(); - if (!_triangleSetsValid) { calculateTriangleSets(geometry); } @@ -399,39 +402,75 @@ bool Model::findRayIntersectionAgainstSubMeshes(const glm::vec3& origin, const g glm::vec3 meshFrameDirection = glm::vec3(worldToMeshMatrix * glm::vec4(direction, 0.0f)); int shapeID = 0; + int subMeshIndex = 0; + + std::vector sortedTriangleSets; for (auto& meshTriangleSets : _modelSpaceMeshTriangleSets) { int partIndex = 0; - for (auto &partTriangleSet : meshTriangleSets) { - float triangleSetDistance; - BoxFace triangleSetFace; - Triangle triangleSetTriangle; - if (partTriangleSet.findRayIntersection(meshFrameOrigin, meshFrameDirection, triangleSetDistance, triangleSetFace, triangleSetTriangle, pickAgainstTriangles, allowBackface)) { - glm::vec3 meshIntersectionPoint = meshFrameOrigin + (meshFrameDirection * triangleSetDistance); - glm::vec3 worldIntersectionPoint = glm::vec3(meshToWorldMatrix * glm::vec4(meshIntersectionPoint, 1.0f)); - float worldDistance = glm::distance(origin, worldIntersectionPoint); - - if (worldDistance < bestDistance) { - bestDistance = worldDistance; - intersectedSomething = true; - face = triangleSetFace; - bestModelTriangle = triangleSetTriangle; - bestWorldTriangle = triangleSetTriangle * meshToWorldMatrix; - extraInfo["worldIntersectionPoint"] = vec3toVariant(worldIntersectionPoint); - extraInfo["meshIntersectionPoint"] = vec3toVariant(meshIntersectionPoint); - extraInfo["partIndex"] = partIndex; - extraInfo["shapeID"] = shapeID; - bestSubMeshIndex = subMeshIndex; + for (auto& partTriangleSet : meshTriangleSets) { + float priority = FLT_MAX; + if (partTriangleSet.getBounds().contains(meshFrameOrigin)) { + priority = 0.0f; + } else { + float partBoundDistance = FLT_MAX; + BoxFace partBoundFace; + glm::vec3 partBoundNormal; + if (partTriangleSet.getBounds().findRayIntersection(meshFrameOrigin, meshFrameDirection, partBoundDistance, + partBoundFace, partBoundNormal)) { + priority = partBoundDistance; } } + + if (priority < FLT_MAX) { + sortedTriangleSets.emplace_back(priority, &partTriangleSet, partIndex, shapeID, subMeshIndex); + } partIndex++; shapeID++; } subMeshIndex++; } + if (sortedTriangleSets.size() > 1) { + static auto comparator = [](const SortedTriangleSet& left, const SortedTriangleSet& right) { return left.distance > right.distance; }; + std::sort(sortedTriangleSets.begin(), sortedTriangleSets.end(), comparator); + } + + for (auto it = sortedTriangleSets.begin(); it != sortedTriangleSets.end(); ++it) { + const SortedTriangleSet& sortedTriangleSet = *it; + // We can exit once triangleSetDistance > bestDistance + if (sortedTriangleSet.distance > bestDistance) { + break; + } + float triangleSetDistance = FLT_MAX; + BoxFace triangleSetFace; + Triangle triangleSetTriangle; + if (sortedTriangleSet.triangleSet->findRayIntersection(meshFrameOrigin, meshFrameDirection, triangleSetDistance, triangleSetFace, + triangleSetTriangle, pickAgainstTriangles, allowBackface)) { + if (triangleSetDistance < bestDistance) { + bestDistance = triangleSetDistance; + intersectedSomething = true; + bestFace = triangleSetFace; + bestModelTriangle = triangleSetTriangle; + bestWorldTriangle = triangleSetTriangle * meshToWorldMatrix; + glm::vec3 meshIntersectionPoint = meshFrameOrigin + (meshFrameDirection * triangleSetDistance); + glm::vec3 worldIntersectionPoint = glm::vec3(meshToWorldMatrix * glm::vec4(meshIntersectionPoint, 1.0f)); + bestWorldIntersectionPoint = worldIntersectionPoint; + bestMeshIntersectionPoint = meshIntersectionPoint; + bestPartIndex = sortedTriangleSet.partIndex; + bestShapeID = sortedTriangleSet.shapeID; + bestSubMeshIndex = sortedTriangleSet.subMeshIndex; + } + } + } + if (intersectedSomething) { distance = bestDistance; + face = bestFace; surfaceNormal = bestWorldTriangle.getNormal(); + extraInfo["worldIntersectionPoint"] = vec3toVariant(bestWorldIntersectionPoint); + extraInfo["meshIntersectionPoint"] = vec3toVariant(bestMeshIntersectionPoint); + extraInfo["partIndex"] = bestPartIndex; + extraInfo["shapeID"] = bestShapeID; if (pickAgainstTriangles) { extraInfo["subMeshIndex"] = bestSubMeshIndex; extraInfo["subMeshName"] = geometry.getModelNameOfMesh(bestSubMeshIndex); @@ -483,13 +522,16 @@ bool Model::findParabolaIntersectionAgainstSubMeshes(const glm::vec3& origin, co QMutexLocker locker(&_mutex); float bestDistance = FLT_MAX; + BoxFace bestFace; Triangle bestModelTriangle; Triangle bestWorldTriangle; - int bestSubMeshIndex = 0; + glm::vec3 bestWorldIntersectionPoint; + glm::vec3 bestMeshIntersectionPoint; + int bestPartIndex; + int bestShapeID; + int bestSubMeshIndex; - int subMeshIndex = 0; const FBXGeometry& geometry = getFBXGeometry(); - if (!_triangleSetsValid) { calculateTriangleSets(geometry); } @@ -503,40 +545,79 @@ bool Model::findParabolaIntersectionAgainstSubMeshes(const glm::vec3& origin, co glm::vec3 meshFrameAcceleration = glm::vec3(worldToMeshMatrix * glm::vec4(acceleration, 0.0f)); int shapeID = 0; + int subMeshIndex = 0; + + std::vector sortedTriangleSets; for (auto& meshTriangleSets : _modelSpaceMeshTriangleSets) { int partIndex = 0; - for (auto &partTriangleSet : meshTriangleSets) { - float triangleSetDistance; - BoxFace triangleSetFace; - Triangle triangleSetTriangle; - if (partTriangleSet.findParabolaIntersection(meshFrameOrigin, meshFrameVelocity, meshFrameAcceleration, - triangleSetDistance, triangleSetFace, triangleSetTriangle, pickAgainstTriangles, allowBackface)) { - if (triangleSetDistance < bestDistance) { - bestDistance = triangleSetDistance; - intersectedSomething = true; - face = triangleSetFace; - bestModelTriangle = triangleSetTriangle; - bestWorldTriangle = triangleSetTriangle * meshToWorldMatrix; - glm::vec3 meshIntersectionPoint = meshFrameOrigin + meshFrameVelocity * triangleSetDistance + - 0.5f * meshFrameAcceleration * triangleSetDistance * triangleSetDistance; - glm::vec3 worldIntersectionPoint = origin + velocity * triangleSetDistance + - 0.5f * acceleration * triangleSetDistance * triangleSetDistance; - extraInfo["worldIntersectionPoint"] = vec3toVariant(worldIntersectionPoint); - extraInfo["meshIntersectionPoint"] = vec3toVariant(meshIntersectionPoint); - extraInfo["partIndex"] = partIndex; - extraInfo["shapeID"] = shapeID; - bestSubMeshIndex = subMeshIndex; + for (auto& partTriangleSet : meshTriangleSets) { + float priority = FLT_MAX; + if (partTriangleSet.getBounds().contains(meshFrameOrigin)) { + priority = 0.0f; + } else { + float partBoundDistance = FLT_MAX; + BoxFace partBoundFace; + glm::vec3 partBoundNormal; + if (partTriangleSet.getBounds().findParabolaIntersection(meshFrameOrigin, meshFrameVelocity, meshFrameAcceleration, + partBoundDistance, partBoundFace, partBoundNormal)) { + priority = partBoundDistance; } } + + if (priority < FLT_MAX) { + sortedTriangleSets.emplace_back(priority, &partTriangleSet, partIndex, shapeID, subMeshIndex); + } partIndex++; shapeID++; } subMeshIndex++; } + if (sortedTriangleSets.size() > 1) { + static auto comparator = [](const SortedTriangleSet& left, const SortedTriangleSet& right) { return left.distance > right.distance; }; + std::sort(sortedTriangleSets.begin(), sortedTriangleSets.end(), comparator); + } + + for (auto it = sortedTriangleSets.begin(); it != sortedTriangleSets.end(); ++it) { + const SortedTriangleSet& sortedTriangleSet = *it; + // We can exit once triangleSetDistance > bestDistance + if (sortedTriangleSet.distance > bestDistance) { + break; + } + float triangleSetDistance = FLT_MAX; + BoxFace triangleSetFace; + Triangle triangleSetTriangle; + if (sortedTriangleSet.triangleSet->findParabolaIntersection(meshFrameOrigin, meshFrameVelocity, meshFrameAcceleration, + triangleSetDistance, triangleSetFace, triangleSetTriangle, + pickAgainstTriangles, allowBackface)) { + if (triangleSetDistance < bestDistance) { + bestDistance = triangleSetDistance; + intersectedSomething = true; + bestFace = triangleSetFace; + bestModelTriangle = triangleSetTriangle; + bestWorldTriangle = triangleSetTriangle * meshToWorldMatrix; + glm::vec3 meshIntersectionPoint = meshFrameOrigin + meshFrameVelocity * triangleSetDistance + + 0.5f * meshFrameAcceleration * triangleSetDistance * triangleSetDistance; + glm::vec3 worldIntersectionPoint = origin + velocity * triangleSetDistance + + 0.5f * acceleration * triangleSetDistance * triangleSetDistance; + bestWorldIntersectionPoint = worldIntersectionPoint; + bestMeshIntersectionPoint = meshIntersectionPoint; + bestPartIndex = sortedTriangleSet.partIndex; + bestShapeID = sortedTriangleSet.shapeID; + bestSubMeshIndex = sortedTriangleSet.subMeshIndex; + // These sets can overlap, so we can't exit early if we find something + } + } + } + if (intersectedSomething) { parabolicDistance = bestDistance; + face = bestFace; surfaceNormal = bestWorldTriangle.getNormal(); + extraInfo["worldIntersectionPoint"] = vec3toVariant(bestWorldIntersectionPoint); + extraInfo["meshIntersectionPoint"] = vec3toVariant(bestMeshIntersectionPoint); + extraInfo["partIndex"] = bestPartIndex; + extraInfo["shapeID"] = bestShapeID; if (pickAgainstTriangles) { extraInfo["subMeshIndex"] = bestSubMeshIndex; extraInfo["subMeshName"] = geometry.getModelNameOfMesh(bestSubMeshIndex); diff --git a/libraries/render-utils/src/Model.h b/libraries/render-utils/src/Model.h index 627e5fddab..adb2fcdd3d 100644 --- a/libraries/render-utils/src/Model.h +++ b/libraries/render-utils/src/Model.h @@ -64,6 +64,16 @@ class Model; using ModelPointer = std::shared_ptr; using ModelWeakPointer = std::weak_ptr; +struct SortedTriangleSet { + SortedTriangleSet(float distance, TriangleSet* triangleSet, int partIndex, int shapeID, int subMeshIndex) : + distance(distance), triangleSet(triangleSet), partIndex(partIndex), shapeID(shapeID), subMeshIndex(subMeshIndex) {} + + float distance; + TriangleSet* triangleSet; + int partIndex; + int shapeID; + int subMeshIndex; +}; /// A generic 3D model displaying geometry loaded from a URL. class Model : public QObject, public std::enable_shared_from_this, public scriptable::ModelProvider { diff --git a/libraries/shared/src/GeometryUtil.cpp b/libraries/shared/src/GeometryUtil.cpp index 6fb06eb624..a93c2ec9f3 100644 --- a/libraries/shared/src/GeometryUtil.cpp +++ b/libraries/shared/src/GeometryUtil.cpp @@ -286,12 +286,13 @@ bool findRaySphereIntersection(const glm::vec3& origin, const glm::vec3& directi distance = 0.0f; return true; // starts inside the sphere } - float b = glm::dot(direction, relativeOrigin); - float radicand = b * b - c; + float b = 2.0f * glm::dot(direction, relativeOrigin); + float a = glm::dot(direction, direction); + float radicand = b * b - 4.0f * a * c; if (radicand < 0.0f) { return false; // doesn't hit the sphere } - float t = -b - sqrtf(radicand); + float t = 0.5f * (-b - sqrtf(radicand)) / a; if (t < 0.0f) { return false; // doesn't hit the sphere } diff --git a/libraries/shared/src/TriangleSet.cpp b/libraries/shared/src/TriangleSet.cpp index cde9c20cab..f3c0ef5776 100644 --- a/libraries/shared/src/TriangleSet.cpp +++ b/libraries/shared/src/TriangleSet.cpp @@ -13,6 +13,8 @@ #include "GLMHelpers.h" +#include + void TriangleSet::insert(const Triangle& t) { _isBalanced = false; @@ -30,47 +32,6 @@ void TriangleSet::clear() { _triangleOctree.clear(); } -bool TriangleSet::findRayIntersection(const glm::vec3& origin, const glm::vec3& direction, - float& distance, BoxFace& face, Triangle& triangle, bool precision, bool allowBackface) { - - // reset our distance to be the max possible, lower level tests will store best distance here - distance = std::numeric_limits::max(); - - if (!_isBalanced) { - balanceOctree(); - } - - int trianglesTouched = 0; - auto result = _triangleOctree.findRayIntersection(origin, direction, distance, face, triangle, precision, trianglesTouched, allowBackface); - - #if WANT_DEBUGGING - if (precision) { - qDebug() << "trianglesTouched :" << trianglesTouched << "out of:" << _triangleOctree._population << "_triangles.size:" << _triangles.size(); - } - #endif - return result; -} - -bool TriangleSet::findParabolaIntersection(const glm::vec3& origin, const glm::vec3& velocity, const glm::vec3& acceleration, - float& parabolicDistance, BoxFace& face, Triangle& triangle, bool precision, bool allowBackface) { - // reset our distance to be the max possible, lower level tests will store best distance here - parabolicDistance = FLT_MAX; - - if (!_isBalanced) { - balanceOctree(); - } - - int trianglesTouched = 0; - auto result = _triangleOctree.findParabolaIntersection(origin, velocity, acceleration, parabolicDistance, face, triangle, precision, trianglesTouched, allowBackface); - -#if WANT_DEBUGGING - if (precision) { - qDebug() << "trianglesTouched :" << trianglesTouched << "out of:" << _triangleOctree._population << "_triangles.size:" << _triangles.size(); - } -#endif - return result; -} - bool TriangleSet::convexHullContains(const glm::vec3& point) const { if (!_bounds.contains(point)) { return false; @@ -97,7 +58,7 @@ void TriangleSet::debugDump() { } void TriangleSet::balanceOctree() { - _triangleOctree.reset(_bounds, 0); + _triangleOctree.reset(_bounds); // insert all the triangles for (size_t i = 0; i < _triangles.size(); i++) { @@ -106,79 +67,15 @@ void TriangleSet::balanceOctree() { _isBalanced = true; - #if WANT_DEBUGGING +#if WANT_DEBUGGING debugDump(); - #endif +#endif } - -// Determine of the given ray (origin/direction) in model space intersects with any triangles -// in the set. If an intersection occurs, the distance and surface normal will be provided. -bool TriangleSet::TriangleOctreeCell::findRayIntersectionInternal(const glm::vec3& origin, const glm::vec3& direction, - float& distance, BoxFace& face, Triangle& triangle, bool precision, - int& trianglesTouched, bool allowBackface) { - bool intersectedSomething = false; - float bestDistance = FLT_MAX; - - if (precision) { - for (const auto& triangleIndex : _triangleIndices) { - const auto& thisTriangle = _allTriangles[triangleIndex]; - float thisTriangleDistance; - trianglesTouched++; - if (findRayTriangleIntersection(origin, direction, thisTriangle, thisTriangleDistance, allowBackface)) { - if (thisTriangleDistance < bestDistance) { - bestDistance = thisTriangleDistance; - intersectedSomething = true; - triangle = thisTriangle; - } - } - } - } else { - intersectedSomething = true; - bestDistance = distance; - } - - if (intersectedSomething) { - distance = bestDistance; - } - - return intersectedSomething; -} - -bool TriangleSet::TriangleOctreeCell::findParabolaIntersectionInternal(const glm::vec3& origin, const glm::vec3& velocity, - const glm::vec3& acceleration, float& parabolicDistance, - BoxFace& face, Triangle& triangle, bool precision, - int& trianglesTouched, bool allowBackface) { - bool intersectedSomething = false; - float bestDistance = FLT_MAX; - - if (precision) { - for (const auto& triangleIndex : _triangleIndices) { - const auto& thisTriangle = _allTriangles[triangleIndex]; - float thisTriangleDistance; - trianglesTouched++; - if (findParabolaTriangleIntersection(origin, velocity, acceleration, thisTriangle, thisTriangleDistance, allowBackface)) { - if (thisTriangleDistance < bestDistance) { - bestDistance = thisTriangleDistance; - intersectedSomething = true; - triangle = thisTriangle; - } - } - } - } else { - intersectedSomething = true; - bestDistance = parabolicDistance; - } - - if (intersectedSomething) { - parabolicDistance = bestDistance; - } - - return intersectedSomething; -} - -static const int MAX_DEPTH = 4; // for now -static const int MAX_CHILDREN = 8; +// With an octree: 8 ^ MAX_DEPTH = 4096 leaves +//static const int MAX_DEPTH = 4; +// With a k-d tree: 2 ^ MAX_DEPTH = 4096 leaves +static const int MAX_DEPTH = 12; TriangleSet::TriangleOctreeCell::TriangleOctreeCell(std::vector& allTriangles, const AABox& bounds, int depth) : _allTriangles(allTriangles) @@ -190,7 +87,8 @@ void TriangleSet::TriangleOctreeCell::clear() { _population = 0; _triangleIndices.clear(); _bounds.clear(); - _children.clear(); + _children.first.reset(); + _children.second.reset(); } void TriangleSet::TriangleOctreeCell::reset(const AABox& bounds, int depth) { @@ -201,45 +99,76 @@ void TriangleSet::TriangleOctreeCell::reset(const AABox& bounds, int depth) { void TriangleSet::TriangleOctreeCell::debugDump() { qDebug() << __FUNCTION__; - qDebug() << "bounds:" << getBounds(); - qDebug() << "depth:" << _depth; - qDebug() << "population:" << _population << "this level or below" + qDebug() << " bounds:" << getBounds(); + qDebug() << " depth:" << _depth; + qDebug() << " population:" << _population << "this level or below" << " ---- triangleIndices:" << _triangleIndices.size() << "in this cell"; - qDebug() << "child cells:" << _children.size(); + int numChildren = 0; + if (_children.first) { + numChildren++; + } else if (_children.second) { + numChildren++; + } + qDebug() << "child cells:" << numChildren; if (_depth < MAX_DEPTH) { - int childNum = 0; - for (auto& child : _children) { - qDebug() << "child:" << childNum; - child.second.debugDump(); - childNum++; + if (_children.first) { + qDebug() << "child: 0"; + _children.first->debugDump(); + } + if (_children.second) { + qDebug() << "child: 1"; + _children.second->debugDump(); } } } +std::pair TriangleSet::TriangleOctreeCell::getTriangleOctreeCellChildBounds() { + std::pair toReturn; + int axis = 0; + // find biggest axis + glm::vec3 dimensions = _bounds.getDimensions(); + for (int i = 0; i < 3; i++) { + if (dimensions[i] >= dimensions[(i + 1) % 3] && dimensions[i] >= dimensions[(i + 2) % 3]) { + axis = i; + break; + } + } + + // The new boxes are half the size in the largest dimension + glm::vec3 newDimensions = dimensions; + newDimensions[axis] *= 0.5f; + toReturn.first.setBox(_bounds.getCorner(), newDimensions); + glm::vec3 offset = glm::vec3(0.0f); + offset[axis] = newDimensions[axis]; + toReturn.second.setBox(_bounds.getCorner() + offset, newDimensions); + return toReturn; +} + void TriangleSet::TriangleOctreeCell::insert(size_t triangleIndex) { - const Triangle& triangle = _allTriangles[triangleIndex]; _population++; + // if we're not yet at the max depth, then check which child the triangle fits in if (_depth < MAX_DEPTH) { + const Triangle& triangle = _allTriangles[triangleIndex]; + auto childBounds = getTriangleOctreeCellChildBounds(); - for (int child = 0; child < MAX_CHILDREN; child++) { - AABox childBounds = getBounds().getOctreeChild((AABox::OctreeChild)child); - - + auto insertOperator = [&](const AABox& childBound, std::shared_ptr& child) { // if the child AABox would contain the triangle... - if (childBounds.contains(triangle)) { + if (childBound.contains(triangle)) { // if the child cell doesn't yet exist, create it... - if (_children.find((AABox::OctreeChild)child) == _children.end()) { - _children.insert( - std::pair - ((AABox::OctreeChild)child, TriangleOctreeCell(_allTriangles, childBounds, _depth + 1))); + if (!child) { + child = std::make_shared(_allTriangles, childBound, _depth + 1); } // insert the triangleIndex in the child cell - _children.at((AABox::OctreeChild)child).insert(triangleIndex); - return; + child->insert(triangleIndex); + return true; } + return false; + }; + if (insertOperator(childBounds.first, _children.first) || insertOperator(childBounds.second, _children.second)) { + return; } } // either we're at max depth, or the triangle doesn't fit in one of our @@ -247,6 +176,62 @@ void TriangleSet::TriangleOctreeCell::insert(size_t triangleIndex) { _triangleIndices.push_back(triangleIndex); } +bool TriangleSet::findRayIntersection(const glm::vec3& origin, const glm::vec3& direction, float& distance, + BoxFace& face, Triangle& triangle, bool precision, bool allowBackface) { + if (!_isBalanced) { + balanceOctree(); + } + + float localDistance = distance; + int trianglesTouched = 0; + bool hit = _triangleOctree.findRayIntersection(origin, direction, localDistance, face, triangle, precision, trianglesTouched, allowBackface); + if (hit) { + distance = localDistance; + } + +#if WANT_DEBUGGING + if (precision) { + qDebug() << "trianglesTouched :" << trianglesTouched << "out of:" << _triangleOctree._population << "_triangles.size:" << _triangles.size(); + } +#endif + return hit; +} + +// Determine of the given ray (origin/direction) in model space intersects with any triangles +// in the set. If an intersection occurs, the distance and surface normal will be provided. +bool TriangleSet::TriangleOctreeCell::findRayIntersectionInternal(const glm::vec3& origin, const glm::vec3& direction, + float& distance, BoxFace& face, Triangle& triangle, bool precision, + int& trianglesTouched, bool allowBackface) { + bool intersectedSomething = false; + float bestDistance = FLT_MAX; + Triangle bestTriangle; + + if (precision) { + for (const auto& triangleIndex : _triangleIndices) { + const auto& thisTriangle = _allTriangles[triangleIndex]; + float thisTriangleDistance; + trianglesTouched++; + if (findRayTriangleIntersection(origin, direction, thisTriangle, thisTriangleDistance, allowBackface)) { + if (thisTriangleDistance < bestDistance) { + bestDistance = thisTriangleDistance; + bestTriangle = thisTriangle; + intersectedSomething = true; + } + } + } + } else { + intersectedSomething = true; + bestDistance = distance; + } + + if (intersectedSomething) { + distance = bestDistance; + triangle = bestTriangle; + } + + return intersectedSomething; +} + bool TriangleSet::TriangleOctreeCell::findRayIntersection(const glm::vec3& origin, const glm::vec3& direction, float& distance, BoxFace& face, Triangle& triangle, bool precision, int& trianglesTouched, bool allowBackface) { @@ -257,52 +242,81 @@ bool TriangleSet::TriangleOctreeCell::findRayIntersection(const glm::vec3& origi float bestLocalDistance = FLT_MAX; BoxFace bestLocalFace; Triangle bestLocalTriangle; - glm::vec3 bestLocalNormal; bool intersects = false; - float boxDistance = FLT_MAX; - // if the pick intersects our bounding box, then continue - if (getBounds().findRayIntersection(origin, direction, boxDistance, bestLocalFace, bestLocalNormal)) { - // if the intersection with our bounding box, is greater than the current best distance (the distance passed in) - // then we know that none of our triangles can represent a better intersection and we can return - if (boxDistance > distance) { - return false; - } - - // if we're not yet at the max depth, then check which child the triangle fits in - if (_depth < MAX_DEPTH) { - float bestChildDistance = FLT_MAX; - for (auto& child : _children) { - // check each child, if there's an intersection, it will return some distance that we need - // to compare against the other results, because there might be multiple intersections and - // we will always choose the best (shortest) intersection - float childDistance = bestChildDistance; - BoxFace childFace; - Triangle childTriangle; - if (child.second.findRayIntersection(origin, direction, childDistance, childFace, childTriangle, precision, trianglesTouched)) { - if (childDistance < bestLocalDistance) { - bestLocalDistance = childDistance; - bestChildDistance = childDistance; - bestLocalFace = childFace; - bestLocalTriangle = childTriangle; - intersects = true; - } - } - } - } - // also check our local triangle set - float internalDistance = boxDistance; + // Check our local triangle set first + // The distance passed in here is the distance to our bounding box. If !precision, that distance is used + { + float internalDistance = distance; BoxFace internalFace; Triangle internalTriangle; if (findRayIntersectionInternal(origin, direction, internalDistance, internalFace, internalTriangle, precision, trianglesTouched, allowBackface)) { - if (internalDistance < bestLocalDistance) { - bestLocalDistance = internalDistance; - bestLocalFace = internalFace; - bestLocalTriangle = internalTriangle; - intersects = true; + bestLocalDistance = internalDistance; + bestLocalFace = internalFace; + bestLocalTriangle = internalTriangle; + intersects = true; + } + } + + // if we're not yet at the max depth, then check our children + if (_depth < MAX_DEPTH) { + std::list sortedTriangleCells; + auto sortingOperator = [&](std::shared_ptr& child) { + if (child) { + float priority = FLT_MAX; + if (child->getBounds().contains(origin)) { + priority = 0.0f; + } else { + float childBoundDistance = FLT_MAX; + BoxFace childBoundFace; + glm::vec3 childBoundNormal; + if (child->getBounds().findRayIntersection(origin, direction, childBoundDistance, childBoundFace, childBoundNormal)) { + // We only need to add this cell if it's closer than the local triangle set intersection (if there was one) + if (childBoundDistance < bestLocalDistance) { + priority = childBoundDistance; + } + } + } + + if (priority < FLT_MAX) { + if (sortedTriangleCells.size() > 0 && priority < sortedTriangleCells.front().first) { + sortedTriangleCells.emplace_front(priority, child); + } else { + sortedTriangleCells.emplace_back(priority, child); + } + } + } + }; + sortingOperator(_children.first); + sortingOperator(_children.second); + + for (auto it = sortedTriangleCells.begin(); it != sortedTriangleCells.end(); ++it) { + const SortedTriangleCell& sortedTriangleCell = *it; + float childDistance = sortedTriangleCell.first; + // We can exit once childDistance > bestLocalDistance + if (childDistance > bestLocalDistance) { + break; + } + // If we're inside the child cell and !precision, we need the actual distance to the cell bounds + if (!precision && childDistance < EPSILON) { + BoxFace childBoundFace; + glm::vec3 childBoundNormal; + sortedTriangleCell.second->getBounds().findRayIntersection(origin, direction, childDistance, childBoundFace, childBoundNormal); + } + BoxFace childFace; + Triangle childTriangle; + if (sortedTriangleCell.second->findRayIntersection(origin, direction, childDistance, childFace, childTriangle, precision, trianglesTouched)) { + if (childDistance < bestLocalDistance) { + bestLocalDistance = childDistance; + bestLocalFace = childFace; + bestLocalTriangle = childTriangle; + intersects = true; + break; + } } } } + if (intersects) { distance = bestLocalDistance; face = bestLocalFace; @@ -311,6 +325,61 @@ bool TriangleSet::TriangleOctreeCell::findRayIntersection(const glm::vec3& origi return intersects; } +bool TriangleSet::findParabolaIntersection(const glm::vec3& origin, const glm::vec3& velocity, const glm::vec3& acceleration, + float& parabolicDistance, BoxFace& face, Triangle& triangle, bool precision, bool allowBackface) { + if (!_isBalanced) { + balanceOctree(); + } + + float localDistance = parabolicDistance; + int trianglesTouched = 0; + bool hit = _triangleOctree.findParabolaIntersection(origin, velocity, acceleration, localDistance, face, triangle, precision, trianglesTouched, allowBackface); + if (hit) { + parabolicDistance = localDistance; + } + +#if WANT_DEBUGGING + if (precision) { + qDebug() << "trianglesTouched :" << trianglesTouched << "out of:" << _triangleOctree._population << "_triangles.size:" << _triangles.size(); + } +#endif + return hit; +} + +bool TriangleSet::TriangleOctreeCell::findParabolaIntersectionInternal(const glm::vec3& origin, const glm::vec3& velocity, + const glm::vec3& acceleration, float& parabolicDistance, + BoxFace& face, Triangle& triangle, bool precision, + int& trianglesTouched, bool allowBackface) { + bool intersectedSomething = false; + float bestDistance = FLT_MAX; + Triangle bestTriangle; + + if (precision) { + for (const auto& triangleIndex : _triangleIndices) { + const auto& thisTriangle = _allTriangles[triangleIndex]; + float thisTriangleDistance; + trianglesTouched++; + if (findParabolaTriangleIntersection(origin, velocity, acceleration, thisTriangle, thisTriangleDistance, allowBackface)) { + if (thisTriangleDistance < bestDistance) { + bestDistance = thisTriangleDistance; + bestTriangle = thisTriangle; + intersectedSomething = true; + } + } + } + } else { + intersectedSomething = true; + bestDistance = parabolicDistance; + } + + if (intersectedSomething) { + parabolicDistance = bestDistance; + triangle = bestTriangle; + } + + return intersectedSomething; +} + bool TriangleSet::TriangleOctreeCell::findParabolaIntersection(const glm::vec3& origin, const glm::vec3& velocity, const glm::vec3& acceleration, float& parabolicDistance, BoxFace& face, Triangle& triangle, bool precision, @@ -322,52 +391,81 @@ bool TriangleSet::TriangleOctreeCell::findParabolaIntersection(const glm::vec3& float bestLocalDistance = FLT_MAX; BoxFace bestLocalFace; Triangle bestLocalTriangle; - glm::vec3 bestLocalNormal; bool intersects = false; - float boxDistance = FLT_MAX; - // if the pick intersects our bounding box, then continue - if (getBounds().findParabolaIntersection(origin, velocity, acceleration, boxDistance, bestLocalFace, bestLocalNormal)) { - // if the intersection with our bounding box, is greater than the current best distance (the distance passed in) - // then we know that none of our triangles can represent a better intersection and we can return - if (boxDistance > parabolicDistance) { - return false; - } - - // if we're not yet at the max depth, then check which child the triangle fits in - if (_depth < MAX_DEPTH) { - float bestChildDistance = FLT_MAX; - for (auto& child : _children) { - // check each child, if there's an intersection, it will return some distance that we need - // to compare against the other results, because there might be multiple intersections and - // we will always choose the best (shortest) intersection - float childDistance = bestChildDistance; - BoxFace childFace; - Triangle childTriangle; - if (child.second.findParabolaIntersection(origin, velocity, acceleration, childDistance, childFace, childTriangle, precision, trianglesTouched)) { - if (childDistance < bestLocalDistance) { - bestLocalDistance = childDistance; - bestChildDistance = childDistance; - bestLocalFace = childFace; - bestLocalTriangle = childTriangle; - intersects = true; - } - } - } - } - // also check our local triangle set - float internalDistance = boxDistance; + // Check our local triangle set first + // The distance passed in here is the distance to our bounding box. If !precision, that distance is used + { + float internalDistance = parabolicDistance; BoxFace internalFace; Triangle internalTriangle; if (findParabolaIntersectionInternal(origin, velocity, acceleration, internalDistance, internalFace, internalTriangle, precision, trianglesTouched, allowBackface)) { - if (internalDistance < bestLocalDistance) { - bestLocalDistance = internalDistance; - bestLocalFace = internalFace; - bestLocalTriangle = internalTriangle; - intersects = true; + bestLocalDistance = internalDistance; + bestLocalFace = internalFace; + bestLocalTriangle = internalTriangle; + intersects = true; + } + } + + // if we're not yet at the max depth, then check our children + if (_depth < MAX_DEPTH) { + std::list sortedTriangleCells; + auto sortingOperator = [&](std::shared_ptr& child) { + if (child) { + float priority = FLT_MAX; + if (child->getBounds().contains(origin)) { + priority = 0.0f; + } else { + float childBoundDistance = FLT_MAX; + BoxFace childBoundFace; + glm::vec3 childBoundNormal; + if (child->getBounds().findParabolaIntersection(origin, velocity, acceleration, childBoundDistance, childBoundFace, childBoundNormal)) { + // We only need to add this cell if it's closer than the local triangle set intersection (if there was one) + if (childBoundDistance < bestLocalDistance) { + priority = childBoundDistance; + } + } + } + + if (priority < FLT_MAX) { + if (sortedTriangleCells.size() > 0 && priority < sortedTriangleCells.front().first) { + sortedTriangleCells.emplace_front(priority, child); + } else { + sortedTriangleCells.emplace_back(priority, child); + } + } + } + }; + sortingOperator(_children.first); + sortingOperator(_children.second); + + for (auto it = sortedTriangleCells.begin(); it != sortedTriangleCells.end(); ++it) { + const SortedTriangleCell& sortedTriangleCell = *it; + float childDistance = sortedTriangleCell.first; + // We can exit once childDistance > bestLocalDistance + if (childDistance > bestLocalDistance) { + break; + } + // If we're inside the child cell and !precision, we need the actual distance to the cell bounds + if (!precision && childDistance < EPSILON) { + BoxFace childBoundFace; + glm::vec3 childBoundNormal; + sortedTriangleCell.second->getBounds().findParabolaIntersection(origin, velocity, acceleration, childDistance, childBoundFace, childBoundNormal); + } + BoxFace childFace; + Triangle childTriangle; + if (sortedTriangleCell.second->findParabolaIntersection(origin, velocity, acceleration, childDistance, childFace, childTriangle, precision, trianglesTouched)) { + if (childDistance < bestLocalDistance) { + bestLocalDistance = childDistance; + bestLocalFace = childFace; + bestLocalTriangle = childTriangle; + intersects = true; + break; + } } } } + if (intersects) { parabolicDistance = bestLocalDistance; face = bestLocalFace; diff --git a/libraries/shared/src/TriangleSet.h b/libraries/shared/src/TriangleSet.h index 0b0d0a9ac5..a838e2d9e2 100644 --- a/libraries/shared/src/TriangleSet.h +++ b/libraries/shared/src/TriangleSet.h @@ -12,6 +12,7 @@ #pragma once #include +#include #include "AABox.h" #include "GeometryUtil.h" @@ -20,9 +21,8 @@ class TriangleSet { class TriangleOctreeCell { public: - TriangleOctreeCell(std::vector& allTriangles) : - _allTriangles(allTriangles) - { } + TriangleOctreeCell(std::vector& allTriangles) : _allTriangles(allTriangles) {} + TriangleOctreeCell(std::vector& allTriangles, const AABox& bounds, int depth); void insert(size_t triangleIndex); void reset(const AABox& bounds, int depth = 0); @@ -40,8 +40,6 @@ class TriangleSet { void debugDump(); protected: - TriangleOctreeCell(std::vector& allTriangles, const AABox& bounds, int depth); - // checks our internal list of triangles bool findRayIntersectionInternal(const glm::vec3& origin, const glm::vec3& direction, float& distance, BoxFace& face, Triangle& triangle, bool precision, int& trianglesTouched, @@ -50,20 +48,22 @@ class TriangleSet { float& parabolicDistance, BoxFace& face, Triangle& triangle, bool precision, int& trianglesTouched, bool allowBackface = false); + std::pair getTriangleOctreeCellChildBounds(); + std::vector& _allTriangles; - std::map _children; - int _depth{ 0 }; - int _population{ 0 }; + std::pair, std::shared_ptr> _children; + int _depth { 0 }; + int _population { 0 }; AABox _bounds; std::vector _triangleIndices; friend class TriangleSet; }; + using SortedTriangleCell = std::pair>; + public: - TriangleSet() : - _triangleOctree(_triangles) - {} + TriangleSet() : _triangleOctree(_triangles) {} void debugDump(); @@ -87,8 +87,7 @@ public: const AABox& getBounds() const { return _bounds; } protected: - - bool _isBalanced{ false }; + bool _isBalanced { false }; std::vector _triangles; TriangleOctreeCell _triangleOctree; AABox _bounds;