fear

FastOctree.lua

--- Octree implementation
-- @classmod Octree

-- i see no god up here
-- EXCEPT FOR ME

local OctreeNode = require(script.OctreeNode)
local OctreeRegionUtils = require(script.OctreeRegionUtils)

local EPSILON = 1e-9
local SQRT_3_OVER_2 = math.sqrt(3) / 2
local SUB_REGION_POSITION_OFFSET = {
	{0.25, 0.25, -0.25};
	{-0.25, 0.25, -0.25};
	{0.25, 0.25, 0.25};
	{-0.25, 0.25, 0.25};
	{0.25, -0.25, -0.25};
	{-0.25, -0.25, -0.25};
	{0.25, -0.25, 0.25};
	{-0.25, -0.25, 0.25};
}

local Octree = {ClassName = "Octree"}
Octree.__index = Octree

local OctreeNode_new = OctreeNode.new
local OctreeRegionUtils_GetNeighborsWithinRadius = OctreeRegionUtils.GetNeighborsWithinRadius

function Octree.new()
	return setmetatable({
		MaxDepth = 4;
		MaxRegionSize = table.create(3, 512);
		RegionHashMap = {};
	}, Octree)
end

function Octree:ClearNodes()
	self.MaxDepth = 4
	self.MaxRegionSize = table.create(3, 512)
	table.clear(self.RegionHashMap)
end

function Octree:GetAllNodes()
	local Options = {}
	local Length = 0

	for _, RegionList in next, self.RegionHashMap do
		for _, Region in ipairs(RegionList) do
			for Node in next, Region.Nodes do
				Length += 1
				Options[Length] = Node
			end
		end
	end

	return Options
end

function Octree:CreateNode(Position: Vector3, Object)
	assert(typeof(Position) == "Vector3", "Bad position value")
	assert(Object, "Bad object value")

	local Node = OctreeNode_new(self, Object)
	Node:SetPosition(Position)
	return Node
end

function Octree:RadiusSearch(Position: Vector3, Radius: number)
	assert(typeof(Position) == "Vector3")
	assert(type(Radius) == "number")
	return self:_RadiusSearch(Position.X, Position.Y, Position.Z, Radius)
end

local function NearestNeighborSort(A, B)
	return A.Distance2 < B.Distance2
end

function Octree:KNearestNeighborsSearch(Position: Vector3, K: number, Radius: number)
	assert(typeof(Position) == "Vector3")
	assert(type(Radius) == "number")

	local Objects, NodeDistances2 = self:_RadiusSearch(Position.X, Position.Y, Position.Z, Radius)

	local SortableLength = #NodeDistances2
	local Sortable = table.create(SortableLength)
	for Index, Distance2 in ipairs(NodeDistances2) do
		Sortable[Index] = {
			Distance2 = Distance2;
			Index = Index;
		}
	end

	table.sort(Sortable, NearestNeighborSort)

	local ArrayLength = math.min(SortableLength, K)
	local KNearest = table.create(ArrayLength)
	local KNearestDistance2 = table.create(ArrayLength)
	local Length = 0
	for Index = 1, ArrayLength do
		local Sorted = Sortable[Index]
		Length += 1
		KNearestDistance2[Length] = Sorted.Distance2
		KNearest[Length] = Objects[Sorted.Index]
	end

	return KNearest, KNearestDistance2
end

