KdTree 3D transform

source.cpp

#include <iostream>
#include "GL\glut.h"
#include "Vec3.h"
#include "Particle.h"
#include "HashTable.h"
#include "QuadTree.h"
#include "KdTree.h"
#include "KdTree3D.h"
#include <vector>

using namespace std;

#define NUM_PARTICLE 10000

//#define HASH_TABLE
//#define QUAD_TREE
//#define KD_TREE
#define KD_TREE_3D

bool userDraw = true;

vector<Particle*> m_Particles;
HashTable* m_Hashing;
QuadTree* m_QuadTree;
KdTree* m_KdTree;
KdTree3D* m_KdTree3d;

#ifdef KD_TREE_3D
	float _zoom = 15.0f; // 화면 확대,축소
	float _rot_x = 0.0f; // x축 회전
	float _rot_y = 0.001f; // y축 회전
	float _trans_x = 0.0f; // x축 이동
	float _trans_y = 0.0f; // y축 이동
	int _last_x = 0; // 이전 마우스 클릭 x위치
	int _last_y = 0; // 이전 마우스 클릭 y위치
unsigned char _buttons[3] = { 0 }; // 마우스 상태(왼쪽,오른쪽,휠 버튼)
#endif

void InitParticles(void)
{
	for (int i = 0; i < NUM_PARTICLE; i++) {
		double x = (double)(rand()) / RAND_MAX;
		double y = (double)(rand()) / RAND_MAX;
		double sdf1 = x * x + y * y - 0.4 * 0.4;
		double sdf2 = (1.0 - x) * (1.0 - x) + (1.0 - y) * (1.0 - y) - 0.4 * 0.4;
		double sdf3 = (0.5 - x) * (0.5 - x) + (0.5 - y) * (0.5 - y) - 0.3 * 0.3;
		double sdf = fmin(fmin(sdf1, sdf2), sdf3);
		if (sdf <= 0.0) {
			m_Particles.push_back(new Particle(false, Vec3<double>(x, y, 0.0)));
		}
	}
}

void InitParticles3D(void)
{
	for (int i = 0; i < NUM_PARTICLE; i++) {
		double x = (double)(rand()) / RAND_MAX;
		double y = (double)(rand()) / RAND_MAX;
		double z = (double)(rand()) / RAND_MAX;
		double sdf1 = x * x + y * y + z * z - 0.3 * 0.3;
		double sdf2 = (1.0 - x) * (1.0 - x) + (1.0 - y) * (1.0 - y) + (1.0 - z) * (1.0 - z) - 0.4 * 0.4;
		double sdf3 = (0.5 - x) * (0.5 - x) + (0.5 - y) * (0.5 - y) + (0.5 - z) * (0.5 - z) - 0.3 * 0.3;
		double sdf = fmin(fmin(sdf1, sdf2), sdf3);
		if (sdf <= 0.0) {
			m_Particles.push_back(new Particle(false, Vec3<double>(x, y, z)));
		}
	}
}

void InitFlag(void)
{
	for (auto p : m_Particles) {
		p->m_Active = false;
		p->m_Collision = false;
	}
}

void Init(void)
{
	glEnable(GL_DEPTH_TEST);

#ifdef QUAD_TREE
	InitParticles();
	m_QuadTree = new QuadTree(7, 2);
	m_QuadTree->BuildTree(m_Particles);
	m_QuadTree->Query(m_Particles, 0.08, 33);
#endif

#ifdef HASH_TABLE
	InitParticles();
	m_Hashing = new HashTable(30);
	m_Hashing->Sort(m_Particles);
	m_Hashing->Query(m_Particles, 0.08, 33);
#endif

#ifdef KD_TREE
	InitParticles();
	m_KdTree = new KdTree(100);
	m_KdTree->BuildTree(m_Particles);
	m_KdTree->Query(m_Particles, 0.08, 33);
#endif

#ifdef KD_TREE_3D
	InitParticles3D();
	m_KdTree3d = new KdTree3D(50);
	m_KdTree3d->BuildTree(m_Particles);
	m_KdTree3d->Query(m_Particles, 0.08, 33);
#endif
}

