From 4b2fbbd3d8576a7bdda824d17abdbdc6e2bab0d9 Mon Sep 17 00:00:00 2001
From: Daniel Evans <danharibo@gmail.com>
Date: Mon, 20 Apr 2015 02:19:30 +0100
Subject: [PATCH] Fix AI Graph discombobulation and debug rendering

---
 rwengine/src/ai/AIGraph.cpp | 18 ++++++++++--------
 rwgame/RWGame.cpp           | 36 ++++++++++++++++++++++++++++++++++--
 rwgame/RWGame.hpp           |  2 ++
 3 files changed, 46 insertions(+), 10 deletions(-)

diff --git a/rwengine/src/ai/AIGraph.cpp b/rwengine/src/ai/AIGraph.cpp
index 67ccccdf..9ba881f0 100644
--- a/rwengine/src/ai/AIGraph.cpp
+++ b/rwengine/src/ai/AIGraph.cpp
@@ -14,8 +14,8 @@ AIGraph::~AIGraph()
 void AIGraph::createPathNodes(const glm::vec3& position, const glm::quat& rotation, PathData& path)
 {
 	size_t startIndex = nodes.size();
-	std::vector<size_t> realNodes;
-	realNodes.reserve(path.nodes.size());
+	std::vector<AIGraphNode*> pathNodes;
+	pathNodes.reserve(path.nodes.size());
 
 	for( size_t n = 0; n < path.nodes.size(); ++n ) {
 		auto& node = path.nodes[n];
@@ -27,7 +27,7 @@ void AIGraph::createPathNodes(const glm::vec3& position, const glm::quat& rotati
 				auto& realNode = externalNodes[rn];
 				auto d = glm::distance2(realNode->position, nodePosition);
 				if( d < 1.f ) {
-					realNodes.push_back(rn);
+					pathNodes.push_back(realNode);
 					ainode = realNode;
 					break;
 				}
@@ -46,7 +46,7 @@ void AIGraph::createPathNodes(const glm::vec3& position, const glm::quat& rotati
 			ainode->external = node.type == PathNode::EXTERNAL;
 			ainode->disabled = false;
 
-			realNodes.push_back(nodes.size());
+			pathNodes.push_back(ainode);
 			nodes.push_back(ainode);
 
 			if( ainode->external )
@@ -68,10 +68,12 @@ void AIGraph::createPathNodes(const glm::vec3& position, const glm::quat& rotati
 	}
 
 	for(size_t pn = 0; pn < path.nodes.size(); ++pn) {
-		if(path.nodes[pn].next >= 0 && (unsigned) path.nodes[pn].next < realNodes.size()) {
-			auto node = nodes[realNodes[pn]];
-			node->connections.push_back(nodes[realNodes[path.nodes[pn].next]]);
-			nodes[realNodes[path.nodes[pn].next]]->connections.push_back(node);
+		if(path.nodes[pn].next >= 0 && (unsigned) path.nodes[pn].next < pathNodes.size()) {
+			auto node = pathNodes[pn];
+			auto next = pathNodes[path.nodes[pn].next];
+			
+			node->connections.push_back(next);
+			next->connections.push_back(node);
 		}
 	}
 }
diff --git a/rwgame/RWGame.cpp b/rwgame/RWGame.cpp
index bbb3818d..ee994fb7 100644
--- a/rwgame/RWGame.cpp
+++ b/rwgame/RWGame.cpp
@@ -26,8 +26,9 @@ DebugDraw* debug;
 StdOutReciever logPrinter;
 
 RWGame::RWGame(const std::string& gamepath, int argc, char* argv[])
-	: engine(nullptr), renderer(nullptr), script(nullptr), inFocus(true), showDebugStats(false),
-	  accum(0.f), timescale(1.f)
+	: engine(nullptr), renderer(nullptr), script(nullptr), inFocus(true),
+	showDebugStats(false), showDebugPaths(false),
+	accum(0.f), timescale(1.f)
 {
 	size_t w = GAME_WINDOW_WIDTH, h = GAME_WINDOW_HEIGHT;
 	bool fullscreen = false;
@@ -440,6 +441,11 @@ void RWGame::render(float alpha, float time)
 	debug->flush(&engine->renderer);
 #endif
 
+	if( showDebugPaths )
+	{
+		renderDebugPaths(time);
+	}
+
 	if ( showDebugStats )
 	{
 		renderDebugStats(time, rendertime);
@@ -549,6 +555,29 @@ void RWGame::renderDebugStats(float time, Renderer::ProfileInfo& worldRenderTime
 	}*/
 }
 
+void RWGame::renderDebugPaths(float time)
+{
+	btVector3 roadColour(1.f, 0.f, 0.f);
+	btVector3 pedColour(0.f, 0.f, 1.f);
+	
+	for( AIGraphNode* n : engine->aigraph.nodes )
+	{
+		btVector3 p( n->position.x, n->position.y, n->position.z );
+		auto& col = n->type == AIGraphNode::Pedestrian ? pedColour : roadColour;
+		debug->drawLine( p - btVector3(0.f, 0.f, 1.f), p + btVector3(0.f, 0.f, 1.f), col);
+		debug->drawLine( p - btVector3(1.f, 0.f, 0.f), p + btVector3(1.f, 0.f, 0.f), col);
+		debug->drawLine( p - btVector3(0.f, 1.f, 0.f), p + btVector3(0.f, 1.f, 0.f), col);
+
+		for( AIGraphNode* c : n->connections )
+		{
+			btVector3 f( c->position.x, c->position.y, c->position.z );
+			debug->drawLine( p, f, col);
+		}
+	}
+
+	debug->flush(renderer);
+}
+
 void RWGame::globalKeyEvent(const sf::Event& event)
 {
 	switch (event.key.code) {
@@ -567,6 +596,9 @@ void RWGame::globalKeyEvent(const sf::Event& event)
 	case sf::Keyboard::F1:
 		showDebugStats = ! showDebugStats;
 		break;
+	case sf::Keyboard::F2:
+		showDebugPaths = ! showDebugPaths;
+		break;
 	default: break;
 	}
 }
diff --git a/rwgame/RWGame.hpp b/rwgame/RWGame.hpp
index 009a1fba..35a94e16 100644
--- a/rwgame/RWGame.hpp
+++ b/rwgame/RWGame.hpp
@@ -23,6 +23,7 @@ class RWGame
 	bool inFocus;
 	ViewCamera lastCam, nextCam;
 	bool showDebugStats;
+	bool showDebugPaths;
 	int lastDraws; /// Number of draws issued for the last frame.
 
 	float accum;
@@ -91,6 +92,7 @@ private:
 	void render(float alpha, float dt);
 	
 	void renderDebugStats(float time, Renderer::ProfileInfo& worldRenderTime);
+	void renderDebugPaths(float time);
 
 	void globalKeyEvent(const sf::Event& event);
 };