Quad Tree in CPP

quad_tree.cpp

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

using namespace std;

struct point
{
	float x;
	float y;
};

struct QuadNode
{
	point key;
	point p;
	QuadNode *first;
	QuadNode *second;
	QuadNode *third;
	QuadNode *forth;
	bool isleaf;
};

struct region
{
	point p1;
	point p2;
};

struct HeapElement
{
    QuadNode *p;
    region r;
    float dist;
    bool isData;
};

class mycomparison
{
public:
    bool operator() (const HeapElement& lhs, const HeapElement &rhs) const
    {
        return lhs.dist >= rhs.dist;
    }
};

// 1: contains, 2: intersect 0: disjoint
int statusofRange(region r1, region r2)
{
	if( r1.p1.x >= r2.p1.x && r1.p2.x <= r2.p2.x && r1.p1.y >= r2.p1.y && r1.p2.y <= r2.p2.y)
		return 1;
	else if(r1.p2.x < r2.p1.x || r1.p1.x > r2.p2.x || r1.p2.y < r2.p1.y || r1.p1.y > r2.p2.y)
		return 0;
	else
		return 2;
}

void reportSubroot(vector<point> &p, QuadNode *tree)
{
    if(tree->isleaf)
    {
        p.push_back(tree->p);
        printf("%f\t%f point found\n", tree->p.x, tree->p.y);
    }
    if(tree->first != NULL)
        reportSubroot(p, tree->first);
    if(tree->second != NULL)
        reportSubroot(p, tree->second);
    if(tree->third != NULL)
        reportSubroot(p, tree->third);
    if(tree->forth != NULL)
        reportSubroot(p, tree->forth);
}

bool pointInRange(point p, point p1, point p2)
{
    change to handle float precision

	return ( p.x >= p1.x && p.x <= p2.x) && ( p.y >= p1.y && p.y <= p2.y);
}

region calcRegion(region rnode, QuadNode *root,int quad)
{
    point p1, p2;

    if(quad == 1)
    {
        region fqr;
        fqr.p1 = rnode.p1;
        fqr.p2 = root->key;

        return fqr;
    }
    else if(quad == 2)
    {
        region sqr;
        p1.x = root->key.x;
        p1.y = rnode.p1.y;
        p2.x = rnode.p2.x;
        p2.y = root->key.y;
        sqr.p1 = p1;
        sqr.p2 = p2;

        return sqr;
    }
    else if(quad == 3)
    {
        region tqr;
        tqr.p1 = root->key;
        tqr.p2 = rnode.p2;

        return tqr;
    }
    else
    {
        region foqr;
        p1.x = rnode.p1.x;
        p1.y = root->key.y;
        p2.x = root->key.x;
        p2.y = rnode.p2.y;
        foqr.p1 = p1;
        foqr.p2 = p2;

        return foqr;
    }
}

float calcMindist(region r, point p)
{
    float x, y;
    if( p.x >= r.p1.x && p.x <= r.p2.x)
        x = 0;
    else
        x = min(fabs(p.x - r.p1.x), fabs(p.x - r.p2.x));

    if( p.y >= r.p1.y && p.y <= r.p2.y)
        y = 0;
    else
        y = min(fabs(p.y - r.p1.y), fabs(p.y - r.p2.y));

    printf( "x\t%f\ty\t%f\n", x, y);

    return sqrt(pow(x, 2) + pow(y, 2));
}

float calcDist(point p1, point p2)
{
    return sqrt(pow(p1.x - p2.x, 2) + pow(p1.y - p2.y, 2));
}

point calcKey(point p1, point p2)
{
	point key;
	key.x = (p1.x + p2.x) / 2;
	key.y = (p1.y + p2.y) / 2;

	return key;
}

void printTree(QuadNode *root)
{
    if(root->isleaf)
        printf("%f\t%f\n", root->p.x, root->p.y);
    if(root->first != NULL)
        printTree(root->first);
    if(root->second != NULL)
        printTree(root->second);
    if(root->third != NULL)
        printTree(root->third);
    if(root->forth != NULL)
        printTree(root->forth);
}

