//Codes of Quadtree-Based Path planning

/* 
Author: Srikanta Patnaik and Team 
Machine Intelligence Laboratory, University College of Engineering, Burla, India
Date of Writing: April, 2003; Version: 1.0
Book: Robot Cognition and Navigation: An Experiment with Mobile Robots
Publisher: Springer
ISBN: 978-3-540-23446-3
URL: http://www.springer.com/3-540-23446-2
 
*/

#include<iostream.h>
#include<conio.h>
#include<math.h>
#include<graphics.h>
#include<time.h>
#include<dos.h>
#include<stdlib.h>

# define ROBOSIZE 9
# define EMPTY 0
# define FILLED 1
# define PARTIAL 2
# define BNDX1 80
# define BNDY1 80
# define BNDX2 400
# define BNDY2 400
/*structure of each quadtree node*/

struct node
{
int x1;
int y1;
int x2;
int y2;
int finished;
node* child1; //pointers to children
node* child2;
node* child3;
node* child4;
node* parent; //pointer to parent
short status;  //node status
int visited;   //visited or not (flag)
};

int  ADJ[5][5]={
		{1,1,0,0},
		{0,1,0,1},
		{0,0,1,1},
		{1,0,1,0}
			};
int REFLECT[5][5]={
		{2,3,0,1},
		{1,0,3,2},
		{2,3,0,1},
		{1,0,3,2}
			  };
/*starting x and y and goal x and goal_y*/
int start_x;
int start_y;
int goal_x;
int goal_y;
node* current_node;//node which has to be updated after each movement
node* goal_node;
node* neighbours[50]; //neighbours of the current node
int neighbour_count;
node* path[100];      //path of the total nodes
int path_count;       //count the no. of nodes in the path


/*setup of 2D workspace*/
class setup
{
int bx1;
int bx2;
int by1;
int by2;
int obs1[10];
int obs2[10];
int obs3[10];
int obs4[10];
public:
setup(void);
void draw_border(void);
void draw_obstacles(void);
void cleanup(void);
};
void setup::setup(void)
{
bx1=BNDX1;
bx2=BNDX2;
by1=BNDY1;
by2=BNDY2;
}
void setup::draw_border(void)
{	
setcolor(LIGHTRED);
setlinestyle(SOLID_LINE,0,THICK_WIDTH);
rectangle(bx1,by1,bx2,by2);
}
void draw_boundary(void)
{
int arr2[20]={80,80,400,80,400,400,80,400};
int arr1[20]={50,50,430,50,430,430,50,430};
setcolor(LIGHTRED);
setfillstyle(SOLID_FILL,LIGHTRED);
setlinestyle(SOLID_LINE,0,THICK_WIDTH);
fillpoly(4,arr1);
setcolor(LIGHTRED);
setfillstyle(SOLID_FILL,WHITE);
setlinestyle(SOLID_LINE,0,THICK_WIDTH);
fillpoly(4,arr2);
}
void setup::draw_obstacles(void)
{
setcolor(LIGHTRED);
setfillstyle(SOLID_FILL,LIGHTRED);
setlinestyle(SOLID_LINE,0,THICK_WIDTH);
rectangle(100,100,120,120);
floodfill(110,110,LIGHTRED);
rectangle(200,100,220,120);
floodfill(210,110,LIGHTRED);
rectangle(220,100,240,120);
floodfill(230,110,LIGHTRED);
rectangle(240,100,260,120);
floodfill(250,110,LIGHTRED);
rectangle(260,100,280,120);
floodfill(270,110,LIGHTRED);
rectangle(320,100,340,120);
floodfill(330,110,LIGHTRED);
rectangle(220,140,240,160);
floodfill(230,150,LIGHTRED);
rectangle(120,160,160,200);
floodfill(140,180,LIGHTRED);
rectangle(200,200,240,240);
floodfill(220,220,LIGHTRED);
rectangle(280,160,320,200);
floodfill(300,180,LIGHTRED);
rectangle(300,220,320,240);
floodfill(310,230,LIGHTRED);
rectangle(360,160,380,180);
floodfill(370,170,LIGHTRED);
rectangle(360,180,380,200);
floodfill(370,190,LIGHTRED);
rectangle(360,200,380,220);
floodfill(370,210,LIGHTRED);
rectangle(360,220,380,240);
floodfill(370,230,LIGHTRED);
rectangle(120,240,160,280);
floodfill(140,260,LIGHTRED);
rectangle(240,280,260,300);
floodfill(250,290,LIGHTRED);
rectangle(260,280,280,300);
floodfill(270,290,LIGHTRED);
rectangle(280,280,300,300);
floodfill(290,290,LIGHTRED);
rectangle(300,280,320,300);
floodfill(310,290,LIGHTRED);
rectangle(100,320,120,340);
floodfill(110,330,LIGHTRED);
rectangle(100,360,120,380);
floodfill(110,370,LIGHTRED);
rectangle(160,320,200,360);
floodfill(180,340,LIGHTRED);
rectangle(240,340,260,360);
floodfill(250,350,LIGHTRED);
rectangle(240,360,260,380);
floodfill(250,370,LIGHTRED);
rectangle(300,340,320,360);
floodfill(310,350,LIGHTRED);
rectangle(320,340,340,360);
floodfill(330,350,LIGHTRED);
rectangle(340,340,360,360);
floodfill(350,350,LIGHTRED);
rectangle(360,340,380,360);
floodfill(370,350,LIGHTRED);
}
/*quadtree class having all the functions that are operated on
quadtree*/
class quadtree
{
node* root;  //pointer to the workspace as a whole

public:
quadtree(void);
void develope_tree(node* n); //developing the quad tree from map
void traverse(node* n);
node* return_root(void) { return root;} //returns the private data to the main;
void set_current_node(node* n,int x,int y);//set the initial node
void set_goal_node(node* n,int x,int y); //setting the final node
node* find_neighbour(node* p, int direction); //finding neighbours
void cleanup(node* n); //cleanup total quad tree after the program is over
};