void Rendering(void)
{
	if (userDraw) {
#ifdef QUAD_TREE
		m_QuadTree->Darw();
#endif

#ifdef HASH_TABLE
		m_Hashing->Draw();
#endif

#ifdef KD_TREE
		m_KdTree->Darw();
#endif
#ifdef KD_TREE_3D
		m_KdTree3d->Draw();
#endif
	}

	glPushMatrix();
	glDisable(GL_LIGHTING);
	glPushMatrix();
	glPointSize(2.0);
	glEnable(GL_POINT_SMOOTH);
	glBegin(GL_POINTS);
	for (auto p : m_Particles) {
		glColor3f(0.0f, 0.0f, 0.0f);
		if (p->m_Active) {
			glColor3f(1.0f, 0.0f, 0.0f);
		}
		if (p->m_Collision) {
			glColor3f(1.0f, 1.0f, 0.0f);
		}
		auto pos = p->m_Pos;
		glVertex3f((float)pos.x(), (float)pos.y(), (float)pos.z());
	}
	glEnd();
	glPopMatrix();
	glEnable(GL_LIGHTING);
	glColor3f(1.0f, 1.0f, 1.0f);
	glPopMatrix();


}

void Display(void)
{
	glClearColor(0.8f, 0.8f, 0.8f, 1.0f);
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
	glLoadIdentity();

	glTranslatef(0, 0, -_zoom);
	glTranslatef(_trans_x, _trans_y, 0);
	glRotatef(_rot_x, 1, 0, 0);
	glRotatef(_rot_y, 0, 1, 0);
	////glTranslatef(0.3, 0, 0);
	//glTranslatef(0, -0.3, 0);
	//glRotatef(45.0f, 1, 0, 0);
	//glRotatef(45.0f, 0, 1, 0);

	Rendering();
	glutSwapBuffers();
}

void Reshape(int w, int h)
{
	glViewport(0, 0, w, h);
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	double r = (float)w / h;
	double xw = 1.0 * r;
	double yw = 1.0 / r;
	if (w < h) {
		glOrtho(-0.0, 1.0, -(yw - 1) / 2, (yw + 1) / 2, -10, 100);
	}
	else {
		glOrtho(-(xw - 1) / 2, (xw + 1) / 2, -0.0, 1.0, -10, 100);
	}
	glMatrixMode(GL_MODELVIEW);
	glutPostRedisplay();
}

void Keyboard(unsigned char key, int x, int y)
{
	switch (key)
	{
	case 'Q':
	case 'q':
		exit(0);
	case 's':
	case 'S':
		InitFlag();
#ifdef QUAD_TREE
		m_QuadTree->Query(m_Particles, 0.08, rand() % m_Particles.size());
#endif

#ifdef HASH_TABLE
		m_Hashing->Query(m_Particles, 0.08, rand() % m_Particles.size());
#endif

#ifdef KD_TREE
		m_KdTree->Query(m_Particles, 0.08, rand() % m_Particles.size());
#endif
#ifdef KD_TREE_3D
		m_KdTree3d->Query(m_Particles, 0.08, rand() % m_Particles.size());
#endif
		break;
	case 'd':
	case 'D':
		userDraw = !userDraw;
		break;
	}
	glutPostRedisplay();
}

#ifdef KD_TREE_3D
void Mouse(int button, int state, int x, int y)
{
	// 이전 마우스 클릭 위치
	_last_x = x;
	_last_y = y;

	switch (button)
	{
	case GLUT_LEFT_BUTTON:
		_buttons[0] = state == GLUT_DOWN ? 1 : 0;
		break;
	case GLUT_MIDDLE_BUTTON:
		_buttons[1] = state == GLUT_DOWN ? 1 : 0;
		break;
	case GLUT_RIGHT_BUTTON:
		_buttons[2] = state == GLUT_DOWN ? 1 : 0;
		break;
	default:
		break;
	}
	glutPostRedisplay(); // 화면갱신
}

