From f1cbb3171cdf9b5c2292510223a537bd1a1e09ee Mon Sep 17 00:00:00 2001
From: saurabhkgpee <shourabh.agarwal@gmail.com>
Date: Sun, 13 Aug 2017 02:44:11 +0530
Subject: [PATCH 1/6] RRT Connect

---
 include/RRT.hpp        |  58 ++++++--
 include/RRTConnect.hpp | 331 +++++++++++++++++++++++++++++++++++++++++
 src/main.cpp           |  38 +++--
 3 files changed, 406 insertions(+), 21 deletions(-)
 create mode 100644 include/RRTConnect.hpp

diff --git a/include/RRT.hpp b/include/RRT.hpp
index e718bac..df00797 100644
--- a/include/RRT.hpp
+++ b/include/RRT.hpp
@@ -19,19 +19,19 @@ namespace rrt
 		double stepLength;
 		std::vector<Utils::Point<T> > pathPoints;
 		int maxIterations;
-		std::vector< std::pair< Utils::Point<T>, Utils::Point<T> > > tree;
+		std::vector< std::pair< Utils::Point<T>, Utils::Point<T> > > tree; 
 		unsigned int biasParameter;
-
+		
 	public:
 		
 		RRT(){};
 		RRT(Utils::Point<T> start,Utils::Point<T> end)
 		{
-			//srand(time(NULL));
+			//srand(time(NULL));			
 			startPoint=start;
 			endPoint=end;
 		}
-
+		
 		virtual bool plan();
 		virtual std::vector<Utils::Point<T> > getPointsOnPath();
 
@@ -59,13 +59,51 @@ namespace rrt
 		void generatePath(Utils::Point<T> first,Utils::Point<T> last);
 		double dist(Utils::Point<T> a,Utils::Point<T> b);
 	};
+
+	template <class T>
+	class RRTConnect: public RRT<T>{
+
+	public:
+		bool plan();
+		 std::vector<Utils::Point<T> > getPointsOnPath();
+
+		void setEndPoints(Utils::Point<T> start, Utils::Point<T> end);
+		void setCheckPointFunction(bool (*f)(Utils::Point<T>));
+		void setStepLength(double value);
+		void setOrigin(Utils::Point<T> origin);
+		void setHalfDimensions(double x,double y);
+		void setBiasParameter(unsigned int);
+		void setMaxIterations(int);
+	private:
+		double halfDimensionX;
+		double halfDimensionY;
+		Utils::Point<T> origin;
+		Utils::Point<T> connectA;
+		Utils::Point<T> connectB;
+		Utils::Point<T> startPoint;
+		Utils::Point<T> endPoint;
+		double stepLength;
+		std::vector<Utils::Point<T> > pathPoints;
+		int maxIterations;
+		std::vector< std::pair< Utils::Point<T>, Utils::Point<T> > > treeA; 
+		std::vector< std::pair< Utils::Point<T>, Utils::Point<T> > > treeB;
+		std::vector< std::pair< Utils::Point<T>, Utils::Point<T> > > mainTree;
+		std::vector< std::pair< Utils::Point<T>, Utils::Point<T> > > supportTree; 
+
+		unsigned int biasParameter;
+		bool (*userCheck)(Utils::Point<T>);
+		bool checkPoint(Utils::Point<T> pt);
+		Utils::Point<T> generatePoint(std::vector< std::pair< Utils::Point<T>, Utils::Point<T> > >);
+		Utils::Point<T> generateBiasedPoint(Utils::Point<T> ,std::vector< std::pair< Utils::Point<T>, Utils::Point<T> > >);
+		Utils::Point<T> findClosestNode(Utils::Point<T>,std::vector< std::pair< Utils::Point<T>, Utils::Point<T> > > tree);
+		void growTree(Utils::Point<T>);
+		Utils::Point<T> getParent(Utils::Point<T>,std::vector< std::pair< Utils::Point<T>, Utils::Point<T> > > tree);
+		bool treeComplete();
+		void generatePath(Utils::Point<T> first,Utils::Point<T> last);
+		double dist(Utils::Point<T> a,Utils::Point<T> b);
+	};
 }
 
-//************* ToDo *************
-// Implement derived classes for variants of RRT
-// Optimize the generate path and other
-// Tweak with the parameters to check their effects on runtime and path
-// Implement Pruning function to keep a check on size of tree
 
 
-#endif
+#endif
\ No newline at end of file
diff --git a/include/RRTConnect.hpp b/include/RRTConnect.hpp
new file mode 100644
index 0000000..b5ca20d
--- /dev/null
+++ b/include/RRTConnect.hpp
@@ -0,0 +1,331 @@
+#include <iostream>
+#include "./RRT.hpp"
+#include "./RRT_implementation.hpp"
+#include "./utils.hpp"
+
+using namespace std;
+namespace rrt
+{
+	template <class T>
+	void RRTConnect<T>::setEndPoints(Utils::Point<T> start, Utils::Point<T> end)
+	{
+		startPoint=start;
+		endPoint=end;
+	}
+
+	template <class T>
+	void RRTConnect<T>::setHalfDimensions(double a, double b)
+	{
+		halfDimensionX=a;
+		halfDimensionY=b;
+	}
+
+	template <class T>
+	void RRTConnect<T>::setOrigin(Utils::Point<T> pt)
+	{
+		origin=pt;
+	}
+
+	template <class T>
+	std::vector<Utils::Point<T> > RRTConnect<T>::getPointsOnPath()
+	{
+		return pathPoints;
+	}
+
+	template <class T>
+	void RRTConnect<T>::setStepLength(double value)
+	{
+		stepLength=value;
+	}
+
+	template <class T>
+	void RRTConnect<T>::setMaxIterations(int value)
+	{
+		maxIterations=value;
+	}
+
+	template <class T>
+	bool RRTConnect<T>:: plan()
+	{
+		int count=0;
+		int check=0;
+		treeA.push_back(std::pair< Utils::Point<T>, Utils::Point<T> > (startPoint,startPoint));
+		treeB.push_back(std::pair< Utils::Point<T>, Utils::Point<T> > (endPoint,endPoint));
+
+		int start_time=clock();
+		while( check < maxIterations)
+		{
+			//std::cout<<"In Planning Loop"<<std::endl;
+			Utils::Point<T> next;
+
+
+			if (check%2 == 0)
+			{
+				mainTree = treeA;
+				supportTree = treeB;
+			}
+			else{
+				mainTree = treeB;
+				supportTree = treeA;
+			}
+
+			if(treeComplete()==true)
+			{
+				std::cout<<"Tree complete!!"<<std::endl;
+				// completeTree(treeA);
+				int end_time=clock();
+				std::cout<<"Time to generate path = "<<(float)(end_time-start_time)/CLOCKS_PER_SEC<<std::endl;
+				generatePath(startPoint,endPoint);
+				return true;
+			}
+			else if(count%biasParameter==0)
+			{
+				// std::cout<<"Adding Next Biased Point to tree!!"<<std::endl;
+
+				// count=0;
+				do{
+					if (check%2 == 0)
+					{
+						next = endPoint;
+					}
+					else{
+						next = startPoint;
+					}
+				}while(checkPoint(next)!=true);
+
+			}
+			else
+			{
+				// std::cout<<"Adding next point to tree!!"<<std::endl;
+				do{
+
+					next = generatePoint(mainTree);
+				}while(checkPoint(next)!=true);
+
+			}
+			// std::cout<<" Growing Tree next : "<<next.x<<","<<next.y<<std::endl;
+			growTree(next);
+			count++;
+			check++;
+			//std::cout<<"check= "<<check<<", count= "<<count<<std::endl;
+		}
+		std::cout<<"Path Not Found"<<std::endl;
+		return false;
+	}
+
+	template <class T>
+
+	void RRTConnect<T>::growTree(Utils::Point<T> temp)
+	{
+		//growing the tree by adding node 
+		//std::cout<<"finding parent in tree of size = "<<tree.size()<<std::endl;
+
+		Utils::Point<T> closest;
+		closest = findClosestNode(temp,treeA);
+		double theta = atan2(temp.y-closest.y,temp.x-closest.x);
+		double dis = dist(temp,closest);
+		if (dis > stepLength)
+		{
+		temp.x=closest.x+stepLength*cos(theta);
+		temp.y=closest.y+stepLength*sin(theta);
+		}
+		treeA.push_back( std::pair< Utils::Point<T>, Utils::Point<T> > (temp,closest));
+		
+		closest = findClosestNode(temp,treeB);
+		theta = atan2(temp.y - closest.y , temp.x - closest.x);
+		dis = dist(temp,closest);
+		if (dis > stepLength)
+		{
+		temp.x=closest.x+stepLength*cos(theta);
+		temp.y=closest.y+stepLength*sin(theta);
+		}
+		treeB.push_back( std::pair< Utils::Point<T>, Utils::Point<T> > (temp,closest));
+		//std::cout<<"Point Generated = "<<temp.x<<","<<temp.y<<std::endl;
+		//std::cout<<"Tree grown"<<std::endl;
+		//add pruning code to keep size of tree under control
+	}
+	
+
+	template <class T>
+	void RRTConnect<T>::setCheckPointFunction(bool (*f)(Utils::Point<T>))
+	{
+		userCheck=f;
+	}
+
+	template <class T>
+	Utils::Point<T> RRTConnect<T>::findClosestNode(Utils::Point<T> cur,std::vector< std::pair< Utils::Point<T>, Utils::Point<T> > >tree)
+	{
+		double min_dist=INT_MAX;
+		Utils::Point<T> closest;
+		for(int i=0;i<tree.size();i++)
+		{
+			////std::cout<<"Itering the tree to find closest Node"<<std::endl;
+			if(dist(tree[i].first,cur) < min_dist )
+			{
+				min_dist=dist(tree[i].first,cur);
+				closest=tree[i].first;
+			}
+		}
+		return closest;
+	}
+
+	template <class T>
+	Utils::Point<T> RRTConnect<T>::generatePoint(std::vector< std::pair< Utils::Point<T>, Utils::Point<T> > > tree)
+	{
+		Utils::Point<T> temp;
+		int signX,signY;
+
+		if(rand()%2==0)
+			signX=-1;
+		else
+			signX=1;
+		if(rand()%2==0)
+			signY=-1;
+		else 
+			signY=1;
+		temp.x = origin.x + signX* ( rand() % (int)halfDimensionX );
+		temp.y = origin.y + signY* ( rand() % (int)halfDimensionY );
+
+
+		return temp;
+		Utils::Point<T> closest=findClosestNode(temp,tree);
+		double theta = atan2(temp.y-closest.y,temp.x-closest.x);
+		double dis = dist(temp,closest);
+		if (dis < stepLength)
+		{
+			return temp;
+		}
+		Utils::Point<T> next;
+
+		next.x=closest.x+stepLength*cos(theta);
+		next.y=closest.y+stepLength*sin(theta);
+		// std::cout<<"origin,"<<origin.x<<" "<<origin.y<<" "<<temp.x<<" "<<temp.y<<" "<<next.x<<" "<<next.y<<std::endl;
+		//std::cout<<"Point Generated = "<<temp.x<<","<<temp.y<<std::endl;
+		return next;
+	}
+
+	template <class T>
+	Utils::Point<T> RRTConnect<T>::generateBiasedPoint(Utils::Point<T> endPoint,std::vector< std::pair< Utils::Point<T>, Utils::Point<T> > >tree)
+	{
+		Utils::Point<T> closest=findClosestNode(endPoint,tree);
+		double theta = atan2(endPoint.y-closest.y,endPoint.x-closest.x);
+
+		Utils::Point<T> next;
+		next.x=closest.x+stepLength*cos(theta);
+		next.y=closest.y+stepLength*sin(theta);
+		//std::cout<<"Biased Point Generated = "<<next.x<<","<<next.y<<std::endl;
+		return next;
+	}
+
+	template <class T>
+	bool RRTConnect<T>::checkPoint(Utils::Point<T> next)
+	{
+		return userCheck(next);
+	}
+
+	template <class T>
+	bool RRTConnect<T>::treeComplete()
+	{
+		std::vector< std::pair< Utils::Point<T>, Utils::Point<T> > > conn;
+		//std::cout<<"Checking if tree is complete? "<<std::endl;
+		//std::cout<<"tree size = "<<tree.size()<<std::endl;
+		int min_dist=INT_MAX;	
+		for(int i=0;i<treeA.size();i++)
+		{
+			for (int j = 0; j < treeB.size(); ++j)
+			{
+				double dis= dist(treeA[i].first,treeB[j].first);
+				if(dis < min_dist)
+				{	min_dist=dis;
+					// std::cout<<treeA.size()<<" "<<treeB.size()<<std::endl;
+				}
+				if(dis < stepLength )
+				{
+					connectA = treeA[i].first;
+					connectB = treeB[j].first;
+
+					conn.push_back( std::pair< Utils::Point<T>, Utils::Point<T> > (connectA,connectB));
+					// std::cout<<"distance = "<<dis<<" ";
+					// std::cout<<"points "<<treeA[i].first.x<<" "<<treeA[i].first.y;
+					// std::cout<<"points "<<treeB[j].first.x<<" "<<treeB[j].first.y;
+
+				}
+			}
+			
+		}
+		if (conn.size() )
+		{
+			double thetaDiffMax = 9999;
+			double thetaA,thetaB,thetaDiff,thetaMain;
+			int a  = 0;
+			for (int i = 0; i < conn.size(); ++i)
+			{
+				thetaMain = atan2(conn[i].second.y - conn[i].first.y ,conn[i].second.x - conn[i].first.x);
+				thetaA = atan2(conn[i].first.y - getParent(conn[i].first,treeA).y, conn[i].first.x - getParent(conn[i].first,treeA).x);
+				thetaB = atan2(getParent(conn[i].second,treeB).y - conn[i].second.y , getParent(conn[i].second,treeB).x - conn[i].second.x);
+				thetaDiff = abs(abs(thetaMain - thetaA) - abs(thetaB - thetaMain));
+				if (thetaDiff < thetaDiffMax)
+				{
+					thetaDiffMax = thetaDiff;
+					a=i;
+				}
+			}
+			connectA = conn[a].first;
+			connectB = conn[a].second;
+			// cout<<"Found Two Probable Joints\n";
+			// cout<<"--------"<<a<<"-------\n";
+			return true;
+		}
+		return false;
+	}
+
+	template <class T>
+	void RRTConnect<T>::setBiasParameter(unsigned int param)
+	{
+		biasParameter=param;
+	}
+
+	template <class T>
+	void RRTConnect<T>::generatePath(Utils::Point<T> first,Utils::Point<T> last)
+	{
+
+		Utils::Point<T> cur=connectA;
+		while(cur!=startPoint){
+			pathPoints.insert(pathPoints.begin(),cur);
+			cur = getParent(cur,treeA);
+		}
+
+		pathPoints.insert(pathPoints.begin(),cur);
+
+		cur = connectB;
+
+		// std::cout<<first.x<<" "<<first.y<<" "<<startPoint.x<<" "<<startPoint.y<<std::endl;
+		while(cur!=endPoint)
+		{
+			pathPoints.push_back(cur);
+			cur = getParent(cur,treeB);
+			// Utils::Point<T> parent= getParent(cur);
+			// std::cout<<"current : "<<cur.x<<","<<cur.y<<"| parent : "<<parent.x<<","<<parent.y<<std::endl;
+			// pathPoints.push_back(parent);
+			// cur=parent;
+		}
+		pathPoints.push_back(cur);
+	}
+
+	template <class T>
+	Utils::Point<T> RRTConnect<T>::getParent(Utils::Point<T> cur,std::vector< std::pair< Utils::Point<T>, Utils::Point<T> > >tree)
+	{
+		for(int i=0;i<tree.size();i++)
+		{
+			if(tree[i].first==cur)
+				return tree[i].second;
+		}
+	}
+
+	template <class T>
+	double RRTConnect<T>::dist(Utils::Point<T> a,Utils::Point<T> b)
+	{
+		return sqrt((a.x-b.x)*(a.x-b.x) + (a.y-b.y)*(a.y-b.y));
+	}
+}
+
diff --git a/src/main.cpp b/src/main.cpp
index 8e5a1d1..0e712f2 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -1,13 +1,18 @@
 #include <iostream>
