Plurk paste

#include "assignment8.h"
#include <iostream>
#include <string>
#include <vector>
#include <stdlib.h>
#include <queue>
#include <algorithm>
#include <math.h>

using std::vector;
using std::cout;
using std::endl;

//=========== Constructor ========================================================

RRT::RRT(const cimg_library::CImg<unsigned char>& image) {
	image_ = image;
	node_vector_ = vector<Node>();
	path_found_ = false;
	sample_x_ = -1;
	sample_y_ = -1;
}

//=========== Required Functions =================================================

/*
A method which, given an input vector of nodes, and an input node, 
finds the closest Node in node vector to input node
*/
Node RRT::NearestNeighbor(vector<Node> node_vector,
					Node input_node) {
	//set a local variable to keep track of the distance. 
	float smallest_dist = Distance(node_vector[0], input_node);
	int smallest_index = 0;
		//set to the distance between the first node in node_vector and input_node.
	//set a local variable to keep track of the index in vector.
	if(node_vector.size() < 2) {
		return node_vector[0];
	}
	//if there is only one node, return that node
	//else
	else {
		for(int i = 0; i <= node_vector.size()-1; i++) {
		//go through each node
			if(Distance(node_vector[i], input_node) < smallest_dist) {
			//calculate it's distance between the input_node
			//if the new distance is shorter than the local distance variable
				smallest_dist = Distance(node_vector[i], input_node);
				//update local with new distance.
				smallest_index = i;
				//update the index
			}
		//identify the 
		}
	} 
	
	return node_vector[smallest_index];
}

/*
A method which given two Nodes, returns true if there is a collision on the path 
between those nodes in the given map, and false otherwise. 
Will not be expected to handle input nodes that are outside the map.
Check collision using Bresenham’s line algorithm.
*/
bool RRT::CheckCollision(Node start, Node end) {
	int x1 = start.x, y1 = start.y, x2 = end.x, y2 = end.y;

	int delta_x(x2 - x1);
    // if x1 == x2, then it does not matter what we set here
    signed char const ix((delta_x > 0) - (delta_x < 0));
    delta_x = std::abs(delta_x) << 1;
 
    int delta_y(y2 - y1);
    // if y1 == y2, then it does not matter what we set here
    signed char const iy((delta_y > 0) - (delta_y < 0));
    delta_y = std::abs(delta_y) << 1;
 
    int value = image_(x1, y1, 0, 0);

    if(value == 0) {
    	return false;
    }
 
    if (delta_x >= delta_y)
    {
        // error may go below zero
        int error(delta_y - (delta_x >> 1));
 
        while (x1 != x2)
        {
            // reduce error, while taking into account the corner case of error == 0
            if ((error > 0) || (!error && (ix > 0)))
            {
                error -= delta_x;
                y1 += iy;
            }
            // else do nothing
 
            error += delta_y;
            x1 += ix;
 
 			value = image_(x1, y1, 0, 0);
            if(value == 0) {
		    	return false;
		    }
        }
    }
    else
    {
        // error may go below zero
        int error(delta_x - (delta_y >> 1));
 
        while (y1 != y2)
        {
            // reduce error, while taking into account the corner case of error == 0
            if ((error > 0) || (!error && (iy > 0)))
            {
                error -= delta_y;
                x1 += ix;
            }
            // else do nothing
 
            error += delta_x;
            y1 += iy;
 
 			value = image_(x1, y1, 0, 0);
            if(value == 0) {
		    	return false;
		    }
        }
    }

    return true;
	// return BresenhamPoints(start.x, start.y, end.x, end.y);
}

/*
A method which given two Nodes, and goal radius, returns a vector of Nodes representing a collision
free path from the start Node to a node within the goal radius of the end node..
*/
vector<Node> RRT::FindPath(Node start, Node end, int goal_radius, int extension_distance) {
	//initialize vector with the start and end Nodes
	cout << "start node: " << start.x << "," << start.y << "\nend node: " << end.x << "," << end.y << endl;
	node_vector_.clear();
	start.SetID(0);
	start.SetParentId(-1);
	node_vector_.push_back(start);
	// node_vector_.push_back(end);

	//if the distance between the start and end is greater than threshold
	if(Distance(end, start) > goal_radius) {
		//path not found.
		path_found_ = false;
	}
		// update the end node's parent to be start node's id.
	// While the path not found
	while(!path_found_) {
		//generate random coordinate x,y
		Sample();
		Node sample_node(sample_x_, sample_y_);
		Node temp_nearest_sample = NearestNeighbor(node_vector_, sample_node);
		//check if the nearest_node and sample are too far from each other.
		if(Distance(temp_nearest_sample, sample_node) > extension_distance) 
		{
			//project sample from the neariest exisiting node toward new node
			Project(sample_x_, sample_y_, 
				temp_nearest_sample, 
				extension_distance);
			Node projected_sample(sample_x_, sample_y_);
			//check collision
			//if path is clear
			Node tempt_nearest = NearestNeighbor(node_vector_, projected_sample);
			bool collision = CheckCollision(tempt_nearest, projected_sample);
			if(collision) {
				projected_sample.SetID(GenerateNodeID());
				Link_Path(projected_sample, tempt_nearest);
				if(!CheckNodeExist(projected_sample)){
					node_vector_.push_back(projected_sample); //add the node into the vector
					// cout << "Node ADDED" << endl;
				}
				//check if the new node is within the threhold of the goal
				if(Distance(projected_sample, end) < goal_radius) {
					//found path
					path_found_ = true;
				}
			}
		}
					
	//if Path found
		//return the updated vector
	}
	PrintNodeVector(node_vector_);
	// BresenhamPoints(start.x, start.y, end.x, end.y);
	return node_vector_;
}