void Motion(int x, int y)
{
	int diff_x = x - _last_x;
	int diff_y = y - _last_y;
	_last_x = x;
	_last_y = y;

	if (_buttons[2]) {
		_zoom -= (float)0.02f * diff_x;
	}
	else if (_buttons[1]) {
		_trans_x += (float)0.02f * diff_x;
		_trans_y -= (float)0.02f * diff_y;
	}
	else if (_buttons[0]) {
		_rot_x += (float)0.2f * diff_y;
		_rot_y += (float)0.2f * diff_x;
	}
	glutPostRedisplay(); // 화면갱신
}
#endif

int main(int argc, char** argv)
{
	glutInit(&argc, argv);
	glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGBA | GLUT_DEPTH);
	glutInitWindowSize(600, 600);
	glutInitWindowPosition(100, 100);
	glutCreateWindow("Multi-Resolution Grids: HashTable, QuadTree, KdTree");
	glutDisplayFunc(Display);
	glutReshapeFunc(Reshape);
#ifdef KD_TREE_3D
	glutMouseFunc(Mouse); // 마우스 클릭 이벤트	
	glutMotionFunc(Motion); // 마우스 이동 이벤트
#endif
	glutKeyboardFunc(Keyboard);
	Init();
	glutMainLoop();
}

kdTree3D.h

#ifndef __KD_TREE3D_H__
#define __KD_TREE3D_H__

#pragma once
#include "Particle.h"
#include "Vec3.h"
#include <vector>
#include <algorithm>

using namespace std;

class KdNode3D
{
public:
	Vec3<double>		m_Min;
	Vec3<double>		m_Max;
	vector<Particle*>	m_Particles;
	KdNode3D* m_ChildNodes[2];
public:
	KdNode3D(void);
	KdNode3D(Vec3<double> min, Vec3<double> max);
	KdNode3D(int depth, Vec3<double> min, Vec3<double> max);
	~KdNode3D(void) {}
public:
	inline Vec3<double> GetCenter(void) { return (m_Max + m_Min) / 2.0; }
public:
	bool	IsEmpty(void);
	bool	IsContain(Vec3<double> p);
	bool	IsContain(Vec3<double> p, double r);
	bool	IsIntersect(Vec3<double> p, double r);
	void	Subdivide(void);
};

class KdTree3D
{
public:
	int					m_Bucket;
	KdNode3D* m_Root;
	vector<KdNode3D*>	m_DrawNodes; //render
public:
	KdTree3D();
	KdTree3D(int bucket)
	{
		m_Bucket = bucket;
	}
	~KdTree3D();
public:
	void	BuildTree(vector<Particle*>& particles);
	void	Query(vector<Particle*>& particles, double r, int index);
public:
	void	Draw(void);
};

#endif

KdTree3D.cpp

#include "KdTree3D.h"
#include "GL\glut.h"

KdNode3D::KdNode3D(void)
{
	for (int i = 0; i < 2; i++) {
		m_ChildNodes[i] = nullptr;
	}
}

KdNode3D::KdNode3D(Vec3<double> min, Vec3<double> max)
{
	m_Min = min;
	m_Max = max;
	for (int i = 0; i < 2; i++) {
		m_ChildNodes[i] = nullptr;
	}
}

KdNode3D::KdNode3D(int depth, Vec3<double> min, Vec3<double> max)
{
	m_Min = min;
	m_Max = max;
	for (int i = 0; i < 2; i++) {
		m_ChildNodes[i] = nullptr;
	}
}