quadtree::quadtree()
{
root=new node;
root->x1=80;
root->y1=80;
root->x2=400;
root->y2=400;
root->child1=NULL;
root->child2=NULL;
root->child3=NULL;
root->child4=NULL;
root->parent=NULL;
root->finished=0; //0=for mixed node and 1= for total free or total blocked node
		 }
void  quadtree::develope_tree(node* n)
{
int i,j;
int cnt=0,cnt1=0,cnt2=0; //cnt1=used for obstacle pixel, cnt2=for free region
node* temp=n;
////////////////////////////////////////////
/* this is for the first region*/
node* ptr1=new node;
ptr1->parent=n;
ptr1->x1=temp->x1;
ptr1->y1=temp->y1;
ptr1->x2=temp->x1+(temp->x2-temp->x1)/2;
ptr1->y2=temp->y1+(temp->y2-temp->y1)/2;

for( i=ptr1->y1+2;i<ptr1->y2-2; i++)
for( j=ptr1->x1+2;j<ptr1->x2-2; j++)
{
cnt++;
if(getpixel(j,i)==WHITE) cnt1++;
if(getpixel(j,i)==LIGHTRED) cnt2++;
}
if((cnt1==0)||(cnt2==0))   ptr1->finished=1;
else ptr1->finished=0;
if((ptr1->x2-ptr1->x1)<=ROBOSIZE)ptr1->finished=1;
if((cnt1==0)&&(cnt2!=0)) ptr1->status=FILLED;
if((cnt1!=0)&&(cnt2==0)) ptr1->status=EMPTY;
if((cnt1!=0)&&(cnt2!=0)) ptr1->status=PARTIAL;

n->child1=ptr1;
//cout<<cnt1<<","<<cnt2<<"for";
//cout<<"1 passed,";getch();
if(ptr1->finished==0) develope_tree(n->child1);
else
{
n->child1->child1=NULL;
n->child1->child2=NULL;
n->child1->child3=NULL;
n->child1->child4=NULL;
}
/////////////////////////////////////////////////////////////
/*this is for second region*/
temp=n;
cnt=0;cnt1=0;cnt2=0;
node* ptr2=new node;
ptr2->parent=n;
ptr2->x1=temp->x1+(temp->x2-temp->x1)/2;
ptr2->y1=temp->y1;
ptr2->x2=temp->x2;
ptr2->y2=temp->y1+(temp->y2-temp->y1)/2;

for( i=ptr2->y1+2;i<ptr2->y2-2; i++)
for( j=ptr2->x1+2;j<ptr2->x2-2; j++)
{
cnt++;
if(getpixel(j,i)==WHITE) cnt1++;
if(getpixel(j,i)==LIGHTRED) cnt2++;
}
if((cnt1==0)||(cnt2==0))   ptr2->finished=1;
 else ptr2->finished=0;

if((ptr2->x2-ptr2->x1)<=ROBOSIZE)ptr2->finished=1;
if((cnt1==0)&&(cnt2!=0)) ptr2->status=FILLED;
if((cnt1!=0)&&(cnt2==0)) ptr2->status=EMPTY;
if((cnt1!=0)&&(cnt2!=0)) ptr2->status=PARTIAL;

n->child2=ptr2;
//	cout<<cnt1<<","<<cnt2<<"for";
//	cout<<"2 passed,";getch();

if(ptr2->finished==0) develope_tree(n->child2);
else
{
n->child2->child1=NULL;
n->child2->child2=NULL;
n->child2->child3=NULL;
n->child2->child4=NULL;
}

//////////////////////////////////////
/*this is for the third region*/
cnt=0;cnt1=0;cnt2=0 ;
temp=n;
node* ptr3=new node;
ptr3->parent=n;
ptr3->x1=temp->x1;
ptr3->y1=temp->y1+(temp->y2-temp->y1)/2;
ptr3->x2=temp->x1+(temp->x2-temp->x1)/2;
ptr3->y2=temp->y2;

//cout<<ptr4->x1<<","<<ptr4->y1<<","<<ptr4->x2<<","<<ptr4->y2;
for( i=ptr3->y1+2;i<ptr3->y2-2; i++)
for( j=ptr3->x1+2;j<ptr3->x2-2; j++)
	{

cnt++;
if(getpixel(j,i)==WHITE) cnt1++;
if(getpixel(j,i)==LIGHTRED) cnt2++;
	}
if((cnt1==0)||(cnt2==0))   ptr3->finished=1;
else ptr3->finished=0;

if((ptr3->x2-ptr3->x1)<=ROBOSIZE)ptr3->finished=1;
if((cnt1==0)&&(cnt2!=0)) ptr3->status=FILLED;
if((cnt1!=0)&&(cnt2==0)) ptr3->status=EMPTY;
if((cnt1!=0)&&(cnt2!=0)) ptr3->status=PARTIAL;

n->child3=ptr3;
//	cout<<cnt1<<","<<cnt2<<"for";
//	cout<<"4 passed,";getch();
if(ptr3->finished==0) develope_tree(n->child3);
else
{
n->child3->child1=NULL;
n->child3->child2=NULL;
n->child3->child3=NULL;
n->child3->child4=NULL;
}

///////////////////////////////////////////////////////
/*this is for the fourth region*/
cnt=0;cnt2=0;cnt1=0;
temp=n;
node* ptr4=new node;
ptr4->parent=n;
ptr4->x1=temp->x1+(temp->x2-temp->x1)/2;
ptr4->y1=temp->y1+(temp->y2-temp->y1)/2;
ptr4->x2=temp->x2;
ptr4->y2=temp->y2;

for( i=ptr4->y1+2;i<ptr4->y2-2; i++)
for( j=ptr4->x1+2;j<ptr4->x2-2; j++)
{
cnt++;
if(getpixel(j,i)==WHITE) cnt1++;
if(getpixel(j,i)==LIGHTRED) cnt2++;
}
if((cnt1==0)||(cnt2==0))   ptr4->finished=1;
else ptr4->finished=0;
if((ptr4->x2-ptr4->x1)<=ROBOSIZE)ptr4->finished=1;
if((cnt1==0)&&(cnt2!=0)) ptr4->status=FILLED;
if((cnt1!=0)&&(cnt2==0)) ptr4->status=EMPTY;
if((cnt1!=0)&&(cnt2!=0)) ptr4->status=PARTIAL;

n->child4=ptr4;
//	cout<<cnt1<<","<<cnt2<<"for";
//	cout<<"3 passed,";getch();
if(ptr4->finished==0) develope_tree(n->child4);
else
{
n->child4->child1=NULL;
n->child4->child2=NULL;
n->child4->child3=NULL;
n->child4->child4=NULL;
}
return;
}

