void aco::Graph::evaporatePheromone( float evaporationRate )
{
int nNodes = getNumberOfNodes();
for (int i = 0; i < nNodes; ++i)
{
std::vector<int> adjacent;
getAdjacentNodes( i, adjacent );
int nAdjacent = adjacent.size();
for (int j = 0; j < nAdjacent; ++j)
{
float oldPheromone = getPheromone( i, j );
float newPheromone = oldPheromone * ( 1.0f - evaporationRate );
addPheromone( i, j, newPheromone - oldPheromone );
}
}
}
void TopologicalMapper::computeGraph(double merge_threshold) {
// First for each critical point, find out the neigbouring regions
std::vector<std::set<uint32_t> > point_neighbour_sets;
point_neighbour_sets.resize(critical_points_.size());
// Draw the critical lines as their indexes on the map
cv::Mat lines(map_resp_.map.info.height, map_resp_.map.info.width, CV_16UC1,
cv::Scalar((uint16_t)-1));
drawCriticalLines(lines, 0, 0, true);
// Go over all the pixels in the lines image and find neighbouring critical
// regions
for (int j = 0; j < lines.rows; ++j) {
uint16_t* image_row_j = lines.ptr<uint16_t>(j);
for (int i = 0; i < lines.cols; ++i) {
uint16_t pixel = image_row_j[i];
if (pixel == (uint16_t)-1) {
continue;
}
int x_offset[] = {0, -1, 1, 0};
int y_offset[] = {-1, 0, 0, 1};
size_t num_neighbours = 4;
for (size_t count = 0; count < num_neighbours; ++count) {
uint32_t x_n = i + x_offset[count];
uint32_t y_n = j + y_offset[count];
if (x_n >= (uint16_t) lines.cols || y_n >= (uint16_t) lines.rows) {
continue;
}
size_t map_idx = MAP_IDX(lines.cols, x_n, y_n);
if (component_map_[map_idx] >= 0 &&
component_map_[map_idx] < (int32_t) num_components_) {
point_neighbour_sets[pixel].insert(component_map_[map_idx]);
}
}
}
}
// Throw out any critical points that do not have 2 adjoining regions.
// This can happen if the regions are too small
std::vector<std::vector<uint32_t> > point_neighbours;
point_neighbours.resize(critical_points_.size());
std::vector<int> remove_crit_points;
for (size_t i = 0; i < critical_points_.size(); ++i) {
if (point_neighbour_sets[i].size() == 2) {
point_neighbours[i] =
std::vector<uint32_t>(point_neighbour_sets[i].begin(),
point_neighbour_sets[i].end());
} else {
remove_crit_points.push_back(i);
}
}
for (int i = remove_crit_points.size() - 1; i >=0 ; --i) {
critical_points_.erase(critical_points_.begin() + i);
point_neighbour_sets.erase(point_neighbour_sets.begin() + i);
point_neighbours.erase(point_neighbours.begin() + i);
}
// Find connectivity to remove any isolated sub-graphs
std::vector<bool> checked_region(critical_points_.size(), false);
std::vector<std::set<int> > graph_sets;
for (size_t i = 0; i < num_components_; ++i) {
if (checked_region[i]) {
continue;
}
std::set<int> open_set, closed_set;
open_set.insert(i);
while (open_set.size() != 0) {
int current_region = *(open_set.begin());
open_set.erase(open_set.begin());
closed_set.insert(current_region);
checked_region[current_region] = true;
for (int j = 0; j < critical_points_.size(); ++j) {
for (int k = 0; k < 2; ++k) {
if (point_neighbours[j][k] == current_region &&
std::find(closed_set.begin(), closed_set.end(),
point_neighbours[j][1-k]) ==
closed_set.end()) {
open_set.insert(point_neighbours[j][1-k]);
}
}
}
}
graph_sets.push_back(closed_set);
}
std::set<int> master_region_set;
if (graph_sets.size() != 1) {
std::cout << "WARNING: Master graph is fragmented into " <<
graph_sets.size() << " sub-graphs!!!" << std::endl;
int max_idx = 0;
int max_size = graph_sets[0].size();
for (size_t i = 1; i < graph_sets.size(); ++i) {
if (graph_sets[i].size() > max_size) {
max_idx = i;
max_size = graph_sets[i].size();
}
}
master_region_set = graph_sets[max_idx];
} else {
master_region_set = graph_sets[0];
}
// Create the region graph next
std::map<int, int> region_to_vertex_map;
int vertex_count = 0;
for (size_t r = 0; r < num_components_; ++r) {
if (std::find(master_region_set.begin(), master_region_set.end(), r) ==
master_region_set.end()) {
region_to_vertex_map[r] = -1;
continue;
}
Graph::vertex_descriptor vi = boost::add_vertex(region_graph_);
// Calculate the centroid
uint32_t avg_i = 0, avg_j = 0, pixel_count = 0;
for (size_t j = 0; j < map_resp_.map.info.height; ++j) {
for (size_t i = 0; i < map_resp_.map.info.width; ++i) {
size_t map_idx = MAP_IDX(map_resp_.map.info.width, i, j);
if (component_map_[map_idx] == (int)r) {
avg_j += j;
avg_i += i;
pixel_count++;
}
}
}
region_graph_[vi].location.x = ((float) avg_i) / pixel_count;
region_graph_[vi].location.y = ((float) avg_j) / pixel_count;
region_graph_[vi].pixels = floor(sqrt(pixel_count));
// This map is only required till the point we form edges on this graph
region_to_vertex_map[r] = vertex_count;
++vertex_count;
}
// Now that region_to_vertex_map is complete,
// forward critical points to next graph
std::map<int, std::map<int, VoronoiPoint> >
region_vertex_crit_points;
for (size_t r = 0; r < num_components_; ++r) {
if (std::find(master_region_set.begin(), master_region_set.end(), r) ==
master_region_set.end()) {
region_to_vertex_map[r] = -1;
continue;
}
// Store the critical points corresponding to this vertex
for (int j = 0; j < critical_points_.size(); ++j) {
for (int k = 0; k < 2; ++k) {
if (point_neighbours[j][k] == r) {
int region_vertex = region_to_vertex_map[r];
int other_region_vertex = region_to_vertex_map[point_neighbours[j][1-k]];
if (region_vertex == 192 && other_region_vertex == 202) {
std::cout << "192-202: " << r << " " << point_neighbours[j][1-k] << " " << critical_points_[j] << std::endl;
}
if (region_vertex == 202 && other_region_vertex == 192) {
std::cout << "192-202: " << r << " " << point_neighbours[j][1-k] << " " << critical_points_[j] << std::endl;
}
region_vertex_crit_points[region_vertex]
[region_to_vertex_map[point_neighbours[j][1-k]]] =
critical_points_[j];
}
}
}
}
// Create 1 edge per critical point
for (size_t i = 0; i < critical_points_.size(); ++i) {
int region1 = region_to_vertex_map[point_neighbours[i][0]];
int region2 = region_to_vertex_map[point_neighbours[i][1]];
if (region1 == -1) {
// part of one of the sub-graphs that was discarded
continue;
}
int count = 0;
Graph::vertex_descriptor vi,vj;
vi = boost::vertex(region1, region_graph_);
vj = boost::vertex(region2, region_graph_);
Graph::edge_descriptor e; bool b;
boost::tie(e,b) = boost::edge(vi, vj, region_graph_);
if (!b) {
boost::tie(e,b) = boost::add_edge(vi, vj, region_graph_);
}
/* boost::tie(e,b) = boost::add_edge(vi, vj, region_graph_); */
// region_graph_[e].weight = bwi_mapper::getMagnitude(
// region_graph_[vi].location - region_graph_[vj].location);
}
// Refine the region graph into the point graph:
int pixel_threshold = merge_threshold / map_resp_.map.info.resolution;
enum {
PRESENT = 0,
REMOVED_REGION_VERTEX = 1,
CONVERT_TO_CRITICAL_POINT = 2,
MERGE_VERTEX = 3
};
Graph::vertex_iterator vi, vend;
// PASS 1 - resolve all vertices that are too big and have more than 2
// critical points to their underlying critical points
std::cout << std::endl << "==============================" << std::endl;
std::cout << "PASS 1" << std::endl;
std::cout << "==============================" << std::endl << std::endl;
Graph pass_0_graph = region_graph_;
Graph pass_1_graph;
int pass_0_count = 0;
int pass_1_count = 0;
std::vector<int> pass_0_vertex_status(
boost::num_vertices(pass_0_graph), PRESENT);
std::map<int, int> pass_0_vertex_to_pass_1_map;
for (boost::tie(vi, vend) = boost::vertices(pass_0_graph); vi != vend;
++vi, ++pass_0_count) {
std::cout << "Analyzing pass 0 graph vertex: " << pass_0_count << std::endl;
std::vector<size_t> adj_vertices;
getAdjacentNodes(pass_0_count, pass_0_graph, adj_vertices);
// See if the area of this region is too big to be directly pushed into
// the pass 3 graph
if (pass_0_graph[*vi].pixels >= pixel_threshold &&
adj_vertices.size() > 2) {
pass_0_vertex_status[pass_0_count] = CONVERT_TO_CRITICAL_POINT;
std::cout << " - throwing it out (needs to be resolved to CPs)" <<
std::endl;
continue;
}
// Otherwise insert this as is into the point graph
Graph::vertex_descriptor point_vi = boost::add_vertex(pass_1_graph);
pass_1_graph[point_vi] = pass_0_graph[*vi];
pass_0_vertex_to_pass_1_map[pass_0_count] = pass_1_count;
++pass_1_count;
}
// Now for each vertex that needs to be resolved into critical points,
// check neighbours recursively to convert these vertices into critical
// points
std::map<int, std::map<int, int> > pass_0_vertex_to_cp_map;
pass_0_count = 0;
for (boost::tie(vi, vend) = boost::vertices(pass_0_graph); vi != vend;
++vi, ++pass_0_count) {
if (pass_0_vertex_status[pass_0_count] != CONVERT_TO_CRITICAL_POINT) {
continue;
}
std::cout << "Converting pass1 vtx to CPs: " << pass_0_count << std::endl;
std::vector<size_t> adj_vertices;
getAdjacentNodes(pass_0_count, pass_0_graph, adj_vertices);
std::vector<int> connect_edges;
BOOST_FOREACH(size_t vtx, adj_vertices) {
if (pass_0_vertex_status[vtx] == PRESENT) {
Graph::vertex_descriptor point_vi = boost::add_vertex(pass_1_graph);
pass_1_graph[point_vi].location =
region_vertex_crit_points[pass_0_count][vtx];
std::cout << " - added vtx at " << pass_1_graph[point_vi].location
<< std::endl;
int pass_1_vertex = pass_0_vertex_to_pass_1_map[vtx];
std::cout << " - connecting vtx to pass_1_vertex: " << pass_1_vertex
<< " (pass0 = " << vtx << ")" << std::endl;
Graph::vertex_descriptor vj;
vj = boost::vertex(pass_1_vertex, pass_1_graph);
Graph::edge_descriptor e; bool b;
boost::tie(e,b) = boost::add_edge(point_vi, vj, pass_1_graph);
connect_edges.push_back(pass_1_count);
++pass_1_count;
} else if (vtx > pass_0_count) {
// The CP is shared, but has not been added
Graph::vertex_descriptor point_vi = boost::add_vertex(pass_1_graph);
pass_1_graph[point_vi].location =
region_vertex_crit_points[pass_0_count][vtx];
std::cout << " - added shared cp with " << vtx << " at "
<< pass_1_graph[point_vi].location << std::endl;
pass_0_vertex_to_cp_map[pass_0_count][vtx] = pass_1_count;
connect_edges.push_back(pass_1_count);
++pass_1_count;
} else {
// Retrieve existing CP
int cp_vtx = pass_0_vertex_to_cp_map[vtx][pass_0_count];
connect_edges.push_back(cp_vtx);
}
}
/* Connect all the edges */
for (int i = 0; i < connect_edges.size(); ++i) {
for (int j = 0; j < i; ++j) {
Graph::vertex_descriptor vi, vj;
vi = boost::vertex(connect_edges[i], pass_1_graph);
vj = boost::vertex(connect_edges[j], pass_1_graph);
Graph::edge_descriptor e; bool b;
boost::tie(e,b) = boost::add_edge(vi, vj, pass_1_graph);
}
}
}
// Connect all the edges as is from pass 1
pass_0_count = 0;
for (boost::tie(vi, vend) = boost::vertices(pass_0_graph); vi != vend;
++vi, ++pass_0_count) {
if (pass_0_vertex_status[pass_0_count] != PRESENT) {
continue;
}
std::cout << "Adding pass 1 edges for: " << pass_0_count << std::endl;
std::vector<size_t> adj_vertices;
getAdjacentNodes(pass_0_count, pass_0_graph, adj_vertices);
BOOST_FOREACH(size_t vtx, adj_vertices) {
if (pass_0_vertex_status[vtx] == PRESENT && vtx < pass_0_count) {
Graph::vertex_descriptor vi, vj;
vi = boost::vertex(
pass_0_vertex_to_pass_1_map[pass_0_count], pass_1_graph);
vj = boost::vertex(
pass_0_vertex_to_pass_1_map[vtx], pass_1_graph);
Graph::edge_descriptor e; bool b;
boost::tie(e,b) = boost::add_edge(vi, vj, pass_1_graph);
}
}
}
pass_1_graph_ = pass_1_graph;
// PASS 2 - remove any vertices that are adjacent to only 2 other vertices,
// where both those vertices are visible to each other
std::cout << std::endl << "==============================" << std::endl;
std::cout << "PASS 2" << std::endl;
std::cout << "==============================" << std::endl << std::endl;
Graph pass_2_graph;
std::vector<int> pass_1_vertex_status(boost::num_vertices(pass_1_graph), PRESENT);
std::map<int, int> pass_1_vertex_to_pass_2_vertex_map;
std::vector<std::pair<int, int> > rr_extra_edges;
std::map<int, std::pair<int, int> > removed_vertex_map;
pass_1_count = 0;
int pass_2_count = 0;
for (boost::tie(vi, vend) = boost::vertices(pass_1_graph); vi != vend;
++vi, ++pass_1_count) {
std::cout << "Analyzing pass_1 graph vertex: " << pass_1_count << std::endl;
if (pass_1_vertex_status[pass_1_count] != PRESENT) {
std::cout << " - the vertex has been thrown out already " << std::endl;
continue;
}
// See if this only has 2 neighbours, and the 2 neighbours are visible to
// each other
std::vector<size_t> open_vertices;
int current_vertex = pass_1_count;
getAdjacentNodes(current_vertex, pass_1_graph, open_vertices);
std::vector<int> removed_vertices;
std::pair<int, int> edge;
if (open_vertices.size() == 2 &&
pass_1_vertex_status[open_vertices[0]] == PRESENT &&
pass_1_vertex_status[open_vertices[1]] == PRESENT) {
while(true) {
std::cout << " - has 2 adjacent vertices " << open_vertices[0] << ", "
<< open_vertices[1] << std::endl;
// Check if the 2 adjacent vertices are visible
Point2f location1 =
getLocationFromGraphId(open_vertices[0], pass_1_graph);
Point2f location2 =
getLocationFromGraphId(open_vertices[1], pass_1_graph);
bool locations_visible =
locationsInDirectLineOfSight(location1, location2, map_resp_.map);
if (locations_visible) {
std::cout << " - the 2 adjacent vertices are visible "
<< "- removing vertex." << std::endl;
pass_1_vertex_status[current_vertex] = REMOVED_REGION_VERTEX;
removed_vertices.push_back(current_vertex);
edge = std::make_pair(open_vertices[0], open_vertices[1]);
bool replacement_found = false;
for (int i = 0; i < 2; ++i) {
std::vector<size_t> adj_vertices;
getAdjacentNodes(open_vertices[i], pass_1_graph,
adj_vertices);
if (adj_vertices.size() == 2) {
size_t new_vertex = (adj_vertices[0] == current_vertex) ?
adj_vertices[1] : adj_vertices[0];
if (pass_1_vertex_status[new_vertex] == PRESENT) {
current_vertex = open_vertices[i];
std::cout << " - neighbours may suffer from the same problem"
<< ". checking vertex " << current_vertex << std::endl;
open_vertices[i] = new_vertex;
replacement_found = true;
break;
}
}
}
if (!replacement_found) {
// Both neighbours on either side had 1 or more than 2 adjacent
// vertices
break;
}
} else {
// left and right vertices not visible
break;
}
}
}
BOOST_FOREACH(int removed_vertex, removed_vertices) {
removed_vertex_map[removed_vertex] = edge;
}
if (removed_vertices.size() != 0) {
// Add the extra edge
std::cout << " - adding extra edge between " << edge.first <<
" " << edge.second << std::endl;
rr_extra_edges.push_back(edge);
continue;
}
// Otherwise insert this as is into the point graph
Graph::vertex_descriptor point_vi = boost::add_vertex(pass_2_graph);
pass_2_graph[point_vi] = pass_1_graph[*vi];
pass_1_vertex_to_pass_2_vertex_map[pass_1_count] =
pass_2_count;
++pass_2_count;
}
// Insert all edges that can be inserted
// Add edges from the pass_1 graph that can be placed as is
pass_1_count = 0;
for (boost::tie(vi, vend) = boost::vertices(pass_1_graph); vi != vend;
++vi, ++pass_1_count) {
if (pass_1_vertex_status[pass_1_count] == PRESENT) {
std::vector<size_t> adj_vertices;
getAdjacentNodes(pass_1_count, pass_1_graph, adj_vertices);
BOOST_FOREACH(size_t adj_vertex, adj_vertices) {
if (pass_1_vertex_status[adj_vertex] == PRESENT &&
adj_vertex > pass_1_count) {
Graph::vertex_descriptor vi,vj;
int vertex1 =
pass_1_vertex_to_pass_2_vertex_map[pass_1_count];
int vertex2 = pass_1_vertex_to_pass_2_vertex_map[adj_vertex];
vi = boost::vertex(vertex1, pass_2_graph);
vj = boost::vertex(vertex2, pass_2_graph);
Graph::edge_descriptor e; bool b;
boost::tie(e,b) = boost::add_edge(vi, vj, pass_2_graph);
}
}
}
}
void Pathfinder::step()
{
if (m_PathFound) return;
//std::cout << "\nStepping\n";
if (!m_Map.inMapBounds(m_SourceNodePosition) || //Make sure the source and target are valid
!m_Map.inMapBounds(m_TargetNodePosition))
{
//std::cout << "Source or Target is invalid\n";
std::cout << m_SourceNodePosition.x << ", " << m_SourceNodePosition.y << "\n";
std::cout << m_TargetNodePosition.x << ", " << m_TargetNodePosition.y << "\n";
return;
}
//std::cout << "Source and Target are valid\n";
if (m_OpenList.size() == 0 &&
m_ClosedList.size() == 0)
{
//std::cout << "Adding source node\n";
m_OpenList.push_back(m_SourceNodePosition);
}
else if ((m_ClosedList.size() != 0 && //make sure there is an object at the back
m_ClosedList.back() == m_TargetNodePosition) || //If the target is in the closed list then we found a path
m_OpenList.size() == 0) //If the open list is empty then we have checked the entire map and not found a path
{
//std::cout << "Path found\n";
//Reset the nodes we have modified
for (sf::Vector2i pos : m_ClosedList)
{
m_Map.getTile(pos).getNode().resetCosts();
m_Map.getTile(pos).getNode().resetState();
}
for (sf::Vector2i pos : m_OpenList)
{
m_Map.getTile(pos).getNode().resetCosts();
m_Map.getTile(pos).getNode().resetState();
}
//Draw the path
sf::Vector2i parentPos = m_ClosedList.back();
while (m_Map.inMapBounds(parentPos))
{
//std::cout << "Parent Position = [" << parentPos.x << ", " << parentPos.y << "]\n";
m_Map.getTile(parentPos).getNode().setState(NodeState::ClosedList);
parentPos = m_Map.getTile(parentPos).getNode().getParentNodePosition();
}
//Draw source and target
m_Map.getTile(m_ClosedList.front()).getNode().setState(NodeState::Source);
m_Map.getTile(m_ClosedList.back()).getNode().setState(NodeState::Target);
m_PathFound = true;
}
else
{
//std::cout << "Finding lowest scored node\n";
//Find node in open list with lowest score, and move it to closed list.
unsigned lowestScoreNodeIndex = getLowestScoreNodeIndex(m_OpenList);
m_ClosedList.push_back(m_OpenList[lowestScoreNodeIndex]);
{
Node& n = m_Map.getTile(m_ClosedList.back()).getNode();
if (n.getState() != NodeState::Source && //If the node isn't the source or target node;
n.getState() != NodeState::Target)
{
n.setState(NodeState::ClosedList);
}
}
m_OpenList.erase(m_OpenList.begin() + lowestScoreNodeIndex);
//std::cout << "Finding adjacent nodes\n";
//Find adjacent nodes of current node
auto adjNodePoses = getAdjacentNodes(m_ClosedList.back());
for (sf::Vector2i& nodePos : adjNodePoses)
{
if (m_Map.getTile(nodePos).getState() != TileState::Wall && //Can be walked on
std::find(m_ClosedList.begin(), m_ClosedList.end(), nodePos) == m_ClosedList.end()) //is not on the closed list
{
Node& n = m_Map.getTile(nodePos).getNode();
if (std::find(m_OpenList.begin(), m_OpenList.end(), nodePos) == m_OpenList.end()) //is not on the open list
{
//std::cout << "Found undiscovered valid node\n";
//Recalculated costs and set parent node
updateNodeInfo(nodePos, m_ClosedList.back());
if (n.getState() != NodeState::Source && //Make sure we don't override the source or target tile.
n.getState() != NodeState::Target)
{
n.setState(NodeState::OpenList);
}
m_OpenList.push_back(nodePos);
}
else if (n.getMovementCost() > calculateMovementCost(m_ClosedList.back(), nodePos))//this path to the node is shorter
{
//std::cout << "Found better path\n";
//Recalculated costs and set parent node
updateNodeInfo(nodePos, m_ClosedList.back());
}
}
}
}
}
//returns the correct triangle fan assuming that only at most 4 bools on
void MarchingCubesGenerator::getTriangleFan(bool * nodesVal, AllFans allfans, int &numFans, AllFans allFansNorm, bool flipped)
{
int onPoint = 0;
bool onPointset;
numFans = 0;
int vertexCounter= 0;
intQueue * first;
intQueue * last;
AdjacentNodes adj;
McVertex tempVert;
McFan tempFan;
memcpy(tempFan, emptyFan, sizeof(McFan));
bool marked[8];
for(int i = 0; i<8; i++)
{
marked[i] = false;
}
for(int i=0; i<8; i++)
{
onPointset=false;
if((nodesVal[i] == true) && (marked[i] == false))
{
onPoint= i;
marked[i]=true;
first = new intQueue;
first->key = i;
first->next = NULL;
last = first;
while(first != NULL)
{
getAdjacentNodes(first->key, adj);
for(int j=0; j<3; j++)
{
if(nodesVal[adj[j]] == true)
{
if(marked[adj[j]] == false)
{
marked[adj[j]] = true;
intQueue * adjacentOn = new intQueue;
adjacentOn->key = adj[j];
adjacentOn->next = NULL;
last->next = adjacentOn;
last = last->next;
}
}
else
{
//add a triangle to the fan list
getVertexBetween(first->key, adj[j], tempVert);
memcpy(tempFan[vertexCounter], tempVert, sizeof(McVertex));
vertexCounter++;
}
}
first = first->next;
}
//return a sorted fan list
sortVerts(allfans[numFans], tempFan, vertexCounter, allFansNorm[numFans], flipped, onPoint);
vertexCounter=0;
numFans++;
}
}
}