-#include "./RRT_implementation.hpp"
+#include "./../include/RRTConnect.hpp"
 
 using namespace std;
 using namespace rrt;
 
 bool check(Utils::Point<int> cur)
 {
-	if(abs(cur.x)<2000 && abs(cur.y)<2000)
+	if(abs(cur.x)<2000 && abs(cur.y)<2000){
+		// if (cur.x < 550 && cur.x > 450 && cur.y < 550 & cur.y > 450)
+		// {
+		// 	return false;
+		// }
 		return true;
+	}
 	return false;
 }
 
@@ -15,24 +20,35 @@ int main()
 {
 	srand(time(NULL));
 	Utils::Point<int> start,finish,origin;
-	start.x=-10;
-	start.y=10;
-	finish.x=1000;
-	finish.y=730;
+	start.x=30;
+	start.y=30;
+	finish.x=1500;
+	finish.y=1730;
 	origin.x=0;
 	origin.y=0;
 
-	RRT<int> test;
+	RRTConnect<int> test;
 	test.setEndPoints(start,finish);
 	test.setCheckPointFunction(*(check));
-	test.setStepLength(200);
+	test.setStepLength(50);
 	test.setHalfDimensions(2000.0,2000.0);
 	test.setBiasParameter(100);
 	test.setOrigin(origin);
 	test.setMaxIterations(10000);
 	test.plan();
-	cout<<"#################################################"<<endl;
+	// test1.setEndPoints(start,finish);
+	// test1.setCheckPointFunction(*(check));
+	// test1.setStepLength(200);
+	// test1.setHalfDimensions(2000.0,2000.0);
+	// test1.setBiasParameter(100);
+	// test1.setOrigin(origin);
+	// test1.setMaxIterations(10000);
+	// test2.plan();
+
 	vector<Utils::Point<int> > path=test.getPointsOnPath();
+	cout<<"#################################################"<<endl;
 	for(int i=0;i<path.size();i++)
-		cout<<path[i].x<<","<<path[i].y<<endl;
-}
+		cout<<"("<<path[i].x<<","<<path[i].y<<")";
+
+	// cout<<"sfd"<<connectA.x<<","<<connectA.y<<" "<<connectB.x<<","<<connectB.y;
+}	
\ No newline at end of file

From 5ac60e6cdea3c76915d2ef75bd4c157e3abce91c Mon Sep 17 00:00:00 2001
From: kvmanohar22 <kvmanohar22@gmail.com>
Date: Sat, 2 Sep 2017 23:42:00 +0530
Subject: [PATCH 2/6] fix conficts

---
 include/RT_RRT_star.hpp | 30 ++++++++++++----
 src/RT_RRT_star.cpp     | 77 +++++++++++++++++++++++++++++++++++++++++
 2 files changed, 100 insertions(+), 7 deletions(-)

