def __insert_node(self, root, dim, parent, data_point): # TODO implement recursive insertion
if root is None:
pos = parent.bounding_box.get_centroid()
x_offset = dim[0] / 2.0
y_offset = dim[1] / 2.0
z_offset = dim[2] / 2.0
branch = self.__find_branch(parent, data_point)
if branch == 0:
new_box = AxisAlignedBox3D.init_box_center(
np.asarray([pos[0] - x_offset, pos[1] - y_offset, pos[2] - z_offset]), x_offset, y_offset, z_offset)
if branch == 1:
new_box = AxisAlignedBox3D.init_box_center(
np.asarray([pos[0] - x_offset, pos[1] - y_offset, pos[2] + z_offset]), x_offset, y_offset, z_offset)
if branch == 2:
new_box = AxisAlignedBox3D.init_box_center(
np.asarray([pos[0] - x_offset, pos[1] + y_offset, pos[2] - z_offset]), x_offset, y_offset, z_offset)
if branch == 3:
new_box = AxisAlignedBox3D.init_box_center(
np.asarray([pos[0] - x_offset, pos[1] + y_offset, pos[2] + z_offset]), x_offset, y_offset, z_offset)
if branch == 4:
new_box = AxisAlignedBox3D.init_box_center(
np.asarray([pos[0] + x_offset, pos[1] - y_offset, pos[2] - z_offset]), x_offset, y_offset, z_offset)
if branch == 5:
new_box = AxisAlignedBox3D.init_box_center(
np.asarray([pos[0] + x_offset, pos[1] - y_offset, pos[2] + z_offset]), x_offset, y_offset, z_offset)
if branch == 6:
new_box = AxisAlignedBox3D.init_box_center(
np.asarray([pos[0] + x_offset, pos[1] + y_offset, pos[2] - z_offset]), x_offset, y_offset, z_offset)
if branch == 7:
new_box = AxisAlignedBox3D.init_box_center(
np.asarray([pos[0] + x_offset, pos[1] + y_offset, pos[2] + z_offset]), x_offset, y_offset, z_offset)
return OctreeNode(new_box, parent.depth + 1, [data_point])
elif not root.isLeafNode and (np.any(root.bounding_box.get_centroid() != data_point)
or root.bounding_box.get_centroid() != data_point):
branch = self.__find_branch(root, data_point)
new_dim = root.bounding_box.get_dimensions() / 2.0
root.branches[branch] = self.__insert_node(root.branches[branch], new_dim, root, data_point)
elif root.isLeafNode:
if len(root.data_points) < self.MAX_OBJECTS_PER_CUBE:
root.data_points.append(data_point)
# print(len(root.data_points))
# try:
# root.data_points.append(data_point)
# except AttributeError:
# print(root.data_points)
# print(type(root.data_points))
# exit(1)
else:
root.data_points.append(data_point)
objects = copy.deepcopy(root.data_points) # TODO do we need deepcopy here?
root.data_points = []
root.isLeafNode = False
new_dim = root.bounding_box.get_dimensions() / 2.0
for k in objects:
branch = self.__find_branch(root, k)
root.branches[branch] = self.__insert_node(root.branches[branch], new_dim, root, k)
return root
import numpy as np from Utils.AxisAlignedBox3D import AxisAlignedBox3D x = AxisAlignedBox3D(np.array([0, 0, 0], dtype=np.float32), np.array([1, 1, 1], dtype=np.float32)) y = AxisAlignedBox3D.init_box_center(np.array([.5, .5, .5], dtype=np.float32), .5, .5, .5) print(x.get_corners()) print(y.get_corners()) assert x.equals_box(y), "fail equality"