diff --git a/include/RRT.hpp b/include/RRT.hpp
index 70c728d..85e1b19 100644
--- a/include/RRT.hpp
+++ b/include/RRT.hpp
@@ -17,7 +17,10 @@ namespace rrt
 		Utils::Point<T> startPoint;
 		Utils::Point<T> endPoint;
 		double stepLength;
+		double obstacleradius;
 		std::deque<Utils::Point<T> > pathPoints;
+		std::vector<Utils::Point<T> > finalPath;
+		std::vector<Utils::Point<T> > ObstaclePoints;
 		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 +38,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>));
@@ -44,6 +47,7 @@ namespace rrt
 		virtual void setHalfDimensions(double x,double y);
 		virtual void setBiasParameter(unsigned int);
 		virtual void setMaxIterations(int);
+		virtual void setObstacleRadius(int);
 		//TODO : To be implemented in the derived classes
 		// virtual void interpolate();
 		// virtual void fitVelocityProfile();
@@ -51,15 +55,16 @@ namespace rrt
 
 	private:
 		bool (*userCheck)(Utils::Point<T>);
-		bool checkPoint(Utils::Point<T> pt);
+		bool checkPoint(Utils::Point<T>, Utils::Point<T>);
 		Utils::Point<T> generatePoint();
 		Utils::Point<T> generateBiasedPoint(int);
-		void growTree(Utils::Point<T>);
+		void growTree(std::pair<Utils::Point<T>,int>,Utils::Point<T>);
 		std::pair<Utils::Point<T>, int>  findClosestNode(Utils::Point<T>);
 		Utils::Point<T> getParent(Utils::Point<T>);
 		std::pair <Utils::Point <T>,Utils::Point <T> > treeComplete(int*);
 		void generatePath(Utils::Point<T> first,Utils::Point<T> last);
 		double dist(Utils::Point<T> a,Utils::Point<T> b);
+		bool obstacle_here(int, int);
 	};
 }
 
diff --git a/include/RRTConnect_implementation.hpp b/include/RRTConnect_implementation.hpp
index fb15906..f9ffa4b 100644
--- a/include/RRTConnect_implementation.hpp
+++ b/include/RRTConnect_implementation.hpp
@@ -25,9 +25,24 @@ namespace rrt
 	}
 
 	template <class T>
-	std::deque<Utils::Point<T> > RRT<T>::getPointsOnPath()
+	std::vector<Utils::Point<T> > RRT<T>::getPointsOnPath()
 	{
-		return pathPoints;
+		Utils::Point<T> parent = pathPoints[0];
+		finalPath.push_back(parent);
+		for (int i=1; i<pathPoints.size();i++)
+		{
+			Utils::Point<T> now = pathPoints[1];
+			if(checkPoint(parent,now)!=true)
+			{
+				parent = pathPoints[i-1];
+				i--;
+				if (i!=0)
+					finalPath.push_back(parent);
+			}
+
+		}
+		finalPath.push_back(pathPoints[pathPoints.size()-1]);
+		return finalPath;
 	}
 
 	template <class T>
@@ -42,6 +57,12 @@ namespace rrt
 		maxIterations=value;
 	}
 
+	template <class T>
+	void RRT<T>::setObstacleRadius(int value)
+	{
+		obstacleradius=value;
+	}
+
 	template <class T>
 	bool RRT<T>:: plan()
 	{
@@ -50,9 +71,10 @@ namespace rrt
 		int check=0;
 		tree1.push_back(std::pair< Utils::Point<T>, Utils::Point<T> > (startPoint,startPoint));
 		tree2.push_back(std::pair< Utils::Point<T>, Utils::Point<T> > (endPoint,endPoint));
+		std::pair < Utils::Point<T> , int > both;
 		while( check < maxIterations)
 		{
-			//std::cout<<"In Planning Loop"<<std::endl;
+			// std::cout<<"In Planning Loop"<<std::endl;
 			Utils::Point<T> next;
 			int flag[]={0};
 			std::pair <Utils::Point<T>, Utils::Point<T> > mid = treeComplete(flag);
@@ -68,24 +90,29 @@ namespace rrt
 			}
 			else if(count%biasParameter==0)
 			{
-				//std::cout<<"Adding Next Biased Point to tree!!"<<std::endl;
+				// std::cout<<"Adding Next Biased Point to tree!!"<<std::endl;
 				count=0;
 				do{
 					next= generateBiasedPoint(which);
 					which*=-1;
-				}while(checkPoint(next)!=true);
+					both= findClosestNode(next);
+					// std::cout<<"Case 1: "<<checkPoint(both.first, next);
+
+				}while(checkPoint(both.first, next)!=true);
 				//std::cout<<" : "<<next.x<<","<<next.y<<std::endl;
 			}
 			else
 			{
-				//std::cout<<"Adding next point to tree!!"<<std::endl;
+				// std::cout<<"Adding next point to tree!!"<<std::endl;
 				do{
 					next = generatePoint();
-				}while(checkPoint(next)!=true);
+					both= findClosestNode(next);
+					// std::cout<<"Case 2: "<<checkPoint(both.first, next);
+				}while(checkPoint(both.first,next)!=true);
 				//std::cout<<" : "<<next.x<<","<<next.y<<std::endl;
 			}
 			//std::cout<<" Growing Tree next : "<<next.x<<","<<next.y<<std::endl;