diff --git a/include/RT_RRT_star.hpp b/include/RT_RRT_star.hpp
index 81bde9d..74927a5 100644
--- a/include/RT_RRT_star.hpp
+++ b/include/RT_RRT_star.hpp
@@ -47,8 +47,11 @@ namespace rrt {
     // Grid density
     long int k_max;
 
+    // Useful parameters for setting some search parameters
+    unsigned int width, length;
+
   public:
-    RT_RRT(){};
+    RT_RRT();
 
     RT_RRT(T goal_radius, T obstacle_radius, T search_radius, T node_thresh,
       T epsilon_radius, long int k_max);
@@ -60,9 +63,10 @@ namespace rrt {
     {
       return sqrt(pow(first.x-second.x,2)+pow(first.y-second.y,2));
     }
+
     /** @brief Return Grid Id
      */
-    std::pair<unsigned int, unsigned int> Grid_Id(Utils::point<T> gride_idx);
+    std::pair<unsigned int, unsigned int> Grid_Id(Utils::Point<T> gride_idx);
 
     /** @brief Return cost
      */
@@ -93,8 +97,7 @@ namespace rrt {
 
     /** @brief Add a new point to the tree
      */
-    void add_node_to_tree(Utils::Point<T> rand, Utils::Point<T> closest,
-      std::vector<Utils::Point<T> > nearest_nodes);
+    void add_node_to_tree(Utils::Point<T> rand);
 
     /** @brief Rewire the tree from the latest node added to the tree
      */
@@ -103,11 +106,11 @@ namespace rrt {
       Utils::Point<T> me = Qr.pop();
       std::pair<int,Utils::Point<T>> now = getCost(me);
       int cost = now.first;
-      Utils::Point<T> parent = now.second; 
+      Utils::Point<T> parent = now.second;
       std::vector<Utils::Point<T> > neighbours= find_near_nodes(Qr);
       for (int i=0;i<neighbours.size();i++)
       {
-        Utils::Point<T> = neighbours[i];
+        Utils::Point<T> neighbour = neighbours[i];
         if (cost > cost - dist(me,parent) + dist(parent,neighbour) + dist(neighbour,me))
         {
               for(int j=0;i<tree.size();j++)
@@ -126,7 +129,7 @@ namespace rrt {
       std::vector<Utils::Point<T> > neighbours= find_near_nodes(Xa);
       for (int i=0;i<neighbours.size();i++)
       {
-        Utils::Point<T> = neighbours[i];
+        Utils::Point<T> neighbour = neighbours[i];
         if (cost > cost - dist(me,Xa) + dist(Xa,neighbour) + dist(neighbour,me))
         {
               for(int j=0;i<tree.size();j++)
@@ -135,7 +138,20 @@ namespace rrt {
                   tree[j].second = Xa;
               }
         }
+      }
     }
+
+    /** @brief Update the radius of neareset nodes prior to calling `find_near_nodes`
+    */
+    void update_epsilon_radius();
+
+    /** @brief Find the closest node
+    */
+    Utils::Point<T> closest_node(Utils::Point<T> rand);
+
+    /** @brief Verify if the line crosses any obstacle
+    */
+    bool line_path_obs(Utils::Point<T> p1, Utils::Point<T> p2);
   };
 
 }
diff --git a/src/RT_RRT_star.cpp b/src/RT_RRT_star.cpp
index 2f35b65..8a6cb42 100644
--- a/src/RT_RRT_star.cpp
+++ b/src/RT_RRT_star.cpp
@@ -1 +1,78 @@
 #include "RT_RRT_star.hpp"
+
+namespace rrt {
+
+  template <class T>
+  RT_RRT<T>::RT_RRT() {
+    // TODO : set these parameters
+    // TODO : Set all parameters in one scale
+    goal_radius = 20.0;
+    obstacle_radius = 20.0;
+    search_radius = 20.0;
+    node_thresh = 20.0;
+    epsilon_radius = 20.0;
+    k_max = 20.0;
+
+    width = 6;
+    length = 9;
+  }
+
+  template <class T>
+  RT_RRT<T>::RT_RRT(T goal_radius, T obstacle_radius, T search_radius,
+    T node_thresh, T epsilon_radius, long int k_max) {
+
+    this->goal_radius = goal_radius;
+    this->obstacle_radius = obstacle_radius;
+    this->search_radius = search_radius;
+    this->node_thresh = node_thresh;
+    this->epsilon_radius = epsilon_radius;
+    this->k_max = k_max;
+    width = 6;
+    length = 9;
+  }
+
+  template <class T>
+  void RT_RRT<T>::update_epsilon_radius() {
+    size_t total_nodes = this->tree.size();
+
+  }
+
+  template <class T>
+  std::vector<Utils::Point<T> > RT_RRT<T>::find_near_nodes(Utils::Point<T> query) {
+
+    std::pair<unsigned int, unsigned int> grid_idx = this->Grid_Id(query);
+    std::vector<Utils::Point<T> > nearest_points;
+    for (typename std::vector<Utils::Point<T> >::iterator
+      itr = grid_nodes[grid_idx].begin(); itr != grid_nodes[grid_idx].end();
+      ++itr) {
+        this->update_epsilon_radius();
+        if (this->dist(query, *itr) < this->epsilon_radius) {
+          nearest_points.push_back(*itr);
+        }
+      }
+    return nearest_points;
+  }
+
+  template <class T>
+  std::pair<unsigned int, unsigned int> RT_RRT<T>::Grid_Id(Utils::Point<T> node) {
+
+  }
+
+  template <class T>
+  void RT_RRT<T>::add_node_to_tree(Utils::Point<T> rand) {
+
+    Utils::Point<T> closest = closest_node(rand);
+    std::vector<Utils::Point<T> > x_near = find_near_nodes(rand);
+
+    Utils::Point<T> x_min = closest;
+    int c_min = cost(closest) + dist(closest, rand);
+    for (typename std::vector<Utils::Point<T> >::iterator itr = x_near.begin();
+      itr != x_near.end(); ++itr) {
+
+      int c_new = cost(*itr) + dist(*itr, rand);
+
+    }
+
+  }
+
+}

From 4e8eb0ba50d74b7fadcc4dc189f866e43919d84f Mon Sep 17 00:00:00 2001
From: saurabhkgpee <shourabh.agarwal@gmail.com>
Date: Sun, 3 Sep 2017 01:20:00 +0530
Subject: [PATCH 3/6] Planner

---
 CMakeLists.txt                 |   6 +-
 include/DRRT.hpp               | 570 +++++++++++++++++++++++++++++++++
 include/RRT.hpp                |  78 ++++-
 include/RRT_implementation.hpp |  35 +-
 test/main.cpp                  |  67 ++++
 5 files changed, 723 insertions(+), 33 deletions(-)
 create mode 100644 include/DRRT.hpp
 create mode 100644 test/main.cpp

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 3334ca3..aa66045 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -4,8 +4,10 @@ set(CMAKE_CXX_FLAGS "-std=c++11")
 
 include_directories("include")
 
-set(SRCS src/Random_sampling.cpp
-  src/RT_RRT_star.cpp)
+set(SRCS src/Random_sampling.cpp)
 
 set(PLANNERS planners)
 add_library(${PLANNERS} SHARED ${SRCS})
+
+add_executable(target test/main.cpp)
+target_link_libraries(target ${PLANNERS})
diff --git a/include/DRRT.hpp b/include/DRRT.hpp
new file mode 100644
index 0000000..ef1b96a
--- /dev/null
+++ b/include/DRRT.hpp
@@ -0,0 +1,570 @@
+
+#include <bits/stdc++.h>
+#include "./RRT.hpp"
+#include "./RRT_implementation.hpp"
+#include "./utils.hpp"
+
+using namespace std;
+namespace rrt
+{
+
+	template<class T>
+	void DRRT<T>::setBucketSize(unsigned int size){
+		BucketSize = size;
+	}
+
+	template<class T>
+	void DRRT<T>::setPointsInBucket(unsigned int num){
+		PointsInBucket = num;
+	}
+
+	template <class T>
+	void DRRT<T>::generateGrid(){
+		numX = 2*halfDimensionX/BucketSize;
+		if (numX != (int)numX)
+		{
+			numX = (int)numX + 1;
+		}
+		numY = 2*halfDimensionY/BucketSize;
+		if (numY != (int)numY)
+		{
+			numY = (int)numY + 1;
+		}
+		int num = numX*numY;
+		for (int i = 0; i < num; ++i)
+		{
+			std::vector<int> v;
+			bucket.push_back(std::pair<int,std::vector<int> > (i,v));
+		}
+	}
+
+	template < class T>
+	int DRRT<T>::findBucket(Utils::Point<T> pt){
+		int x = pt.x + halfDimensionX ;
+		int y = pt.y + halfDimensionX ;
+		int numx = (int)(x/BucketSize) + 1;
+		if (numx > numX)
+		{
+			numx = numx - 1;
+		}
+		int numy = (int)(y/BucketSize) + 1;
+		if (numy > numY)
+		{
+			numy = numy - 1;
+		}
+		int num = (numy - 1)*numX + numx - 1;
+		return num;
+	}
+
+	template <class T>
+	void DRRT<T>::setEndPoints(Utils::Point<T> start, Utils::Point<T> end)
+	{
+		startPoint=start;
+		endPoint=end;
+	}
+
+	template <class T>
+	void DRRT<T>::setTimeOut(float time)
+	{
+		timeOut = time;
+	}
+
+	template <class T>
+	void DRRT<T>::setHalfDimensions(double a, double b)
+	{
+		halfDimensionX=a;
+		halfDimensionY=b;
+	}
+
+	template <class T>
+	void DRRT<T>::setOrigin(Utils::Point<T> pt)
+	{
+		origin=pt;
+	}
+
+	template <class T>
+	std::vector<Utils::Point<T> > DRRT<T>::getPointsOnPath(Utils::Point<T> goal)
+	{
+		// clear vector before use
+		pathPoints.clear();
+		Utils::Point <T> cur;
+		int numBucket = findBucket(goal);
+		float minDist = 9999;
+		int min = -1;
+
+		// checking point at minimum distance from goal
+		// in goal's bucket, 
+		// If any change......  do here
+		for (int i = 0; i < bucket[numBucket].second.size(); ++i)
+		{
+			int id = bucket[numBucket].second[i];
+			float Dist = dist(goal,tree[id].first);
+			if (Dist < minDist)
+			{
+				minDist = Dist;
+				min = id;
+			}
+		}
+		if (goal != tree[min].first)
+		{
+			pathPoints.push_back(goal);
+		}
+		cur = tree[min].first;
+		while(cur != getParent(cur)){
+			pathPoints.push_back(cur);
+			cur = getParent(cur);
+		}
+		pathPoints.push_back(cur);
+		return pathPoints;
+	}
+
+	template <class T>
+	std::vector<Utils::Point<T> > DRRT<T>::getPath(Utils::Point<T> start,Utils::Point<T> goal){
+		reWireRoot(start);
+		std::vector<Utils::Point<T> > path;
+		cout<<"Current Root"<<root.x<<" "<<root.y<<endl;
+		path = getPointsOnPath(goal);
+		if (path[path.size() - 1] != start)
+		{
+			path.push_back(start);
+		}
+		return path;
+	}
+
+	template <class T>
+	void DRRT<T>::setStepLength(double length)
+	{
+		stepLength=length;
+	}
+
+	template <class T>
+	void DRRT<T>::setMaxIterations(int maxIt)
+	{
+		maxIterations=maxIt;
+	}
+
+	
+	template<class T>
+	bool DRRT<T>::saturation(){
+		for (int i = 0; i < bucket.size(); ++i)
+		{
+			if (bucket[i].second.size() < PointsInBucket)
+			{
+				return true;
+			}
+		}
+
+		return false;
+	}
+
+	template<class T>
+	bool DRRT<T>::dense(Utils::Point<T> pt){
+		int numBucket = findBucket(pt);
+		int density = bucket[numBucket].second.size();
+		if (density > PointsInBucket)
+		{
+			return false;
+		}
+		return true;
+
+	}
+
+	template <class T>
+	bool DRRT<T>::collision(Utils::Point<T> pt){
+		for (int i = 0; i < obstacles.size(); ++i)
+		{
+			std::vector<int> obstacleBucket;
+			Utils::Point<T> ob = obstacles[i].first;
+			int radius = obstacles[i].second;
+			int numbucket = findBucket(ob);
+			obstacleBucket.push_back(numbucket);
+			if (numbucket%(int)numX == 0)
+			{
+				obstacleBucket.push_back(numbucket + 1);
+			}
+			else if(numbucket % (int)numX == (int)numX - 1)
+				obstacleBucket.push_back(numbucket - 1);
+			else{
+				obstacleBucket.push_back(numbucket + 1);
+				obstacleBucket.push_back(numbucket - 1);
+			}
+			
+			if (numbucket/(int)numX == 0)
+			{
+				obstacleBucket.push_back(numbucket + (int)numX);	
+			}
+
+			else if(numbucket/(int)numX == (int)numY - 1){
+				obstacleBucket.push_back(numbucket - (int)numX);
+			}
+
+			else{
+				obstacleBucket.push_back(numbucket + (int)numX);
+				obstacleBucket.push_back(numbucket - (int)numX);
+			}
+
+			for (int n = 0; n < obstacleBucket.size(); ++n)
+			{
+				int numBucket = obstacleBucket[n];
+				for (int j = 0; j < bucket[numBucket].second.size(); ++j)
+				{
+					int id = bucket[numBucket].second[j];
+					Utils::Point<T> node = tree[id].first;
+					if (dist(pt,node) < radius)
+					{
+						cout<<"Obstacle\n";
+						return false;
+					}
+				}
+			}
+		}
+		
+
+		return true;
+	}
+
+	template <class T>
+	bool DRRT<T>:: plan()
+	{
+		// unordered_map<Utils:Point<T>, float > costMap;
+		cout<<"Planning\n";
+		int count=0;
+		int check=0;
+		int numBucket = findBucket(startPoint);
+		bucket[numBucket].second.push_back(tree.size());		
+		//storing index of node in bucket
+		tree.push_back(std::pair< Utils::Point<T>, Utils::Point<T> > (startPoint,startPoint));
+		root = startPoint;
+		Utils::Point<T> next;
+		float start_time = clock();
+		while( check < maxIterations && saturation())
+		{
+			do{
+				next = generatePoint();
+			}while(checkPoint(next)==false || dense(next)==false);
+			int numBucket = findBucket(next);
+			/*storing index of node in bucket*/
+			bucket[numBucket].second.push_back(tree.size());	
+			growTree(next);
+			check ++;
+			
+		}
+		float end_time = clock();
+
+		
+		cout<<tree.size();
+		std::cout<<"\nTime to generate path = "<<(float)(end_time-start_time)/CLOCKS_PER_SEC<<std::endl;
+
+		// for (int i = 0; i < tree.size(); ++i)
+		// {
+		// 	cout<<"("<<tree[i].first.x<<","<<tree[i].first.y<<")";
+		// }
+		cout<<endl;
+		return true;
+
+
+	}
+
+	template <class T>
+
+	void DRRT<T>::growTree(Utils::Point<T> temp)
+	{
+		Utils::Point<T> parent;
+		parent = findClosestNode(temp);
+		tree.push_back(std::pair< Utils::Point<T>,Utils::Point<T> >(temp,parent));
+	}
+	
+
+	template <class T>
+	void DRRT<T>::setCheckPointFunction(bool (*f)(Utils::Point<T>))
+	{
+		userCheck=f;
+	}
+
+	template <class T>
+	Utils::Point<T> DRRT<T>::findClosestNode(Utils::Point<T> cur)
+	{
+		double min_dist=INT_MAX;
+		Utils::Point<T> closest;
+		for(int i=0;i<tree.size();i++)
+		{
+			if(dist(tree[i].first,cur) < min_dist )
+			{
+				min_dist=dist(tree[i].first,cur);
+				closest=tree[i].first;
+			}
+		}
+		return closest;
+	}
+
+	template <class T>
+	Utils::Point<T> DRRT<T>::generatePoint()
+	{
+		Utils::Point<T> temp;
+		int signX,signY;
+
+		if(rand()%2==0)
+			signX=-1;
+		else
+			signX=1;
+		if(rand()%2==0)
+			signY=-1;
+		else 
+			signY=1;
+		temp.x = origin.x + signX* ( rand() % (int)halfDimensionX );
+		temp.y = origin.y + signY* ( rand() % (int)halfDimensionY );
+
+
+		Utils::Point<T> closest=findClosestNode(temp);
+		double theta = atan2(temp.y-closest.y,temp.x-closest.x);
+		double dis = dist(temp,closest);
+		if (dis < stepLength)
+		{
+			return temp;
+		}
+		Utils::Point<T> next;
+
+		next.x=closest.x+stepLength*cos(theta);
+		next.y=closest.y+stepLength*sin(theta);
+		return next;
+	}
+
+
+	template <class T>
+	bool DRRT<T>::checkPoint(Utils::Point<T> next)
+	{
+		return userCheck(next);
+	}
+
+	template <class T>
+	float DRRT<T>::cost(Utils::Point <T> point){
+
+
+		Utils::Point <T> parent;
+		parent = getParent(point);
+		if (point == parent)
+		{
+			if (point == root)
+			{
+				return 0;
+			}
+			return 9999;
+		}
+
+		else{
+			float costValue;
+			costValue = cost(parent) + dist(parent,point);
+			// costMap[point] = costValue;
+			return costValue;
+		}
+	}
+
+	template<class T>
+	std::vector< Utils::Point<T> > DRRT<T>::getChild(Utils::Point<T> point){
+		std::vector< Utils::Point<T> > child;
+		int numBucket = findBucket(point);
+		std::vector<int> list;
+		list.push_back(numBucket);
+
+		// only searching one layer around newRoot
+		if (numBucket%(int)numX == 0)
+		{
+			list.push_back(numBucket + 1);
+		}
+		else if(numBucket % (int)numX == (int)numX - 1)
+			list.push_back(numBucket - 1);
+		else{
+			list.push_back(numBucket + 1);
+			list.push_back(numBucket - 1);
+		}
+		
+		if (numBucket/(int)numX == 0)
+		{
+			list.push_back(numBucket + (int)numX);	
+		}
+
+		else if(numBucket/(int)numX == (int)numY - 1){
+			list.push_back(numBucket - (int)numX);
+		}
+
+		else{
+			list.push_back(numBucket + (int)numX);
+			list.push_back(numBucket - (int)numX);
+		}
+
+		for (int i = 0; i < list.size(); ++i)
+		{
+			int numBucket = list[i];
+			for (int j = 0; j < bucket[numBucket].second.size(); ++j)
+			{
+				int id = bucket[numBucket].second[j];
+				if (tree[id].second == point)
+				{
+					child.push_back(tree[id].first);
+				}
+			}
+		}
+
+		return child;
+	};
+
+
+	template <class T>
+	Utils::Point<T> DRRT<T>::getParent(Utils::Point<T> cur)
+	{
+		int numBucket = findBucket(cur);
+		// iterate over cur point bucket
+		for(int i=0; i<bucket[numBucket].second.size() ;i++)
+		{
+			int id = bucket[numBucket].second[i];
+			if(tree[id].first==cur){
+				return tree[id].second;
+			}
+		}
+	}
+
+	template <class T>
+	double DRRT<T>::dist(Utils::Point<T> a,Utils::Point<T> b)
+	{
+		return sqrt((a.x-b.x)*(a.x-b.x) + (a.y-b.y)*(a.y-b.y));
+	}
+
+
+	template<class T>
+	void DRRT<T>::reWireRoot(Utils::Point <T> newRoot){
+		// unordered_map<Utils::Point<T>, float> costMap;
+		std::vector<Utils::Point<T> > path = getPointsOnPath(newRoot);
+
+		std::vector<int> ids;
+		for (int i = 0; i < path.size(); ++i)
+		{
+			int numBucket = findBucket(path[i]);
+			for (int j = 0; j < bucket[numBucket].second.size(); ++j)
+			{	
+				int id = bucket[numBucket].second[j]; 
+				if (path[i] ==tree[id].first)
+				{
+					ids.push_back(id);
+					break;
+				}
+			}
+		}
+		if (path.size() != ids.size())
+		{
+			path.erase(path.begin());
+		}
+		cout<<"\n"<<path.size()<<","<<ids.size();
+		tree[ids[0]] = pair< Utils::Point<T> , Utils::Point<T> > (path[0],path[0]);
+		root = path[0];
+		for (int i = 1; i < ids.size(); ++i)
+		{
+			int id = ids[i];
+			tree[id] = std::pair< Utils::Point<T>, Utils::Point<T> > (path[i],path[i - 1]);
+		}
+
+		// New Root of tree is "nearest point" to given point if point is not in tree
+		// std::vector<Utils::Point<T> > temppath = getPointsOnPath(startPoint);
+
+		std::vector<Utils::Point<T> > nodes;
+		nodes.push_back(path[0]);
+
+		float start = clock();
+		int ptr = 0;
+		cout<<"\n";
+		while(((clock() - start)/CLOCKS_PER_SEC) < timeOut && ptr < nodes.size())
+		{
+			int numBucket = findBucket(nodes[ptr]);
+			
+			int myId = -1;
+			for (int i = 0; i < bucket[numBucket].second.size(); ++i)
+			{
+				int id = bucket[numBucket].second[i];
+				if (tree[id].first == nodes[ptr])
+				{
+					myId = id;
+					break;
+				}
+			}
+
+			std::vector<int> list;		//buckets adjacent to rewiring point
+			std::vector<int> ids;		//id of nodes in adjacent buckets
+			list.push_back(numBucket);
+
+			// only searching one layer around newRoot
+			if (numBucket%(int)numX == 0)
+			{
+				list.push_back(numBucket + 1);
+			}
+			else if(numBucket % (int)numX == (int)numX - 1)
+				list.push_back(numBucket - 1);
+			else{
+				list.push_back(numBucket + 1);
+				list.push_back(numBucket - 1);
+			}
+			
+			if (numBucket/(int)numX == 0)
+			{
+				list.push_back(numBucket + (int)numX);	
+			}
+
+			else if(numBucket/(int)numX == (int)numY - 1){
+				list.push_back(numBucket - (int)numX);
+			}
+
+			else{
+				list.push_back(numBucket + (int)numX);
+				list.push_back(numBucket - (int)numX);
+			}
+
+			//list contains bucket id for all buckets surrounding newroot
+			for (int i = 0; i < list.size(); ++i)
+			{
+				int numBucket = list[i];
+				for (int j = 0; j < bucket[numBucket].second.size(); ++j)
+				{
+					int id = bucket[numBucket].second[j];
+					if (id != myId)
+					{
+						ids.push_back(id);
+					}
+				}
+			}
+
+			float score = cost(nodes[ptr]);
+
+
+			for (int i = 0; i < ids.size(); ++i)
+			{
+				int id = ids[i];
+				if (dist(tree[id].first,tree[myId].first) < stepLength)
+				{
+					if ((cost(tree[id].first) + dist(tree[id].first, tree[myId].first)) < score)
+					{
+						tree[myId] = pair< Utils::Point<T>, Utils::Point<T> > (tree[myId].first, tree[id].first);
+						score = cost(tree[id].first) + dist(tree[id].first, tree[myId].first);
+					}
+				}
+				
+
+				bool flag = false;
+				for (int j = 0; j < nodes.size(); ++j)
+				{
+					if (nodes[j] == tree[id].first)
+					{
+						flag = true;
+						break;
+					}
+				}
+
+				if (!flag)
+				{
+					nodes.push_back(tree[id].first);
+				}
+			}
+			ptr++;
+		}
+		cout<<ptr;
+	}
+
+}
+
diff --git a/include/RRT.hpp b/include/RRT.hpp
index e1d9045..63affe2 100644
--- a/include/RRT.hpp
+++ b/include/RRT.hpp
@@ -3,6 +3,7 @@
 
 #include <iostream>
 #include <bits/stdc++.h>
+// #include <unordered_map>
 #include "utils.hpp"
 
 namespace rrt
@@ -17,12 +18,9 @@ namespace rrt
 		Utils::Point<T> startPoint;
 		Utils::Point<T> endPoint;
 		double stepLength;
-		std::deque<Utils::Point<T> > pathPoints;
+		std::vector<Utils::Point<T> > pathPoints;
 		int maxIterations;
 		std::vector< std::pair< Utils::Point<T>, Utils::Point<T> > > tree; 
-
-		std::vector< std::pair< Utils::Point<T>, Utils::Point<T> > > tree1; 
-		std::vector< std::pair< Utils::Point<T>, Utils::Point<T> > > tree2; 
 		unsigned int biasParameter;
 		
 	public:
@@ -36,7 +34,7 @@ namespace rrt
 		}
 		
 		virtual bool plan();
-		virtual std::deque<Utils::Point<T> > getPointsOnPath();
+		virtual std::vector<Utils::Point<T> > getPointsOnPath();
 
 		virtual void setEndPoints(Utils::Point<T> start, Utils::Point<T> end);
 		virtual void setCheckPointFunction(bool (*f)(Utils::Point<T>));
@@ -54,11 +52,11 @@ namespace rrt
 		bool (*userCheck)(Utils::Point<T>);
 		bool checkPoint(Utils::Point<T> pt);
 		Utils::Point<T> generatePoint();
-		Utils::Point<T> generateBiasedPoint(int);
+		Utils::Point<T> generateBiasedPoint();
 		void growTree(Utils::Point<T>);
-		std::pair<Utils::Point<T>, int>  findClosestNode(Utils::Point<T>);
+		Utils::Point<T> findClosestNode(Utils::Point<T>);
 		Utils::Point<T> getParent(Utils::Point<T>);
-		std::pair <Utils::Point <T>,Utils::Point <T> > treeComplete(int*);
+		bool treeComplete();
 		void generatePath(Utils::Point<T> first,Utils::Point<T> last);
 		double dist(Utils::Point<T> a,Utils::Point<T> b);
 	};
@@ -105,13 +103,67 @@ namespace rrt
 		void generatePath(Utils::Point<T> first,Utils::Point<T> last);
 		double dist(Utils::Point<T> a,Utils::Point<T> b);
 	};
+
+
+	template <class T>
+	class DRRT: public RRT<T>{
+	private:
+		std::vector<int> obstacleBucket;
+		std::vector<std::pair< int, std::vector <int> > > bucket;
+		float numX;
+		float numY;
+		double halfDimensionX;
+		double halfDimensionY;
+		Utils::Point<T> origin;
+		Utils::Point<T> startPoint;
+		Utils::Point<T> endPoint;
+		double stepLength;
+		std::vector<Utils::Point<T> > pathPoints;
+		int maxIterations;
+		std::vector< std::pair< Utils::Point<T>, Utils::Point<T> > > tree; 
+		unsigned int biasParameter;
+		unsigned int BucketSize;
+		unsigned int PointsInBucket;
+		float timeOut;
+		Utils::Point<T> root;
+
+	public:
+		bool plan();
+		std::vector<Utils::Point<T> > getPath(Utils::Point<T>,Utils::Point<T>);
+		std::vector<Utils::Point<T> > getPointsOnPath(Utils::Point<T>);
+		std::vector<std::pair<Utils::Point<T>, int > > obstacles;
+		void setEndPoints(Utils::Point<T> start, Utils::Point<T> end);
+		void setCheckPointFunction(bool (*f)(Utils::Point<T>));
+		void setStepLength(double value);
+		void setOrigin(Utils::Point<T> origin);
+		void setHalfDimensions(double x,double y);
+		void setMaxIterations(int);
+		void setBucketSize(unsigned int);
+		void setPointsInBucket(unsigned int);
+		void generateGrid();
+		void setTimeOut(float time);
+
+	
+	private:
+		void reWireRoot(Utils::Point<T> point);
+		int findBucket(Utils::Point<T>);
+		bool saturation();
+		bool collision(Utils::Point<T>);
+		bool dense(Utils::Point<T>);
+		bool (*userCheck)(Utils::Point<T>);
+		bool checkPoint(Utils::Point<T> pt);
+		Utils::Point<T> generatePoint();
+		Utils::Point<T> findClosestNode(Utils::Point<T>);
+		void growTree(Utils::Point<T>);
+		Utils::Point<T> getParent(Utils::Point<T>);
+		bool treeComplete();
+		void generatePath(Utils::Point<T> first,Utils::Point<T> last);
+		double dist(Utils::Point<T> a,Utils::Point<T> b);
+		float cost(Utils::Point<T> point);
+		std::vector< Utils::Point<T> > getChild(Utils::Point<T> point);
+	};
 }
 
