Added FindPath() function to NavigationMesh.

Added navigation test script.

Bin/Data/Scripts/Navigation.as

@@ -0,0 +1,404 @@
+Scene@ testScene;
+Camera@ camera;
+Node@ cameraNode;
+
+float yaw = 0.0;
+float pitch = 0.0;
+int drawDebug = 0;
+
+NavigationMesh@ navMesh;
+
+Vector3 startPos;
+Vector3 endPos;
+
+bool setStartPos = true;
+bool startPosSet = false;
+bool endPosSet = false;
+
+Array<Vector3> path;
+
+void Start()
+{
+    if (!engine.headless)
+    {
+        InitConsole();
+        InitUI();
+    }
+    else
+        OpenConsoleWindow();
+
+    InitScene();
+
+    SubscribeToEvent("Update", "HandleUpdate");
+    SubscribeToEvent("KeyDown", "HandleKeyDown");
+    SubscribeToEvent("MouseMove", "HandleMouseMove");
+    SubscribeToEvent("MouseButtonDown", "HandleMouseButtonDown");
+    SubscribeToEvent("MouseButtonUp", "HandleMouseButtonUp");
+    SubscribeToEvent("PostRenderUpdate", "HandlePostRenderUpdate");
+}
+
+void InitConsole()
+{
+    XMLFile@ uiStyle = cache.GetResource("XMLFile", "UI/DefaultStyle.xml");
+
+    engine.CreateDebugHud();
+    debugHud.style = uiStyle;
+    debugHud.mode = DEBUGHUD_SHOW_ALL;
+
+    engine.CreateConsole();
+    console.style = uiStyle;
+}
+
+void InitUI()
+{
+    XMLFile@ uiStyle = cache.GetResource("XMLFile", "UI/DefaultStyle.xml");
+
+    Cursor@ newCursor = Cursor("Cursor");
+    newCursor.style = uiStyle;
+    newCursor.position = IntVector2(graphics.width / 2, graphics.height / 2);
+    ui.cursor = newCursor;
+    if (GetPlatform() == "Android" || GetPlatform() == "iOS")
+        ui.cursor.visible = false;
+}
+
+void InitScene()
+{
+    testScene = Scene("TestScene");
+
+    // Enable access to this script file & scene from the console
+    script.defaultScene = testScene;
+    script.defaultScriptFile = scriptFile;
+
+    // Create the camera outside the scene so it is unaffected by scene load/save
+    cameraNode = Node();
+    camera = cameraNode.CreateComponent("Camera");
+    cameraNode.position = Vector3(0, 2, 0);
+
+    if (!engine.headless)
+    {
+        renderer.viewports[0] = Viewport(testScene, camera);
+        
+        // Add bloom & FXAA effects to the renderpath. Clone the default renderpath so that we don't affect it
+        RenderPath@ newRenderPath = renderer.viewports[0].renderPath.Clone();
+        newRenderPath.Append(cache.GetResource("XMLFile", "PostProcess/Bloom.xml"));
+        newRenderPath.Append(cache.GetResource("XMLFile", "PostProcess/EdgeFilter.xml"));
+        newRenderPath.SetEnabled("Bloom", false);
+        newRenderPath.SetEnabled("EdgeFilter", false);
+        renderer.viewports[0].renderPath = newRenderPath;
+
+        audio.listener = cameraNode.CreateComponent("SoundListener");
+    }
+
+    PhysicsWorld@ world = testScene.CreateComponent("PhysicsWorld");
+    testScene.CreateComponent("Octree");
+    testScene.CreateComponent("DebugRenderer");
+
+    Node@ zoneNode = testScene.CreateChild("Zone");
+    Zone@ zone = zoneNode.CreateComponent("Zone");
+    zone.ambientColor = Color(0.15, 0.15, 0.15);
+    zone.fogColor = Color(0.5, 0.5, 0.7);
+    zone.fogStart = 100.0;
+    zone.fogEnd = 300.0;
+    zone.boundingBox = BoundingBox(-1000, 1000);
+
+    {
+        Node@ lightNode = testScene.CreateChild("GlobalLight");
+        lightNode.direction = Vector3(0.3, -0.5, 0.425);
+
+        Light@ light = lightNode.CreateComponent("Light");
+        light.lightType = LIGHT_DIRECTIONAL;
+        light.castShadows = true;
+        light.shadowBias = BiasParameters(0.0001, 0.5);
+        light.shadowCascade = CascadeParameters(10.0, 50.0, 200.0, 0.0, 0.8);
+        light.specularIntensity = 0.5;
+    }
+
+    {
+        Node@ objectNode = testScene.CreateChild("Floor");
+        objectNode.position = Vector3(0, -0.5, 0);
+        objectNode.scale = Vector3(200, 1, 200);
+
+        StaticModel@ object = objectNode.CreateComponent("StaticModel");
+        object.model = cache.GetResource("Model", "Models/Box.mdl");
+        object.material = cache.GetResource("Material", "Materials/StoneTiled.xml");
+        object.occluder = true;
+
+        RigidBody@ body = objectNode.CreateComponent("RigidBody");
+        CollisionShape@ shape = objectNode.CreateComponent("CollisionShape");
+        shape.SetBox(Vector3(1, 1, 1));
+    }
+
+    for (uint i = 0; i < 50; ++i)
+    {
+        Node@ objectNode = testScene.CreateChild("Box");
+        objectNode.position = Vector3(Random() * 180 - 90, 1, Random() * 180 - 90);
+        objectNode.SetScale(2);
+
+        StaticModel@ object = objectNode.CreateComponent("StaticModel");
+        object.model = cache.GetResource("Model", "Models/Box.mdl");
+        object.material = cache.GetResource("Material", "Materials/Stone.xml");
+        object.castShadows = true;
+
+        RigidBody@ body = objectNode.CreateComponent("RigidBody");
+        CollisionShape@ shape = objectNode.CreateComponent("CollisionShape");
+        shape.SetBox(Vector3(1, 1, 1));
+    }
+
+    for (uint i = 0; i < 10; ++i)
+    {
+        Node@ objectNode = testScene.CreateChild("Box");
+        objectNode.position = Vector3(Random() * 180 - 90, 10, Random() * 180 - 90);
+        objectNode.SetScale(20);
+
+        StaticModel@ object = objectNode.CreateComponent("StaticModel");
+        object.model = cache.GetResource("Model", "Models/Box.mdl");
+        object.material = cache.GetResource("Material", "Materials/Stone.xml");
+        object.castShadows = true;
+        object.occluder = true;
+
+        RigidBody@ body = objectNode.CreateComponent("RigidBody");
+        CollisionShape@ shape = objectNode.CreateComponent("CollisionShape");
+        shape.SetBox(Vector3(1, 1, 1));
+    }
+
+    for (uint i = 0; i < 50; ++i)
+    {
+        Node@ objectNode = testScene.CreateChild("Mushroom");
+        objectNode.position = Vector3(Random() * 180 - 90, 0, Random() * 180 - 90);
+        objectNode.rotation = Quaternion(0, Random(360.0), 0);
+        objectNode.SetScale(5);
+
+        StaticModel@ object = objectNode.CreateComponent("StaticModel");
+        object.model = cache.GetResource("Model", "Models/Mushroom.mdl");
+        object.material = cache.GetResource("Material", "Materials/Mushroom.xml");
+        object.castShadows = true;
+
+        RigidBody@ body = objectNode.CreateComponent("RigidBody");
+        CollisionShape@ shape = objectNode.CreateComponent("CollisionShape");
+        shape.SetTriangleMesh(object.model, 0);
+    }
+    
+    testScene.CreateComponent("Navigable");
+    navMesh = testScene.CreateComponent("NavigationMesh");
+    navMesh.Build();
+}
+
+void HandleUpdate(StringHash eventType, VariantMap& eventData)
+{
+    float timeStep = eventData["TimeStep"].GetFloat();
+
+    if (ui.focusElement is null)
+    {
+        float speedMultiplier = 1.0;
+        if (input.keyDown[KEY_LSHIFT])
+            speedMultiplier = 5.0;
+        if (input.keyDown[KEY_LCTRL])
+            speedMultiplier = 0.1;
+
+        if (input.keyDown['W'])
+            cameraNode.TranslateRelative(Vector3(0, 0, 10) * timeStep * speedMultiplier);
+        if (input.keyDown['S'])
+            cameraNode.TranslateRelative(Vector3(0, 0, -10) * timeStep * speedMultiplier);
+        if (input.keyDown['A'])
+            cameraNode.TranslateRelative(Vector3(-10, 0, 0) * timeStep * speedMultiplier);
+        if (input.keyDown['D'])
+            cameraNode.TranslateRelative(Vector3(10, 0, 0) * timeStep * speedMultiplier);
+    }
+}
+
+void HandleKeyDown(StringHash eventType, VariantMap& eventData)
+{
+    int key = eventData["Key"].GetInt();
+
+    if (key == KEY_ESC)
+    {
+        if (ui.focusElement is null)
+            engine.Exit();
+        else
+            console.visible = false;
+    }
+    
+    if (key == KEY_F1)
+        console.Toggle();
+
+    if (ui.focusElement is null)
+    {
+        if (key == '1')
+        {
+            int quality = renderer.textureQuality;
+            ++quality;
+            if (quality > 2)
+                quality = 0;
+            renderer.textureQuality = quality;
+        }
+
+        if (key == '2')
+        {
+            int quality = renderer.materialQuality;
+            ++quality;
+            if (quality > 2)
+                quality = 0;
+            renderer.materialQuality = quality;
+        }
+
+        if (key == '3')
+            renderer.specularLighting = !renderer.specularLighting;
+
+        if (key == '4')
+            renderer.drawShadows = !renderer.drawShadows;
+
+        if (key == '5')
+        {
+            int size = renderer.shadowMapSize;
+            size *= 2;
+            if (size > 2048)
+                size = 512;
+            renderer.shadowMapSize = size;
+        }
+
+        if (key == '6')
+            renderer.shadowQuality = renderer.shadowQuality + 1;
+
+        if (key == '7')
+        {
+            bool occlusion = renderer.maxOccluderTriangles > 0;
+            occlusion = !occlusion;
+            renderer.maxOccluderTriangles = occlusion ? 5000 : 0;
+        }
+
+        if (key == '8')
+            renderer.dynamicInstancing = !renderer.dynamicInstancing;
+
+        if (key == ' ')
+        {
+            drawDebug++;
+            if (drawDebug > 3)
+                drawDebug = 0;
+        }
+
+        if (key == 'B')
+            renderer.viewports[0].renderPath.ToggleEnabled("Bloom");
+
+        if (key == 'F')
+            renderer.viewports[0].renderPath.ToggleEnabled("EdgeFilter");
+
+        if (key == 'O')
+            camera.orthographic = !camera.orthographic;
+
+        if (key == 'T')
+            debugHud.Toggle(DEBUGHUD_SHOW_PROFILER);
+
+        if (key == KEY_F5)
+        {
+            File@ xmlFile = File(fileSystem.programDir + "Data/Scenes/TestScene.xml", FILE_WRITE);
+            testScene.SaveXML(xmlFile);
+        }
+
+        if (key == KEY_F7)
+        {
+            File@ xmlFile = File(fileSystem.programDir + "Data/Scenes/TestScene.xml", FILE_READ);
+            if (xmlFile.open)
+                testScene.LoadXML(xmlFile);
+        }
+    }
+}
+
+void HandleMouseMove(StringHash eventType, VariantMap& eventData)
+{
+    if (eventData["Buttons"].GetInt() & MOUSEB_RIGHT != 0)
+    {
+        int mousedx = eventData["DX"].GetInt();
+        int mousedy = eventData["DY"].GetInt();
+        yaw += mousedx / 10.0;
+        pitch += mousedy / 10.0;
+        if (pitch < -90.0)
+            pitch = -90.0;
+        if (pitch > 90.0)
+            pitch = 90.0;
+
+        cameraNode.rotation = Quaternion(pitch, yaw, 0);
+    }
+}
+
+void HandleMouseButtonDown(StringHash eventType, VariantMap& eventData)
+{
+    int button = eventData["Button"].GetInt();
+    if (button == MOUSEB_RIGHT)
+        ui.cursor.visible = false;
+
+    if (button == MOUSEB_LEFT && ui.GetElementAt(ui.cursorPosition, true) is null && ui.focusElement is null)
+    {
+        IntVector2 pos = ui.cursorPosition;
+        Ray cameraRay = camera.GetScreenRay(float(pos.x) / graphics.width, float(pos.y) / graphics.height);
+        RayQueryResult result = testScene.octree.RaycastSingle(cameraRay, RAY_TRIANGLE, 250.0, DRAWABLE_GEOMETRY);
+    
+        if (result.drawable !is null)
+        {
+            Vector3 rayHitPos = cameraRay.origin + cameraRay.direction * result.distance;
+            if (setStartPos)
+            {
+                startPos = rayHitPos;
+                startPosSet = true;
+            }
+            else
+            {
+                endPos = rayHitPos;
+                endPosSet = true;
+            }
+            setStartPos = !setStartPos;
+            
+            if (startPosSet && endPosSet)
+                path = navMesh.FindPath(startPos, endPos);
+        }
+    }
+}
+
+void HandleMouseButtonUp(StringHash eventType, VariantMap& eventData)
+{
+    if (eventData["Button"].GetInt() == MOUSEB_RIGHT)
+        ui.cursor.visible = true;
+}
+
+void HandlePostRenderUpdate()
+{
+    if (engine.headless)
+        return;
+
+    // Draw rendering debug geometry without depth test to see the effect of occlusion
+    if (drawDebug == 1)
+        renderer.DrawDebugGeometry(false);
+    if (drawDebug == 2)
+        testScene.physicsWorld.DrawDebugGeometry(true);
+    if (drawDebug == 3)
+        navMesh.DrawDebugGeometry(testScene.debugRenderer, false);
+
+    IntVector2 pos = ui.cursorPosition;
+    if (ui.GetElementAt(pos, true) is null && testScene.octree !is null)
+    {
+        Ray cameraRay = camera.GetScreenRay(float(pos.x) / graphics.width, float(pos.y) / graphics.height);
+        RayQueryResult result = testScene.octree.RaycastSingle(cameraRay, RAY_TRIANGLE, 250.0, DRAWABLE_GEOMETRY);
+        if (result.drawable !is null)
+        {
+            Vector3 rayHitPos = cameraRay.origin + cameraRay.direction * result.distance;
+            testScene.debugRenderer.AddBoundingBox(BoundingBox(rayHitPos + Vector3(-0.01, -0.01, -0.01), rayHitPos +
+                Vector3(0.01, 0.01, 0.01)), Color(1.0, 1.0, 1.0), true);
+        }
+    }
+    
+    if (startPosSet)
+    {
+        testScene.debugRenderer.AddBoundingBox(BoundingBox(startPos + Vector3(-0.1, -0.1, -0.1), startPos +
+                Vector3(0.1, 0.1, 0.1)), Color(1.0, 1.0, 1.0), true);
+    }
+    if (endPosSet)
+    {
+        testScene.debugRenderer.AddBoundingBox(BoundingBox(endPos + Vector3(-0.1, -0.1, -0.1), endPos +
+                Vector3(0.1, 0.1, 0.1)), Color(1.0, 1.0, 1.0), true);
+    }
+
+    if (path.length > 1)
+    {
+        for (uint i = 0; i < path.length - 1; ++i)
+            testScene.debugRenderer.AddLine(path[i], path[i + 1], Color(1.0, 1.0, 1.0), false);
+    }
+}