function Octree:GetOrCreateLowestSubRegion(PositionX: number, PositionY: number, PositionZ: number)
	local Region = self:_GetOrCreateRegion(PositionX, PositionY, PositionZ)
	local MaxDepth = self.MaxDepth
	local Current = Region
	for _ = Region.Depth, MaxDepth do
		local Position = Current.Position
		local Index = PositionX > Position[1] and 1 or 2
		if PositionY <= Position[2] then
			Index += 4
		end

		if PositionZ >= Position[3] then
			Index += 2
		end

		local Next = Current.SubRegions[Index]

		-- construct
		if not Next then
			local Size = Current.Size
			local CurrentPosition = Current.Position
			local Multiplier = SUB_REGION_POSITION_OFFSET[Index]

			local X, Y, Z = Size[1], Size[2], Size[3]
			local CurrentPositionX = CurrentPosition[1] + Multiplier[1] * X
			local CurrentPositionY = CurrentPosition[2] + Multiplier[2] * Y
			local CurrentPositionZ = CurrentPosition[3] + Multiplier[3] * Z
			local SizeX, SizeY, SizeZ = X / 2, Y / 2, Z / 2

			local HalfSizeX, HalfSizeY, HalfSizeZ = SizeX / 2, SizeY / 2, SizeZ / 2

			local LowerBoundsArray = table.create(3)
			LowerBoundsArray[1], LowerBoundsArray[2], LowerBoundsArray[3] = CurrentPositionX - HalfSizeX, CurrentPositionY - HalfSizeY, CurrentPositionZ - HalfSizeZ

			local PositionArray = table.create(3)
			PositionArray[1], PositionArray[2], PositionArray[3] = CurrentPositionX, CurrentPositionY, CurrentPositionZ

			local SizeArray = table.create(3)
			SizeArray[1], SizeArray[2], SizeArray[3] = SizeX, SizeY, SizeZ

			local UpperBoundsArray = table.create(3)
			UpperBoundsArray[1], UpperBoundsArray[2], UpperBoundsArray[3] = CurrentPositionX + HalfSizeX, CurrentPositionY + HalfSizeY, CurrentPositionZ + HalfSizeZ

			Next = {
				Depth = Current and (Current.Depth + 1) or 1;
				LowerBounds = LowerBoundsArray;
				NodeCount = 0;
				Nodes = {}; -- [node] = true (contains subchild nodes too)
				Parent = Current;
				ParentIndex = Index;
				Position = PositionArray;
				Size = SizeArray; -- { sx, sy, sz }
				SubRegions = {};
				UpperBounds = UpperBoundsArray;
			}

			-- Next = OctreeRegionUtils.CreateSubRegion(Current, Index)
			Current.SubRegions[Index] = Next
		end

		-- iterate
		Current = Next
	end

	return Current
end

-- Private
function Octree:_RadiusSearch(PositionX: number, PositionY: number, PositionZ: number, Radius: number)
	local ObjectsFound = {}
	local NodeDistances2 = {}

	local Diameter = self.MaxRegionSize[1]
	local SearchRadius = Radius + SQRT_3_OVER_2 * Diameter
	local SearchRadiusSquared = SearchRadius * SearchRadius + EPSILON

	for _, RegionList in next, self.RegionHashMap do
		for _, Region in ipairs(RegionList) do
			local Position = Region.Position
			local RegionPositionX = Position[1]
			local RegionPositionY = Position[2]
			local RegionPositionZ = Position[3]

			local OffsetX, OffsetY, OffsetZ = PositionX - RegionPositionX, PositionY - RegionPositionY, PositionZ - RegionPositionZ
			local Distance2 = OffsetX * OffsetX + OffsetY * OffsetY + OffsetZ * OffsetZ

			if Distance2 <= SearchRadiusSquared then
				OctreeRegionUtils_GetNeighborsWithinRadius(Region, Radius, PositionX, PositionY, PositionZ, ObjectsFound, NodeDistances2, self.MaxDepth)
			end
		end
	end

	return ObjectsFound, NodeDistances2
end

function Octree:_GetRegion(PositionX: number, PositionY: number, PositionZ: number)
	local RegionHashMap = self.RegionHashMap
	local MaxRegionSize = self.MaxRegionSize
	local X, Y, Z = MaxRegionSize[1], MaxRegionSize[2], MaxRegionSize[3]
	local CX, CY, CZ = math.floor(PositionX / X + 0.5), math.floor(PositionY / Y + 0.5), math.floor(PositionZ / Z + 0.5)
	local Hash = CX * 73856093 + CY * 19351301 + CZ * 83492791

	local RegionList = RegionHashMap[Hash]
	if not RegionList then
		return nil
	end

	local RegionPositionX, RegionPositionY, RegionPositionZ = X * CX, Y * CY, Z * CZ
	for _, Region in ipairs(RegionList) do
		local Position = Region.Position
		if Position[1] == RegionPositionX and Position[2] == RegionPositionY and Position[3] == RegionPositionZ then
			return Region
		end
	end

	return nil