-//************* ToDo ************* 
-// Implement derived classes for variants of RRT
-// Optimize the generate path and other 
-// Tweak with the parameters to check their effects on runtime and path 
-// Implement Pruning function to keep a check on size of tree
 
 
 #endif
\ No newline at end of file
diff --git a/include/RRT_implementation.hpp b/include/RRT_implementation.hpp
index 5e9bbd9..7220afa 100644
--- a/include/RRT_implementation.hpp
+++ b/include/RRT_implementation.hpp
@@ -25,7 +25,7 @@ namespace rrt
 	}
 
 	template <class T>
-	std::deque<Utils::Point<T> > RRT<T>::getPointsOnPath()
+	std::vector<Utils::Point<T> > RRT<T>::getPointsOnPath()
 	{
 		return pathPoints;
 	}
@@ -52,9 +52,7 @@ namespace rrt
 		{
 			//std::cout<<"In Planning Loop"<<std::endl;
 			Utils::Point<T> next;
-			int arr[]={1};
-			std::pair  <Utils::Point <T>,Utils::Point <T> >  mid = treeComplete(arr);
-			if(mid.first!=mid.second)
+			if(treeComplete()==true)
 			{
 				std::cout<<"Tree complete!!"<<std::endl;
 				int start_time=clock();
@@ -69,7 +67,7 @@ namespace rrt
 				//std::cout<<"Adding Next Biased Point to tree!!"<<std::endl;
 				count=0;
 				do{
-					next= generateBiasedPoint(1);
+					next= generateBiasedPoint();
 				}while(checkPoint(next)!=true);
 				//std::cout<<" : "<<next.x<<","<<next.y<<std::endl;
 			}
@@ -94,11 +92,11 @@ namespace rrt
 	template <class T>
 	void RRT<T>::growTree(Utils::Point<T> next)
 	{
-		//growing the tree by adding node
+		//growing the tree by adding node 
 		//std::cout<<"finding parent in tree of size = "<<tree.size()<<std::endl;
 		Utils::Point<T> parent;
-		parent= findClosestNode(next).first;
-		//std::cout<<"current : "<<next.x<<","<<next.y<<"| parent : "<<parent.x<<","<<parent.y<<std::endl;
+		parent= findClosestNode(next);
+		//std::cout<<"current : "<<next.x<<","<<next.y<<"| parent : "<<parent.x<<","<<parent.y<<std::endl;			
 		tree.push_back( std::pair< Utils::Point<T>, Utils::Point<T> > (next,parent));
 		//std::cout<<"Tree grown"<<std::endl;
 		//add pruning code to keep size of tree under control
@@ -111,7 +109,7 @@ namespace rrt
 	}
 
 	template <class T>
-	std::pair<Utils::Point<T>, int>  RRT<T>::findClosestNode(Utils::Point<T> cur)
+	Utils::Point<T> RRT<T>::findClosestNode(Utils::Point<T> cur)
 	{
 		double min_dist=INT_MAX;
 		Utils::Point<T> closest;
@@ -124,7 +122,7 @@ namespace rrt
 				closest=tree[i].first;
 			}
 		}
-		return std::make_pair(closest,1);
+		return closest;
 	}
 
 	template <class T>
@@ -139,13 +137,13 @@ namespace rrt
 			signX=1;
 		if(rand()%2==0)
 			signY=-1;
-		else
+		else 
 			signY=1;
 
 		temp.x = origin.x + signX* ( rand() % (int)halfDimensionX );
 		temp.y = origin.y + signY* ( rand() % (int)halfDimensionY );
 
-		Utils::Point<T> closest=findClosestNode(temp).first;
+		Utils::Point<T> closest=findClosestNode(temp);
 		double theta = atan2(temp.y-closest.y,temp.x-closest.x);
 
 		Utils::Point<T> next;
@@ -156,9 +154,9 @@ namespace rrt
 	}
 
 	template <class T>
-	Utils::Point<T> RRT<T>::generateBiasedPoint(int which)
+	Utils::Point<T> RRT<T>::generateBiasedPoint()
 	{
-		Utils::Point<T> closest=findClosestNode(endPoint).first;
+		Utils::Point<T> closest=findClosestNode(endPoint);
 		double theta = atan2(endPoint.y-closest.y,endPoint.x-closest.x);
 
 		Utils::Point<T> next;
@@ -175,11 +173,11 @@ namespace rrt
 	}
 
 	template <class T>
-	std::pair <Utils::Point <T>,Utils::Point <T> > RRT<T>::treeComplete(int arr[1])
+	bool RRT<T>::treeComplete()
 	{
 		//std::cout<<"Checking if tree is complete? "<<std::endl;
 		//std::cout<<"tree size = "<<tree.size()<<std::endl;
-		int min_dist=INT_MAX;
+		int min_dist=INT_MAX;	
 		for(int i=0;i<tree.size();i++)
 		{
 			double dis= dist(tree[i].first,endPoint);
@@ -187,11 +185,11 @@ namespace rrt
 				min_dist=dis;
 			if(dis < stepLength )
 			{
-				return std::make_pair(tree[i].first,endPoint);
+				return true;
 			}
 		}
 		//std::cout<<"Min Distance In this iteration = "<<min_dist<<std::endl;
-		return std::make_pair(endPoint,endPoint);
+		return false;
 	}
 
 	template <class T>
@@ -230,3 +228,4 @@ namespace rrt
 		return sqrt((a.x-b.x)*(a.x-b.x) + (a.y-b.y)*(a.y-b.y));
 	}
 }
+
diff --git a/test/main.cpp b/test/main.cpp
new file mode 100644
index 0000000..3d21239
--- /dev/null
+++ b/test/main.cpp
@@ -0,0 +1,67 @@
+#include <iostream>
+#include "./../include/DRRT.hpp"
+using namespace std;
+using namespace rrt;
+
+bool check(Utils::Point<int> cur)
+{
+	if(abs(cur.x)<2000 && abs(cur.y)<2000){
+		Utils::Point<int> a,b;
+		a.x = 530;
+		a.y = 570;
+		b = cur;
+		double dist = sqrt((a.x-b.x)*(a.x-b.x) + (a.y-b.y)*(a.y-b.y));
+		return true;
+	}
+	cout<<"False check";
+	return false;
+}
+
+int main()
+{
+	srand(time(NULL));
+	Utils::Point<int> start,finish,origin,checkpt;
+	start.x=30;
+	start.y=30;
+	finish.x=1500;
+	finish.y=1730;
+	origin.x=0;
+	origin.y=0;
+	checkpt.x = 400;
+	checkpt.y = 0;
+
+	// steplength << bucketsize (finding child using bucket)
+	DRRT<int> test;
+	test.setEndPoints(start,finish);
+	test.setCheckPointFunction(*(check));
+	test.setStepLength(50);
+	test.setHalfDimensions(2000.0,2000.0);
+	test.setBucketSize(100);
+	test.setPointsInBucket(3);
+	test.setBiasParameter(100);
+	test.setOrigin(origin);
+	test.setMaxIterations(50000);
+	test.generateGrid();
+	test.setTimeOut(0.010);
+	Utils::Point<int> ob;
+	ob.x = 200;
+	ob.y = 0;
+
+	
+	test.plan();
+	vector<Utils::Point<int> > path;
+
+	float starttime = clock();
+	path =test.getPath(start,checkpt);
+	float endtime = clock();
+
+	cout<<"Time taken = "<<(endtime - starttime)/CLOCKS_PER_SEC;
+	cout<<"\n"<<path.size();
+	cout<<"###########################IN Main######################"<<endl;
+	for(int i=0;i<path.size();i++)
+		cout<<"("<<path[i].x<<","<<path[i].y<<")";
+
+	cout<<endl<<endl;
+
+	
+}	
\ No newline at end of file

From 131c7b5a83c4c0999f098cb6c1deeee3ba7612b3 Mon Sep 17 00:00:00 2001
From: saurabhkgpee <shourabh.agarwal@gmail.com>
Date: Sun, 13 Aug 2017 02:44:11 +0530
Subject: [PATCH 4/6] RRT Connect

---
 include/RRT.hpp        |  44 ++++++
 include/RRTConnect.hpp | 331 +++++++++++++++++++++++++++++++++++++++++
 src/main.cpp           |  27 ++--
 3 files changed, 393 insertions(+), 9 deletions(-)
 create mode 100644 include/RRTConnect.hpp

diff --git a/include/RRT.hpp b/include/RRT.hpp
index 70c728d..c542273 100644
--- a/include/RRT.hpp
+++ b/include/RRT.hpp
@@ -61,6 +61,49 @@ namespace rrt
 		void generatePath(Utils::Point<T> first,Utils::Point<T> last);
 		double dist(Utils::Point<T> a,Utils::Point<T> b);
 	};