bool KdNode3D::IsContain(Vec3<double> p)
{
	if (p.x() >= m_Min.x() && p.x() <= m_Max.x() && p.y() >= m_Min.y() && p.y() <= m_Max.y() && p.z() >= m_Min.z() && p.z() <= m_Max.z()) {
		return true;
	}
	return false;
}

bool KdNode3D::IsIntersect(Vec3<double> p, double r)
{
	double dist = 0.0;
	for (int i = 0; i < 2; i++) {
		if (p(i) < m_Min(i)) {
			double d = m_Min(i) - p(i);
			dist += d * d;
		}
		else if (p(i) > m_Max(i)) {
			double d = p(i) - m_Max(i);
			dist += d * d;
		}
	}
	return dist <= r * r;
}

bool KdNode3D::IsEmpty(void)
{
	return m_ChildNodes[0] == nullptr;
}

bool KdNode3D::IsContain(Vec3<double> p, double r)
{
	double minx = p.x() - r;
	double maxx = p.x() + r;
	double miny = p.y() - r;
	double maxy = p.y() + r;
	double minz = p.z() - r;
	double maxz = p.z() + r;
	return (minx >= m_Min.x() && maxx <= m_Max.x() && miny >= m_Min.y() && maxy <= m_Max.y() && minz >= m_Min.z() && maxz <= m_Max.z());
}

void KdNode3D::Subdivide(void)
{
	for (int i = 0; i < 2; i++) {
		m_ChildNodes[i] = new KdNode3D();
	}

	auto corssVec = m_Max - m_Min;
	int mid = m_Particles.size() / 2;
	int splitAxis;
	if (corssVec.x() >= corssVec.y())
	{
		if (corssVec.x() >= corssVec.z())
		{
			splitAxis = 0;
		}
		else
		{
			splitAxis = 2;
		}
	}
	else
	{
		if (corssVec.y() >= corssVec.z())
		{
			splitAxis = 1;
		}
		else
		{
			splitAxis = 2;
		}
	}
	nth_element(m_Particles.begin(), m_Particles.begin() + mid, m_Particles.end(), [splitAxis](Particle* a, Particle* b) {return a->m_Pos(splitAxis) < b->m_Pos(splitAxis); });

	auto separation = m_Particles[mid]->m_Pos(splitAxis);
	auto localMin = m_Min;
	auto localMax = m_Max;
	localMin[splitAxis] = separation;
	localMax[splitAxis] = separation;

	m_ChildNodes[0]->m_Min = m_Min;
	m_ChildNodes[0]->m_Max = localMax;
	m_ChildNodes[1]->m_Min = localMin;
	m_ChildNodes[1]->m_Max = m_Max;

	// Transfer particles
	m_ChildNodes[0]->m_Particles.clear();
	m_ChildNodes[0]->m_Particles.assign(m_Particles.begin(), m_Particles.begin() + mid);
	m_ChildNodes[1]->m_Particles.clear();
	m_ChildNodes[1]->m_Particles.assign(m_Particles.begin() + mid, m_Particles.end());
	m_Particles.clear();
}

KdTree3D::KdTree3D()
{
}

KdTree3D::~KdTree3D()
{
}

void KdTree3D::BuildTree(vector<Particle*>& particles)
{
	m_Root = new KdNode3D(1, Vec3<double>(-0.05, -0.05, -0.05), Vec3<double>(1.05, 1.05, 1.05));

	// Transfer particles
	for (auto p : particles) {
		m_Root->m_Particles.push_back(p);
	}

	vector<KdNode3D*> queue;
	queue.push_back(m_Root);

	int debug = 0;

	while (queue.size() != 0) {
		auto node = queue[0];
		m_DrawNodes.push_back(node);
		queue.erase(queue.begin());
		if (node->m_Particles.size() > m_Bucket) {
			node->Subdivide();
			for (int i = 0; i < 2; i++) {
				queue.push_back(node->m_ChildNodes[i]);
			}
		}
	}
	printf("KdTree : build tree!\n");
}