-			growTree(next);
+			growTree(both,next);
 			count++;
 			check++;
 			//std::cout<<"check= "<<check<<", count= "<<count<<std::endl;
@@ -95,11 +122,11 @@ namespace rrt
 	}
 
 	template <class T>
-	void RRT<T>::growTree(Utils::Point<T> next)
+	void RRT<T>::growTree(std::pair < Utils::Point<T>, int > both,Utils::Point<T> next)
 	{
 		//growing the tree by adding node 
 		//std::cout<<"finding parent in tree of size = "<<tree.size()<<std::endl;
-		std::pair < Utils::Point<T> , int > both= findClosestNode(next);
+		// std::pair < Utils::Point<T> , int > both= findClosestNode(next);
 		Utils::Point<T> parent=both.first;
 		int which = both.second;
 		//std::cout<<"current : "<<next.x<<","<<next.y<<"| parent : "<<parent.x<<","<<parent.y<<std::endl;
@@ -107,7 +134,6 @@ namespace rrt
 			tree2.push_back( std::pair< Utils::Point<T>, Utils::Point<T> > (next,parent));
 		else
 			tree1.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
 	}
@@ -199,10 +225,90 @@ namespace rrt
 		return next;
 	}
 
+	// float dist(int x1, int y1, int x2, int y2)
+	// {
+	// 	return sqrt(pow((x1-x2),2)+pow((y1-y2),2));
+	// }
+
 	template <class T>
-	bool RRT<T>::checkPoint(Utils::Point<T> next)
+	bool RRT<T>::obstacle_here(int x, int y)
 	{
-		return userCheck(next);
+		for (int i=0;i<ObstaclePoints.size();i++)
+		{
+			Utils::Point<T> pratham;
+			pratham.x = x; pratham.y = y;
+			Utils::Point<T> dwitiya;
+			dwitiya.x = ObstaclePoints[i].x; dwitiya.y = ObstaclePoints[i].y;
+			if (dist(pratham,dwitiya)<obstacleradius)
+				return true;
+		}
+		return false;
+	}
+
+	template <class T>
+	bool RRT<T>::checkPoint(Utils::Point<T> parent, Utils::Point<T> next)
+	{
+		int x1=parent.x, x2=next.x, y1=parent.y,y2=next.y;
+		float x=x1, count;
+		try
+		{
+		 // std::cout<<"\nIn try ";
+		 // std::cout<<x1<<std::endl;
+		 // std::cout<<x2<<std::endl;
+		 // std::cout<<y1<<std::endl;
+		 // std::cout<<y2<<std::endl;
+
+		        int m=float(y2-y1)/(x2-x1);
+		        if (m==0) throw 20;
+		        int c=y2-m*x2;
+		        count = fabs(1.0/m);
+		        // while(1)
+		        if (count>1) count=1;
+		        if (count<-1) count=-1;
+		        if (x2<x1) count*=-1;
+		        // std::cout<<"\nm is "<<m<<" and count is: "<<count<<"\n";
+		        while (1)
+		        {
+		            x+=count;
+		            int y=m*x+c;
+		            if ((count>0 and x>=x2) || (count<0 and x<=x2))
+		            {
+		                // std::cout<<"Return true from try\n";
+		                return true;
+		            }
+		            else
+		            {
+		        // std::cout<<"\nm is "<<m<<" and count is: "<<count<<"\n";
+		            	// std::cout<<std::endl<<"x: "<<x<<" x2: "<<x2<<std::endl;
+		            	// std::cout<<"count: "<<count<<std::endl;
+		            }
+		            if (obstacle_here(x,y))
+		            {
+		                // std::cout<<"Return false from try\n";
+		                return false;
+		            }
+		        }
+		}
+		catch(int e)
+		{
+		        count=1;
+		        int y=y1;
+		        if (y2<y1) count*=-1;
+		        while (1)
+		        {
+		            y+=count;
+		            if ((count>0 and y>=y2) || (count<0 and y<=y2))
+		            {
+		            	// std::cout<<"Return true from catch\n";
+		                return true;
+		            }
+		            if (obstacle_here(x,y))
+		            {
+		            	// std::cout<<"Return false from catch\n";
+		                return false;
+		            }
+		        }
+		}
 	}
 
 	template <class T>