+
+	template <class T>
+	class RRTConnect: public RRT<T>{
+
+	public:
+		bool plan();
+		 std::vector<Utils::Point<T> > getPointsOnPath();
+
+		void setEndPoints(Utils::Point<T> start, Utils::Point<T> end);
+		void setCheckPointFunction(bool (*f)(Utils::Point<T>));
+		void setStepLength(double value);
+		void setOrigin(Utils::Point<T> origin);
+		void setHalfDimensions(double x,double y);
+		void setBiasParameter(unsigned int);
+		void setMaxIterations(int);
+	private:
+		double halfDimensionX;
+		double halfDimensionY;
+		Utils::Point<T> origin;
+		Utils::Point<T> connectA;
+		Utils::Point<T> connectB;
+		Utils::Point<T> startPoint;
+		Utils::Point<T> endPoint;
+		double stepLength;
+		std::vector<Utils::Point<T> > pathPoints;
+		int maxIterations;
+		std::vector< std::pair< Utils::Point<T>, Utils::Point<T> > > treeA; 
+		std::vector< std::pair< Utils::Point<T>, Utils::Point<T> > > treeB;
+		std::vector< std::pair< Utils::Point<T>, Utils::Point<T> > > mainTree;
+		std::vector< std::pair< Utils::Point<T>, Utils::Point<T> > > supportTree; 
+
+		unsigned int biasParameter;
+		bool (*userCheck)(Utils::Point<T>);
+		bool checkPoint(Utils::Point<T> pt);
+		Utils::Point<T> generatePoint(std::vector< std::pair< Utils::Point<T>, Utils::Point<T> > >);
+		Utils::Point<T> generateBiasedPoint(Utils::Point<T> ,std::vector< std::pair< Utils::Point<T>, Utils::Point<T> > >);
+		Utils::Point<T> findClosestNode(Utils::Point<T>,std::vector< std::pair< Utils::Point<T>, Utils::Point<T> > > tree);
+		void growTree(Utils::Point<T>);
+		Utils::Point<T> getParent(Utils::Point<T>,std::vector< std::pair< Utils::Point<T>, Utils::Point<T> > > tree);
+		bool treeComplete();
+		void generatePath(Utils::Point<T> first,Utils::Point<T> last);
+		double dist(Utils::Point<T> a,Utils::Point<T> b);
+	};
 }
 
 //************* ToDo ************* 
@@ -70,4 +113,5 @@ namespace rrt
 // Implement Pruning function to keep a check on size of tree
 
 
+
 #endif
\ No newline at end of file
diff --git a/include/RRTConnect.hpp b/include/RRTConnect.hpp
new file mode 100644
index 0000000..b5ca20d
--- /dev/null
+++ b/include/RRTConnect.hpp
@@ -0,0 +1,331 @@
+#include <iostream>
+#include "./RRT.hpp"
+#include "./RRT_implementation.hpp"
+#include "./utils.hpp"
+
+using namespace std;
+namespace rrt
+{
+	template <class T>
+	void RRTConnect<T>::setEndPoints(Utils::Point<T> start, Utils::Point<T> end)
+	{
+		startPoint=start;
+		endPoint=end;
+	}
+
+	template <class T>
+	void RRTConnect<T>::setHalfDimensions(double a, double b)
+	{
+		halfDimensionX=a;
+		halfDimensionY=b;
+	}
+
+	template <class T>
+	void RRTConnect<T>::setOrigin(Utils::Point<T> pt)
+	{
+		origin=pt;
+	}
+
+	template <class T>
+	std::vector<Utils::Point<T> > RRTConnect<T>::getPointsOnPath()
+	{
+		return pathPoints;
+	}
+
+	template <class T>
+	void RRTConnect<T>::setStepLength(double value)
+	{
+		stepLength=value;
+	}
+
+	template <class T>
+	void RRTConnect<T>::setMaxIterations(int value)
+	{
+		maxIterations=value;
+	}
+
+	template <class T>
+	bool RRTConnect<T>:: plan()
+	{
+		int count=0;
+		int check=0;
+		treeA.push_back(std::pair< Utils::Point<T>, Utils::Point<T> > (startPoint,startPoint));
+		treeB.push_back(std::pair< Utils::Point<T>, Utils::Point<T> > (endPoint,endPoint));
+
+		int start_time=clock();
+		while( check < maxIterations)
+		{
+			//std::cout<<"In Planning Loop"<<std::endl;
+			Utils::Point<T> next;
+
+
+			if (check%2 == 0)
+			{
+				mainTree = treeA;
+				supportTree = treeB;
+			}
+			else{
+				mainTree = treeB;
+				supportTree = treeA;
+			}
+
+			if(treeComplete()==true)
+			{
+				std::cout<<"Tree complete!!"<<std::endl;
+				// completeTree(treeA);
+				int end_time=clock();
+				std::cout<<"Time to generate path = "<<(float)(end_time-start_time)/CLOCKS_PER_SEC<<std::endl;
+				generatePath(startPoint,endPoint);
+				return true;
+			}
+			else if(count%biasParameter==0)
+			{
+				// std::cout<<"Adding Next Biased Point to tree!!"<<std::endl;
+
+				// count=0;
+				do{
+					if (check%2 == 0)
+					{
+						next = endPoint;
+					}
+					else{
+						next = startPoint;
+					}
+				}while(checkPoint(next)!=true);
+
+			}
+			else
+			{
+				// std::cout<<"Adding next point to tree!!"<<std::endl;
+				do{
+
+					next = generatePoint(mainTree);
+				}while(checkPoint(next)!=true);
+
+			}
+			// std::cout<<" Growing Tree next : "<<next.x<<","<<next.y<<std::endl;
+			growTree(next);
+			count++;
+			check++;
+			//std::cout<<"check= "<<check<<", count= "<<count<<std::endl;
+		}
+		std::cout<<"Path Not Found"<<std::endl;
+		return false;
+	}
+
+	template <class T>
+
+	void RRTConnect<T>::growTree(Utils::Point<T> temp)
+	{
+		//growing the tree by adding node 
+		//std::cout<<"finding parent in tree of size = "<<tree.size()<<std::endl;
+
+		Utils::Point<T> closest;
+		closest = findClosestNode(temp,treeA);
+		double theta = atan2(temp.y-closest.y,temp.x-closest.x);
+		double dis = dist(temp,closest);
+		if (dis > stepLength)
+		{
+		temp.x=closest.x+stepLength*cos(theta);
+		temp.y=closest.y+stepLength*sin(theta);
+		}
+		treeA.push_back( std::pair< Utils::Point<T>, Utils::Point<T> > (temp,closest));
+		
+		closest = findClosestNode(temp,treeB);
+		theta = atan2(temp.y - closest.y , temp.x - closest.x);
+		dis = dist(temp,closest);
+		if (dis > stepLength)
+		{
+		temp.x=closest.x+stepLength*cos(theta);
+		temp.y=closest.y+stepLength*sin(theta);
+		}
+		treeB.push_back( std::pair< Utils::Point<T>, Utils::Point<T> > (temp,closest));
+		//std::cout<<"Point Generated = "<<temp.x<<","<<temp.y<<std::endl;
+		//std::cout<<"Tree grown"<<std::endl;
+		//add pruning code to keep size of tree under control
+	}
+	
+
+	template <class T>
+	void RRTConnect<T>::setCheckPointFunction(bool (*f)(Utils::Point<T>))
+	{
+		userCheck=f;
+	}
+
+	template <class T>
+	Utils::Point<T> RRTConnect<T>::findClosestNode(Utils::Point<T> cur,std::vector< std::pair< Utils::Point<T>, Utils::Point<T> > >tree)
+	{
+		double min_dist=INT_MAX;
+		Utils::Point<T> closest;
+		for(int i=0;i<tree.size();i++)
+		{
+			////std::cout<<"Itering the tree to find closest Node"<<std::endl;
+			if(dist(tree[i].first,cur) < min_dist )
+			{
+				min_dist=dist(tree[i].first,cur);
+				closest=tree[i].first;
+			}
+		}
+		return closest;
+	}
+
+	template <class T>
+	Utils::Point<T> RRTConnect<T>::generatePoint(std::vector< std::pair< Utils::Point<T>, Utils::Point<T> > > tree)
+	{
+		Utils::Point<T> temp;
+		int signX,signY;
+
+		if(rand()%2==0)
+			signX=-1;
+		else
+			signX=1;
+		if(rand()%2==0)
+			signY=-1;
+		else 
+			signY=1;
+		temp.x = origin.x + signX* ( rand() % (int)halfDimensionX );
+		temp.y = origin.y + signY* ( rand() % (int)halfDimensionY );
+
+
+		return temp;
+		Utils::Point<T> closest=findClosestNode(temp,tree);
+		double theta = atan2(temp.y-closest.y,temp.x-closest.x);
+		double dis = dist(temp,closest);
+		if (dis < stepLength)
+		{
+			return temp;
+		}
+		Utils::Point<T> next;
+
+		next.x=closest.x+stepLength*cos(theta);
+		next.y=closest.y+stepLength*sin(theta);
+		// std::cout<<"origin,"<<origin.x<<" "<<origin.y<<" "<<temp.x<<" "<<temp.y<<" "<<next.x<<" "<<next.y<<std::endl;
+		//std::cout<<"Point Generated = "<<temp.x<<","<<temp.y<<std::endl;
+		return next;
+	}
+
+	template <class T>
+	Utils::Point<T> RRTConnect<T>::generateBiasedPoint(Utils::Point<T> endPoint,std::vector< std::pair< Utils::Point<T>, Utils::Point<T> > >tree)
+	{
+		Utils::Point<T> closest=findClosestNode(endPoint,tree);
+		double theta = atan2(endPoint.y-closest.y,endPoint.x-closest.x);
+
+		Utils::Point<T> next;
+		next.x=closest.x+stepLength*cos(theta);
+		next.y=closest.y+stepLength*sin(theta);
+		//std::cout<<"Biased Point Generated = "<<next.x<<","<<next.y<<std::endl;
+		return next;
+	}
+
+	template <class T>
+	bool RRTConnect<T>::checkPoint(Utils::Point<T> next)
+	{
+		return userCheck(next);
+	}
+
+	template <class T>
+	bool RRTConnect<T>::treeComplete()
+	{
+		std::vector< std::pair< Utils::Point<T>, Utils::Point<T> > > conn;
+		//std::cout<<"Checking if tree is complete? "<<std::endl;
+		//std::cout<<"tree size = "<<tree.size()<<std::endl;
+		int min_dist=INT_MAX;	
+		for(int i=0;i<treeA.size();i++)
+		{
+			for (int j = 0; j < treeB.size(); ++j)
+			{
+				double dis= dist(treeA[i].first,treeB[j].first);
+				if(dis < min_dist)
+				{	min_dist=dis;
+					// std::cout<<treeA.size()<<" "<<treeB.size()<<std::endl;
+				}
+				if(dis < stepLength )
+				{
+					connectA = treeA[i].first;
+					connectB = treeB[j].first;
+
+					conn.push_back( std::pair< Utils::Point<T>, Utils::Point<T> > (connectA,connectB));
+					// std::cout<<"distance = "<<dis<<" ";
+					// std::cout<<"points "<<treeA[i].first.x<<" "<<treeA[i].first.y;
+					// std::cout<<"points "<<treeB[j].first.x<<" "<<treeB[j].first.y;
+
+				}
+			}
+			
+		}
+		if (conn.size() )
+		{
+			double thetaDiffMax = 9999;
+			double thetaA,thetaB,thetaDiff,thetaMain;
+			int a  = 0;
+			for (int i = 0; i < conn.size(); ++i)
+			{
+				thetaMain = atan2(conn[i].second.y - conn[i].first.y ,conn[i].second.x - conn[i].first.x);
+				thetaA = atan2(conn[i].first.y - getParent(conn[i].first,treeA).y, conn[i].first.x - getParent(conn[i].first,treeA).x);
+				thetaB = atan2(getParent(conn[i].second,treeB).y - conn[i].second.y , getParent(conn[i].second,treeB).x - conn[i].second.x);
+				thetaDiff = abs(abs(thetaMain - thetaA) - abs(thetaB - thetaMain));
+				if (thetaDiff < thetaDiffMax)
+				{
+					thetaDiffMax = thetaDiff;
+					a=i;
+				}
+			}
+			connectA = conn[a].first;
+			connectB = conn[a].second;
+			// cout<<"Found Two Probable Joints\n";
+			// cout<<"--------"<<a<<"-------\n";
+			return true;
+		}
+		return false;
+	}
+
+	template <class T>
+	void RRTConnect<T>::setBiasParameter(unsigned int param)
+	{
+		biasParameter=param;
+	}
+
+	template <class T>
+	void RRTConnect<T>::generatePath(Utils::Point<T> first,Utils::Point<T> last)
+	{
+
+		Utils::Point<T> cur=connectA;
+		while(cur!=startPoint){
+			pathPoints.insert(pathPoints.begin(),cur);
+			cur = getParent(cur,treeA);
+		}
+
+		pathPoints.insert(pathPoints.begin(),cur);
+
+		cur = connectB;
+
+		// std::cout<<first.x<<" "<<first.y<<" "<<startPoint.x<<" "<<startPoint.y<<std::endl;
+		while(cur!=endPoint)
+		{
+			pathPoints.push_back(cur);
+			cur = getParent(cur,treeB);
+			// Utils::Point<T> parent= getParent(cur);
+			// std::cout<<"current : "<<cur.x<<","<<cur.y<<"| parent : "<<parent.x<<","<<parent.y<<std::endl;
+			// pathPoints.push_back(parent);
+			// cur=parent;
+		}
+		pathPoints.push_back(cur);
+	}
+
+	template <class T>
+	Utils::Point<T> RRTConnect<T>::getParent(Utils::Point<T> cur,std::vector< std::pair< Utils::Point<T>, Utils::Point<T> > >tree)
+	{
+		for(int i=0;i<tree.size();i++)
+		{
+			if(tree[i].first==cur)
+				return tree[i].second;
+		}
+	}
+
+	template <class T>
+	double RRTConnect<T>::dist(Utils::Point<T> a,Utils::Point<T> b)
+	{
+		return sqrt((a.x-b.x)*(a.x-b.x) + (a.y-b.y)*(a.y-b.y));
+	}
+}
+
diff --git a/src/main.cpp b/src/main.cpp
index bc9d7da..10911d4 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -1,13 +1,19 @@
 #include <iostream>
 #include "../include/RRT_implementation.hpp"
 
+
 using namespace std;
 using namespace rrt;
 
 bool check(Utils::Point<int> cur)
 {
-	if(abs(cur.x)<2000 && abs(cur.y)<2000)
+	if(abs(cur.x)<2000 && abs(cur.y)<2000){
+		// if (cur.x < 550 && cur.x > 450 && cur.y < 550 & cur.y > 450)
+		// {
+		// 	return false;
+		// }
 		return true;
+	}
 	return false;
 }
 
