diff --git a/Modules/Shared/Region3/Octree/Octree.lua b/Modules/Shared/Region3/Octree/Octree.lua index 41d19fdb8b..3d0b92d960 100644 --- a/Modules/Shared/Region3/Octree/Octree.lua +++ b/Modules/Shared/Region3/Octree/Octree.lua @@ -13,11 +13,17 @@ Octree.ClassName = "Octree" Octree.__index = Octree function Octree.new() - local self = setmetatable({}, Octree) + return setmetatable({ + _maxRegionSize = {512, 512, 512}, -- these should all be the same number + _maxDepth = 4, + _regionHashMap = {} + }, Octree) +end +function Octree:ClearNodes() self._maxRegionSize = { 512, 512, 512 } -- these should all be the same number self._maxDepth = 4 - self._regionHashMap = {} -- [hash] = region + table.clear(self._regionHashMap) return self end @@ -28,7 +34,7 @@ function Octree:GetAllNodes() for _, regionList in pairs(self._regionHashMap) do for _, region in pairs(regionList) do for node, _ in pairs(region.nodes) do - options[#options+1] = node + table.insert(options, node) end end end @@ -48,19 +54,14 @@ function Octree:CreateNode(position, object) end function Octree:RadiusSearch(position, radius) - assert(typeof(position) == "Vector3") - assert(type(radius) == "number") - - local px, py, pz = position.x, position.y, position.z - return self:_radiusSearch(px, py, pz, radius) + return self:_radiusSearch(assert(typeof(position) == "Vector3") and position.X, position.Y, position.Z, assert(type(radius) == "number") and radius) end function Octree:KNearestNeighborsSearch(position, k, radius) assert(typeof(position) == "Vector3") assert(type(radius) == "number") - local px, py, pz = position.x, position.y, position.z - local objects, nodeDistances2 = self:_radiusSearch(px, py, pz, radius) + local objects, nodeDistances2 = self:_radiusSearch(position.x, position.y, position.z, radius) local sortable = {} for index, dist2 in pairs(nodeDistances2) do @@ -78,38 +79,34 @@ function Octree:KNearestNeighborsSearch(position, k, radius) local knearestDist2 = {} for i = 1, math.min(#sortable, k) do local sorted = sortable[i] - knearestDist2[#knearestDist2 + 1] = sorted.dist2 - knearest[#knearest + 1] = objects[sorted.index] + table.insert(knearestDist2, sorted.dist2) + table.insert(knearest, objects[sorted.index]) end return knearest, knearestDist2 end function Octree:GetOrCreateLowestSubRegion(px, py, pz) - local region = self:_getOrCreateRegion(px, py, pz) - return OctreeRegionUtils.getOrCreateSubRegionAtDepth(region, px, py, pz, self._maxDepth) + return OctreeRegionUtils.getOrCreateSubRegionAtDepth(self:_getOrCreateRegion(px, py, pz), px, py, pz, self._maxDepth) end function Octree:_radiusSearch(px, py, pz, radius) local objectsFound = {} local nodeDistances2 = {} - local diameter = self._maxRegionSize[1] - local searchRadiusSquared = OctreeRegionUtils.getSearchRadiusSquared(radius, diameter, EPSILON) + local searchRadiusSquared = OctreeRegionUtils.getSearchRadiusSquared(radius, self._maxRegionSize[1], EPSILON) + --debug.profilebegin('_regionHashMap loop') for _, regionList in pairs(self._regionHashMap) do for _, region in pairs(regionList) do local rpos = region.position - local rpx, rpy, rpz = rpos[1], rpos[2], rpos[3] - local ox, oy, oz = px - rpx, py - rpy, pz - rpz - local dist2 = ox*ox + oy*oy + oz*oz - if dist2 <= searchRadiusSquared then - OctreeRegionUtils.getNeighborsWithinRadius( - region, radius, px, py, pz, objectsFound, nodeDistances2, self._maxDepth) + if (px - rpos[1])^2 + (py - rpos[2])^2 + (pz - rpos[3])^2 <= searchRadiusSquared then + OctreeRegionUtils.getNeighborsWithinRadius(region, radius, px, py, pz, objectsFound, nodeDistances2, maxDepth) end end end + --debug.profileend() return objectsFound, nodeDistances2 end diff --git a/Modules/Shared/Region3/Octree/OctreeNode.lua b/Modules/Shared/Region3/Octree/OctreeNode.lua index 3dd20c5ef2..804e11685b 100644 --- a/Modules/Shared/Region3/Octree/OctreeNode.lua +++ b/Modules/Shared/Region3/Octree/OctreeNode.lua @@ -10,15 +10,13 @@ OctreeNode.ClassName = "OctreeNode" OctreeNode.__index = OctreeNode function OctreeNode.new(octree, object) - local self = setmetatable({}, OctreeNode) + return setmetatable({ + _octree = octree or error("No octree"), + _object = object or error("No object"), - self._octree = octree or error("No octree") - self._object = object or error("No object") - - self._currentLowestRegion = nil - self._position = nil - - return self + _currentLowestRegion = nil, + _position = nil + }, OctreeNode) end function OctreeNode:KNearestNeighborsSearch(k, radius) @@ -46,15 +44,18 @@ function OctreeNode:SetPosition(position) return end - local px, py, pz = position.x, position.y, position.z + local px = position.x + local py = position.y + local pz = position.z self._px = px self._py = py self._pz = pz self._position = position - if self._currentLowestRegion then - if OctreeRegionUtils.inRegionBounds(self._currentLowestRegion, px, py, pz) then + local currentLowestRegion = self._currentLowestRegion + if currentLowestRegion then + if OctreeRegionUtils.inRegionBounds(currentLowestRegion, px, py, pz) then return end end @@ -66,18 +67,18 @@ function OctreeNode:SetPosition(position) -- error("[OctreeNode.SetPosition] newLowestRegion is not in region bounds!") -- end - if self._currentLowestRegion then - OctreeRegionUtils.moveNode(self._currentLowestRegion, newLowestRegion, self) + if currentLowestRegion then + OctreeRegionUtils.moveNode(currentLowestRegion, newLowestRegion, self) else OctreeRegionUtils.addNode(newLowestRegion, self) end - self._currentLowestRegion = newLowestRegion end function OctreeNode:Destroy() - if self._currentLowestRegion then - OctreeRegionUtils.removeNode(self._currentLowestRegion, self) + local currentLowestRegion = self._currentLowestRegion + if currentLowestRegion then + OctreeRegionUtils.removeNode(currentLowestRegion, self) end end diff --git a/Modules/Shared/Region3/Octree/OctreeRegionUtils.lua b/Modules/Shared/Region3/Octree/OctreeRegionUtils.lua index bc8f155ba9..8d4ca475bc 100644 --- a/Modules/Shared/Region3/Octree/OctreeRegionUtils.lua +++ b/Modules/Shared/Region3/Octree/OctreeRegionUtils.lua @@ -34,7 +34,9 @@ function OctreeRegionUtils.visualize(region) end function OctreeRegionUtils.create(px, py, pz, sx, sy, sz, parent, parentIndex) - local hsx, hsy, hsz = sx/2, sy/2, sz/2 + local hsx = sx * 0.5 + local hsy = sy * 0.5 + local hsz = sz * 0.5 local region = { subRegions = { @@ -72,7 +74,7 @@ function OctreeRegionUtils.addNode(lowestSubregion, node) while current do if not current.nodes[node] then current.nodes[node] = node - current.node_count = current.node_count + 1 + current.node_count += 1 end current = current.parent end @@ -87,17 +89,23 @@ function OctreeRegionUtils.moveNode(fromLowest, toLowest, node) while currentFrom ~= currentTo do -- remove from current do - assert(currentFrom.nodes[node]) - assert(currentFrom.node_count > 0) + local currentFromNodes = currentFrom.nodes + assert(currentFromNodes[node]) + local currentFromNodeCount = currentFrom.node_count - 1 + assert(currentFromNodeCount > 1) - currentFrom.nodes[node] = nil - currentFrom.node_count = currentFrom.node_count - 1 + currentFromNodes[node] = nil + currentFrom.node_count = currentFromNodeCount -- remove subregion! - if currentFrom.node_count <= 0 and currentFrom.parentIndex then - assert(currentFrom.parent) - assert(currentFrom.parent.subRegions[currentFrom.parentIndex] == currentFrom) - currentFrom.parent.subRegions[currentFrom.parentIndex] = nil + local currentFromParentIndex = currentFromNodeCount < 2 and currentFrom.parentIndex + if currentFromParentIndex then + local currentFromParent = currentFrom.parent + + assert(currentFromParent) + local currentFromParentSubRegions = currentFromParent.subRegions + assert(currentFromParentSubRegions[currentFromParentIndex] == currentFrom) + currentFromParentSubRegions[currentFromParentIndex] = nil end end @@ -105,7 +113,7 @@ function OctreeRegionUtils.moveNode(fromLowest, toLowest, node) do assert(not currentTo.nodes[node]) currentTo.nodes[node] = node - currentTo.node_count = currentTo.node_count + 1 + currentTo.node_count += 1 end currentFrom = currentFrom.parent @@ -118,65 +126,59 @@ function OctreeRegionUtils.removeNode(lowestSubregion, node) local current = lowestSubregion while current do - assert(current.nodes[node]) - assert(current.node_count > 0) + local currentNodes = current.nodes + assert(currentNodes[node]) + local currentNodeCount = current.node_count - 1 + assert(currentNodeCount > 1) - current.nodes[node] = nil - current.node_count = current.node_count - 1 + currentNodes[node] = nil + current.node_count = currentNodeCount -- remove subregion! - if current.node_count <= 0 and current.parentIndex then - assert(current.parent) - assert(current.parent.subRegions[current.parentIndex] == current) - current.parent.subRegions[current.parentIndex] = nil + local currentParent = current.parent + local currentParentIndex = current.parentIndex + if currentNodeCount <= 0 and currentParentIndex then + assert(currentParent) + assert(currentParent.subRegions[currentParentIndex] == current) + + currentParent.subRegions[currentParentIndex] = nil end - current = current.parent + current = currentParent end end function OctreeRegionUtils.getSearchRadiusSquared(radius, diameter, epsilon) - local diagonal = SQRT_3_OVER_2*diameter - local searchRadius = radius + diagonal - return searchRadius*searchRadius + epsilon + -- calculating directly is faster as Luau folds the expressions. + return (radius + SQRT_3_OVER_2*diameter)^2 + epsilon end -- See basic algorithm: -- luacheck: push ignore -- https://github.com/PointCloudLibrary/pcl/blob/29f192af57a3e7bdde6ff490669b211d8148378f/octree/include/pcl/octree/impl/octree_search.hpp#L309 -- luacheck: pop -function OctreeRegionUtils.getNeighborsWithinRadius( - region, radius, px, py, pz, objectsFound, nodeDistances2, maxDepth) - assert(maxDepth) - - local childDiameter = region.size[1]/2 - local searchRadiusSquared = OctreeRegionUtils.getSearchRadiusSquared(radius, childDiameter, EPSILON) - +function OctreeRegionUtils.getNeighborsWithinRadius(region, radius, px, py, pz, objectsFound, nodeDistances2, maxDepth) + local diameter = region.size[1] * 0.5 + local searchRadiusSquared = (radius + SQRT_3_OVER_2*diameter)^2 + EPSILON local radiusSquared = radius*radius -- for each child for _, childRegion in pairs(region.subRegions) do local cposition = childRegion.position - local cpx, cpy, cpz = cposition[1], cposition[2], cposition[3] - - local ox, oy, oz = px - cpx, py - cpy, pz - cpz - local dist2 = ox*ox + oy*oy + oz*oz -- within search radius - if dist2 <= searchRadiusSquared then + if (px - cposition[1])^2 + (py - cposition[2])^2 + (pz - cposition[3])^2 <= searchRadiusSquared then if childRegion.depth == maxDepth then - for node, _ in pairs(childRegion.nodes) do - local npx, npy, npz = node:GetRawPosition() - local nox, noy, noz = px - npx, py - npy, pz - npz - local ndist2 = nox*nox + noy*noy + noz*noz + for node in pairs(childRegion.nodes) do + local ndist2 = (px - node._px)^2 + (py - node._py)^2 + (pz - node._pz)^2 + if ndist2 <= radiusSquared then - objectsFound[#objectsFound + 1] = node:GetObject() - nodeDistances2[#nodeDistances2 + 1] = ndist2 + table.insert(objectsFound, node._object) + table.insert(nodeDistances2, ndist2) end end else - OctreeRegionUtils.getNeighborsWithinRadius( - childRegion, radius, px, py, pz, objectsFound, nodeDistances2, maxDepth) + OctreeRegionUtils.getNeighborsWithinRadius(childRegion, radius, px, py, pz, objectsFound, nodeDistances2, maxDepth) end end end @@ -185,7 +187,9 @@ end function OctreeRegionUtils.getOrCreateSubRegionAtDepth(region, px, py, pz, maxDepth) local current = region for _ = region.depth, maxDepth do - local index = OctreeRegionUtils.getSubRegionIndex(current, px, py, pz) + local position = current.position + local index = (px > position[1] and 1 or 2) + (py <= position[2] and 4 or 0) + (pz >= position[3] and 2 or 0) + local _next = current.subRegions[index] -- construct @@ -205,35 +209,34 @@ function OctreeRegionUtils.createSubRegion(parentRegion, parentIndex) local position = parentRegion.position local multiplier = SUB_REGION_POSITION_OFFSET[parentIndex] - local px = position[1] + multiplier[1]*size[1] - local py = position[2] + multiplier[2]*size[2] - local pz = position[3] + multiplier[3]*size[3] - local sx, sy, sz = size[1]/2, size[2]/2, size[3]/2 + local sizeX = size[1] + local sizeY = size[2] + local sizeZ = size[3] + return OctreeRegionUtils.create( + position[1] + multiplier[1]*sizeX, + position[2] + multiplier[2]*sizeY, + position[3] + multiplier[3]*sizeZ, - return OctreeRegionUtils.create(px, py, pz, sx, sy, sz, parentRegion, parentIndex) + sizeX * 0.5, + sizeY * 0.5, + sizeZ * 0.5, + + parentRegion, parentIndex + ) end -- Consider regions to be range [px, y) function OctreeRegionUtils.inRegionBounds(region, px, py, pz) local lowerBounds = region.lowerBounds local upperBounds = region.upperBounds - return ( - px >= lowerBounds[1] and px <= upperBounds[1] and - py >= lowerBounds[2] and py <= upperBounds[2] and - pz >= lowerBounds[3] and pz <= upperBounds[3] - ) + return px >= lowerBounds[1] and px <= upperBounds[1] and + py >= lowerBounds[2] and py <= upperBounds[2] and + pz >= lowerBounds[3] and pz <= upperBounds[3] end function OctreeRegionUtils.getSubRegionIndex(region, px, py, pz) - local index = px > region.position[1] and 1 or 2 - if py <= region.position[2] then - index = index + 4 - end - - if pz >= region.position[3] then - index = index + 2 - end - return index + local position = region.position + return (px > position[1] and 1 or 2) + (py <= position[2] and 4 or 0) + (pz >= position[3] and 2 or 0) end --- This definitely collides @@ -242,17 +245,16 @@ function OctreeRegionUtils.getTopLevelRegionHash(cx, cy, cz) -- Normally you would modulus this to hash table size, but we want as flat of a structure as possible return cx * 73856093 + cy*19351301 + cz*83492791 end - function OctreeRegionUtils.getTopLevelRegionCellIndex(maxRegionSize, px, py, pz) return math.floor(px / maxRegionSize[1] + 0.5), - math.floor(py / maxRegionSize[2] + 0.5), - math.floor(pz / maxRegionSize[3] + 0.5) + math.floor(py / maxRegionSize[2] + 0.5), + math.floor(pz / maxRegionSize[3] + 0.5) end function OctreeRegionUtils.getTopLevelRegionPosition(maxRegionSize, cx, cy, cz) return maxRegionSize[1] * cx, - maxRegionSize[2] * cy, - maxRegionSize[3] * cz + maxRegionSize[2] * cy, + maxRegionSize[3] * cz end function OctreeRegionUtils.areEqualTopRegions(region, rpx, rpy, rpz) @@ -263,17 +265,26 @@ function OctreeRegionUtils.areEqualTopRegions(region, rpx, rpy, rpz) end function OctreeRegionUtils.findRegion(regionHashMap, maxRegionSize, px, py, pz) - local cx, cy, cz = OctreeRegionUtils.getTopLevelRegionCellIndex(maxRegionSize, px, py, pz) - local hash = OctreeRegionUtils.getTopLevelRegionHash(cx, cy, cz) + local maxSizeX = maxRegionSize[1] + local maxSizeY = maxRegionSize[2] + local maxSizeZ = maxRegionSize[3] - local regionList = regionHashMap[hash] + -- directly calculate values + local cx = math.floor(px / maxSizeX + 0.5) + local cy = math.floor(py / maxSizeY + 0.5) + local cz = math.floor(pz / maxSizeZ + 0.5) + + local regionList = regionHashMap[cx * 73856093 + cy*19351301 + cz*83492791] if not regionList then return nil end - local rpx, rpy, rpz = OctreeRegionUtils.getTopLevelRegionPosition(maxRegionSize, cx, cy, cz) + local rpx = maxSizeX * cx + local rpy = maxSizeY * cy + local rpz = maxSizeZ * cz for _, region in pairs(regionList) do - if OctreeRegionUtils.areEqualTopRegions(region, rpx, rpy, rpz) then + local position = region.position + if position[1] == rpx and position[2] == rpy and position[3] == rpz then return region end end @@ -282,8 +293,12 @@ function OctreeRegionUtils.findRegion(regionHashMap, maxRegionSize, px, py, pz) end function OctreeRegionUtils.getOrCreateRegion(regionHashMap, maxRegionSize, px, py, pz) - local cx, cy, cz = OctreeRegionUtils.getTopLevelRegionCellIndex(maxRegionSize, px, py, pz) - local hash = OctreeRegionUtils.getTopLevelRegionHash(cx, cy, cz) + -- directly calculate values + local cx = math.floor(px / maxRegionSize[1] + 0.5) + local cy = math.floor(py / maxRegionSize[2] + 0.5) + local cz = math.floor(pz / maxRegionSize[3] + 0.5) + + local hash = cx * 73856093 + cy*19351301 + cz*83492791 local regionList = regionHashMap[hash] if not regionList then @@ -291,16 +306,15 @@ function OctreeRegionUtils.getOrCreateRegion(regionHashMap, maxRegionSize, px, p regionHashMap[hash] = regionList end - local rpx, rpy, rpz = OctreeRegionUtils.getTopLevelRegionPosition(maxRegionSize, cx, cy, cz) + local rpx, rpy, rpz = maxRegionSize[1] * cx, maxRegionSize[2] * cy, maxRegionSize[3] * cz for _, region in pairs(regionList) do - if OctreeRegionUtils.areEqualTopRegions(region, rpx, rpy, rpz) then + local position = region.position + if position[1] == rpx and position[2] == rpy and position[3] == rpz then return region end end - local region = OctreeRegionUtils.create( - rpx, rpy, rpz, - maxRegionSize[1], maxRegionSize[2], maxRegionSize[3]) + local region = OctreeRegionUtils.create(rpx, rpy, rpz, maxRegionSize[1], maxRegionSize[2], maxRegionSize[3]) table.insert(regionList, region) return region