Docs/ScriptAPI.dox

@@ -5393,6 +5393,7 @@ Methods:<br>
 - void MarkNetworkUpdate() const
 - void DrawDebugGeometry(DebugRenderer@, bool)
 - bool Build()
+- Vector3[]@ FindPath(const Vector3&, const Vector3&, const Vector3& arg2 = Vector3 ( 1.0 , 1.0 , 1.0 ))
 
 Properties:<br>
 - ShortStringHash type (readonly)

Engine/Engine/NavigationAPI.cpp

@@ -33,10 +33,18 @@ void RegisterNavigable(asIScriptEngine* engine)
     RegisterComponent<Navigable>(engine, "Navigable");
 }
 
+static CScriptArray* NavigationMeshFindPath(const Vector3& start, const Vector3& end, const Vector3& extents, NavigationMesh* ptr)
+{
+    PODVector<Vector3> dest;
+    ptr->FindPath(dest, start, end, extents);
+    return VectorToArray<Vector3>(dest, "Array<Vector3>");
+}
+
 void RegisterNavigationMesh(asIScriptEngine* engine)
 {
     RegisterComponent<NavigationMesh>(engine, "NavigationMesh");
     engine->RegisterObjectMethod("NavigationMesh", "bool Build()", asMETHOD(NavigationMesh, Build), asCALL_THISCALL);
+    engine->RegisterObjectMethod("NavigationMesh", "Array<Vector3>@ FindPath(const Vector3&in, const Vector3&in, const Vector3&in extents = Vector3(1.0, 1.0, 1.0))", asFUNCTION(NavigationMeshFindPath), asCALL_CDECL_OBJLAST);
     engine->RegisterObjectMethod("NavigationMesh", "void set_cellSize(float)", asMETHOD(NavigationMesh, SetCellSize), asCALL_THISCALL);
     engine->RegisterObjectMethod("NavigationMesh", "float get_cellSize() const", asMETHOD(NavigationMesh, GetCellSize), asCALL_THISCALL);
     engine->RegisterObjectMethod("NavigationMesh", "void set_cellHeight(float)", asMETHOD(NavigationMesh, SetCellHeight), asCALL_THISCALL);

Engine/Navigation/NavigationMesh.cpp

@@ -36,6 +36,7 @@
 
 #include <DetourNavMesh.h>
 #include <DetourNavMeshBuilder.h>
+#include <DetourNavMeshQuery.h>
 #include <Recast.h>
 
 #include "DebugNew.h"
@@ -56,6 +57,8 @@ static const float DEFAULT_EDGE_MAX_ERROR = 1.3f;
 static const float DEFAULT_DETAIL_SAMPLE_DISTANCE = 6.0f;
 static const float DEFAULT_DETAIL_SAMPLE_MAX_ERROR = 1.0f;
 
+static const int MAX_POLYS = 256;
+
 /// Temporary data for building the Recast navigation mesh.
 struct NavigationBuildData
 {
@@ -124,13 +127,18 @@ NavigationMesh::NavigationMesh(Context* context) :
     edgeMaxError_(DEFAULT_EDGE_MAX_ERROR),
     detailSampleDistance_(DEFAULT_DETAIL_SAMPLE_DISTANCE),
     detailSampleMaxError_(DEFAULT_DETAIL_SAMPLE_MAX_ERROR),
-    navMesh_(0)
+    navMesh_(0),
+    navMeshQuery_(0),
+    queryFilter_(new dtQueryFilter())
 {
 }
 
 NavigationMesh::~NavigationMesh()
 {
     ReleaseNavMesh();
+    
+    delete queryFilter_;
+    queryFilter_ = 0;
 }
 
 void NavigationMesh::RegisterObject(Context* context)
@@ -379,6 +387,14 @@ bool NavigationMesh::Build()
             return false;
         }
         
+        // Set polygon flags.
+        /// \todo Allow to define custom flags
+        for (int i = 0; i < build.polyMesh_->npolys; ++i)
+        {
+            if (build.polyMesh_->areas[i] == RC_WALKABLE_AREA)
+                build.polyMesh_->flags[i] = 0x1;
+        }
+        
         unsigned char* navData = 0;
         int navDataSize = 0;
         
@@ -419,6 +435,46 @@ bool NavigationMesh::Build()
     }
 }
 