/*traversing the total quadtree*/
void quadtree::traverse(node* n)
{
node* t=n;
if(t->child1!=NULL) traverse( t->child1);
if(t->child2!=NULL) traverse( t->child2);
if(t->child3!=NULL) traverse( t->child3);
if(t->child4!=NULL) traverse( t->child4);
if(t->finished==1)
{
cout<<endl<<t->x1<<","<<t->y1<<" and "<<t->x2<<","<<t->y2<<"  -----> ";
switch(t->status)
{
case 0: {cout<<" empty node"; break;}
case 1: {cout<<" filled node";break;}
case 2: {cout<<" partially filled node";break;}
}
}
}
/*setting the current node as fisrt node*/

void quadtree::set_current_node(node* n,int x,int y)
{
node* t=n;
if(t->child1!=NULL) set_current_node( t->child1,x,y);
if(t->child2!=NULL) set_current_node( t->child2,x,y);
if(t->child3!=NULL) set_current_node( t->child3,x,y);
if(t->child4!=NULL) set_current_node( t->child4,x,y);
if(t->finished==1)

{if((t->x1<=x)&&(t->y1<=y)&&(t->x2>=x)&&(t->y2>=y)) current_node= t;}
}

void quadtree::set_goal_node(node* n,int x,int y)
{
node* t=n;
if(t->child1!=NULL) set_goal_node( t->child1,x,y);
if(t->child2!=NULL) set_goal_node( t->child2,x,y);
if(t->child3!=NULL) set_goal_node( t->child3,x,y);
if(t->child4!=NULL) set_goal_node( t->child4,x,y);
if(t->finished==1)

{if((t->x1<=x)&&(t->y1<=y)&&(t->x2>=x)&&(t->y2>=y)) goal_node= t;}
}

