KD Tree in CPP

kd_tree.cpp

#include <stdio.h>
#include <vector>
#include <algorithm>
#include <math.h>
#define E 0.000001

using namespace std;

struct point
{
	float p[2];
};

struct KDnode
{
	float key;
	point p;
	KDnode *left;
	KDnode *right;
	bool isleaf;
};

struct region
{
	point p1;
	point p2;
};


int statusofRange(region r1, region r2)
{
	if( r1.p1.p[0] >= r2.p1.p[0] && r1.p2.p[0] <= r2.p2.p[0] && r1.p1.p[1] >= r2.p1.p[1] && r1.p2.p[1] <= r2.p2.p[1])
		return 1;
	else if(r1.p2.p[0] < r2.p1.p[0] || r1.p1.p[0] > r2.p2.p[0] || r1.p2.p[1] < r2.p1.p[1] || r1.p1.p[1] > r2.p2.p[1])
		return 0;
	else
		return 2;
}

void reportSubtree(vector<point>& list, KDnode *tree)
{
	if(tree->isleaf)
	{
		list.push_back(tree->p);
	}
	else
	{
		reportSubtree(list, tree->left);
		reportSubtree(list, tree->right);
	}
}

void printTree(KDnode *tree)
{
	if(tree->isleaf)
	{
		printf(" %f\t%f\n", tree->p.p[0], tree->p.p[1]);
	}
	else
	{
		printTree(tree->left);
		printTree(tree->right);
	}
}
//laod data into RAM i.e. store into array
vector<point> loadData(FILE *fp)
{
	vector<point> data;
	float r, j;
	while(fscanf(fp, "%f%f", &r, &j) == 2)
	{
		point p;
		p.p[0] = r;
		p.p[1] = j;
		data.push_back(p);
	}
	return data;
}

bool pointInRange(point p, point p1, point p2)
{
	return ( p.p[0] >= p1.p[0] && p.p[0] >= p2.p[0]) && ( p.p[1] >= p1.p[1] && p.p[1] >= p2.p[1]);
}


//error is there
KDnode* insertPoint(KDnode *root, point p, int depth)
{
	if(root == NULL)
	{
		KDnode *leaf = new KDnode;
		leaf->p = p;
		leaf->left = NULL;
		leaf->right = NULL;
		leaf->isleaf = true;

		root = leaf;
	}
	else if(!root->isleaf)
	{
        if(root->key >= p.p[depth])
            root->left = insertPoint(root->left, p, (depth + 1) % 2);
        else
            root->right = insertPoint(root->right, p, (depth + 1) % 2);

	}
	else
	{
        //change leaf to internal node
        root->isleaf = false;
        root->key = (root->p.p[depth] + p.p[depth]) / 2;

        //create leaf node that contains same info of root
        KDnode *leaf = new KDnode;
		leaf->p = root->p;
		leaf->left = NULL;
		leaf->right = NULL;
		leaf->isleaf = true;

        //create new leaf node that contain the given info
        KDnode *newleaf = new KDnode;
		newleaf->p = p;
		newleaf->left = NULL;
		newleaf->right = NULL;
		newleaf->isleaf = true;

        // insert both these
        if(root->key >= newleaf->p.p[depth])
        {
            root->left = newleaf;
            root->right = leaf;
        }
        else
        {
            root->left = leaf;
            root->right = newleaf;
        }
    }

    return root;
}


KDnode* buildKDtree(FILE *fp)
{
	KDnode *tree = NULL;
	float r, j;
	int i = 0;
	while(fscanf(fp, "%f%f", &r, &j) == 2)
	{
		point p;
		p.p[0] = r;
		p.p[1] = j;
		printf("insert %d Point \n", ++i);
		tree = insertPoint(tree, p, 0);
	}
	return tree;
}

bool pointQuery(KDnode *root, point p)
{

	return false;
}

// query : query window rnode : tree window
void windowQuery(vector<point> &points, KDnode *root, region query, region rnode, int depth)
{

	if(root->isleaf)
	{
		if(pointInRange(root->p, query.p1, query.p2))
			points.push_back(root->p);
	}
	else
	{
		int position = 0;
		for left subtree
		region lcr;
		if(depth % 2 == 0)
		{
			lcr.p1 = rnode.p1;
			point p2;
			p2.x = root->key;
			p2.y = rnode.p2.y;
			lcr.p2 = p2;
		}
		else
		{
			lcr.p1 = rnode.p1;
			point p2;
			p2.x = rnode.p2.x;
			p2.y = root->key;
			lcr.p2 = p2;
		}
		position = statusofRange(lcr, query);
		if(position == 1)
		{
			reportSubtree(points, root->left);
		}
		else if(position == 2)
		{
			windowQuery(points, root->left, query, lcr, depth + 1);
		}

		for right subtree
		region rcr;
		if(depth % 2 == 0)
		{
			lcr.p2 = rnode.p2;
			point p1;
			p1.x = root->key;
			p1.y = rnode.p1.y;
			lcr.p1 = p1;
		}
		else
		{
			lcr.p2 = rnode.p2;
			point p1;
			p1.x = rnode.p1.x;
			p1.y = root->key;
			lcr.p1 = p1;
		}
		position = statusofRange(lcr, query);
		if(position == 1)
		{
			reportSubtree(points, root->right);
		}
		else if(position == 2)
		{
			windowQuery(points, root->right, query, rcr, depth + 1);
		}
	}
}


int main()
{
	//testing
	FILE *fi = fopen("test.txt", "r");
	KDnode *T = buildKDtree(fi);
	vector<point> ans;
	printTree(T);
	reportSubtree(ans, T);
	printf("%d size \n", ans.size());
	return 0;
}