@@ -15,17 +21,17 @@ int main()
 {
 	srand(time(NULL));
 	Utils::Point<int> start,finish,origin;
-	start.x=-10;
-	start.y=10;
-	finish.x=1000;
-	finish.y=730;
+	start.x=30;
+	start.y=30;
+	finish.x=1500;
+	finish.y=1730;
 	origin.x=0;
 	origin.y=0;
 
-	RRT<int> test;
+	RRTConnect<int> test;
 	test.setEndPoints(start,finish);
 	test.setCheckPointFunction(*(check));
-	test.setStepLength(200);
+	test.setStepLength(50);
 	test.setHalfDimensions(2000.0,2000.0);
 	test.setBiasParameter(100);
 	test.setOrigin(origin);
@@ -33,6 +39,9 @@ int main()
 	test.plan();
 	cout<<"#################################################"<<endl;
 	deque<Utils::Point<int> > path=test.getPointsOnPath();
+
 	for(int i=0;i<path.size();i++)
-		cout<<path[i].x<<","<<path[i].y<<endl;
-}
+		cout<<"("<<path[i].x<<","<<path[i].y<<")";
+
+	// cout<<"sfd"<<connectA.x<<","<<connectA.y<<" "<<connectB.x<<","<<connectB.y;
+}	
\ No newline at end of file

From 75935a24581386269717aa0d6f40a0ab77d83e10 Mon Sep 17 00:00:00 2001
From: saurabhkgpee <shourabh.agarwal@gmail.com>
Date: Sun, 3 Sep 2017 01:20:00 +0530
Subject: [PATCH 5/6] Planner

---
 CMakeLists.txt                 |   6 +-
 include/DRRT.hpp               | 570 +++++++++++++++++++++++++++++++++
 include/RRT.hpp                |  75 ++++-
 include/RRT_implementation.hpp |  35 +-
 test/main.cpp                  |  67 ++++
 5 files changed, 723 insertions(+), 30 deletions(-)
 create mode 100644 include/DRRT.hpp
 create mode 100644 test/main.cpp

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 3334ca3..aa66045 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -4,8 +4,10 @@ set(CMAKE_CXX_FLAGS "-std=c++11")
 
 include_directories("include")
 
-set(SRCS src/Random_sampling.cpp
-  src/RT_RRT_star.cpp)
+set(SRCS src/Random_sampling.cpp)
 
 set(PLANNERS planners)
 add_library(${PLANNERS} SHARED ${SRCS})
+
+add_executable(target test/main.cpp)
+target_link_libraries(target ${PLANNERS})
diff --git a/include/DRRT.hpp b/include/DRRT.hpp
new file mode 100644
index 0000000..ef1b96a
--- /dev/null
+++ b/include/DRRT.hpp
@@ -0,0 +1,570 @@
+
+#include <bits/stdc++.h>
+#include "./RRT.hpp"
+#include "./RRT_implementation.hpp"
+#include "./utils.hpp"
+
+using namespace std;
+namespace rrt
+{
+
+	template<class T>
+	void DRRT<T>::setBucketSize(unsigned int size){
+		BucketSize = size;
+	}
+
+	template<class T>
+	void DRRT<T>::setPointsInBucket(unsigned int num){
+		PointsInBucket = num;
+	}
+
+	template <class T>
+	void DRRT<T>::generateGrid(){
+		numX = 2*halfDimensionX/BucketSize;
+		if (numX != (int)numX)
+		{
+			numX = (int)numX + 1;
+		}
+		numY = 2*halfDimensionY/BucketSize;
+		if (numY != (int)numY)
+		{
+			numY = (int)numY + 1;
+		}
+		int num = numX*numY;
+		for (int i = 0; i < num; ++i)
+		{
+			std::vector<int> v;
+			bucket.push_back(std::pair<int,std::vector<int> > (i,v));
+		}
+	}
+
+	template < class T>
+	int DRRT<T>::findBucket(Utils::Point<T> pt){
+		int x = pt.x + halfDimensionX ;
+		int y = pt.y + halfDimensionX ;
+		int numx = (int)(x/BucketSize) + 1;
+		if (numx > numX)
+		{
+			numx = numx - 1;
+		}
+		int numy = (int)(y/BucketSize) + 1;
+		if (numy > numY)
+		{
+			numy = numy - 1;
+		}
+		int num = (numy - 1)*numX + numx - 1;
+		return num;
+	}
+
+	template <class T>
+	void DRRT<T>::setEndPoints(Utils::Point<T> start, Utils::Point<T> end)
+	{
+		startPoint=start;
+		endPoint=end;
+	}
+
+	template <class T>
+	void DRRT<T>::setTimeOut(float time)
+	{
+		timeOut = time;
+	}
+
+	template <class T>
+	void DRRT<T>::setHalfDimensions(double a, double b)
+	{
+		halfDimensionX=a;
+		halfDimensionY=b;
+	}
+
+	template <class T>
+	void DRRT<T>::setOrigin(Utils::Point<T> pt)
+	{
+		origin=pt;
+	}
+
+	template <class T>
+	std::vector<Utils::Point<T> > DRRT<T>::getPointsOnPath(Utils::Point<T> goal)
+	{
+		// clear vector before use
+		pathPoints.clear();
+		Utils::Point <T> cur;
+		int numBucket = findBucket(goal);
+		float minDist = 9999;
+		int min = -1;
+
+		// checking point at minimum distance from goal
+		// in goal's bucket, 
+		// If any change......  do here
+		for (int i = 0; i < bucket[numBucket].second.size(); ++i)
+		{
+			int id = bucket[numBucket].second[i];
+			float Dist = dist(goal,tree[id].first);
+			if (Dist < minDist)
+			{
+				minDist = Dist;
+				min = id;
+			}
+		}
+		if (goal != tree[min].first)
+		{
+			pathPoints.push_back(goal);
+		}
+		cur = tree[min].first;
+		while(cur != getParent(cur)){
+			pathPoints.push_back(cur);
+			cur = getParent(cur);
+		}
+		pathPoints.push_back(cur);
+		return pathPoints;
+	}
+
+	template <class T>
+	std::vector<Utils::Point<T> > DRRT<T>::getPath(Utils::Point<T> start,Utils::Point<T> goal){
+		reWireRoot(start);
+		std::vector<Utils::Point<T> > path;
+		cout<<"Current Root"<<root.x<<" "<<root.y<<endl;
+		path = getPointsOnPath(goal);
+		if (path[path.size() - 1] != start)
+		{
+			path.push_back(start);
+		}
+		return path;
+	}
+
+	template <class T>
+	void DRRT<T>::setStepLength(double length)
+	{
+		stepLength=length;
+	}
+
+	template <class T>
+	void DRRT<T>::setMaxIterations(int maxIt)
+	{
+		maxIterations=maxIt;
+	}
+
+	
+	template<class T>
+	bool DRRT<T>::saturation(){
+		for (int i = 0; i < bucket.size(); ++i)
+		{
+			if (bucket[i].second.size() < PointsInBucket)
+			{
+				return true;
+			}
+		}
+
+		return false;
+	}
+
+	template<class T>
+	bool DRRT<T>::dense(Utils::Point<T> pt){
+		int numBucket = findBucket(pt);
+		int density = bucket[numBucket].second.size();
+		if (density > PointsInBucket)
+		{
+			return false;
+		}
+		return true;
+
+	}
+
+	template <class T>
+	bool DRRT<T>::collision(Utils::Point<T> pt){
+		for (int i = 0; i < obstacles.size(); ++i)
+		{
+			std::vector<int> obstacleBucket;
+			Utils::Point<T> ob = obstacles[i].first;
+			int radius = obstacles[i].second;
+			int numbucket = findBucket(ob);
+			obstacleBucket.push_back(numbucket);
+			if (numbucket%(int)numX == 0)
+			{
+				obstacleBucket.push_back(numbucket + 1);
+			}
+			else if(numbucket % (int)numX == (int)numX - 1)
+				obstacleBucket.push_back(numbucket - 1);
+			else{
+				obstacleBucket.push_back(numbucket + 1);
+				obstacleBucket.push_back(numbucket - 1);
+			}
+			
+			if (numbucket/(int)numX == 0)
+			{
+				obstacleBucket.push_back(numbucket + (int)numX);	
+			}
+
+			else if(numbucket/(int)numX == (int)numY - 1){
+				obstacleBucket.push_back(numbucket - (int)numX);
+			}
+
+			else{
+				obstacleBucket.push_back(numbucket + (int)numX);
+				obstacleBucket.push_back(numbucket - (int)numX);
+			}
+
+			for (int n = 0; n < obstacleBucket.size(); ++n)
+			{
+				int numBucket = obstacleBucket[n];
+				for (int j = 0; j < bucket[numBucket].second.size(); ++j)
+				{
+					int id = bucket[numBucket].second[j];
+					Utils::Point<T> node = tree[id].first;
+					if (dist(pt,node) < radius)
+					{
+						cout<<"Obstacle\n";
+						return false;
+					}
+				}
+			}
+		}
+		
+
+		return true;
+	}
+
+	template <class T>
+	bool DRRT<T>:: plan()
+	{
+		// unordered_map<Utils:Point<T>, float > costMap;
+		cout<<"Planning\n";
+		int count=0;
+		int check=0;
+		int numBucket = findBucket(startPoint);
+		bucket[numBucket].second.push_back(tree.size());		
+		//storing index of node in bucket
+		tree.push_back(std::pair< Utils::Point<T>, Utils::Point<T> > (startPoint,startPoint));
+		root = startPoint;
+		Utils::Point<T> next;
+		float start_time = clock();
+		while( check < maxIterations && saturation())
+		{
+			do{
+				next = generatePoint();
+			}while(checkPoint(next)==false || dense(next)==false);
+			int numBucket = findBucket(next);
+			/*storing index of node in bucket*/
+			bucket[numBucket].second.push_back(tree.size());	
+			growTree(next);
+			check ++;
+			
+		}
+		float end_time = clock();
+
+		
+		cout<<tree.size();
+		std::cout<<"\nTime to generate path = "<<(float)(end_time-start_time)/CLOCKS_PER_SEC<<std::endl;
+
+		// for (int i = 0; i < tree.size(); ++i)
+		// {
+		// 	cout<<"("<<tree[i].first.x<<","<<tree[i].first.y<<")";
+		// }
+		cout<<endl;
+		return true;
+
+
+	}
+
+	template <class T>
+
+	void DRRT<T>::growTree(Utils::Point<T> temp)
+	{
+		Utils::Point<T> parent;
+		parent = findClosestNode(temp);
+		tree.push_back(std::pair< Utils::Point<T>,Utils::Point<T> >(temp,parent));
+	}
+	
+
+	template <class T>
+	void DRRT<T>::setCheckPointFunction(bool (*f)(Utils::Point<T>))
+	{
+		userCheck=f;
+	}
+
+	template <class T>
+	Utils::Point<T> DRRT<T>::findClosestNode(Utils::Point<T> cur)
+	{
+		double min_dist=INT_MAX;
+		Utils::Point<T> closest;
+		for(int i=0;i<tree.size();i++)
+		{
+			if(dist(tree[i].first,cur) < min_dist )
+			{
+				min_dist=dist(tree[i].first,cur);
+				closest=tree[i].first;
+			}
+		}
+		return closest;
+	}
+
+	template <class T>
+	Utils::Point<T> DRRT<T>::generatePoint()
+	{
+		Utils::Point<T> temp;
+		int signX,signY;
+
+		if(rand()%2==0)
+			signX=-1;
+		else
+			signX=1;
+		if(rand()%2==0)
+			signY=-1;
+		else 
+			signY=1;
+		temp.x = origin.x + signX* ( rand() % (int)halfDimensionX );
+		temp.y = origin.y + signY* ( rand() % (int)halfDimensionY );
+
+
+		Utils::Point<T> closest=findClosestNode(temp);
+		double theta = atan2(temp.y-closest.y,temp.x-closest.x);
+		double dis = dist(temp,closest);
+		if (dis < stepLength)
+		{
+			return temp;
+		}
+		Utils::Point<T> next;
+
+		next.x=closest.x+stepLength*cos(theta);
+		next.y=closest.y+stepLength*sin(theta);
+		return next;
+	}
+
+
+	template <class T>
+	bool DRRT<T>::checkPoint(Utils::Point<T> next)
+	{
+		return userCheck(next);
+	}
+
+	template <class T>
+	float DRRT<T>::cost(Utils::Point <T> point){
+
+
+		Utils::Point <T> parent;
+		parent = getParent(point);
+		if (point == parent)
+		{
+			if (point == root)
+			{
+				return 0;
+			}
+			return 9999;
+		}
+
+		else{
+			float costValue;
+			costValue = cost(parent) + dist(parent,point);
+			// costMap[point] = costValue;
+			return costValue;
+		}
+	}
+
+	template<class T>
+	std::vector< Utils::Point<T> > DRRT<T>::getChild(Utils::Point<T> point){
+		std::vector< Utils::Point<T> > child;
+		int numBucket = findBucket(point);
+		std::vector<int> list;
+		list.push_back(numBucket);
+
+		// only searching one layer around newRoot
+		if (numBucket%(int)numX == 0)
+		{
+			list.push_back(numBucket + 1);
+		}
+		else if(numBucket % (int)numX == (int)numX - 1)
+			list.push_back(numBucket - 1);
+		else{
+			list.push_back(numBucket + 1);
+			list.push_back(numBucket - 1);
+		}
+		
+		if (numBucket/(int)numX == 0)
+		{
+			list.push_back(numBucket + (int)numX);	
+		}
+
+		else if(numBucket/(int)numX == (int)numY - 1){
+			list.push_back(numBucket - (int)numX);
+		}
+
+		else{
+			list.push_back(numBucket + (int)numX);
+			list.push_back(numBucket - (int)numX);
+		}
+
+		for (int i = 0; i < list.size(); ++i)
+		{
+			int numBucket = list[i];
+			for (int j = 0; j < bucket[numBucket].second.size(); ++j)
+			{
+				int id = bucket[numBucket].second[j];
+				if (tree[id].second == point)
+				{
+					child.push_back(tree[id].first);
+				}
+			}
+		}
+
+		return child;
+	};
+
+
+	template <class T>
+	Utils::Point<T> DRRT<T>::getParent(Utils::Point<T> cur)
+	{
+		int numBucket = findBucket(cur);
+		// iterate over cur point bucket
+		for(int i=0; i<bucket[numBucket].second.size() ;i++)
+		{
+			int id = bucket[numBucket].second[i];
+			if(tree[id].first==cur){
+				return tree[id].second;
+			}
+		}
+	}
+
+	template <class T>
+	double DRRT<T>::dist(Utils::Point<T> a,Utils::Point<T> b)
+	{
+		return sqrt((a.x-b.x)*(a.x-b.x) + (a.y-b.y)*(a.y-b.y));
+	}
+
+
+	template<class T>
+	void DRRT<T>::reWireRoot(Utils::Point <T> newRoot){
+		// unordered_map<Utils::Point<T>, float> costMap;
+		std::vector<Utils::Point<T> > path = getPointsOnPath(newRoot);
+
+		std::vector<int> ids;
+		for (int i = 0; i < path.size(); ++i)
+		{
+			int numBucket = findBucket(path[i]);
+			for (int j = 0; j < bucket[numBucket].second.size(); ++j)
+			{	
+				int id = bucket[numBucket].second[j]; 
+				if (path[i] ==tree[id].first)
+				{
+					ids.push_back(id);
+					break;
+				}
+			}
+		}
+		if (path.size() != ids.size())
+		{
+			path.erase(path.begin());
+		}
+		cout<<"\n"<<path.size()<<","<<ids.size();
+		tree[ids[0]] = pair< Utils::Point<T> , Utils::Point<T> > (path[0],path[0]);
+		root = path[0];
+		for (int i = 1; i < ids.size(); ++i)
+		{
+			int id = ids[i];
+			tree[id] = std::pair< Utils::Point<T>, Utils::Point<T> > (path[i],path[i - 1]);
+		}
+
+		// New Root of tree is "nearest point" to given point if point is not in tree
+		// std::vector<Utils::Point<T> > temppath = getPointsOnPath(startPoint);
+
+		std::vector<Utils::Point<T> > nodes;
+		nodes.push_back(path[0]);
+
+		float start = clock();
+		int ptr = 0;
+		cout<<"\n";
+		while(((clock() - start)/CLOCKS_PER_SEC) < timeOut && ptr < nodes.size())
+		{
+			int numBucket = findBucket(nodes[ptr]);
+			
+			int myId = -1;
+			for (int i = 0; i < bucket[numBucket].second.size(); ++i)
+			{
+				int id = bucket[numBucket].second[i];
+				if (tree[id].first == nodes[ptr])
+				{
+					myId = id;
+					break;
+				}
+			}
+
+			std::vector<int> list;		//buckets adjacent to rewiring point
+			std::vector<int> ids;		//id of nodes in adjacent buckets
+			list.push_back(numBucket);
+
+			// only searching one layer around newRoot
+			if (numBucket%(int)numX == 0)
+			{
+				list.push_back(numBucket + 1);
+			}
+			else if(numBucket % (int)numX == (int)numX - 1)
+				list.push_back(numBucket - 1);
+			else{
+				list.push_back(numBucket + 1);
+				list.push_back(numBucket - 1);
+			}
+			
+			if (numBucket/(int)numX == 0)
+			{
+				list.push_back(numBucket + (int)numX);	
+			}
+
+			else if(numBucket/(int)numX == (int)numY - 1){
+				list.push_back(numBucket - (int)numX);
+			}
+
+			else{
+				list.push_back(numBucket + (int)numX);
+				list.push_back(numBucket - (int)numX);
+			}
+
+			//list contains bucket id for all buckets surrounding newroot
+			for (int i = 0; i < list.size(); ++i)
+			{
+				int numBucket = list[i];
+				for (int j = 0; j < bucket[numBucket].second.size(); ++j)
+				{
+					int id = bucket[numBucket].second[j];
+					if (id != myId)
+					{
+						ids.push_back(id);
+					}
+				}
+			}
+
+			float score = cost(nodes[ptr]);
+
+
+			for (int i = 0; i < ids.size(); ++i)
+			{
+				int id = ids[i];
+				if (dist(tree[id].first,tree[myId].first) < stepLength)
+				{
+					if ((cost(tree[id].first) + dist(tree[id].first, tree[myId].first)) < score)
+					{
+						tree[myId] = pair< Utils::Point<T>, Utils::Point<T> > (tree[myId].first, tree[id].first);
+						score = cost(tree[id].first) + dist(tree[id].first, tree[myId].first);
+					}
+				}
+				
+
+				bool flag = false;
+				for (int j = 0; j < nodes.size(); ++j)
+				{
+					if (nodes[j] == tree[id].first)
+					{
+						flag = true;
+						break;
+					}
+				}
+
+				if (!flag)
+				{
+					nodes.push_back(tree[id].first);
+				}
+			}
+			ptr++;
+		}
+		cout<<ptr;
+	}
+
+}
+
diff --git a/include/RRT.hpp b/include/RRT.hpp
index c542273..09917c9 100644
--- a/include/RRT.hpp
+++ b/include/RRT.hpp
@@ -3,6 +3,7 @@
 
 #include <iostream>
 #include <bits/stdc++.h>