void quadtree::cleanup(node* t)
{
//	node* t=n;
if(t->child1!=NULL) cleanup( t->child1);
if(t->child2!=NULL) cleanup( t->child2);
if(t->child3!=NULL) cleanup( t->child3);
if(t->child4!=NULL) cleanup( t->child4);
delete t;
}
/* finding the neighbours for the current node*/
node* quadtree::find_neighbour(node* p,int direction)
{
node* n;
int sontype;
if(p==p->parent->child1) sontype=0;
if(p==p->parent->child2) sontype=1;
if(p==p->parent->child3) sontype=2;
if(p==p->parent->child4) sontype=3;

if((p->parent!=NULL)&& (ADJ[direction][sontype]))
n=find_neighbour(p->parent,direction);
else n=p->parent;
// next line extra

if(n->finished==1) return n;
int r=REFLECT[direction][sontype];

//	 if((n!=NULL)&&(n->status==PARTIAL))
{
if(r==0) return n->child1;
if(r==1) return n->child2;
if(r==2) return n->child3;
if(r==3) return n->child4;
}
//  else
return n;
}

/*traversing the total quadtree*/

int having_common_side(int x1,int y1,int x2,int y2,int X1,int Y1,int X2,int Y2)
{
int xmid,ymid;
xmid=(x1+x2)/2;
ymid=y2;
for(int i=X1;i<=X2;i++)
{ if((i==xmid)&&(ymid==Y1)) return 1;}
xmid=x2;
ymid=(y1+y2)/2;
for( i=Y1;i<=Y2;i++)
{ if((i==ymid)&&(xmid==X1)) return 1;}
xmid=(x1+x2)/2;
ymid=y1;
for( i=X1;i<=X2;i++)
{ if((i==xmid)&&(ymid==Y2)) return 1;}

xmid=x1;
ymid=(y1+y2)/2;
for( i=Y1;i<=Y2;i++)
{ if((i==ymid)&&(xmid==X2)) return 1;}

return 0;
}