QuadNode* insertQuadpoint(QuadNode *root, point p)
{
    printf("working \n");
	if(root == NULL)
	{
	    printf( "in null \n");
		//crate a leaf and return it
		QuadNode *leaf = new QuadNode;
		leaf->p = p;
		leaf->first = NULL;
		leaf->second = NULL;
		leaf->third = NULL;
		leaf->forth = NULL;
		leaf->isleaf = true;

		root = leaf;
	}
	else if(!root->isleaf)
	{
	    printf("not leaf\n");

		if(root->key.x >= p.x && root->key.y >= p.y)
			root->first = insertQuadpoint(root->first, p);
		else if(root->key.x < p.x && root->key.y >= p.y)
			root->second = insertQuadpoint(root->second, p);
		else if(root->key.x < p.x && root->key.y < p.y)
			root->third = insertQuadpoint(root->third, p);
		else
			root->forth = insertQuadpoint(root->forth, p);
	}
	else
	{
	   printf("leaf \n");
	    //change to internal node
        root->isleaf = false;
        root->key = calcKey(root->p, p);

		//create a new leaf node
		QuadNode *newleaf = new QuadNode;
		newleaf->first = NULL;
		newleaf->second = NULL;
		newleaf->third = NULL;
		newleaf->forth = NULL;
		newleaf->isleaf = true;
		newleaf->p = root->p;

		//crate a leaf and return it
		QuadNode *leaf = new QuadNode;
		leaf->p = p;
		leaf->first = NULL;
		leaf->second = NULL;
		leaf->third = NULL;
		leaf->forth = NULL;
		leaf->isleaf = true;

		//insert newleaf proper location
		if(root->key.x >= newleaf->p.x && root->key.y >= newleaf->p.y)
			root->first = newleaf;
		else if(root->key.x < newleaf->p.x && root->key.y >= newleaf->p.y)
			root->second = newleaf;
		else if(root->key.x < newleaf->p.x && root->key.y < newleaf->p.y)
			root->third = newleaf;
		else
			root->forth = newleaf;

		//insert leaf proper location
		if(root->key.x >= leaf->p.x && root->key.y >= leaf->p.y)
			root->first = leaf;
		else if(root->key.x < leaf->p.x && root->key.y >= leaf->p.y)
			root->second = leaf;
		else if(root->key.x < leaf->p.x && root->key.y < leaf->p.y)
			root->third = leaf;
		else
			root->forth = leaf;
	}

    printf( "is root is still root %d\n", root->isleaf);
    return root;
}

QuadNode *BuildQuadTree(FILE *fp)
{
	QuadNode *tree = NULL;
	float r, j;
	while(fscanf(fp, "%f%f", &r, &j) == 2)
	{
		point p;
		p.x = r;
		p.y = j;
        printf("inserting %f \t %f \n", r, j);
		tree = insertQuadpoint(tree, p);
		//printTree(tree);
	}
	return tree;
}

void windowQuery(vector<point> &points, QuadNode *root, region query, region rnode)
{
    //something fishy
    if(root->isleaf)
    {
        if(pointInRange(root->p, query.p1, query.p2))
        {
            points.push_back(root->p);
            printf("%f\t%f in leaf point found\n", root->p.x, root->p.y);
        }
    }
    else
    {

        if(root->first != NULL)
        {
            region fqr = calcRegion(rnode, root, 1);
            //region contained in query or intersecting
            int stat = statusofRange(fqr, query);
            if(stat == 1)
            {
                printf( "fqr : p1.x %f p1.y %f p2.x %f p2.y %f\n", fqr.p1.x, fqr.p1.y, fqr.p2.x, fqr.p2.y);
                printf("%d status \n", stat);
                reportSubroot(points, root->first);
            }
            else if(stat = 2)
            {
                windowQuery(points, root->first, query, fqr);
            }
        }

        if(root->second != NULL)
        {
            region sqr = calcRegion(rnode, root, 2);
            //same for 2nd
            int stat = statusofRange(sqr, query);
            if(stat == 1)
            {
                printf( "sqr: p1.x %f p1.y %f p2.x %f p2.y %f\n", sqr.p1.x, sqr.p1.y, sqr.p2.x, sqr.p2.y);
                printf("%d status \n", stat);
                reportSubroot(points, root->second);
            }
            else if(stat = 2)
            {
                windowQuery(points, root->second, query, sqr);
            }
        }

        if(root->third != NULL)
        {
            region tqr = calcRegion(rnode, root, 3);

            //same for 3rd
            int stat = statusofRange(tqr, query);
            if(stat == 1)
            {
                printf( "trq: p1.x %f p1.y %f p2.x %f p2.y %f\n", tqr.p1.x, tqr.p1.y, tqr.p2.x, tqr.p2.y);
                printf("%d status \n", stat);
                reportSubroot(points, root->third);
            }
            else if(stat = 2)
            {
                windowQuery(points, root->third, query, tqr);
            }
        }
        if(root->forth != NULL)
        {
            region foqr = calcRegion(rnode, root, 4);
            //same for 4th
            int stat = statusofRange(foqr, query);
            if(stat == 1)
            {
                printf( "fort : p1.x %f p1.y %f p2.x %f p2.y %f\n", foqr.p1.x, foqr.p1.y, foqr.p2.x, foqr.p2.y);
                printf("%d status \n", stat);
                reportSubroot(points, root->forth);
            }
            else if(stat = 2)
            {
                windowQuery(points, root->forth, query, foqr);
            }
        }
    }
}


