Octree新 20210422

#pragma once
#include <vector>
#include <iostream>
#include <fstream>
#include <string>

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/cloud_viewer.h>

#include <GL/GLU.h>


class Octree {
public:
	Octree() {}
	~Octree() { std::cout << "delete"; destory(root); }
	struct Point
	{
		float x, y, z;
		float r, g, b;
		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,float r=1,float g=1,float b=1) {
			this->x = x;
			this->y = y;
			this->z = z;
			this->r = r;
			this->g = g;
			this->b = b;
		}
		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;
	std::vector<Point> resPointCloud;
	float octLength;
	bool oldPoint = true;

	bool created = false;

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


	Octree& operator-(const Octree &b) {
		Octree *newOctree = new Octree();
		newOctree->root = NULL;
		resPointCloud.clear();
		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));
		}
		getSubResault(*newOctree, newOctree->root);
		return *newOctree;
	}

	int subLimt = 20;
	void getSubResault(Octree &root, Octree::Octree_Struct node) {
		if (node->level > 1) {
			for (int i = 0; i < 8; i++) {
				if (node->nodes[i] != NULL) {
					getSubResault(root, node->nodes[i]);
				}
			}
		}
		else {
			if (node->newCount + subLimt <= node->count)
			{
				int size = node->points.size();
				for (int i = 0; i < size; i++) {
					root.resPointCloud.push_back(node->points.at(i));
				}
			}
		}
	}

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


			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;
					root->nodes[i]->newCount = 0;
				}

				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*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() {
		std::cout << "deleted";
		destory(root);
	}
	void destory(Octree_Struct &root) {
		if (created != true)return;
		if (root != NULL)
			for (int i = 0; i < 8; i++) {
				if (root->nodes[i] != NULL) {
					destory(root->nodes[i]);

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

	

public://后添加
	struct drawOptions {
		enum showType { POINT };
		showType showOption;
		float pointSize;
		int drawPointCount;

		int newPointMoreSize;
		drawOptions(showType _showOption, float _pointSize, int _drawPointCount, int _newPointMoreSize) {
			showOption = _showOption;
			pointSize = _pointSize;
			drawPointCount = _drawPointCount;
			newPointMoreSize = _newPointMoreSize;
		}
	};
	void loadFile(const std::string str) {
		int x, y, z, color;
		FILE *fp;
		fp = fopen(str.c_str(), "r");
		if (!fp) {
			std::cout << "0.can't open " << std::endl;
			return;
		}
		while (feof(fp) == 0) {
			fscanf(fp, "%d %d %d %d", &x, &y, &z, &color);
			insertPoint(root, Octree::Point((float)x, (float)y, (float)z));
		}
		fclose(fp);
	}
	void loadPoints(pcl::PointCloud<pcl::PointXYZRGB>::Ptr src) {
		int size = src->size();
		for (int i = 0; i < size; i++) {
			Point point;
			point.x = src->at(i).x;
			point.y = src->at(i).y;
			point.z = src->at(i).z;
			point.r = src->at(i).r; 
			point.g = src->at(i).g;
			point.b = src->at(i).b;
			insertPoint(root,point);
		}
	}

	void getPointsFromOctree(pcl::PointCloud<pcl::PointXYZRGB>::Ptr &src, drawOptions drawOption) {
		getPointsFromOctree(root, src, drawOption);
	}
	void getPointsFromOctree(Octree_Struct root, pcl::PointCloud<pcl::PointXYZRGB>::Ptr &src, drawOptions drawOption) {
		if (root == NULL)return;

		if (root->level == 1){
			if(root->newCount<root->count)
			if(root->newCount<drawOption.newPointMoreSize)
			{
				pcl::PointXYZRGB point;
				if (root->count > 0)
				for (auto it = root->points.begin(); it != root->points.end(); it++) {
					point.x = (*it).x;
					point.y = (*it).y;
					point.z = (*it).z;
					point.r = (*it).r;
					point.g = (*it).g;
					point.b = (*it).b;
					src->push_back(point);
				}
				cout << root->points.size() << endl;
			}
		}

		for (int i = 0; i < 8; i++) {
			getPointsFromOctree(root->nodes[i],src, drawOption);
		}
	}

	
	void gglDrawOctree(drawOptions drawOption) {
		gglDrawOctree(root, drawOption);
	}
	void gglDrawOctree(Octree_Struct root, drawOptions drawOption) {
		if (root == NULL)return;

		if(root->level==1)
		if (root->points.size() >= drawOption.drawPointCount) 
		{
			std::cout << root->points.size() << std::endl;
			if (drawOption.showOption == drawOptions::POINT) {
				glPointSize(drawOption.pointSize);
				glBegin(GL_POINTS);

				for (auto it = root->points.begin(); it != root->points.end(); it++) {
					glColor3f((*it).r, (*it).g, (*it).b);
					glVertex3f((*it).x, (*it).y, (*it).z);
				}
				glEnd();
			}
		}

		for (int i = 0; i < 8; i++) {
			gglDrawOctree(root->nodes[i], drawOption);
		}
	}
};

oct.octLength  = 75;
oct.root       = NULL;
oct2.octLength = 75;
oct2.root      = NULL;

openFileSlot(src,"C:\\Users\\90182si\\Desktop\\dianyun2021\\zhangaiwu3.pcd");
openFileSlot(src2,"C:\\Users\\90182si\\Desktop\\dianyun2021\\bei-jing.pcd");
oct.oldPoint = true;
oct.loadPoints(src);
oct.oldPoint = false;
oct.loadPoints(src2);

oct.getPointsFromOctree(octPoint,Octree::drawOptions(Octree::drawOptions::POINT,3.0f,0,10));