void KdTree3D::Query(vector<Particle*>& particles, double r, int index)
{
	auto pos = particles[index]->m_Pos;

	vector<KdNode3D*> queue;
	queue.push_back(m_Root);

	while (queue.size() != 0) {
		auto node = queue[0];
		queue.erase(queue.begin());
		bool contain = node->IsContain(pos, r);
		bool intersect = node->IsIntersect(pos, r);
		if (contain == true || intersect == true) {
			for (auto p : node->m_Particles) {
				p->m_Active = true;
				double sdf = (p->m_Pos.x() - pos.x()) * (p->m_Pos.x() - pos.x()) + (p->m_Pos.y() - pos.y()) * (p->m_Pos.y() - pos.y()) + (p->m_Pos.z() - pos.z()) * (p->m_Pos.z() - pos.z()) - r * r;
				if (sdf <= 0.0) {
					p->m_Collision = true;
				}
			}
			if (node->IsEmpty() == false) {
				for (int i = 0; i < 2; i++) {
					queue.push_back(node->m_ChildNodes[i]);
				}
			}
		}
	}
}

void KdTree3D::Draw(void)
{
	glLineWidth(1.0);
	glDisable(GL_LIGHTING);
	glPushMatrix();
	for (auto n : m_DrawNodes) {
		glColor3f(0.0f, 0.0f, 1.0f);
		glBegin(GL_LINES);
		glVertex3f(n->m_Min.x(), n->m_Min.y(), n->m_Min.z());
		glVertex3f(n->m_Max.x(), n->m_Min.y(), n->m_Min.z());

		glVertex3f(n->m_Max.x(), n->m_Min.y(), n->m_Min.z());
		glVertex3f(n->m_Max.x(), n->m_Max.y(), n->m_Min.z());

		glVertex3f(n->m_Max.x(), n->m_Max.y(), n->m_Min.z());
		glVertex3f(n->m_Min.x(), n->m_Max.y(), n->m_Min.z());

		glVertex3f(n->m_Min.x(), n->m_Max.y(), n->m_Min.z());
		glVertex3f(n->m_Min.x(), n->m_Min.y(), n->m_Min.z());

		glVertex3f(n->m_Min.x(), n->m_Min.y(), n->m_Min.z());
		glVertex3f(n->m_Min.x(), n->m_Min.y(), n->m_Max.z());

		glVertex3f(n->m_Max.x(), n->m_Min.y(), n->m_Min.z());
		glVertex3f(n->m_Max.x(), n->m_Min.y(), n->m_Max.z());

		glVertex3f(n->m_Max.x(), n->m_Max.y(), n->m_Min.z());
		glVertex3f(n->m_Max.x(), n->m_Max.y(), n->m_Max.z());

		glVertex3f(n->m_Min.x(), n->m_Max.y(), n->m_Min.z());
		glVertex3f(n->m_Min.x(), n->m_Max.y(), n->m_Max.z());

		glVertex3f(n->m_Min.x(), n->m_Min.y(), n->m_Max.z());
		glVertex3f(n->m_Max.x(), n->m_Min.y(), n->m_Max.z());

		glVertex3f(n->m_Max.x(), n->m_Min.y(), n->m_Max.z());
		glVertex3f(n->m_Max.x(), n->m_Max.y(), n->m_Max.z());

		glVertex3f(n->m_Max.x(), n->m_Max.y(), n->m_Max.z());
		glVertex3f(n->m_Min.x(), n->m_Max.y(), n->m_Max.z());

		glVertex3f(n->m_Min.x(), n->m_Max.y(), n->m_Max.z());
		glVertex3f(n->m_Min.x(), n->m_Max.y(), n->m_Max.z());

		glEnd();
	}
	glPopMatrix();
	glEnable(GL_LIGHTING);
	glLineWidth(1.0);
}