end

function Octree:_GetOrCreateRegion(PositionX: number, PositionY: number, PositionZ: number)
	local RegionHashMap = self.RegionHashMap
	local MaxRegionSize = self.MaxRegionSize
	local X, Y, Z = MaxRegionSize[1], MaxRegionSize[2], MaxRegionSize[3]
	local CX, CY, CZ = math.floor(PositionX / X + 0.5), math.floor(PositionY / Y + 0.5), math.floor(PositionZ / Z + 0.5)
	local Hash = CX * 73856093 + CY * 19351301 + CZ * 83492791

	local RegionList = RegionHashMap[Hash]
	if not RegionList then
		RegionList = {}
		RegionHashMap[Hash] = RegionList
	end

	local RegionPositionX, RegionPositionY, RegionPositionZ = X * CX, Y * CY, Z * CZ
	for _, Region in ipairs(RegionList) do
		local Position = Region.Position
		if Position[1] == RegionPositionX and Position[2] == RegionPositionY and Position[3] == RegionPositionZ then
			return Region
		end
	end

	local HalfSizeX, HalfSizeY, HalfSizeZ = X / 2, Y / 2, Z / 2

	local LowerBoundsArray = table.create(3)
	LowerBoundsArray[1], LowerBoundsArray[2], LowerBoundsArray[3] = RegionPositionX - HalfSizeX, RegionPositionY - HalfSizeY, RegionPositionZ - HalfSizeZ

	local PositionArray = table.create(3)
	PositionArray[1], PositionArray[2], PositionArray[3] = RegionPositionX, RegionPositionY, RegionPositionZ

	local SizeArray = table.create(3)
	SizeArray[1], SizeArray[2], SizeArray[3] = X, Y, Z

	local UpperBoundsArray = table.create(3)
	UpperBoundsArray[1], UpperBoundsArray[2], UpperBoundsArray[3] = RegionPositionX + HalfSizeX, RegionPositionY + HalfSizeY, RegionPositionZ + HalfSizeZ

	local Region = {
		Depth = 1;
		LowerBounds = LowerBoundsArray;
		NodeCount = 0;
		Nodes = {}; -- [node] = true (contains subchild nodes too)
		Parent = nil;
		ParentIndex = nil;
		Position = PositionArray;
		Size = SizeArray; -- { sx, sy, sz }
		SubRegions = {};
		UpperBounds = UpperBoundsArray;
	}

	table.insert(RegionList, Region)
	return Region
end

return Octree

OctreeNode.lua

--- Basic node interacting with the octree
-- @classmod OctreeNode

local OctreeNode = {ClassName = "OctreeNode"}
OctreeNode.__index = OctreeNode

function OctreeNode.new(Octree, Object)
	return setmetatable({
		Octree = Octree or error("No octree");
		Object = Object or error("No object");

		CurrentLowestRegion = nil;
		Position = nil;
		PositionX = nil;
		PositionY = nil;
		PositionZ = nil;
	}, OctreeNode)
end

function OctreeNode:KNearestNeighborsSearch(K: number, Radius: number)
	return self.Octree:KNearestNeighborsSearch(self.Position, K, Radius)
end

function OctreeNode:GetObject()
	warn("OctreeNode:GetObject is deprecated.")
	return self.Object
end

function OctreeNode:RadiusSearch(Radius: number)
	return self.Octree:RadiusSearch(self.Position, Radius)
end

function OctreeNode:GetPosition()
	warn("OctreeNode:GetPosition is deprecated.")
	return self.Position
end

function OctreeNode:GetRawPosition(): (number, number, number)
	return self.PositionX, self.PositionY, self.PositionZ
end

