Update QueryGraph and demo NavMesh

Projects/Skylicht/Graph/Graph/CGraphQuery.cpp

@@ -97,7 +97,7 @@ namespace Skylicht
 
 			core::aabbox3d<f32> childOctreeBox;
 			core::array<core::triangle3df> keepTriangles;
-			core::array<Graph::SObstacleSegment> keepSegments;
+			core::array<core::line3df> keepSegments;
 
 			// calculate children
 			if (!node->Box.isEmpty() && node->Triangles.size() > m_minimalPolysPerNode)
@@ -161,17 +161,17 @@ namespace Skylicht
 					keepTriangles.clear();
 
 					// step 3: collect obstacle segment
-					core::array<SObstacleSegment>& segs = node->Obstacle.getSegments();
+					core::array<core::line3df>& segs = node->Obstacle.getSegments();
 					for (u32 i = 0, n = segs.size(); i < n; i++)
 					{
-						SObstacleSegment& s = segs[i];
+						core::line3df& s = segs[i];
 
-						if (childNode->OctreeBox.isPointInside(s.Begin) && childNode->OctreeBox.isPointInside(s.End))
+						if (childNode->OctreeBox.isPointInside(s.start) && childNode->OctreeBox.isPointInside(s.end))
 						{
-							childNode->Obstacle.addSegment(s.Begin, s.End);
+							childNode->Obstacle.addSegment(s.start, s.end);
 
-							childNode->Box.addInternalPoint(s.Begin);
-							childNode->Box.addInternalPoint(s.End);
+							childNode->Box.addInternalPoint(s.start);
+							childNode->Box.addInternalPoint(s.end);
 						}
 						else
 						{
@@ -275,11 +275,17 @@ namespace Skylicht
 
 		void CGraphQuery::getTriangles(const core::aabbox3df& box, core::array<core::triangle3df*>& result)
 		{
+			if (m_root == NULL)
+				return;
+
 			getTrianglesFromOctree(result, m_root, box);
 		}
 
 		void CGraphQuery::getObstacles(const core::aabbox3df& box, CObstacleAvoidance& obstacle)
 		{
+			if (m_root == NULL)
+				return;
+
 			getObstacleFromOctree(obstacle, m_root, box);
 		}
 
@@ -359,13 +365,13 @@ namespace Skylicht
 			COctreeNode* node,
 			const core::aabbox3df& box)
 		{
-			core::array<SObstacleSegment>& segs = node->Obstacle.getSegments();
+			core::array<core::line3df>& segs = node->Obstacle.getSegments();
 			for (u32 i = 0, n = segs.size(); i < n; i++)
 			{
-				SObstacleSegment& s = segs[i];
-				if (box.isPointInside(s.Begin) || box.isPointInside(s.End))
+				core::line3df& s = segs[i];
+				if (box.intersectsWithLine(s))
 				{
-					obstacle.addSegment(s.Begin, s.End);
+					obstacle.addSegment(s.start, s.end);
 				}
 			}
 

Projects/Skylicht/Graph/ObstacleAvoidance/CObstacleAvoidance.cpp

@@ -43,20 +43,20 @@ namespace Skylicht
 
 		void CObstacleAvoidance::addSegment(const core::vector3df& begin, const core::vector3df& end)
 		{
-			m_segments.push_back(SObstacleSegment());
-			SObstacleSegment& segment = m_segments.getLast();
-			segment.Begin = begin;
-			segment.End = end;
+			m_segments.push_back(core::line3df());
+			core::line3df& segment = m_segments.getLast();
+			segment.start = begin;
+			segment.end = end;
 		}
 
-		void CObstacleAvoidance::addSegments(const core::array<SObstacleSegment>& segments)
+		void CObstacleAvoidance::addSegments(const core::array<core::line3df>& segments)
 		{
 			for (u32 i = 0, n = segments.size(); i < n; i++)
 			{
-				m_segments.push_back(SObstacleSegment());
-				SObstacleSegment& segment = m_segments.getLast();
-				segment.Begin = segments[i].Begin;
-				segment.End = segments[i].End;
+				m_segments.push_back(core::line3df());
+				core::line3df& segment = m_segments.getLast();
+				segment.start = segments[i].start;
+				segment.end = segments[i].end;
 			}
 		}
 
@@ -101,17 +101,17 @@ namespace Skylicht
 
 		bool CObstacleAvoidance::isLineHit(const core::vector3df& a, const core::vector3df& b, float h)
 		{
-			SObstacleSegment* segs = m_segments.pointer();
+			core::line3df* segs = m_segments.pointer();
 			core::vector3df v = b - a;
 			float t = 0.0f;
 
 			for (u32 i = 0, n = m_segments.size(); i < n; i++)
 			{
-				SObstacleSegment& s = segs[i];
+				core::line3df& s = segs[i];
 
-				if (fabs(s.Begin.Y - a.Y) < h && fabs(s.End.Y - a.Y) < h)
+				if (fabs(s.start.Y - a.Y) < h && fabs(s.end.Y - a.Y) < h)
 				{
-					int intersection = isectRaySeg(a, v, s.Begin, s.End, t);
+					int intersection = isectRaySeg(a, v, s.start, s.end, t);
 					if (intersection)
 					{
 						return true;
@@ -123,16 +123,16 @@ namespace Skylicht
 
 		void CObstacleAvoidance::copySegments(CObstacleAvoidance* toTarget, const core::aabbox3df& box)
 		{
-			SObstacleSegment* segs = m_segments.pointer();
+			core::line3df* segs = m_segments.pointer();
 			core::line3df line;
 
 			for (u32 i = 0, n = m_segments.size(); i < n; i++)
 			{
-				SObstacleSegment& s = segs[i];
-				line.setLine(s.Begin, s.End);
+				core::line3df& s = segs[i];
+				line.setLine(s.start, s.end);
 				if (box.intersectsWithLine(line))
 				{
-					toTarget->addSegment(s.Begin, s.End);
+					toTarget->addSegment(s.start, s.end);
 				}
 			}
 		}
@@ -145,29 +145,29 @@ namespace Skylicht
 				return position;
 			}
 
-			SObstacleSegment* segs = m_segments.pointer();
+			core::line3df* segs = m_segments.pointer();
 
 			core::vector3df intersectionPoint;
 			core::vector3df intersectionNormal;
 
-			SObstacleSegment* intersectionSeg = NULL;
+			core::line3df* intersectionSeg = NULL;
 			float tmin = 2.0f;
 
 			core::vector3df r = vel;
 			r.normalize();
 
 			for (u32 i = 0, n = m_segments.size(); i < n; i++)
 			{
-				SObstacleSegment& s = segs[i];
+				core::line3df& s = segs[i];
 
-				float bY = fabsf(s.Begin.Y - position.Y);
-				float eY = fabsf(s.End.Y - position.Y);
+				float bY = fabsf(s.start.Y - position.Y);
+				float eY = fabsf(s.end.Y - position.Y);
 
 				if (bY > stepHeight && eY > stepHeight)
 					continue;
 
 				float t = 0.0f;
-				int intersection = isectRaySeg(position, vel, s.Begin, s.End, t);
+				int intersection = isectRaySeg(position, vel, s.start, s.end, t);
 				if (intersection)
 				{
 					if (t < tmin)
@@ -183,7 +183,7 @@ namespace Skylicht
 
 			if (intersectionSeg)
 			{
-				core::vector3df v = intersectionSeg->End - intersectionSeg->Begin;
+				core::vector3df v = intersectionSeg->end - intersectionSeg->start;
 
 				intersectionNormal = v.crossProduct(core::vector3df(0.0f, 1.0f, 0.0f));
 				intersectionNormal.normalize();

Projects/Skylicht/Graph/ObstacleAvoidance/CObstacleAvoidance.h

@@ -34,16 +34,10 @@ namespace Skylicht
 			float Radius;
 		};
 
-		struct SObstacleSegment
-		{
-			core::vector3df Begin;
-			core::vector3df End;
-		};
-
 		class CObstacleAvoidance
 		{
 		protected:
-			core::array<SObstacleSegment> m_segments;
+			core::array<core::line3df> m_segments;
 
 		public:
 			CObstacleAvoidance();
@@ -52,13 +46,13 @@ namespace Skylicht
 
 			void addSegment(const core::vector3df& begin, const core::vector3df& end);
 
-			void addSegments(const core::array<SObstacleSegment>& segments);
+			void addSegments(const core::array<core::line3df>& segments);
 
 			void clear();
 
 			void copySegments(CObstacleAvoidance* toTarget, const core::aabbox3df& box);
 
-			inline core::array<SObstacleSegment>& getSegments()
+			inline core::array<core::line3df>& getSegments()
 			{
 				return m_segments;
 			}

Samples/Graph/Source/CDemoNavMesh.cpp

@@ -80,6 +80,8 @@ void CDemoNavMesh::init()
 	if (m_map)
 		m_map->setVisible(true);
 
+	m_agent->getComponent<CMoveAgent>()->setGraphQuery(m_query);
+
 	m_clickPosition.set(0.0f, 0.0f, 0.0f);
 }
 
@@ -158,10 +160,10 @@ void CDemoNavMesh::update()
 	// draw bound obstacle
 	if (m_drawDebugObstacle)
 	{
-		core::array<Graph::SObstacleSegment>& segments = m_obstacle->getSegments();
+		core::array<core::line3df>& segments = m_obstacle->getSegments();
 		for (u32 i = 0, n = segments.size(); i < n; i++)
 		{
-			debug->addLine(segments[i].Begin, segments[i].End, red);
+			debug->addLine(segments[i], red);
 		}
 	}
 
@@ -221,6 +223,11 @@ void CDemoNavMesh::update()
 
 void CDemoNavMesh::onGUI()
 {
+	ImGui::Text("Demo NavMesh");
+	ImGui::Text("After build NavMesh");
+	ImGui::Text("- Left mouse to set Agent position");
+	ImGui::Text("- Right mouse to move Agent");
+
 	if (ImGui::CollapsingHeader("Draw Debug"))
 	{
 		ImGui::Checkbox("Recast mesh", &m_drawDebugRecastMesh);
@@ -279,20 +286,22 @@ void CDemoNavMesh::onViewRayClick(const core::line3df& ray, int button, bool hol
 		m_clickPosition = outIntersection;
 		m_pickTile = m_walkingTileMap->getTileByPosition(m_clickPosition);
 
+		CMoveAgent* moveAgent = m_agent->getComponent<CMoveAgent>();
+
 		if (button == 0)
 		{
-			CMoveAgent* moveAgent = m_agent->getComponent<CMoveAgent>();
+			// left click
 			moveAgent->setPosition(m_clickPosition);
 			moveAgent->setTargetPosition(m_clickPosition);
 		}
+		else
+		{
+			// right click
+			moveAgent->setTargetPosition(m_clickPosition);
+		}
 	}
 }
 
-void CDemoNavMesh::onLeftClickPosition(bool holdShift, const core::vector3df& pos)
-{
-
-}
-
 void CDemoNavMesh::buildNavMesh()
 {
 	m_builder->build(m_recastMesh, m_outputNavMesh, m_obstacle);

Samples/Graph/Source/CDemoNavMesh.h

@@ -51,8 +51,6 @@ class CDemoNavMesh : public CDemo
 
 	virtual void onViewRayClick(const core::line3df& ray, int button, bool holdShift);
 
-	virtual void onLeftClickPosition(bool holdShift, const core::vector3df& pos);
-
 	void buildNavMesh();
 
 	void buildWalkingMap();

Samples/Graph/Source/CDemoObstacleAvoidance.cpp

@@ -77,10 +77,10 @@ void CDemoObstacleAvoidance::update()
 
 	SColor red(255, 200, 0, 0);
 
-	core::array<Graph::SObstacleSegment>& segments = m_obstacle->getSegments();
+	core::array<core::line3df>& segments = m_obstacle->getSegments();
 	for (u32 i = 0, n = segments.size(); i < n; i++)
 	{
-		debug->addLine(segments[i].Begin, segments[i].End, red);
+		debug->addLine(segments[i], red);
 	}
 }
 

Samples/Graph/Source/CMoveAgent.cpp

@@ -2,19 +2,20 @@
 #include "CMoveAgent.h"
 
 #include "GameObject/CGameObject.h"
-
+#include "Debug/CSceneDebug.h"
 #include "Utils/CVector.h"
 
 CMoveAgent::CMoveAgent() :
 	m_obstacle(NULL),
+	m_graphQuery(NULL),
 	m_agentRadius(0.5f)
 {
-
+	m_obstacle = new Graph::CObstacleAvoidance();
 }
 
 CMoveAgent::~CMoveAgent()
 {
-
+	delete m_obstacle;
 }
 
 void CMoveAgent::initComponent()
@@ -32,6 +33,27 @@ void CMoveAgent::updateComponent()
 
 	if (m_obstacle)
 	{
+		if (m_graphQuery)
+		{
+			core::aabbox3df box;
+			core::vector3df r(m_agentRadius, 0.0f, m_agentRadius);
+
+			box.MinEdge = newPosition - r;
+			box.MaxEdge = newPosition + core::vector3df(0.0f, 2.0f, 0.0f) + r;
+
+			m_obstacle->clear();
+			m_graphQuery->getObstacles(box, *m_obstacle);
+
+			/*
+			core::array<core::line3df>& segs = m_obstacle->getSegments();
+			SColor c(255, 255, 0, 255);
+			for (u32 i = 0, n = segs.size(); i < n; i++)
+			{
+				CSceneDebug::getInstance()->addLine(segs[i], c);
+			}
+			*/
+		}
+
 		core::vector3df velocity = newPosition - position;
 		newPosition = m_obstacle->collide(position, velocity);
 	}

Samples/Graph/Source/CMoveAgent.h

@@ -2,11 +2,13 @@
 
 #include "Components/CComponentSystem.h"
 #include "ObstacleAvoidance/CObstacleAvoidance.h"
+#include "Graph/CGraphQuery.h"
 
 class CMoveAgent : public CComponentSystem
 {
 protected:
 	Graph::CObstacleAvoidance* m_obstacle;
+	Graph::CGraphQuery* m_graphQuery;
 
 	core::vector3df m_targetPosition;
 
@@ -23,14 +25,23 @@ class CMoveAgent : public CComponentSystem
 
 	void setPosition(const core::vector3df& position);
 
+	inline void setGraphQuery(Graph::CGraphQuery* query)
+	{
+		m_graphQuery = query;
+	}
+
 	inline void setTargetPosition(const core::vector3df& position)
 	{
 		m_targetPosition = position;
 	}
 
 	inline void setObstacle(Graph::CObstacleAvoidance* obstacle)
 	{
-		m_obstacle = obstacle;
+		if (m_obstacle)
+		{
+			m_obstacle->clear();
+			m_obstacle->addSegments(obstacle->getSegments());
+		}
 	}
 
 	inline void setAgentRadius(float radius)