/*finding neibours of the small size than the current node*/
void find_small_neighbours(node* n)
{
node* t=n;
if(t->child1!=NULL) find_small_neighbours( t->child1);
if(t->child2!=NULL) find_small_neighbours( t->child2);
if(t->child3!=NULL) find_small_neighbours( t->child3);
if(t->child4!=NULL) find_small_neighbours( t->child4);
if(t->finished==1)
{
if(having_common_side(t->x1,t->y1,t->x2,t->y2,current_node->x1,current_node->y1,current_node->x2,current_node->y2))
{neighbours[neighbour_count++]=t;}
}
}

double distance(int x1,int y1,int x2,int y2)
{
double dist= sqrt((double)(x2-x1)*(double)(x2-x1)+(double)(y2-y1)*(double)(y2-y1));
return dist;
}
/*finding the path from starting to goal node*/
void find_path(void)
{
int i;

/*	node* t;
for(int i=0;i<neighbour_count;i++)
{
t=neighbours[i];
if((t->x1==goal_node->x1)&&(t->y1==goal_node->y1)&&(t->x2== goal_node->x2)&&(t->y2==goal_node->y2))
{ current_node=goal_node;
return;
}
}
*/
			   
node* tem;
cout<<endl<<"Following are the possible neighbouring node"<<endl;
cout<<	"_________________________________"<<endl;
for( i=0;i<neighbour_count;i++)
{
tem=neighbours[i];

cout<<tem->x1<<","<<tem->y1<<" and "<<tem->x2<<","<<tem->y2<<"  :  ";
if(tem->status==EMPTY) cout<< "status :"<<"Empty node"<<endl;
if(tem->status==FILLED) cout<<"status :"<<"Occupied node"<<endl;
if(tem->status==PARTIAL)cout<<"status :"<<"Mixed node"<<endl;
}
//////////////////////////////////////////////////
node* best_node;
int midx1=(current_node->x1+current_node->x2)/2;
int midy1=(current_node->y1+current_node->y2)/2;
int midx2=0;
int midy2=0;
int midx3=(goal_node->x1+goal_node->x2)/2;
int midy3=(goal_node->y1+goal_node->y2)/2;
double dist1=0.0;
double dist2=0.0;
double mindist=10000.0;
cout<<endl<< "Total numbers of neighbouring nodes="<<neighbour_count<<endl;
for( i=0;i<neighbour_count;i++)
{
//	cout<<"examining node"<<neighbours[i]->x1<<","<<neighbours[i]->y1<<endl;
if(neighbours[i]->visited!=1)
{
if(neighbours[i]->status==EMPTY)
{
midx2=(neighbours[i]->x1+neighbours[i]->x2)/2;
midy2=(neighbours[i]->y1+neighbours[i]->y2)/2;
dist1=distance(midx1,midy1,midx2,midy2);
dist2=distance(midx2,midy2,midx3,midy3);
if(dist1<=0.0) dist1=0.0;
if(dist2<=0.0) dist2=0.0;
if( (dist1+dist2)<=mindist)
{
mindist=dist2+dist1;
best_node=neighbours[i];
}
}
}
//	mindist=10000.0;
}
//	  cout<<"with distance" <<mindist;
best_node->visited=1;
path[path_count++]=best_node;
current_node=best_node;
cout<<"Best node selected out of the above nodes:";
cout<<"[ ("<<best_node->x1<<","<<best_node->y1<<") and (";
cout<<best_node->x2<<","<<best_node->y2<<") ]"<<endl;
getch();
}