function OctreeNode:SetPosition(Position: Vector3)
	if self.Position == Position then
		return
	end

	local PositionX, PositionY, PositionZ = Position.X, Position.Y, Position.Z

	self.PositionX = PositionX
	self.PositionY = PositionY
	self.PositionZ = PositionZ
	self.Position = Position

	if self.CurrentLowestRegion then
		local Region = self.CurrentLowestRegion
		local LowerBounds = Region.LowerBounds
		local UpperBounds = Region.UpperBounds
		if PositionX >= LowerBounds[1] and PositionX <= UpperBounds[1] and PositionY >= LowerBounds[2] and PositionY <= UpperBounds[2] and PositionZ >= LowerBounds[3] and PositionZ <= UpperBounds[3] then
			return
		end
	end

	local NewLowestRegion = self.Octree:GetOrCreateLowestSubRegion(PositionX, PositionY, PositionZ)
	if self.CurrentLowestRegion then
		-- OctreeRegionUtils_MoveNode(self.CurrentLowestRegion, NewLowestRegion, self)
		local FromLowest = self.CurrentLowestRegion
		assert(FromLowest.Depth == NewLowestRegion.Depth, "fromLowest.Depth ~= toLowest.Depth")
		assert(FromLowest ~= NewLowestRegion, "fromLowest == toLowest")
		local CurrentFrom = FromLowest
		local CurrentTo = NewLowestRegion

		while CurrentFrom ~= CurrentTo do
			-- remove from current
			if not CurrentFrom.Nodes[self] then
				error("CurrentFrom.Nodes doesn't have a node here.")
			end

			if CurrentFrom.NodeCount <= 0 then
				error("NodeCount is <= 0.")
			end

			local NodeCount = CurrentFrom.NodeCount - 1
			CurrentFrom.Nodes[self] = nil
			CurrentFrom.NodeCount = NodeCount

			-- remove subregion!
			if NodeCount <= 0 and CurrentFrom.ParentIndex then
				local Parent = CurrentFrom.Parent
				if not Parent then
					error("CurrentFrom.Parent doesn't exist.")
				end

				if Parent.SubRegions[CurrentFrom.ParentIndex] ~= CurrentFrom then
					error("Failed equality check.")
				end

				Parent.SubRegions[CurrentFrom.ParentIndex] = nil
			end

			if CurrentTo.Nodes[self] then
				error("CurrentTo.Nodes already has a node here.")
			end

			CurrentTo.Nodes[self] = self
			CurrentTo.NodeCount += 1

			CurrentFrom = CurrentFrom.Parent
			CurrentTo = CurrentTo.Parent
		end
	else
		local Current = NewLowestRegion
		while Current do
			local CurrentNodes = Current.Nodes
			if not CurrentNodes[self] then
				CurrentNodes[self] = self
				Current.NodeCount += 1
			end

			Current = Current.Parent
		end
	end

	self.CurrentLowestRegion = NewLowestRegion
end

function OctreeNode:Destroy()
	local LowestSubregion = self.CurrentLowestRegion
	if LowestSubregion then
		local Current = LowestSubregion

		while Current do
			if not Current.Nodes[self] then
				error("CurrentFrom.Nodes doesn't have a node here.")
			end

			if Current.NodeCount <= 0 then
				error("NodeCount is <= 0.")
			end

			local NodeCount = Current.NodeCount - 1
			Current.Nodes[self] = nil
			Current.NodeCount = NodeCount

			-- remove subregion!
			local Parent = Current.Parent
			if NodeCount <= 0 and Current.ParentIndex then
				if not Parent then
					error("Current.Parent doesn't exist.")
				end

				if Parent.SubRegions[Current.ParentIndex] ~= Current then
					error("Failed equality check.")
				end

				Parent.SubRegions[Current.ParentIndex] = nil
			end

			Current = Parent
		end
	end

	-- setmetatable(self, nil)
end

return OctreeNode

OctreeRegionUtils.lua

--- Octree implementation
-- @module OctreeRegionUtils