diff --git a/include/RRT_implementation.hpp b/include/RRT_implementation.hpp
index 5e9bbd9..62423b3 100644
--- a/include/RRT_implementation.hpp
+++ b/include/RRT_implementation.hpp
@@ -25,9 +25,24 @@ namespace rrt
 	}
 
 	template <class T>
-	std::deque<Utils::Point<T> > RRT<T>::getPointsOnPath()
+	std::vector<Utils::Point<T> > RRT<T>::getPointsOnPath()
 	{
-		return pathPoints;
+		Utils::Point<T> parent = pathPoints[0];
+		finalPath.push_back(parent);
+		for (int i=1; i<pathPoints.size();i++)
+		{
+			Utils::Point<T> now = pathPoints[1];
+			if(checkPoint(parent,now)!=true)
+			{
+				parent = pathPoints[i-1];
+				i--;
+				if (i!=0)
+					finalPath.push_back(parent);
+			}
+
+		}
+		finalPath.push_back(pathPoints[pathPoints.size()-1]);
+		return finalPath;
 	}
 
 	template <class T>
@@ -42,6 +57,12 @@ namespace rrt
 		maxIterations=value;
 	}
 
+	template <class T>
+	void RRT<T>::setObstacleRadius(int value)
+	{
+		obstacleradius=value;
+	}
+
 	template <class T>
 	bool RRT<T>:: plan()
 	{
@@ -54,11 +75,14 @@ namespace rrt
 			Utils::Point<T> next;
 			int arr[]={1};
 			std::pair  <Utils::Point <T>,Utils::Point <T> >  mid = treeComplete(arr);
+			std::pair  <Utils::Point <T>,int >  both;
+			// Utils::Point<T> both;
 			if(mid.first!=mid.second)
 			{
 				std::cout<<"Tree complete!!"<<std::endl;
 				int start_time=clock();
-				growTree(endPoint);
+				both= findClosestNode(endPoint);
+				growTree(both,endPoint);
 				int end_time=clock();
 				std::cout<<"Time to generate path = "<<end_time-start_time<<std::endl;
 				generatePath(startPoint,endPoint);
@@ -70,7 +94,8 @@ namespace rrt
 				count=0;
 				do{
 					next= generateBiasedPoint(1);
-				}while(checkPoint(next)!=true);
+					both= findClosestNode(next);
+				}while(checkPoint(both.first, next)!=true);
 				//std::cout<<" : "<<next.x<<","<<next.y<<std::endl;
 			}
 			else
@@ -78,11 +103,12 @@ namespace rrt
 				//std::cout<<"Adding next point to tree!!"<<std::endl;
 				do{
 					next = generatePoint();
-				}while(checkPoint(next)!=true);
+					both= findClosestNode(next);
+				}while(checkPoint(both.first, next)!=true);
 				//std::cout<<" : "<<next.x<<","<<next.y<<std::endl;
 			}
 			//std::cout<<" Growing Tree next : "<<next.x<<","<<next.y<<std::endl;
-			growTree(next);
+			growTree(both, next);
 			count++;
 			check++;
 			//std::cout<<"check= "<<check<<", count= "<<count<<std::endl;
@@ -92,14 +118,14 @@ namespace rrt
 	}
 
 	template <class T>
-	void RRT<T>::growTree(Utils::Point<T> next)
+	void RRT<T>::growTree(std::pair  <Utils::Point <T>,int > parent,Utils::Point<T> next)
 	{
 		//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;
+	
+		// parent= findClosestNode(next).first;
 		//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));
+		tree.push_back( std::pair< Utils::Point<T>, Utils::Point<T> > (next,parent.first));
 		//std::cout<<"Tree grown"<<std::endl;
 		//add pruning code to keep size of tree under control
 	}