// ============== Helper Functions =============================================


void Node::SetParentId(const int& pid) {
	parent_id = pid;
}
void Node::SetID(const int& sid) { 
	id = sid;
}


void RRT::DrawNodePath() {
  // if (occupancy_grid_ptr_ == nullptr) {
  //   return;
  // }
  const unsigned char color[] = {200};
  const unsigned char dColor[] = {100};
  // for (size_t i = 0; i < sqrt(map_->GetNodeListLength()); i++) {
  //   for (size_t j = 0; j < sqrt(map_->GetNodeListLength()); j++) {
  //     int id = map_->GenerateNodeID(
  //       16 * i + 9,
  //       16 * j + 9);
  //     Node* current_node = map_->GetNodeList()[id];
  //     if (IsVisited(current_node)) {
  //       occupancy_grid_ptr_->draw_circle(
  //         16 * i + 9,
  //         16 * j + 9,
  //         6,
  //         color);
  //     }
  //     if (id == goal_id) {
  //       occupancy_grid_ptr_->draw_circle(
  //         16 * i + 9,
  //         16 * j + 9,
  //         6,
  //         dColor);
  //     }
  //   }
  // }
  // display = *occupancy_grid_ptr_;
  // display.show();
  // usleep(200000000);

}

void RRT::PrintNodeVector(const vector<Node> node_vector) {
	if(node_vector.size() < 1) {
		return;
	}
	for(Node n: node_vector) {
		cout << n.parent_id << " " << n.id <<"(" << n.x << "," << n.y << ") ";
	}
	cout << endl;
}

//return the sign of the given number as 0, +1, or -1.
int RRT::sign(int& number) {
	if (number == 0) {
		return 0;
	}
	else if(number < 0) {
		return -1;
	}
	else {
		return 1;
	}
}

//return the distance between 2 nodes
float RRT::Distance(Node n1, Node n2) {
	// cout << "distance " << sqrt(pow(n2.x - n1.x, 2) + pow(n2.y - n1.y, 2)) << endl;
	return sqrt(pow(n2.x - n1.x, 2) + pow(n2.y - n1.y, 2));
}


// sample a random x based on given depth away from the 
void RRT::Sample() {
	const int range_from = 0;
	const int range_to_x = image_.width() -1;
	const int range_to_y = image_.height() -1;
	std:: random_device rand_dev;
	std::uniform_int_distribution<int> distribution_x(range_from, range_to_x);
	std::uniform_int_distribution<int> distribution_y(range_from, range_to_y);
	sample_x_ = distribution_x(rand_dev);
	// cout << "sample x: " << sample_x_ << endl;
	sample_y_ = distribution_y(rand_dev);
	// cout << "sample y: " << sample_y_ << endl;
}

//project the sample node to distance away from the nearest_node.
//likely using the 
void RRT::Project(int sample_x, int sample_y, Node nearest_node, int distance) {
	// cout << "original sample_x_, sample_y_ " << sample_x << "," << sample_y << endl;
	float vector_x = sample_x - nearest_node.x;
	float vector_y = sample_y - nearest_node.y;

	//length between sample and nearest_node
	float length = sqrt(vector_x*vector_x + vector_y*vector_y);

	//normalize vector
	vector_x /= length;
	vector_y /= length;

	sample_x_ = static_cast<int>(((static_cast<float>(nearest_node.x)) +
									 vector_x*(static_cast<float>(distance))));
	sample_y_ = static_cast<int>(((static_cast<float>(nearest_node.y)) +
									 vector_y*(static_cast<float>(distance))));
	// cout << "projected sample_x_,sample_y_ " << sample_x_ << "," << sample_y_ << endl;

}

//link the new_node to the nearest.  by assigning the parent id of new_node
//with the id of nearest_node
void RRT::Link_Path(Node& new_node, Node& nearest_node) {
	new_node.SetParentId(nearest_node.id);
}

int RRT::GenerateNodeID(){
	return node_vector_.size();
};

//check if the new_node is an existed node
// return true if it is, false otherwise.
bool RRT::CheckNodeExist(Node new_node) {
	bool found = false;
	while(!found) {
		for (int a = 0; a < node_vector_.size(); a++) {
			if(node_vector_[a].x == new_node.x) {
				if(node_vector_[a].y == new_node.y) {
					found = true;
					// cout << "found!" << endl;
				}
			}
		}
	}

	return found;
}

// End of File