local EPSILON = 1e-6
local SQRT_3_OVER_2 = math.sqrt(3) / 2
local SUB_REGION_POSITION_OFFSET = {
	{0.25, 0.25, -0.25};
	{-0.25, 0.25, -0.25};
	{0.25, 0.25, 0.25};
	{-0.25, 0.25, 0.25};
	{0.25, -0.25, -0.25};
	{-0.25, -0.25, -0.25};
	{0.25, -0.25, 0.25};
	{-0.25, -0.25, 0.25};
}

local OctreeRegionUtils = {}

function OctreeRegionUtils.Create(PositionX, PositionY, PositionZ, SizeX, SizeY, SizeZ, Parent, ParentIndex)
	local HalfSizeX, HalfSizeY, HalfSizeZ = SizeX / 2, SizeY / 2, SizeZ / 2
	return {
		Depth = Parent and (Parent.Depth + 1) or 1;
		LowerBounds = {PositionX - HalfSizeX, PositionY - HalfSizeY, PositionZ - HalfSizeZ};
		NodeCount = 0;
		Nodes = {}; -- [node] = true (contains subchild nodes too)
		Parent = Parent;
		ParentIndex = ParentIndex;
		Position = {PositionX, PositionY, PositionZ};
		Size = {SizeX, SizeY, SizeZ}; -- { sx, sy, sz }
		SubRegions = {};
		UpperBounds = {PositionX + HalfSizeX, PositionY + HalfSizeY, PositionZ + HalfSizeZ};
	}
end

function OctreeRegionUtils.AddNode(LowestSubregion, Node)
	assert(Node)
	local Current = LowestSubregion
	while Current do
		local CurrentNodes = Current.Nodes
		if not CurrentNodes[Node] then
			CurrentNodes[Node] = Node
			Current.NodeCount += 1
		end

		Current = Current.Parent
	end
end

function OctreeRegionUtils.MoveNode(FromLowest, ToLowest, Node)
	assert(FromLowest.Depth == ToLowest.Depth, "fromLowest.Depth ~= toLowest.Depth")
	assert(FromLowest ~= ToLowest, "fromLowest == toLowest")
	local CurrentFrom = FromLowest
	local CurrentTo = ToLowest

	while CurrentFrom ~= CurrentTo do
		-- remove from current
		if not CurrentFrom.Nodes[Node] then
			error("CurrentFrom.Nodes doesn't have a node here.")
		end

		if CurrentFrom.NodeCount <= 0 then
			error("NodeCount is <= 0.")
		end

		local NodeCount = CurrentFrom.NodeCount - 1
		CurrentFrom.Nodes[Node] = nil
		CurrentFrom.NodeCount = NodeCount

		-- remove subregion!
		if NodeCount <= 0 and CurrentFrom.ParentIndex then
			local Parent = CurrentFrom.Parent
			if not Parent then
				error("CurrentFrom.Parent doesn't exist.")
			end

			if Parent.SubRegions[CurrentFrom.ParentIndex] ~= CurrentFrom then
				error("Failed equality check.")
			end

			Parent.SubRegions[CurrentFrom.ParentIndex] = nil
		end

		if CurrentTo.Nodes[Node] then
			error("CurrentTo.Nodes already has a node here.")
		end

		CurrentTo.Nodes[Node] = Node
		CurrentTo.NodeCount += 1

		CurrentFrom = CurrentFrom.Parent
		CurrentTo = CurrentTo.Parent
	end
end

function OctreeRegionUtils.RemoveNode(LowestSubregion, Node)
	assert(Node)
	local Current = LowestSubregion

	while Current do
		if not Current.Nodes[Node] then
			error("CurrentFrom.Nodes doesn't have a node here.")
		end

		if Current.NodeCount <= 0 then
			error("NodeCount is <= 0.")
		end

		local NodeCount = Current.NodeCount - 1
		Current.Nodes[Node] = nil
		Current.NodeCount = NodeCount

		-- remove subregion!
		local Parent = Current.Parent
		if NodeCount <= 0 and Current.ParentIndex then
			if not Parent then
				error("Current.Parent doesn't exist.")
			end

			if Parent.SubRegions[Current.ParentIndex] ~= Current then
				error("Failed equality check.")
			end

			Parent.SubRegions[Current.ParentIndex] = nil
		end

		Current = Parent
	end