+void NavigationMesh::FindPath(PODVector<Vector3>& dest, const Vector3& start, const Vector3& end, const Vector3& extents)
+{
+    PROFILE(FindPath);
+    
+    dest.Clear();
+    
+    if (!navMesh_ || !navMeshQuery_)
+        return;
+    
+    dtPolyRef startRef;
+    dtPolyRef endRef;
+    navMeshQuery_->findNearestPoly(&start.x_, &extents.x_, queryFilter_, &startRef, 0);
+    navMeshQuery_->findNearestPoly(&end.x_, &extents.x_, queryFilter_, &endRef, 0);
+    
+    if (!startRef || !endRef)
+        return;
+    
+    dtPolyRef polys[MAX_POLYS];
+    dtPolyRef pathPolys[MAX_POLYS];
+    Vector3 pathPoints[MAX_POLYS];
+    unsigned char pathFlags[MAX_POLYS];
+    int numPolys = 0;
+    int numPathPoints = 0;
+    
+    navMeshQuery_->findPath(startRef, endRef, &start.x_, &end.x_, queryFilter_, polys, &numPolys, MAX_POLYS);
+    if (!numPolys)
+        return;
+    
+    Vector3 actualEnd = end;
+    
+    // If full path was not found, clamp end point to the end polygon
+    if (polys[numPolys - 1] != endRef)
+        navMeshQuery_->closestPointOnPoly(polys[numPolys - 1], &end.x_, &actualEnd.x_);
+    
+    navMeshQuery_->findStraightPath(&start.x_, &actualEnd.x_, polys, numPolys, &pathPoints[0].x_, pathFlags, pathPolys, &numPathPoints, MAX_POLYS);
+    
+    for (int i = 0; i < numPathPoints; ++i)
+        dest.Push(pathPoints[i]);
+}
+
 void NavigationMesh::SetNavigationDataAttr(const PODVector<unsigned char>& data)
 {
     navigationDataAttr_ = data;
@@ -552,6 +608,22 @@ bool NavigationMesh::CreateNavMesh(unsigned char* navData, unsigned navDataSize)
         return false;
     }
     