@@ -169,9 +195,85 @@ namespace rrt
 	}
 
 	template <class T>
-	bool RRT<T>::checkPoint(Utils::Point<T> next)
+	bool RRT<T>::obstacle_here(int x, int y)
 	{
-		return userCheck(next);
+		for (int i=0;i<ObstaclePoints.size();i++)
+		{
+			Utils::Point<T> pratham;
+			pratham.x = x; pratham.y = y;
+			Utils::Point<T> dwitiya;
+			dwitiya.x = ObstaclePoints[i].x; dwitiya.y = ObstaclePoints[i].y;
+			if (dist(pratham,dwitiya)<obstacleradius)
+				return true;
+		}
+		return false;
+	}
+
+
+	template <class T>
+	bool RRT<T>::checkPoint(Utils::Point<T> parent, Utils::Point<T> next)
+	{
+		int x1=parent.x, x2=next.x, y1=parent.y,y2=next.y;
+		float x=x1, count;
+		try
+		{
+		 // std::cout<<"\nIn try ";
+		 // std::cout<<x1<<std::endl;
+		 // std::cout<<x2<<std::endl;
+		 // std::cout<<y1<<std::endl;
+		 // std::cout<<y2<<std::endl;
+
+		        int m=float(y2-y1)/(x2-x1);
+		        if (m==0) throw 20;
+		        int c=y2-m*x2;
+		        count = fabs(1.0/m);
+		        // while(1)
+		        if (count>1) count=1;
+		        if (count<-1) count=-1;
+		        if (x2<x1) count*=-1;
+		        // std::cout<<"\nm is "<<m<<" and count is: "<<count<<"\n";
+		        while (1)
+		        {
+		            x+=count;
+		            int y=m*x+c;
+		            if ((count>0 and x>=x2) || (count<0 and x<=x2))
+		            {
+		                // std::cout<<"Return true from try\n";
+		                return true;
+		            }
+		            else
+		            {
+		        // std::cout<<"\nm is "<<m<<" and count is: "<<count<<"\n";
+		            	// std::cout<<std::endl<<"x: "<<x<<" x2: "<<x2<<std::endl;
+		            	// std::cout<<"count: "<<count<<std::endl;
+		            }
+		            if (obstacle_here(x,y))
+		            {
+		                // std::cout<<"Return false from try\n";
+		                return false;
+		            }
+		        }
+		}
+		catch(int e)
+		{
+		        count=1;
+		        int y=y1;
+		        if (y2<y1) count*=-1;
+		        while (1)
+		        {
+		            y+=count;
+		            if ((count>0 and y>=y2) || (count<0 and y<=y2))
+		            {
+		            	// std::cout<<"Return true from catch\n";
+		                return true;
+		            }
+		            if (obstacle_here(x,y))
+		            {
+		            	// std::cout<<"Return false from catch\n";
+		                return false;
+		            }
+		        }
+		}
 	}
 
 	template <class T>
diff --git a/src/main.cpp b/src/main.cpp
index bc9d7da..07472ef 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -30,9 +30,10 @@ int main()
 	test.setBiasParameter(100);
 	test.setOrigin(origin);
 	test.setMaxIterations(10000);
+	test.setObstacleRadius(50);
 	test.plan();
 	cout<<"#################################################"<<endl;
-	deque<Utils::Point<int> > path=test.getPointsOnPath();
+	vector<Utils::Point<int> > path=test.getPointsOnPath();
 	for(int i=0;i<path.size();i++)
 		cout<<path[i].x<<","<<path[i].y<<endl;
 }
diff --git a/src/main_connect.cpp b/src/main_connect.cpp
index 0b4b460..11c68a9 100644
--- a/src/main_connect.cpp
+++ b/src/main_connect.cpp
@@ -30,9 +30,10 @@ int main()
 	test.setBiasParameter(100);
 	test.setOrigin(origin);
 	test.setMaxIterations(10000);
+	test.setObstacleRadius(50);
 	test.plan();
 	cout<<"#################################################"<<endl;
-	deque<Utils::Point<int> > path=test.getPointsOnPath();
+	vector<Utils::Point<int> > path=test.getPointsOnPath();
 	for(int i=0;i<path.size();i++)
 		cout<<path[i].x<<","<<path[i].y<<endl;
 }