end

function OctreeRegionUtils.GetSearchRadiusSquared(Radius, Diameter, Epsilon)
	local SearchRadius = Radius + SQRT_3_OVER_2 * Diameter
	return SearchRadius * SearchRadius + 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
local function GetNeighborsWithinRadius(Region, Radius, PositionX, PositionY, PositionZ, ObjectsFound, NodeDistances2, MaxDepth)
	if not MaxDepth then
		error("Missing MaxDepth.")
	end

	local SearchRadius = Radius + SQRT_3_OVER_2 * (Region.Size[1] / 2)
	local SearchRadiusSquared = SearchRadius * SearchRadius + EPSILON
	local RadiusSquared = Radius * Radius

	-- for each child
	for _, ChildRegion in next, Region.SubRegions do
		local ChildPosition = ChildRegion.Position
		local ChildPositionX = ChildPosition[1]
		local ChildPositionY = ChildPosition[2]
		local ChildPositionZ = ChildPosition[3]

		local OffsetX = PositionX - ChildPositionX
		local OffsetY = PositionY - ChildPositionY
		local OffsetZ = PositionZ - ChildPositionZ
		local Distance2 = OffsetX * OffsetX + OffsetY * OffsetY + OffsetZ * OffsetZ

		-- within search radius
		if Distance2 <= SearchRadiusSquared then
			if ChildRegion.Depth == MaxDepth then
				for Node in next, ChildRegion.Nodes do
					local NodePositionX = Node.PositionX
					local NodePositionY = Node.PositionY
					local NodePositionZ = Node.PositionZ

					local NodeOffsetX = NodePositionX - PositionX
					local NodeOffsetY = NodePositionY - PositionY
					local NodeOffsetZ = NodePositionZ - PositionZ
					local NodeDistance2 = NodeOffsetX * NodeOffsetX + NodeOffsetY * NodeOffsetY + NodeOffsetZ * NodeOffsetZ

					if NodeDistance2 <= RadiusSquared then
						table.insert(ObjectsFound, Node.Object)
						table.insert(NodeDistances2, NodeDistance2)
					end
				end
			else
				GetNeighborsWithinRadius(ChildRegion, Radius, PositionX, PositionY, PositionZ, ObjectsFound, NodeDistances2, MaxDepth)
			end
		end
	end
end

OctreeRegionUtils.GetNeighborsWithinRadius = GetNeighborsWithinRadius

function OctreeRegionUtils.GetSubRegionIndex(Region, PositionX, PositionY, PositionZ)
	local Position = Region.Position
	local Index = PositionX > Position[1] and 1 or 2
	if PositionY <= Position[2] then
		Index += 4
	end

	if PositionZ >= Position[3] then
		Index += 2
	end

	return Index
end