+// #include <unordered_map>
 #include "utils.hpp"
 
 namespace rrt
@@ -17,7 +18,7 @@ namespace rrt
 		Utils::Point<T> startPoint;
 		Utils::Point<T> endPoint;
 		double stepLength;
-		std::deque<Utils::Point<T> > pathPoints;
+		std::vector<Utils::Point<T> > pathPoints;
 		int maxIterations;
 		std::vector< std::pair< Utils::Point<T>, Utils::Point<T> > > tree; 
 		std::vector< std::pair< Utils::Point<T>, Utils::Point<T> > > tree1; 
@@ -35,7 +36,7 @@ namespace rrt
 		}
 		
 		virtual bool plan();
-		virtual std::deque<Utils::Point<T> > getPointsOnPath();
+		virtual std::vector<Utils::Point<T> > getPointsOnPath();
 
 		virtual void setEndPoints(Utils::Point<T> start, Utils::Point<T> end);
 		virtual void setCheckPointFunction(bool (*f)(Utils::Point<T>));
@@ -53,11 +54,11 @@ namespace rrt
 		bool (*userCheck)(Utils::Point<T>);
 		bool checkPoint(Utils::Point<T> pt);
 		Utils::Point<T> generatePoint();
-		Utils::Point<T> generateBiasedPoint(int);
+		Utils::Point<T> generateBiasedPoint();
 		void growTree(Utils::Point<T>);
-		std::pair<Utils::Point<T>, int>  findClosestNode(Utils::Point<T>);
+		Utils::Point<T> findClosestNode(Utils::Point<T>);
 		Utils::Point<T> getParent(Utils::Point<T>);
-		std::pair <Utils::Point <T>,Utils::Point <T> > treeComplete(int*);
+		bool treeComplete();
 		void generatePath(Utils::Point<T> first,Utils::Point<T> last);
 		double dist(Utils::Point<T> a,Utils::Point<T> b);
 	};
@@ -104,13 +105,67 @@ namespace rrt
 		void generatePath(Utils::Point<T> first,Utils::Point<T> last);
 		double dist(Utils::Point<T> a,Utils::Point<T> b);
 	};
+
+
+	template <class T>
+	class DRRT: public RRT<T>{
+	private:
+		std::vector<int> obstacleBucket;
+		std::vector<std::pair< int, std::vector <int> > > bucket;
+		float numX;
+		float numY;
+		double halfDimensionX;
+		double halfDimensionY;
+		Utils::Point<T> origin;
+		Utils::Point<T> startPoint;
+		Utils::Point<T> endPoint;
+		double stepLength;
+		std::vector<Utils::Point<T> > pathPoints;
+		int maxIterations;
+		std::vector< std::pair< Utils::Point<T>, Utils::Point<T> > > tree; 
+		unsigned int biasParameter;
+		unsigned int BucketSize;
+		unsigned int PointsInBucket;
+		float timeOut;
+		Utils::Point<T> root;
+
+	public:
+		bool plan();
+		std::vector<Utils::Point<T> > getPath(Utils::Point<T>,Utils::Point<T>);
+		std::vector<Utils::Point<T> > getPointsOnPath(Utils::Point<T>);
+		std::vector<std::pair<Utils::Point<T>, int > > obstacles;
+		void setEndPoints(Utils::Point<T> start, Utils::Point<T> end);
+		void setCheckPointFunction(bool (*f)(Utils::Point<T>));
+		void setStepLength(double value);
+		void setOrigin(Utils::Point<T> origin);
+		void setHalfDimensions(double x,double y);
+		void setMaxIterations(int);
+		void setBucketSize(unsigned int);
+		void setPointsInBucket(unsigned int);
+		void generateGrid();
+		void setTimeOut(float time);
+
+	
+	private:
+		void reWireRoot(Utils::Point<T> point);
+		int findBucket(Utils::Point<T>);
+		bool saturation();
+		bool collision(Utils::Point<T>);
+		bool dense(Utils::Point<T>);
+		bool (*userCheck)(Utils::Point<T>);
+		bool checkPoint(Utils::Point<T> pt);
+		Utils::Point<T> generatePoint();
+		Utils::Point<T> findClosestNode(Utils::Point<T>);
+		void growTree(Utils::Point<T>);
+		Utils::Point<T> getParent(Utils::Point<T>);
+		bool treeComplete();
+		void generatePath(Utils::Point<T> first,Utils::Point<T> last);
+		double dist(Utils::Point<T> a,Utils::Point<T> b);
+		float cost(Utils::Point<T> point);
+		std::vector< Utils::Point<T> > getChild(Utils::Point<T> point);
+	};
 }
 
-//************* ToDo ************* 
-// Implement derived classes for variants of RRT
-// Optimize the generate path and other 
-// Tweak with the parameters to check their effects on runtime and path 
-// Implement Pruning function to keep a check on size of tree
 
 
 
diff --git a/include/RRT_implementation.hpp b/include/RRT_implementation.hpp
index 5e9bbd9..7220afa 100644
--- a/include/RRT_implementation.hpp
+++ b/include/RRT_implementation.hpp
@@ -25,7 +25,7 @@ namespace rrt
 	}
 
 	template <class T>
-	std::deque<Utils::Point<T> > RRT<T>::getPointsOnPath()
+	std::vector<Utils::Point<T> > RRT<T>::getPointsOnPath()
 	{
 		return pathPoints;
 	}
@@ -52,9 +52,7 @@ namespace rrt
 		{
 			//std::cout<<"In Planning Loop"<<std::endl;
 			Utils::Point<T> next;
-			int arr[]={1};
-			std::pair  <Utils::Point <T>,Utils::Point <T> >  mid = treeComplete(arr);
-			if(mid.first!=mid.second)
+			if(treeComplete()==true)
 			{
 				std::cout<<"Tree complete!!"<<std::endl;
 				int start_time=clock();
@@ -69,7 +67,7 @@ namespace rrt
 				//std::cout<<"Adding Next Biased Point to tree!!"<<std::endl;
 				count=0;
 				do{
-					next= generateBiasedPoint(1);
+					next= generateBiasedPoint();
 				}while(checkPoint(next)!=true);
 				//std::cout<<" : "<<next.x<<","<<next.y<<std::endl;
 			}
@@ -94,11 +92,11 @@ namespace rrt
 	template <class T>
 	void RRT<T>::growTree(Utils::Point<T> next)
 	{
-		//growing the tree by adding node
+		//growing the tree by adding node 
 		//std::cout<<"finding parent in tree of size = "<<tree.size()<<std::endl;
 		Utils::Point<T> parent;
-		parent= findClosestNode(next).first;
-		//std::cout<<"current : "<<next.x<<","<<next.y<<"| parent : "<<parent.x<<","<<parent.y<<std::endl;
+		parent= findClosestNode(next);
+		//std::cout<<"current : "<<next.x<<","<<next.y<<"| parent : "<<parent.x<<","<<parent.y<<std::endl;			
 		tree.push_back( std::pair< Utils::Point<T>, Utils::Point<T> > (next,parent));
 		//std::cout<<"Tree grown"<<std::endl;
 		//add pruning code to keep size of tree under control
@@ -111,7 +109,7 @@ namespace rrt
 	}
 
 	template <class T>
-	std::pair<Utils::Point<T>, int>  RRT<T>::findClosestNode(Utils::Point<T> cur)
+	Utils::Point<T> RRT<T>::findClosestNode(Utils::Point<T> cur)
 	{
 		double min_dist=INT_MAX;
 		Utils::Point<T> closest;
@@ -124,7 +122,7 @@ namespace rrt
 				closest=tree[i].first;
 			}
 		}
-		return std::make_pair(closest,1);
+		return closest;
 	}
 
 	template <class T>
@@ -139,13 +137,13 @@ namespace rrt
 			signX=1;
 		if(rand()%2==0)
 			signY=-1;
-		else
+		else 
 			signY=1;
 
 		temp.x = origin.x + signX* ( rand() % (int)halfDimensionX );
 		temp.y = origin.y + signY* ( rand() % (int)halfDimensionY );
 
-		Utils::Point<T> closest=findClosestNode(temp).first;
+		Utils::Point<T> closest=findClosestNode(temp);
 		double theta = atan2(temp.y-closest.y,temp.x-closest.x);
 
 		Utils::Point<T> next;
@@ -156,9 +154,9 @@ namespace rrt
 	}
 
 	template <class T>
-	Utils::Point<T> RRT<T>::generateBiasedPoint(int which)
+	Utils::Point<T> RRT<T>::generateBiasedPoint()
 	{
-		Utils::Point<T> closest=findClosestNode(endPoint).first;
+		Utils::Point<T> closest=findClosestNode(endPoint);
 		double theta = atan2(endPoint.y-closest.y,endPoint.x-closest.x);
 
 		Utils::Point<T> next;
@@ -175,11 +173,11 @@ namespace rrt
 	}
 
 	template <class T>
