八叉树动态Octree(点云) C++代码

动态生成八叉树，就是该八叉树可以根据新加入点的位置自动调整树的结构，将新加入点放到正确的位置。

#pragma once
#include <vector>
class Octree {
public:
	Octree() {}
	~Octree() { std::cout << "delete"; destory(root); }
	struct Point
	{
		float x, y, z;
		void setPoint(float x, float y, float z) {
			this->x = x;
			this->y = y;
			this->z = z;
		}
		Point() {}
		Point(float x, float y, float z) {
			this->x = x;
			this->y = y;
			this->z = z;
		}
		Point& operator = (const Point &b) {
			this->x = b.x;
			this->y = b.y;
			this->z = b.z;
			return *this;
		}
	};
	typedef struct Octree_Node {
		int count;
		int newCount;
		std::vector<Point> points;
		std::vector<Point> newPoints;
		Octree_Node *nodes[8];
		Point centel;
		float length;
		int level;

	}Octree_Node, *Octree_Struct;

	Octree_Struct root;
	std::vector<Point> pointCloud;
	float octLength;
	bool oldPoint = true;

	Octree& operator+(const Octree &b) {
		Octree *newOctree = new Octree();
		newOctree->root = NULL;
		newOctree->octLength = this->octLength;
		for (auto it = this->pointCloud.begin(); it != this->pointCloud.end(); it++) {
			newOctree->insertPoint(newOctree->root, (*it));
		}
		newOctree->oldPoint = false;
		for (auto it = b.pointCloud.begin(); it != b.pointCloud.end(); it++) {
			newOctree->insertPoint(newOctree->root, (*it));
		}
		return *newOctree;
	}