function OctreeRegionUtils.GetOrCreateSubRegionAtDepth(Region, PositionX, PositionY, PositionZ, MaxDepth)
	local Current = Region
	for _ = Region.Depth, MaxDepth do
		local Position = Current.Position
		local Index = PositionX > Position[1] and 1 or 2
		if PositionY <= Position[2] then
			Index += 4
		end

		if PositionZ >= Position[3] then
			Index += 2
		end

		local Next = Current.SubRegions[Index]

		-- construct
		if not Next then
			local Size = Current.Size
			local CurrentPosition = Current.Position
			local Multiplier = SUB_REGION_POSITION_OFFSET[Index]

			local X, Y, Z = Size[1], Size[2], Size[3]
			local CurrentPositionX = CurrentPosition[1] + Multiplier[1] * X
			local CurrentPositionY = CurrentPosition[2] + Multiplier[2] * Y
			local CurrentPositionZ = CurrentPosition[3] + Multiplier[3] * Z
			local SizeX, SizeY, SizeZ = X / 2, Y / 2, Z / 2

			local HalfSizeX, HalfSizeY, HalfSizeZ = SizeX / 2, SizeY / 2, SizeZ / 2

			local LowerBoundsArray = table.create(3)
			LowerBoundsArray[1], LowerBoundsArray[2], LowerBoundsArray[3] = CurrentPositionX - HalfSizeX, CurrentPositionY - HalfSizeY, CurrentPositionZ - HalfSizeZ

			local PositionArray = table.create(3)
			PositionArray[1], PositionArray[2], PositionArray[3] = CurrentPositionX, CurrentPositionY, CurrentPositionZ

			local SizeArray = table.create(3)
			SizeArray[1], SizeArray[2], SizeArray[3] = SizeX, SizeY, SizeZ

			local UpperBoundsArray = table.create(3)
			UpperBoundsArray[1], UpperBoundsArray[2], UpperBoundsArray[3] = CurrentPositionX + HalfSizeX, CurrentPositionY + HalfSizeY, CurrentPositionZ + HalfSizeZ

			Next = {
				Depth = Current and (Current.Depth + 1) or 1;
				LowerBounds = LowerBoundsArray;
				NodeCount = 0;
				Nodes = {}; -- [node] = true (contains subchild nodes too)
				Parent = Current;
				ParentIndex = Index;
				Position = PositionArray;
				Size = SizeArray; -- { sx, sy, sz }
				SubRegions = {};
				UpperBounds = UpperBoundsArray;
			}

			-- Next = OctreeRegionUtils.CreateSubRegion(Current, Index)
			Current.SubRegions[Index] = Next
		end

		-- iterate
		Current = Next
	end

	return Current
end

local OctreeRegionUtils_Create = OctreeRegionUtils.Create

function OctreeRegionUtils.CreateSubRegion(ParentRegion, ParentIndex)
	local Size = ParentRegion.Size
	local Position = ParentRegion.Position
	local Multiplier = SUB_REGION_POSITION_OFFSET[ParentIndex]

	local X, Y, Z = Size[1], Size[2], Size[3]
	local PositionX = Position[1] + Multiplier[1] * X
	local PositionY = Position[2] + Multiplier[2] * Y
	local PositionZ = Position[3] + Multiplier[3] * Z

	return OctreeRegionUtils_Create(PositionX, PositionY, PositionZ, X / 2, Y / 2, Z / 2, ParentRegion, ParentIndex)
end

-- Consider regions to be range [px, y)
function OctreeRegionUtils.InRegionBounds(Region, PositionX, PositionY, PositionZ)
	local LowerBounds = Region.LowerBounds
	local UpperBounds = Region.UpperBounds
	return PositionX >= LowerBounds[1] and PositionX <= UpperBounds[1] and PositionY >= LowerBounds[2] and PositionY <= UpperBounds[2] and PositionZ >= LowerBounds[3] and PositionZ <= UpperBounds[3]
end

--- This definitely collides
-- https://stackoverflow.com/questions/5928725/hashing-2d-3d-and-nd-vectors
function OctreeRegionUtils.GetTopLevelRegionHash(CX: number, CY: number, CZ: number): number
	-- 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, PositionX, PositionY, PositionZ)
	return math.floor(PositionX / MaxRegionSize[1] + 0.5), math.floor(PositionY / MaxRegionSize[2] + 0.5), math.floor(PositionZ / MaxRegionSize[3] + 0.5)
end

function OctreeRegionUtils.GetTopLevelRegionPosition(MaxRegionSize, CX, CY, CZ)
	return MaxRegionSize[1] * CX, MaxRegionSize[2] * CY, MaxRegionSize[3] * CZ
end

function OctreeRegionUtils.AreEqualTopRegions(Region, PositionX, PositionY, PositionZ)
	local Position = Region.Position
	return Position[1] == PositionX and Position[2] == PositionY and Position[3] == PositionZ
end