+    navMeshQuery_ = dtAllocNavMeshQuery();
+    if (!navMeshQuery_)
+    {
+        LOGERROR("Could not create Detour navmesh query");
+        ReleaseNavMesh();
+        return false;
+    }
+    
+    status = navMeshQuery_->init(navMesh_, 2048);
+    if (dtStatusFailed(status))
+    {
+        LOGERROR("Could not init navmesh query");
+        ReleaseNavMesh();
+        return false;
+    }
+    
     LOGDEBUG("Created Detour navmesh, data size " + String(navDataSize) + " bytes");
     return true;
 }
@@ -560,6 +632,9 @@ void NavigationMesh::ReleaseNavMesh()
 {
     dtFreeNavMesh(navMesh_);
     navMesh_ = 0;
+    
+    dtFreeNavMeshQuery(navMeshQuery_);
+    navMeshQuery_ = 0;
 }
 
 }

Engine/Navigation/NavigationMesh.h

@@ -26,14 +26,9 @@
 #include "BoundingBox.h"
 #include "Component.h"
 
-class rcContext;
 class dtNavMesh;
-
-struct rcHeightfield;
-struct rcCompactHeightfield;
-struct rcContourSet;
-struct rcPolyMesh;
-struct rcPolyMeshDetail;
+class dtNavMeshQuery;
+class dtQueryFilter;
 
 namespace Urho3D
 {
@@ -84,6 +79,8 @@ public:
     void SetDetailSampleMaxError(float error);
     /// Rebuild the navigation data. Return true if successful.
     bool Build();
+    /// Find a path. Return non-empty list of points if successful.
+    void FindPath(PODVector<Vector3>& dest, const Vector3& start, const Vector3& end, const Vector3& extents = Vector3::ONE);
     
     /// Return cell size.
     float GetCellSize() const { return cellSize_; }
@@ -153,6 +150,10 @@ private:
     float detailSampleMaxError_;
     /// Detour navmesh.
     dtNavMesh* navMesh_;
+    /// Detour navmesh query.
+    dtNavMeshQuery* navMeshQuery_;
+    /// Detour navmesh query filter.
+    dtQueryFilter* queryFilter_;
     /// Navigation data attribute. Contains the unmodified Recast data.
     PODVector<unsigned char> navigationDataAttr_;
 };