	void insertPoint(Octree_Struct &root, Point point) {
		//第一个点
		if (root == NULL) {
			root = new Octree_Node();
			root->centel = point;
			root->length = octLength;
			root->level = 1;


			for (int i = 0; i < 8; i++) {
				root->nodes[i] = NULL;
			}
			if (oldPoint == true) {
				root->count = 1;
				root->points.push_back(point);
			}
			else {
				root->newCount = 1;
				root->newPoints.push_back(point);
			}

			pointCloud.push_back(point);
			return;
		}

		//如果point在root范围内
		bool inRoot = false;
		if (point.x <= root->centel.x + root->length && point.x > root->centel.x - root->length) {
			if (point.y <= root->centel.y + root->length && point.y > root->centel.y - root->length) {
				if (point.z <= root->centel.z + root->length && point.z > root->centel.z - root->length) {
					inRoot = true;
				}
			}
		}

		//在里面
		if (inRoot == true) {
			//最下面那层
			if (root->level == 1) {

				if (oldPoint == true) {
					root->count++;
					root->points.push_back(point);
				}
				else {
					root->newCount++;
					root->newPoints.push_back(point);
				}
				pointCloud.push_back(point);
			}
			//中间层
			else {
				int i = 0;
				Point centel;
				//不用=
				if (point.x > root->centel.x && point.y > root->centel.y &&point.z > root->centel.z) {
					i = 0;
					centel.x = root->centel.x + root->length / 2;
					centel.y = root->centel.y + root->length / 2;
					centel.z = root->centel.z + root->length / 2;
				}
				else if (point.x <= root->centel.x && point.y > root->centel.y &&point.z > root->centel.z) {
					i = 1;
					centel.x = root->centel.x - root->length / 2;
					centel.y = root->centel.y + root->length / 2;
					centel.z = root->centel.z + root->length / 2;
				}
				else if (point.x > root->centel.x && point.y <= root->centel.y &&point.z > root->centel.z) {
					i = 2;
					centel.x = root->centel.x + root->length / 2;
					centel.y = root->centel.y - root->length / 2;
					centel.z = root->centel.z + root->length / 2;
				}
				else if (point.x <= root->centel.x && point.y <= root->centel.y &&point.z > root->centel.z) {
					i = 3;
					centel.x = root->centel.x - root->length / 2;
					centel.y = root->centel.y - root->length / 2;
					centel.z = root->centel.z + root->length / 2;
				}
				else if (point.x > root->centel.x && point.y > root->centel.y &&point.z <= root->centel.z) {
					i = 4;
					centel.x = root->centel.x + root->length / 2;
					centel.y = root->centel.y + root->length / 2;
					centel.z = root->centel.z - root->length / 2;
				}
				else if (point.x <= root->centel.x && point.y > root->centel.y &&point.z <= root->centel.z) {
					i = 5;
					centel.x = root->centel.x - root->length / 2;
					centel.y = root->centel.y + root->length / 2;
					centel.z = root->centel.z - root->length / 2;
				}
				else if (point.x > root->centel.x && point.y <= root->centel.y &&point.z <= root->centel.z) {
					i = 6;
					centel.x = root->centel.x + root->length / 2;
					centel.y = root->centel.y - root->length / 2;
					centel.z = root->centel.z - root->length / 2;
				}
				else if (point.x <= root->centel.x && point.y <= root->centel.y &&point.z <= root->centel.z) {
					i = 7;
					centel.x = root->centel.x - root->length / 2;
					centel.y = root->centel.y - root->length / 2;
					centel.z = root->centel.z - root->length / 2;
				}

				if (root->nodes[i] == NULL) {
					root->nodes[i] = new Octree_Node();
					for (int j = 0; j < 8; j++) {
						root->nodes[i]->nodes[j] = NULL;
					}
					root->nodes[i]->centel = centel;
					root->nodes[i]->length = root->length / 2;
					root->nodes[i]->level = root->level - 1;
					root->nodes[i]->count = 0;
				}
				else {
					//root->nodes[i]->count++;
				}
				if (oldPoint == true) {
					root->count++;
				}
				else {
					root->newCount++;
				}

				insertPoint(root->nodes[i], point);

			}
		}
		else if (inRoot == false) {
			//      newRoot
			//     /               \
			//  root            insert
			Octree_Struct newRoot = new Octree_Node();
			for (int i = 0; i < 8; i++) {
				newRoot->nodes[i] = NULL;
			}
			newRoot->level = root->level + 1;
			newRoot->count = root->count;
			newRoot->newCount = root->newCount;

			Point centel;
			if (point.x > root->centel.x && point.y > root->centel.y &&point.z > root->centel.z) {
				centel = Point(root->centel.x + root->length, root->centel.y + root->length, root->centel.z + root->length);
			}
			else if (point.x < root->centel.x && point.y > root->centel.y &&point.z > root->centel.z) {
				centel = Point(root->centel.x - root->length, root->centel.y + root->length, root->centel.z + root->length);
			}
			else if (point.x > root->centel.x && point.y < root->centel.y &&point.z > root->centel.z) {
				centel = Point(root->centel.x + root->length, root->centel.y - root->length, root->centel.z + root->length);
			}
			else if (point.x < root->centel.x && point.y < root->centel.y &&point.z > root->centel.z) {
				centel = Point(root->centel.x - root->length, root->centel.y - root->length, root->centel.z + root->length);
			}
			else if (point.x > root->centel.x && point.y > root->centel.y &&point.z < root->centel.z) {
				centel = Point(root->centel.x + root->length, root->centel.y + root->length, root->centel.z - root->length);
			}
			else if (point.x < root->centel.x && point.y > root->centel.y &&point.z < root->centel.z) {
				centel = Point(root->centel.x - root->length, root->centel.y + root->length, root->centel.z - root->length);
			}
			else if (point.x > root->centel.x && point.y < root->centel.y &&point.z < root->centel.z) {
				centel = Point(root->centel.x + root->length, root->centel.y - root->length, root->centel.z - root->length);
			}
			else if (point.x < root->centel.x && point.y < root->centel.y &&point.z < root->centel.z) {
				centel = Point(root->centel.x - root->length, root->centel.y - root->length, root->centel.z - root->length);
			}

			newRoot->centel = centel;

			float length = root->length;

			while (point.x > (centel.x + length) || point.x < (centel.x - length) ||
				point.y>(centel.y + length) || point.y < (centel.y - length) ||
				point.z>(centel.z + length) || point.z < (centel.z - length)
				)length *= 2;

			newRoot->length = length;

			int i = 0;
			if (root->centel.x > newRoot->centel.x && root->centel.y > newRoot->centel.y &&root->centel.z > newRoot->centel.z) {
				i = 0;
			}
			else if (root->centel.x < newRoot->centel.x && root->centel.y > newRoot->centel.y &&root->centel.z > newRoot->centel.z) {
				i = 1;
			}
			else if (root->centel.x > newRoot->centel.x && root->centel.y < newRoot->centel.y &&root->centel.z > newRoot->centel.z) {
				i = 2;
			}
			else if (root->centel.x < newRoot->centel.x && root->centel.y < newRoot->centel.y &&root->centel.z > newRoot->centel.z) {
				i = 3;
			}
			else if (root->centel.x > newRoot->centel.x && root->centel.y > newRoot->centel.y &&root->centel.z < newRoot->centel.z) {
				i = 4;
			}
			else if (root->centel.x < newRoot->centel.x && root->centel.y > newRoot->centel.y &&root->centel.z < newRoot->centel.z) {
				i = 5;
			}
			else if (root->centel.x > newRoot->centel.x && root->centel.y < newRoot->centel.y &&root->centel.z < newRoot->centel.z) {
				i = 6;
			}
			else if (root->centel.x < newRoot->centel.x && root->centel.y < newRoot->centel.y &&root->centel.z < newRoot->centel.z) {
				i = 7;
			}
			newRoot->nodes[i] = root;
			root = newRoot;
			insertPoint(newRoot, point);
		}
	}

	void destory(Octree_Struct &root) {
		if (root != NULL)
			for (int i = 0; i < 8; i++) {
				if (root->nodes[i] != NULL) {
					destory(root->nodes[i]);

				}
			}
		if (root != NULL)
			free(root);
	}
};