457 lines
16 KiB
C++
457 lines
16 KiB
C++
//
|
|
// SuperTuxKart - a fun racing game with go-kart
|
|
// Copyright (C) 2016 SuperTuxKart Team
|
|
//
|
|
// This program is free software; you can redistribute it and/or
|
|
// modify it under the terms of the GNU General Public License
|
|
// as published by the Free Software Foundation; either version 3
|
|
// of the License, or (at your option) any later version.
|
|
//
|
|
// This program is distributed in the hope that it will be useful,
|
|
// but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
// GNU General Public License for more details.
|
|
//
|
|
// You should have received a copy of the GNU General Public License
|
|
// along with this program; if not, write to the Free Software
|
|
// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
|
|
|
|
#include "tracks/arena_graph.hpp"
|
|
|
|
#include "config/user_config.hpp"
|
|
#include "io/file_manager.hpp"
|
|
#include "io/xml_node.hpp"
|
|
#include "race/race_manager.hpp"
|
|
#include "tracks/arena_node.hpp"
|
|
#include "tracks/track.hpp"
|
|
#include "tracks/track_manager.hpp"
|
|
#include "utils/log.hpp"
|
|
|
|
#include <algorithm>
|
|
#include <queue>
|
|
|
|
// -----------------------------------------------------------------------------
|
|
ArenaGraph::ArenaGraph(const std::string &navmesh, const XMLNode *node)
|
|
: Graph()
|
|
{
|
|
loadNavmesh(navmesh);
|
|
buildGraph();
|
|
// Compute shortest distance from all nodes
|
|
for (unsigned int i = 0; i < getNumNodes(); i++)
|
|
computeDijkstra(i);
|
|
|
|
setNearbyNodesOfAllNodes();
|
|
if (node && race_manager->getMinorMode() == RaceManager::MINOR_MODE_SOCCER)
|
|
loadGoalNodes(node);
|
|
|
|
loadBoundingBoxNodes();
|
|
|
|
} // ArenaGraph
|
|
|
|
// -----------------------------------------------------------------------------
|
|
ArenaNode* ArenaGraph::getNode(unsigned int i) const
|
|
{
|
|
assert(i < m_all_nodes.size());
|
|
ArenaNode* n = dynamic_cast<ArenaNode*>(m_all_nodes[i]);
|
|
assert(n != NULL);
|
|
return n;
|
|
} // getNode
|
|
|
|
// -----------------------------------------------------------------------------
|
|
void ArenaGraph::differentNodeColor(int n, video::SColor* c) const
|
|
{
|
|
std::set<int>::iterator it;
|
|
it = m_red_node.find(n);
|
|
if (it != m_red_node.end())
|
|
{
|
|
*c = video::SColor(255, 255, 0, 0);
|
|
return;
|
|
}
|
|
|
|
it = m_blue_node.find(n);
|
|
if (it != m_blue_node.end())
|
|
{
|
|
*c = video::SColor(255, 0, 0, 255);
|
|
return;
|
|
}
|
|
|
|
if (UserConfigParams::m_track_debug)
|
|
{
|
|
if (m_all_nodes[n]->is3DQuad())
|
|
*c = video::SColor(255, 0, 255, 0);
|
|
else
|
|
*c = video::SColor(255, 255, 255, 0);
|
|
}
|
|
|
|
} // differentNodeColor
|
|
|
|
// -----------------------------------------------------------------------------
|
|
void ArenaGraph::loadNavmesh(const std::string &navmesh)
|
|
{
|
|
XMLNode *xml = file_manager->createXMLTree(navmesh);
|
|
if (xml->getName() != "navmesh")
|
|
{
|
|
Log::error("ArenaGraph", "NavMesh is invalid.");
|
|
delete xml;
|
|
return;
|
|
}
|
|
|
|
std::vector<Vec3> all_vertices;
|
|
for (unsigned int i = 0; i < xml->getNumNodes(); i++)
|
|
{
|
|
const XMLNode *xml_node = xml->getNode(i);
|
|
if (xml_node->getName() == "vertices")
|
|
{
|
|
for (unsigned int i = 0; i < xml_node->getNumNodes(); i++)
|
|
{
|
|
const XMLNode *xml_node_node = xml_node->getNode(i);
|
|
if (!(xml_node_node->getName() == "vertex"))
|
|
{
|
|
Log::error("ArenaGraph", "Unsupported type '%s' found"
|
|
"in '%s' - ignored.",
|
|
xml_node_node->getName().c_str(), navmesh.c_str());
|
|
continue;
|
|
}
|
|
|
|
// Reading vertices
|
|
float x, y, z;
|
|
xml_node_node->get("x", &x);
|
|
xml_node_node->get("y", &y);
|
|
xml_node_node->get("z", &z);
|
|
Vec3 p(x, y, z);
|
|
all_vertices.push_back(p);
|
|
}
|
|
}
|
|
|
|
if (xml_node->getName() == "faces")
|
|
{
|
|
for (unsigned int i = 0; i < xml_node->getNumNodes(); i++)
|
|
{
|
|
const XMLNode *xml_node_node = xml_node->getNode(i);
|
|
if (xml_node_node->getName() != "face")
|
|
{
|
|
Log::error("ArenaGraph", "Unsupported type '%s'"
|
|
" found in '%s' - ignored.",
|
|
xml_node_node->getName().c_str(), navmesh.c_str());
|
|
continue;
|
|
}
|
|
|
|
// Reading quads
|
|
std::vector<int> quad_index;
|
|
std::vector<int> adjacent_quad_index;
|
|
xml_node_node->get("indices", &quad_index);
|
|
xml_node_node->get("adjacents", &adjacent_quad_index);
|
|
if (quad_index.size() != 4)
|
|
{
|
|
Log::error("ArenaGraph", "A Node in navmesh is not made"
|
|
" of quad, will only use the first 4 vertices");
|
|
}
|
|
|
|
createQuad(all_vertices[quad_index[0]],
|
|
all_vertices[quad_index[1]], all_vertices[quad_index[2]],
|
|
all_vertices[quad_index[3]], (int)m_all_nodes.size(),
|
|
false/*invisible*/, false/*ai_ignore*/, true/*is_arena*/,
|
|
false/*ignore*/);
|
|
|
|
ArenaNode* cur_node = getNode((int)m_all_nodes.size() - 1);
|
|
cur_node->setAdjacentNodes(adjacent_quad_index);
|
|
}
|
|
}
|
|
}
|
|
const XMLNode* ht = xml->getNode("height-testing");
|
|
if (ht)
|
|
{
|
|
float min = Graph::MIN_HEIGHT_TESTING;
|
|
float max = Graph::MAX_HEIGHT_TESTING;
|
|
ht->get("min", &min);
|
|
ht->get("max", &max);
|
|
for (unsigned i = 0; i < m_all_nodes.size(); i++)
|
|
{
|
|
m_all_nodes[i]->setHeightTesting(min, max);
|
|
}
|
|
}
|
|
delete xml;
|
|
|
|
} // loadNavmesh
|
|
|
|
// ----------------------------------------------------------------------------
|
|
void ArenaGraph::buildGraph()
|
|
{
|
|
const unsigned int n_nodes = getNumNodes();
|
|
|
|
m_distance_matrix = std::vector<std::vector<float>>
|
|
(n_nodes, std::vector<float>(n_nodes, 9999.9f));
|
|
for (unsigned int i = 0; i < n_nodes; i++)
|
|
{
|
|
ArenaNode* cur_node = getNode(i);
|
|
for (const int& adjacent : cur_node->getAdjacentNodes())
|
|
{
|
|
Vec3 diff = getNode(adjacent)->getCenter() - cur_node->getCenter();
|
|
float distance = diff.length();
|
|
m_distance_matrix[i][adjacent] = distance;
|
|
}
|
|
m_distance_matrix[i][i] = 0.0f;
|
|
}
|
|
|
|
// Allocate and initialise the previous node data structure:
|
|
m_parent_node = std::vector<std::vector<int16_t>>
|
|
(n_nodes, std::vector<int16_t>(n_nodes, Graph::UNKNOWN_SECTOR));
|
|
for (unsigned int i = 0; i < n_nodes; i++)
|
|
{
|
|
for (unsigned int j = 0; j < n_nodes; j++)
|
|
{
|
|
if (i == j || m_distance_matrix[i][j] >= 9899.9f)
|
|
m_parent_node[i][j] = -1;
|
|
else
|
|
m_parent_node[i][j] = i;
|
|
} // for j
|
|
} // for i
|
|
|
|
} // buildGraph
|
|
|
|
// ----------------------------------------------------------------------------
|
|
/** Dijkstra shortest path computation. It computes the shortest distance from
|
|
* the specified node 'source' to all other nodes. At the end of the
|
|
* computation, m_distance_matrix[i][j] stores the shortest path distance from
|
|
* source to j and m_parent_node[source][j] stores the last vertex visited on
|
|
* the shortest path from i to j before visiting j. Suppose the shortest path
|
|
* from i to j is i->......->k->j then m_parent_node[i][j] = k
|
|
*/
|
|
void ArenaGraph::computeDijkstra(int source)
|
|
{
|
|
// Stores the distance (float) to 'source' from a specified node (int)
|
|
typedef std::pair<int, float> IndDistPair;
|
|
|
|
class Shortest
|
|
{
|
|
public:
|
|
bool operator()(const IndDistPair &p1, const IndDistPair &p2)
|
|
{
|
|
return p1.second > p2.second;
|
|
}
|
|
};
|
|
|
|
std::priority_queue<IndDistPair, std::vector<IndDistPair>, Shortest> queue;
|
|
IndDistPair begin(source, 0.0f);
|
|
queue.push(begin);
|
|
const unsigned int n = getNumNodes();
|
|
std::vector<bool> visited;
|
|
visited.resize(n, false);
|
|
while (!queue.empty())
|
|
{
|
|
// Get element with shortest path
|
|
IndDistPair current = queue.top();
|
|
queue.pop();
|
|
int cur_index = current.first;
|
|
if (visited[cur_index]) continue;
|
|
visited[cur_index] = true;
|
|
|
|
for (const int& adjacent : getNode(cur_index)->getAdjacentNodes())
|
|
{
|
|
// Distance already computed, can be ignored
|
|
if (visited[adjacent]) continue;
|
|
|
|
float new_dist =
|
|
current.second + m_distance_matrix[cur_index][adjacent];
|
|
if (new_dist < m_distance_matrix[source][adjacent])
|
|
{
|
|
m_distance_matrix[source][adjacent] = new_dist;
|
|
m_parent_node[source][adjacent] = cur_index;
|
|
}
|
|
IndDistPair pair(adjacent, new_dist);
|
|
queue.push(pair);
|
|
}
|
|
}
|
|
} // computeDijkstra
|
|
|
|
// ----------------------------------------------------------------------------
|
|
/** THIS FUNCTION IS ONLY USED FOR UNIT-TESTING, to verify that the new
|
|
* Dijkstra algorithm gives the same results.
|
|
* computeFloydWarshall() computes the shortest distance between any two
|
|
* nodes. At the end of the computation, m_distance_matrix[i][j] stores the
|
|
* shortest path distance from i to j and m_parent_node[i][j] stores the last
|
|
* vertex visited on the shortest path from i to j before visiting j. Suppose
|
|
* the shortest path from i to j is i->......->k->j then
|
|
* m_parent_node[i][j] = k
|
|
*/
|
|
void ArenaGraph::computeFloydWarshall()
|
|
{
|
|
unsigned int n = getNumNodes();
|
|
|
|
for (unsigned int k = 0; k < n; k++)
|
|
{
|
|
for (unsigned int i = 0; i < n; i++)
|
|
{
|
|
for (unsigned int j = 0; j < n; j++)
|
|
{
|
|
if ((m_distance_matrix[i][k] + m_distance_matrix[k][j]) <
|
|
m_distance_matrix[i][j])
|
|
{
|
|
m_distance_matrix[i][j] =
|
|
m_distance_matrix[i][k] + m_distance_matrix[k][j];
|
|
m_parent_node[i][j] = m_parent_node[k][j];
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
} // computeFloydWarshall
|
|
|
|
// -----------------------------------------------------------------------------
|
|
void ArenaGraph::loadGoalNodes(const XMLNode *node)
|
|
{
|
|
const XMLNode *check_node = node->getNode("checks");
|
|
for (unsigned int i = 0; i < check_node->getNumNodes(); i++)
|
|
{
|
|
const XMLNode *goal = check_node->getNode(i);
|
|
if (goal->getName() =="goal")
|
|
{
|
|
Vec3 p1, p2;
|
|
bool first_goal = false;
|
|
goal->get("first_goal", &first_goal);
|
|
goal->get("p1", &p1);
|
|
goal->get("p2", &p2);
|
|
|
|
int first = Graph::UNKNOWN_SECTOR;
|
|
findRoadSector(p1, &first, NULL, true);
|
|
int last = Graph::UNKNOWN_SECTOR;
|
|
findRoadSector(p2, &last, NULL, true);
|
|
|
|
first_goal ? m_blue_node.insert(first) : m_red_node.insert(first);
|
|
first_goal ? m_blue_node.insert(last) : m_red_node.insert(last);
|
|
while (first != last)
|
|
{
|
|
// Find all the nodes which connect the two points of
|
|
// goal, notice: only work if it's a straight line
|
|
first = getNextNode(first, last);
|
|
first_goal ? m_blue_node.insert(first) :
|
|
m_red_node.insert(first);
|
|
}
|
|
}
|
|
}
|
|
} // loadGoalNodes
|
|
|
|
// ----------------------------------------------------------------------------
|
|
void ArenaGraph::setNearbyNodesOfAllNodes()
|
|
{
|
|
// Only save the nearby 8 nodes
|
|
const unsigned int try_count = 8;
|
|
for (unsigned int i = 0; i < getNumNodes(); i++)
|
|
{
|
|
// Get the distance to all nodes at i
|
|
ArenaNode* cur_node = getNode(i);
|
|
std::vector<int> nearby_nodes;
|
|
std::vector<float> dist = m_distance_matrix[i];
|
|
|
|
// Skip the same node
|
|
dist[i] = 999999.0f;
|
|
for (unsigned int j = 0; j < try_count; j++)
|
|
{
|
|
std::vector<float>::iterator it =
|
|
std::min_element(dist.begin(), dist.end());
|
|
const int pos = int(it - dist.begin());
|
|
nearby_nodes.push_back(pos);
|
|
dist[pos] = 999999.0f;
|
|
}
|
|
cur_node->setNearbyNodes(nearby_nodes);
|
|
}
|
|
|
|
} // setNearbyNodesOfAllNodes
|
|
|
|
// ----------------------------------------------------------------------------
|
|
/** Determines the full path from 'from' to 'to' and returns it in a
|
|
* std::vector (in reverse order). Used only for unit testing.
|
|
*/
|
|
std::vector<int16_t> ArenaGraph::getPathFromTo(int from, int to,
|
|
const std::vector< std::vector< int16_t > >& parent_node)
|
|
{
|
|
std::vector<int16_t> path;
|
|
path.push_back(to);
|
|
while(from!=to)
|
|
{
|
|
to = parent_node[from][to];
|
|
path.push_back(to);
|
|
}
|
|
return path;
|
|
} // getPathFromTo
|
|
|
|
// ============================================================================
|
|
/** Unit testing for arena graph distance and parent node computation.
|
|
* Instead of using hand-tuned test cases we use the tested, verified and
|
|
* easier to understand Floyd-Warshall algorithm to compute the distances,
|
|
* and check if the (significanty faster) Dijkstra algorithm gives the same
|
|
* results. For now we use the cave mesh as test case.
|
|
*/
|
|
void ArenaGraph::unitTesting()
|
|
{
|
|
Track *track = track_manager->getTrack("cave");
|
|
std::string navmesh_file_name=track->getTrackFile("navmesh.xml");
|
|
|
|
double s = StkTime::getRealTime();
|
|
ArenaGraph* ag = new ArenaGraph(navmesh_file_name);
|
|
double e = StkTime::getRealTime();
|
|
Log::error("Time", "Dijkstra %lf", e-s);
|
|
|
|
// Save the Dijkstra results
|
|
std::vector< std::vector< float > > distance_matrix = ag->m_distance_matrix;
|
|
std::vector< std::vector< int16_t > > parent_node = ag->m_parent_node;
|
|
ag->buildGraph();
|
|
|
|
// Now compute results with Floyd-Warshall
|
|
s = StkTime::getRealTime();
|
|
ag->computeFloydWarshall();
|
|
e = StkTime::getRealTime();
|
|
Log::error("Time", "Floyd-Warshall %lf", e-s);
|
|
|
|
int error_count = 0;
|
|
for(unsigned int i=0; i<ag->m_distance_matrix.size(); i++)
|
|
{
|
|
for(unsigned int j=0; j<ag->m_distance_matrix[i].size(); j++)
|
|
{
|
|
if(ag->m_distance_matrix[i][j] - distance_matrix[i][j] > 0.001f)
|
|
{
|
|
Log::error("ArenaGraph",
|
|
"Incorrect distance %d, %d: Dijkstra: %f F.W.: %f",
|
|
i, j, distance_matrix[i][j], ag->m_distance_matrix[i][j]);
|
|
error_count++;
|
|
} // if distance is too different
|
|
|
|
// Unortunately it happens frequently that there are different
|
|
// shortest path with the same length. And Dijkstra might find
|
|
// a different path then Floyd-Warshall. So the test for parent
|
|
// polygon often results in false positives, so it is disabled,
|
|
// but I leave the code in place in case it is useful for some
|
|
// debugging in the feature
|
|
#undef TEST_PARENT_POLY_EVEN_THOUGH_MANY_FALSE_POSITIVES
|
|
#ifdef TEST_PARENT_POLY_EVEN_THOUGH_MANY_FALSE_POSITIVES
|
|
if(ag->m_parent_node[i][j] != parent_node[i][j])
|
|
{
|
|
error_count++;
|
|
std::vector<int16_t> dijkstra_path = getPathFromTo(i, j, parent_node);
|
|
std::vector<int16_t> floyd_path = getPathFromTo(i, j, ag->m_parent_node);
|
|
if(dijkstra_path.size()!=floyd_path.size())
|
|
{
|
|
Log::error("ArenaGraph",
|
|
"Incorrect path length %d, %d: Dijkstra: %d F.W.: %d",
|
|
i, j, parent_node[i][j], ag->m_parent_node[i][j]);
|
|
continue;
|
|
}
|
|
Log::error("ArenaGraph", "Path problems from %d to %d:",
|
|
i, j);
|
|
for (unsigned k = 0; k < dijkstra_path.size(); k++)
|
|
{
|
|
if(dijkstra_path[k]!=floyd_path[k])
|
|
Log::error("ArenaGraph", "%d/%d dijkstra: %d floyd %d",
|
|
k, dijkstra_path.size(), dijkstra_path[k],
|
|
floyd_path[k]);
|
|
} // for k<dijkstra_path.size()
|
|
|
|
} // if dijkstra parent_node != floyd parent node
|
|
#endif
|
|
} // for j
|
|
} // for i
|
|
|
|
delete ag;
|
|
|
|
} // unitTesting
|