void KNNQuery(vector<point> &points, QuadNode *root,point query, region rnode, int k)
{
    /*
    1. push root along with region and dist = 0 in min heap based on dist
    2. pop next element from heap
    3. if next == internal node, pop it and push all its children along with calculated region and dist
    4. if next == leaf, push data point in it
    5. if next == isData and count < k, report it
    */

    priority_queue<HeapElement,std::vector<HeapElement>, mycomparison> PQ;
    int count = 0;

    if(root == NULL)
    {
        printf("Error, passed NULL value");
    }
    else
    {
        HeapElement temp;
        temp.p = root;
        temp.r = rnode;
        temp.dist = 0;
        temp.isData = false;
        PQ.push(temp);
        printf( "root is inserted\n");
    }
    while(!PQ.empty())
    {
        HeapElement next = PQ.top();
        PQ.pop();
        printf("next candidate distance %f\n", next.dist);
        if(next.isData)
        {

            if(count++ < k)
            {
                //report data
                points.push_back(next.p->p);
                printf(" %f\t%f point reported\n", next.p->p.x, next.p->p.y);
            }
            else
            {
                break;
            }
        }
        else if(! next.p->isleaf)
        {
            //insert first leaf if not null
            if(next.p->first != NULL)
            {
                HeapElement first;
                first.p = next.p->first;
                first.isData = false;
                first.r = calcRegion(next.r, next.p,1);
                first.dist = calcMindist(first.r, query);

                PQ.push(first);
            }

            if(next.p->second != NULL)
            {
                HeapElement second;
                second.p = next.p->second;
                second.isData = false;
                second.r = calcRegion(next.r, next.p, 2);
                second.dist = calcMindist(second.r, query);

                PQ.push(second);
            }

            if(next.p->third != NULL)
            {
                HeapElement third;
                third.p = next.p->third;
                third.isData = false;
                third.r = calcRegion(next.r, next.p, 3);
                third.dist = calcMindist(third.r, query);

                PQ.push(third);
            }

            if(next.p->forth != NULL)
            {
                HeapElement forth;
                forth.p = next.p->forth;
                forth.isData = false;
                forth.r = calcRegion(next.r, next.p, 4);
                forth.dist = calcMindist(forth.r, query);

                PQ.push(forth);
            }

            printf( "loaded children \n");
        }
        else
        {
            HeapElement data;
            data.p = next.p;
            data.isData = true;
            data.dist = calcDist(next.p->p, query);
            printf("data point dist %f\n", data.dist);
            PQ.push(data);
        }
    }
}
int main()
{
	//testing
	FILE *fi = fopen("test.txt", "r");
	QuadNode *root = BuildQuadTree(fi);
	//printTree(root);
	vector<point> ans;
    //reportSubroot(ans, root);
    //printf( "%d\n", ans.size());

    //testing KNN
    //1. calcdist   -- tested
    //2. calcregion -- tested
    //3. calmindist -- tested
    //4. PQ         -- tested
    //5. KNN on debug data
//
//    QuadNode *dummy = new QuadNode;
//
    point p1, p2, p3, p4;
    p1.x = 0;
    p1.y = 0;
    p2.x = 1;
    p2.y = 1;
    p3.x = 0.25;
    p3.y = 0.25;
    p4.x = 0.75;
    p4.y = 0.75;

    region test, test2;
    test.p1 = p1;
    test.p2 = p2;
    test2.p1 = p3;
    test2.p2 = p4;

    printf("query %d\n", statusofRange(test2, test));
//    dummy->key = p3;

//    printf("calcdist %f\n", calcDist(p1, p2));
//    printf("calcmindist %f\n", calcMindist(test, p3));
//    printf("fabs %f\n", fabs(2.0 - 3.14));

//    for(int i = 1; i < 5; i++)
//    {
//        region ans = calcRegion(test, root->second, i);
//       // printf("key is %f\t%f\n", root->second->)
//        printf( "p1.x %f p1.y %f p2.x %f p2.y %f\n", ans.p1.x, ans.p1.y, ans.p2.x, ans.p2.y);
//    }

//
//    KNNQuery(ans, root, p3, test, 4);
//    printf( "%d\n", ans.size());
//

//    1. status of range -- done
//    2. window on debug
//
    windowQuery(ans, root, test2, test);
    printf( "%d\n", ans.size());

    for(int i = 0; i < ans.size(); i++)
    {
        printf("point %d is %f \t %f \n", i, ans[i].x, ans[i].y);
    }

	return 0;
}