void fill_node(int x1,int y1,int x2,int y2)
{ 
int k;
for(int j=x1; j<=x2;j++)
for(k=y1; k<y2;k++)
{ putpixel(j,k,LIGHTRED);
}
}
/*main program for quad tree*/
char a1[]="S";
char a2[]="G";
void main()
{
double tim;
clrscr();
char a3[]="(80,80)";
char a4[]="(400,80)";
char a5[]="(80,400)";
char a6[]="(400,400)";
int driver,mode;
driver=DETECT;
initgraph(&driver,&mode,"a:\\bgi" );
setup s; /*s is an object for setup class*/
s.draw_border();
s.draw_obstacles();
setcolor(GREEN);
settextstyle(1,0,1);
moveto(55,55);
outtext(a3);
moveto(330,55);
outtext(a4);
moveto(55,405);
outtext(a5);
moveto(330,405);
outtext(a6);
cout<<"\n\t----THIS IS THE MODEL WORKSPACE-----------";
cout<<"\n\tPRESS ENTER TO FEED DATA FOR START AND GOAL POINT";
getch();
closegraph();
cout<<"     set starting x-location:";cin>>start_x;
cout<<"     set starting y-location:";cin>>start_y;
cout<<"     set goal x-location:";cin>>goal_x;
cout<<"     set goal y-location:";cin>>goal_y;
//tobe removed
//  setup s;
/*initialising the graphic mode*/
// int driver,mode;
//  driver=DETECT;

initgraph(&driver,&mode,"a:\\bgi");
draw_boundary();
setcolor(GREEN);
setfillstyle(SOLID_FILL,GREEN);
setlinestyle(SOLID_LINE,0,THICK_WIDTH);
circle(start_x,start_y,5);
circle(goal_x,goal_y,5);
settextstyle(1,0,2);
setcolor(BLACK);
moveto(start_x-5,start_y+5);
outtext(a1);
moveto(goal_x-5,goal_y+5);
outtext(a2);
s.draw_border();
s.draw_obstacles();
if(getpixel(start_x,start_y)==BLACK)
{
cout<<"\n\t\tStarting point you have provided lies out of work space"<<endl;
cout<<"\t\t\t\tPRESS ANY KEY TO TERMINATE";
getch();
closegraph();
exit(0);
}
if(getpixel(start_x,start_y)==LIGHTRED)
{ 
cout<<"\n\t\tStarting point you have provided lies on the obstacle"<<endl;
cout<<"\t\t\t\tPRESS ANY KEY TO TERMINATE";
getch();
closegraph();
exit(0);
}
if(getpixel(goal_x,goal_y)==BLACK)
{
cout<<"\n\t\tThe goal point you have provided lies out of work space."<<endl;
cout<<"\t\t\t\tPRESS ANY KEY TO TERMINATE";
getch();
closegraph();
exit(0);
}
if(getpixel(goal_x,goal_y)==LIGHTRED)
{
cout<<"\n\t\tThe goal point you have provided lies on the obstacle"<<endl;
cout<<"\t\t\t\tPRESS ANY KEY TO TERMINATE";
getch();
closegraph();
exit(0);
}
quadtree q;
q.develope_tree(q.return_root());

getch();
closegraph();
	   
////////////////////////////////////////
q.set_current_node(q.return_root(),start_x,start_y);
q.set_goal_node(q.return_root(),goal_x,goal_y);

current_node->visited=1;
cout<<"current_node="<< current_node->x1<<","<<current_node->y1;
cout<<" and "<<current_node->x2<<","<<current_node->y2<<endl;
path_count=0;
for(int j=0;j<100;j++) path[j]=NULL;
node* nn;
node* en;
node* sn;
node* wn;
	
/* do loop is meant for finding the path iteratively*/
getch();
clock_t  star,end;
star = clock();
do
{
neighbour_count=0;
for( j=0;j<50;j++) neighbours[j]=NULL;
nn=q.find_neighbour(current_node,0);
en=q.find_neighbour(current_node,1);
sn=q.find_neighbour(current_node,2);
wn=q.find_neighbour(current_node,3);

if(!((nn->x1>=BNDX1-5)&&(nn->y1>=BNDY1-5)&&(nn->x2<=BNDX2+5)&&(nn->y2<=BNDY2+5)))
	  nn=NULL;
if(!((en->x1>=BNDX1-5)&&(en->y1>=BNDY1-5)&&(en->x2<=BNDX2+5)&&(en->y2<=BNDY2+5)))
	  en=NULL;
if(!((sn->x1>=BNDX1-5)&&(sn->y1>=BNDY1-5)&&(sn->x2<=BNDX2+5)&&(sn->y2<=BNDY2+5)))
	  sn=NULL;
if(!((wn->x1>=BNDX1-5)&&(wn->y1>=BNDY1-5)&&(wn->x2<=BNDX2+5)&&(wn->y2<=BNDY2+5)))
	  wn=NULL;
/*
if(nn!=NULL)  cout<<nn->x1<<","<<nn->y1<<" and "<<nn->x2<<"," <<nn->y2<<endl;
if(en!=NULL)  cout<<en->x1<<","<<en->y1<<" and "<<en->x2<<"," <<en->y2<<endl;
if(sn!=NULL)  cout<<sn->x1<<","<<sn->y1<<" and "<<sn->x2<<"," <<sn->y2<<endl;
if(wn!=NULL)  cout<<wn->x1<<","<<wn->y1<<" and "<<wn->x2<<"," <<wn->y2<<endl;
			*/
if(nn!=NULL)   if(nn->child1==NULL) neighbours[neighbour_count++]=nn;
if(en!=NULL)   if(en->child1==NULL) neighbours[neighbour_count++]=en;
if(sn!=NULL)   if(sn->child1==NULL) neighbours[neighbour_count++]=sn;
if(wn!=NULL)   if(wn->child1==NULL) neighbours[neighbour_count++]=wn;

if(nn!=NULL)   if(nn->child1!=NULL) find_small_neighbours(nn);
if(en!=NULL)   if(en->child1!=NULL) find_small_neighbours(en);
if(sn!=NULL)   if(sn->child1!=NULL) find_small_neighbours(sn);
if(wn!=NULL)   if(wn->child1!=NULL) find_small_neighbours(wn);

///////////////// displaying all neighbours //////////

/*	node* tem;
cout<<endl<<"total neighbours"<<endl;
for(int i=0;i<neighbour_count;i++)
	{
tem=neighbours[i];
cout<<tem->x1<<","<<tem->y1<<" and "<<tem->x2<<","<<tem->y2<<endl;
if(tem->status==EMPTY) cout<<"status:"<<"empty"<<endl;
	}
	*/

/////////////////////////////////
find_path();

}
while(current_node!=goal_node);
end = clock();
tim=( (end - star) / CLK_TCK);

/*
cout<<endl<<" path blocks are";
for(int l=0;l<path_count;l++)
{cout<<endl<<path[l]->x1<<","<<path[l]->y1<<"and"<<path[l]->x2<<","<<path[l]->y2;}
*/
	 
//	  q.traverse(q.return_root());
//getch();

initgraph(&driver,&mode,"a:\\bgi");
draw_boundary();
s.draw_border();
s.draw_obstacles();
int pp1= (path[0]->x1+path[0]->x2)/2;
int pp2= (path[0]->y1+path[0]->y2)/2;
setcolor(GREEN);
setfillstyle(SOLID_FILL,GREEN);
setlinestyle(SOLID_LINE,0,THICK_WIDTH);
circle(start_x,start_y,5);
circle(goal_x,goal_y,5);
settextstyle(1,0,2);
setcolor(BLACK);
moveto(start_x-5,start_y+5);
outtext(a1);
moveto(goal_x-5,goal_y+5);
outtext(a2);
settextstyle(1,0,2);
setcolor(GREEN);
line(start_x,start_y,pp1,pp2);
int pp3,pp4;
double dist=0;
dist+=distance(start_x,start_y,pp1,pp2);
for(int ss=0; ss<path_count-1; ss++)
{
//fill_node(path[ss]->x1,path[ss]->y1,path[ss]->x2,path[ss]->y2);
pp1= (path[ss]->x1+path[ss]->x2)/2;
pp2= (path[ss]->y1+path[ss]->y2)/2;
pp3= (path[ss+1]->x1+path[ss+1]->x2)/2;
pp4= (path[ss+1]->y1+path[ss+1]->y2)/2;
line(pp1,pp2,pp3,pp4);
dist+=distance(pp1,pp2,pp3,pp4);
}
line( pp3,pp4,goal_x,goal_y);
dist+=distance(pp3,pp4,goal_x,goal_y);
getch();
closegraph();
q.cleanup(q.return_root());

/*	cout<<"\t\tTotal Time taken to search path="<<tim<<endl;
cout<<"\t\tTotal Path traversal="<<dist;
getch(); */
}