function OctreeRegionUtils.FindRegion(RegionHashMap, MaxRegionSize, PositionX, PositionY, PositionZ)
	local X, Y, Z = MaxRegionSize[1], MaxRegionSize[2], MaxRegionSize[3]
	local CX, CY, CZ = math.floor(PositionX / X + 0.5), math.floor(PositionY / Y + 0.5), math.floor(PositionZ / Z + 0.5)
	local Hash = CX * 73856093 + CY * 19351301 + CZ * 83492791

	local RegionList = RegionHashMap[Hash]
	if not RegionList then
		return nil
	end

	local RegionPositionX, RegionPositionY, RegionPositionZ = X * CX, Y * CY, Z * CZ
	for _, Region in ipairs(RegionList) do
		local Position = Region.Position
		if Position[1] == RegionPositionX and Position[2] == RegionPositionY and Position[3] == RegionPositionZ then
			return Region
		end
	end

	return nil
end

-- function OctreeRegionUtils.Create2(RegionPositionX, RegionPositionY, RegionPositionZ, X, Y, Z)
-- 	local HalfSizeX, HalfSizeY, HalfSizeZ = X / 2, Y / 2, Z / 2
-- 	local Region = {
-- 		Depth = 1;
-- 		LowerBounds = {RegionPositionX - HalfSizeX, RegionPositionY - HalfSizeY, RegionPositionZ - HalfSizeZ};
-- 		NodeCount = 0;
-- 		Nodes = {}; -- [node] = true (contains subchild nodes too)
-- 		Parent = nil;
-- 		ParentIndex = nil;
-- 		Position = {RegionPositionX, RegionPositionY, RegionPositionZ};
-- 		Size = {X, Y, Z}; -- { sx, sy, sz }
-- 		SubRegions = {};
-- 		UpperBounds = {RegionPositionX + HalfSizeX, RegionPositionY + HalfSizeY, RegionPositionZ + HalfSizeZ};
-- 	}
-- end

function OctreeRegionUtils.GetOrCreateRegion(RegionHashMap, MaxRegionSize, PositionX, PositionY, PositionZ)
	local X, Y, Z = MaxRegionSize[1], MaxRegionSize[2], MaxRegionSize[3]
	local CX, CY, CZ = math.floor(PositionX / X + 0.5), math.floor(PositionY / Y + 0.5), math.floor(PositionZ / Z + 0.5)
	local Hash = CX * 73856093 + CY * 19351301 + CZ * 83492791

	local RegionList = RegionHashMap[Hash]
	if not RegionList then
		RegionList = {}
		RegionHashMap[Hash] = RegionList
	end

	local RegionPositionX, RegionPositionY, RegionPositionZ = X * CX, Y * CY, Z * CZ
	for _, Region in ipairs(RegionList) do
		local Position = Region.Position
		if Position[1] == RegionPositionX and Position[2] == RegionPositionY and Position[3] == RegionPositionZ then
			return Region
		end
	end

	local HalfSizeX, HalfSizeY, HalfSizeZ = X / 2, Y / 2, Z / 2

	local LowerBoundsArray = table.create(3)
	LowerBoundsArray[1], LowerBoundsArray[2], LowerBoundsArray[3] = RegionPositionX - HalfSizeX, RegionPositionY - HalfSizeY, RegionPositionZ - HalfSizeZ

	local PositionArray = table.create(3)
	PositionArray[1], PositionArray[2], PositionArray[3] = RegionPositionX, RegionPositionY, RegionPositionZ

	local SizeArray = table.create(3)
	SizeArray[1], SizeArray[2], SizeArray[3] = X, Y, Z

	local UpperBoundsArray = table.create(3)
	UpperBoundsArray[1], UpperBoundsArray[2], UpperBoundsArray[3] = RegionPositionX + HalfSizeX, RegionPositionY + HalfSizeY, RegionPositionZ + HalfSizeZ

	local Region = {
		Depth = 1;
		LowerBounds = LowerBoundsArray;
		NodeCount = 0;
		Nodes = {}; -- [node] = true (contains subchild nodes too)
		Parent = nil;
		ParentIndex = nil;
		Position = PositionArray;
		Size = SizeArray; -- { sx, sy, sz }
		SubRegions = {};
		UpperBounds = UpperBoundsArray;
	}

	table.insert(RegionList, Region)
	return Region
end

return OctreeRegionUtils