-	std::pair <Utils::Point <T>,Utils::Point <T> > RRT<T>::treeComplete(int arr[1])
+	bool RRT<T>::treeComplete()
 	{
 		//std::cout<<"Checking if tree is complete? "<<std::endl;
 		//std::cout<<"tree size = "<<tree.size()<<std::endl;
-		int min_dist=INT_MAX;
+		int min_dist=INT_MAX;	
 		for(int i=0;i<tree.size();i++)
 		{
 			double dis= dist(tree[i].first,endPoint);
@@ -187,11 +185,11 @@ namespace rrt
 				min_dist=dis;
 			if(dis < stepLength )
 			{
-				return std::make_pair(tree[i].first,endPoint);
+				return true;
 			}
 		}
 		//std::cout<<"Min Distance In this iteration = "<<min_dist<<std::endl;
-		return std::make_pair(endPoint,endPoint);
+		return false;
 	}
 
 	template <class T>
@@ -230,3 +228,4 @@ namespace rrt
 		return sqrt((a.x-b.x)*(a.x-b.x) + (a.y-b.y)*(a.y-b.y));
 	}
 }
+
diff --git a/test/main.cpp b/test/main.cpp
new file mode 100644
index 0000000..3d21239
--- /dev/null
+++ b/test/main.cpp
@@ -0,0 +1,67 @@
+#include <iostream>
+#include "./../include/DRRT.hpp"
+using namespace std;
+using namespace rrt;
+
+bool check(Utils::Point<int> cur)
+{
+	if(abs(cur.x)<2000 && abs(cur.y)<2000){
+		Utils::Point<int> a,b;
+		a.x = 530;
+		a.y = 570;
+		b = cur;
+		double dist = sqrt((a.x-b.x)*(a.x-b.x) + (a.y-b.y)*(a.y-b.y));
+		return true;
+	}
+	cout<<"False check";
+	return false;
+}
+
+int main()
+{
+	srand(time(NULL));
+	Utils::Point<int> start,finish,origin,checkpt;
+	start.x=30;
+	start.y=30;
+	finish.x=1500;
+	finish.y=1730;
+	origin.x=0;
+	origin.y=0;
+	checkpt.x = 400;
+	checkpt.y = 0;
+
+	// steplength << bucketsize (finding child using bucket)
+	DRRT<int> test;
+	test.setEndPoints(start,finish);
+	test.setCheckPointFunction(*(check));
+	test.setStepLength(50);
+	test.setHalfDimensions(2000.0,2000.0);
+	test.setBucketSize(100);
+	test.setPointsInBucket(3);
+	test.setBiasParameter(100);
+	test.setOrigin(origin);
+	test.setMaxIterations(50000);
+	test.generateGrid();
+	test.setTimeOut(0.010);
+	Utils::Point<int> ob;
+	ob.x = 200;
+	ob.y = 0;
+
+	
+	test.plan();
+	vector<Utils::Point<int> > path;
+
+	float starttime = clock();
+	path =test.getPath(start,checkpt);
+	float endtime = clock();
+
+	cout<<"Time taken = "<<(endtime - starttime)/CLOCKS_PER_SEC;
+	cout<<"\n"<<path.size();
+	cout<<"###########################IN Main######################"<<endl;
+	for(int i=0;i<path.size();i++)
+		cout<<"("<<path[i].x<<","<<path[i].y<<")";
+
+	cout<<endl<<endl;
+
+	
+}	
\ No newline at end of file

From 5180125b05cd76d8f15f2ecf6b71e5df2c763067 Mon Sep 17 00:00:00 2001
From: saurabhkgpee <shourabh.agarwal@gmail.com>
Date: Sun, 3 Sep 2017 14:25:59 +0530
Subject: [PATCH 6/6] Updated
 Grid_Id,cost,rewire_node,rewire_root,closest_node,getParent

---
 CMakeLists.txt          |   3 +-
 include/RT_RRT_star.hpp | 202 ++++++++++++++++++++++++++++++++--------
 src/RT_RRT_star.cpp     |   5 +-
 3 files changed, 164 insertions(+), 46 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index aa66045..412c10b 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -4,7 +4,8 @@ set(CMAKE_CXX_FLAGS "-std=c++11")
 
 include_directories("include")
 
-set(SRCS src/Random_sampling.cpp)
+set(SRCS src/Random_sampling.cpp
+		src/RT_RRT_star.cpp)
 
 set(PLANNERS planners)
 add_library(${PLANNERS} SHARED ${SRCS})
diff --git a/include/RT_RRT_star.hpp b/include/RT_RRT_star.hpp
index 74927a5..f09dc3f 100644
--- a/include/RT_RRT_star.hpp
+++ b/include/RT_RRT_star.hpp
@@ -17,8 +17,8 @@ namespace rrt {
     // Queue at the root of the tree
     std::queue<std::pair<Utils::Point<T>, Utils::Point<T> > > Qs;
 
-    // Grid based subset of nodes for grid based search
-    std::map<std::pair<unsigned int, unsigned int> , std::vector<Utils::Point<T> > > grid_nodes;
+    // Grid based subset of nodes for grid based search, value === id of point in main tree
+    std::map<std::pair<unsigned int, unsigned int> , std::vector<int > > grid_nodes;
 
     // Position of the agent
     Utils::Point<T> Xa;
@@ -50,6 +50,11 @@ namespace rrt {
     // Useful parameters for setting some search parameters
     unsigned int width, length;
 
+    //half Dimension
+    unsigned int halfDimensionX, halfDimensionY;
+
+    unsigned int bucketSize;
+
   public:
     RT_RRT();
 
@@ -66,25 +71,39 @@ namespace rrt {
 
     /** @brief Return Grid Id
      */
-    std::pair<unsigned int, unsigned int> Grid_Id(Utils::Point<T> gride_idx);
+    std::pair<unsigned int, unsigned int> Grid_Id(Utils::Point<T> gride_idx){
+      int x = gride_idx.x + halfDimensionX;
+      int y = gride_idx.y + halfDimensionY;
+
+      unsigned int gridX = x/bucketSize;
+      unsigned int gridY = y/bucketSize;
+
+      unsigned int maxGridInX = 2*halfDimensionX/bucketSize;
+      unsigned int maxGridInY = 2*halfDimensionY/bucketSize;
+
+      if (gridX == maxGridInX)
+      {
+        gridX --;
+      }
+
+      if (gridY == maxGridInY)
+      {
+        gridY --;
+      }
+
+      return std::pair<unsigned int, unsigned int > (gridX, gridY);
+    }
 
     /** @brief Return cost
      */
-    std::pair<int,Utils::point<T> > cost(Utils::point<T> child, int count=0)
+    std::pair<double,Utils::Point<T> > cost(Utils::Point<T> child)
     {
-              if (child==Xa)
-                return std::pair<int,Utils::point<T> > (0,Xa);
-              for(int j=0;i<tree.size();j++)
-              {
-                if(tree[j].first==child)
-                {
-                  std::pair<int,Utils::point<T> > here = cost(tree[j].second,1);
-                  if (count)
-                    return std::pair<int,Utils::point<T> >  (dist(child,tree[j].second)+here.first.first,here.second);
-                  else
-                    return std::pair<int,Utils::point<T> >  (dist(child,tree[j].second)+here.first.first,tree[j].second);
-                }
-              }
+        if (child==Xa)
+          return std::pair<int,Utils::Point<T> > (0,Xa);
+        else{
+          Utils::Point<T> parent = getParent(child);
+          return std::pair<int, Utils::Point<T> >(cost(parent).first + dist(child,parent), parent);
+       }
     }
 
     /** @brief Simultaneously expand and rewire the tree
@@ -103,22 +122,41 @@ namespace rrt {
      */
     void rewire_node(std::queue<std::pair<Utils::Point<T>, Utils::Point<T> > > Qr)
     {
-      Utils::Point<T> me = Qr.pop();
-      std::pair<int,Utils::Point<T>> now = getCost(me);
-      int cost = now.first;
-      Utils::Point<T> parent = now.second;
-      std::vector<Utils::Point<T> > neighbours= find_near_nodes(Qr);
-      for (int i=0;i<neighbours.size();i++)
-      {
-        Utils::Point<T> neighbour = neighbours[i];
-        if (cost > cost - dist(me,parent) + dist(parent,neighbour) + dist(neighbour,me))
+      float start = clock();
+      float timeLimit;
+      while(clock() - start < timeLimit && !Qr.empty()){
+        Utils::Point<T> cur = Qr.front();
+        std::pair<int,Utils::Point<T>> now = cost(cur);
+        double myScore = now.first;
+        Utils::Point<T> parent = now.second;
+        std::vector<Utils::Point<T> > neighbours= find_near_nodes(cur);
+
+        int myId;
+        std::pair<unsigned int, unsigned int> gridId = Grid_Id(cur);
+        std::vector<int> ids = grid_nodes[gridId];
+        for (int i = 0; i < ids.size(); ++i)
+        {
+          if (tree[ids[i]].first == cur)
+          {
+            myId = ids[i];
+            break;
+          }
+        }
+
+        for (int i=0;i<neighbours.size();i++)
         {
-              for(int j=0;i<tree.size();j++)
-              {
-                if(tree[j].first==me)
-                  tree[j].second = neighbour[i];
-              }
+          Utils::Point<T> neighbour = neighbours[i];
+          if (cost(neighbour).first + dist(neighbour,cur) < myScore)
+          {
+            myScore = cost(neighbour).first + dist(neighbour,cur);
+            tree[myId].second = neighbour;
+          }
+          if (find(Qr.front(), Qr.back() + 1 , neighbour) != Qr.back() + 1)
+          {
+            Qr.push(neighbour);
+          }
         }
+        Qr.pop();
       }
     }
 
@@ -126,19 +164,48 @@ namespace rrt {
     */
     void rewire_root(std::queue<std::pair<Utils::Point<T>, Utils::Point<T> > > Qs)
     {
-      std::vector<Utils::Point<T> > neighbours= find_near_nodes(Xa);
-      for (int i=0;i<neighbours.size();i++)
+
+      if (Qs.empty())
       {
-        Utils::Point<T> neighbour = neighbours[i];
-        if (cost > cost - dist(me,Xa) + dist(Xa,neighbour) + dist(neighbour,me))
+        Qs.push(Xa);
+      }
+
+      float start = clock();
+
+
+      float timeLimit; //to declare by calling file
+
+      while(!Qs.empty() && (clock() - start) > timeLimit){
+        Utils::Point<T> cur = Qs.front();
+        std::vector<Utils::Point<T> > neighbours= find_near_nodes(cur);
+        float myScore = cost(cur).first;
+        std::pair<unsigned int, unsigned int> gridID = Grid_Id(cur);
+        int myId;
+        for (int i = 0; i < grid_nodes[gridID].size(); ++i)
         {
-              for(int j=0;i<tree.size();j++)
-              {
-                if(tree[j].first==neighbour[i])
-                  tree[j].second = Xa;
-              }
+          if (tree[i].first == cur)
+          {
+            myId = i;
+            break;
+          }
         }
+        for (int i=0;i<neighbours.size();i++)
+        {
+          Utils::Point<T> neighbour = neighbours[i];
+          float nbScore = cost(neighbour).first;
+          if (nbScore + dist(neighbour,cur) < myScore)
+          {
+            myScore = nbScore;
+            tree[myId].second = neighbour;
+          }
+          if (find(Qs.front(), Qs.back() + 1, neighbour) != Qs.back() +1)
+          {
+            Qs.push(neighbour);
+          }
+        }
+        Qs.pop();
       }
+      
     }
 
     /** @brief Update the radius of neareset nodes prior to calling `find_near_nodes`
@@ -147,11 +214,64 @@ namespace rrt {
 
     /** @brief Find the closest node
     */
-    Utils::Point<T> closest_node(Utils::Point<T> rand);
+    Utils::Point<T> closest_node(Utils::Point<T> rand){
+      Utils::Point<T> closest;
+
+      std::pair<unsigned int, unsigned int> gridId = Grid_Id(rand);
+      unsigned int maxGridInX = 2*halfDimensionX/bucketSize;
+      unsigned int maxGridInY = 2*halfDimensionY/bucketSize;
+      std::vector<std::pair<unsigned int, unsigned int> > adjacentGrid;
+      adjacentGrid.push_back(gridId);
+      int x[] = {-1,-1,-1,0,0,1,1,1};
+      int y[] = {-1,0,1,-1,1,-1,0,1};
+      for (int i = 0; i < 8; ++i)
+      {
+        unsigned int gridX = gridId.first + x[i];
+        unsigned int gridY = gridId.second + y[i];
+        if (gridX >= 0 && gridX < maxGridInX && gridY >=0 && gridY < maxGridInY)
+        {
+          adjacentGrid.push_back(std::pair<unsigned int, unsigned int > (gridX,gridY));
+        }
+      }
+      float minDist = 999999;
+      for (int i = 0; i < adjacentGrid.size(); ++i)
+      {
+        std::vector<int> ids = grid_nodes[adjacentGrid[i]];
+        for (int j = 0; j < ids.size(); ++j)
+        {
+          float distance = dist(rand,tree[ids[j]].first);
+          if (distance < minDist)
+          {
+            minDist = distance;
+            closest = tree[ids[j]].first;
+          }
+        }
+      }
+
+      return closest;
+    }
 
     /** @brief Verify if the line crosses any obstacle
     */
     bool line_path_obs(Utils::Point<T> p1, Utils::Point<T> p2);
+
+    
+    /**@brief Return Parent of Node
+    */
+    Utils::Point<T> getParent(Utils::Point<T> cur)
+    {
+      std::pair<unsigned int, unsigned int> gridId= Grid_Id(cur);
+      std::vector<int> ids = grid_nodes[gridId];
+      // iterate over cur point bucket
+      for(int i=0; i<ids.size() ;i++)
+      {
+        if(tree[ids[i]].first==cur){
+          return tree[ids[i]].second;
+        }
+      }
+    }
+
+
   };
 
 }
diff --git a/src/RT_RRT_star.cpp b/src/RT_RRT_star.cpp
index 8a6cb42..9358c98 100644
--- a/src/RT_RRT_star.cpp
+++ b/src/RT_RRT_star.cpp
@@ -53,10 +53,7 @@ namespace rrt {
     return nearest_points;
   }
 
-  template <class T>
-  std::pair<unsigned int, unsigned int> RT_RRT<T>::Grid_Id(Utils::Point<T> node) {
-
-  }
+  
 
   template <class T>
   void RT_RRT<T>::add_node_to_tree(Utils::Point<T> rand) {