void NavigationMesh::CollectGeometries(Vector<NavigationGeometryInfo>& geometryList)
{
    ATOMIC_PROFILE(CollectNavigationGeometry);

    // Get Navigable components from child nodes, not from whole scene. This makes it possible to partition
    // the scene into several navigation meshes
    PODVector<Navigable*> navigables;
    node_->GetComponents<Navigable>(navigables, true);

    HashSet<Node*> processedNodes;
    for (unsigned i = 0; i < navigables.Size(); ++i)
    {
        if (navigables[i]->IsEnabledEffective())
            CollectGeometries(geometryList, navigables[i]->GetNode(), processedNodes, navigables[i]->IsRecursive());
    }

    // Get offmesh connections
    Matrix3x4 inverse = node_->GetWorldTransform().Inverse();
    PODVector<OffMeshConnection*> connections;
    node_->GetComponents<OffMeshConnection>(connections, true);

    for (unsigned i = 0; i < connections.Size(); ++i)
    {
        OffMeshConnection* connection = connections[i];
        if (connection->IsEnabledEffective() && connection->GetEndPoint())
        {
            const Matrix3x4& transform = connection->GetNode()->GetWorldTransform();

            NavigationGeometryInfo info;
            info.component_ = connection;
            info.boundingBox_ = BoundingBox(Sphere(transform.Translation(), connection->GetRadius())).Transformed(inverse);

            geometryList.Push(info);
        }
    }

    // Get nav area volumes
    PODVector<NavArea*> navAreas;
    node_->GetComponents<NavArea>(navAreas, true);
    areas_.Clear();
    for (unsigned i = 0; i < navAreas.Size(); ++i)
    {
        NavArea* area = navAreas[i];
        if (area->IsEnabledEffective())
        {
            NavigationGeometryInfo info;
            info.component_ = area;
            info.boundingBox_ = area->GetWorldBoundingBox();
            geometryList.Push(info);
            areas_.Push(WeakPtr<NavArea>(area));
        }
    }
}
void DynamicNavigationMesh::DrawDebugGeometry(DebugRenderer* debug, bool depthTest)
{
    if (!debug || !navMesh_ || !node_)
        return;

    const Matrix3x4& worldTransform = node_->GetWorldTransform();

    const dtNavMesh* navMesh = navMesh_;

    for (int z = 0; z < numTilesZ_; ++z)
    {
        for (int x = 0; x < numTilesX_; ++x)
        {
            // Get the layers from the tile-cache
            const dtMeshTile* tiles[TILECACHE_MAXLAYERS];
            int tileCount = navMesh->getTilesAt(x, z, tiles, TILECACHE_MAXLAYERS);
            for (int i = 0; i < tileCount; ++i)
            {
                const dtMeshTile* tile = tiles[i];
                if (!tile)
                    continue;

                for (int i = 0; i < tile->header->polyCount; ++i)
                {
                    dtPoly* poly = tile->polys + i;
                    for (unsigned j = 0; j < poly->vertCount; ++j)
                    {
                        debug->AddLine(
                            worldTransform * *reinterpret_cast<const Vector3*>(&tile->verts[poly->verts[j] * 3]),
                            worldTransform * *reinterpret_cast<const Vector3*>(&tile->verts[poly->verts[(j + 1) % poly->vertCount] * 3]),
                            Color::YELLOW,
                            depthTest
                            );
                    }
                }
            }
        }
    }

    Scene* scene = GetScene();
    if (scene)
    {
        // Draw Obstacle components
        if (drawObstacles_)
        {
            PODVector<Node*> obstacles;
            scene->GetChildrenWithComponent<Obstacle>(obstacles, true);
            for (unsigned i = 0; i < obstacles.Size(); ++i)
            {
                Obstacle* obstacle = obstacles[i]->GetComponent<Obstacle>();
                if (obstacle && obstacle->IsEnabledEffective())
                    obstacle->DrawDebugGeometry(debug, depthTest);
            }
        }

        // Draw OffMeshConnection components
        if (drawOffMeshConnections_)
        {
            PODVector<Node*> connections;
            scene->GetChildrenWithComponent<OffMeshConnection>(connections, true);
            for (unsigned i = 0; i < connections.Size(); ++i)
            {
                OffMeshConnection* connection = connections[i]->GetComponent<OffMeshConnection>();
                if (connection && connection->IsEnabledEffective())
                    connection->DrawDebugGeometry(debug, depthTest);
            }
        }

        // Draw NavArea components
        if (drawNavAreas_)
        {
            PODVector<Node*> areas;
            scene->GetChildrenWithComponent<NavArea>(areas, true);
            for (unsigned i = 0; i < areas.Size(); ++i)
            {
                NavArea* area = areas[i]->GetComponent<NavArea>();
                if (area && area->IsEnabledEffective())
                    area->DrawDebugGeometry(debug, depthTest);
            }
        }
    }
}
void NavigationMesh::GetTileGeometry(NavBuildData* build, Vector<NavigationGeometryInfo>& geometryList, BoundingBox& box)
{
    Matrix3x4 inverse = node_->GetWorldTransform().Inverse();

    for (unsigned i = 0; i < geometryList.Size(); ++i)
    {
        if (box.IsInsideFast(geometryList[i].boundingBox_) != OUTSIDE)
        {
            const Matrix3x4& transform = geometryList[i].transform_;

            if (geometryList[i].component_->GetType() == OffMeshConnection::GetTypeStatic())
            {
                OffMeshConnection* connection = static_cast<OffMeshConnection*>(geometryList[i].component_);
                Vector3 start = inverse * connection->GetNode()->GetWorldPosition();
                Vector3 end = inverse * connection->GetEndPoint()->GetWorldPosition();

                build->offMeshVertices_.Push(start);
                build->offMeshVertices_.Push(end);
                build->offMeshRadii_.Push(connection->GetRadius());
                build->offMeshFlags_.Push((unsigned short)connection->GetMask());
                build->offMeshAreas_.Push((unsigned char)connection->GetAreaID());
                build->offMeshDir_.Push((unsigned char)(connection->IsBidirectional() ? DT_OFFMESH_CON_BIDIR : 0));
                continue;
            }
            else if (geometryList[i].component_->GetType() == NavArea::GetTypeStatic())
            {
                NavArea* area = static_cast<NavArea*>(geometryList[i].component_);
                NavAreaStub stub;
                stub.areaID_ = (unsigned char)area->GetAreaID();
                stub.bounds_ = area->GetWorldBoundingBox();
                build->navAreas_.Push(stub);
                continue;
            }

#ifdef ATOMIC_PHYSICS
            CollisionShape* shape = dynamic_cast<CollisionShape*>(geometryList[i].component_);
            if (shape)
            {
                switch (shape->GetShapeType())
                {
                case SHAPE_TRIANGLEMESH:
                    {
                        Model* model = shape->GetModel();
                        if (!model)
                            continue;

                        unsigned lodLevel = shape->GetLodLevel();
                        for (unsigned j = 0; j < model->GetNumGeometries(); ++j)
                            AddTriMeshGeometry(build, model->GetGeometry(j, lodLevel), transform);
                    }
                    break;

                case SHAPE_CONVEXHULL:
                    {
                        ConvexData* data = static_cast<ConvexData*>(shape->GetGeometryData());
                        if (!data)
                            continue;

                        unsigned numVertices = data->vertexCount_;
                        unsigned numIndices = data->indexCount_;
                        unsigned destVertexStart = build->vertices_.Size();

                        for (unsigned j = 0; j < numVertices; ++j)
                            build->vertices_.Push(transform * data->vertexData_[j]);

                        for (unsigned j = 0; j < numIndices; ++j)
                            build->indices_.Push(data->indexData_[j] + destVertexStart);
                    }
                    break;

                case SHAPE_BOX:
                    {
                        unsigned destVertexStart = build->vertices_.Size();

                        build->vertices_.Push(transform * Vector3(-0.5f, 0.5f, -0.5f));
                        build->vertices_.Push(transform * Vector3(0.5f, 0.5f, -0.5f));
                        build->vertices_.Push(transform * Vector3(0.5f, -0.5f, -0.5f));
                        build->vertices_.Push(transform * Vector3(-0.5f, -0.5f, -0.5f));
                        build->vertices_.Push(transform * Vector3(-0.5f, 0.5f, 0.5f));
                        build->vertices_.Push(transform * Vector3(0.5f, 0.5f, 0.5f));
                        build->vertices_.Push(transform * Vector3(0.5f, -0.5f, 0.5f));
                        build->vertices_.Push(transform * Vector3(-0.5f, -0.5f, 0.5f));

                        const unsigned indices[] = {
                            0, 1, 2, 0, 2, 3, 1, 5, 6, 1, 6, 2, 4, 5, 1, 4, 1, 0, 5, 4, 7, 5, 7, 6,
                            4, 0, 3, 4, 3, 7, 1, 0, 4, 1, 4, 5
                        };

                        for (unsigned j = 0; j < 36; ++j)
                            build->indices_.Push(indices[j] + destVertexStart);
                    }
                    break;

                default:
                    break;
                }

                continue;
            }
#endif
            Drawable* drawable = dynamic_cast<Drawable*>(geometryList[i].component_);
            if (drawable)
            {
                const Vector<SourceBatch>& batches = drawable->GetBatches();

                for (unsigned j = 0; j < batches.Size(); ++j)
                    AddTriMeshGeometry(build, drawable->GetLodGeometry(j, geometryList[i].lodLevel_), transform);
            }
        }
    }
}
void NavigationMesh::FindPath(PODVector<NavigationPathPoint>& dest, const Vector3& start, const Vector3& end,
    const Vector3& extents, const dtQueryFilter* filter)
{
    ATOMIC_PROFILE(FindPath);
    dest.Clear();

    if (!InitializeQuery())
        return;

    // Navigation data is in local space. Transform path points from world to local
    const Matrix3x4& transform = node_->GetWorldTransform();
    Matrix3x4 inverse = transform.Inverse();

    Vector3 localStart = inverse * start;
    Vector3 localEnd = inverse * end;

    const dtQueryFilter* queryFilter = filter ? filter : queryFilter_.Get();
    dtPolyRef startRef;
    dtPolyRef endRef;
    navMeshQuery_->findNearestPoly(&localStart.x_, &extents.x_, queryFilter, &startRef, 0);
    navMeshQuery_->findNearestPoly(&localEnd.x_, &extents.x_, queryFilter, &endRef, 0);

    if (!startRef || !endRef)
        return;

    int numPolys = 0;
    int numPathPoints = 0;

    navMeshQuery_->findPath(startRef, endRef, &localStart.x_, &localEnd.x_, queryFilter, pathData_->polys_, &numPolys,
        MAX_POLYS);
    if (!numPolys)
        return;

    Vector3 actualLocalEnd = localEnd;

    // If full path was not found, clamp end point to the end polygon
    if (pathData_->polys_[numPolys - 1] != endRef)
        navMeshQuery_->closestPointOnPoly(pathData_->polys_[numPolys - 1], &localEnd.x_, &actualLocalEnd.x_, 0);

    navMeshQuery_->findStraightPath(&localStart.x_, &actualLocalEnd.x_, pathData_->polys_, numPolys,
        &pathData_->pathPoints_[0].x_, pathData_->pathFlags_, pathData_->pathPolys_, &numPathPoints, MAX_POLYS);

    // Transform path result back to world space
    for (int i = 0; i < numPathPoints; ++i)
    {
        NavigationPathPoint pt;
        pt.position_ = transform * pathData_->pathPoints_[i];
        pt.flag_ = (NavigationPathPointFlag)pathData_->pathFlags_[i];

        // Walk through all NavAreas and find nearest
        unsigned nearestNavAreaID = 0;       // 0 is the default nav area ID
        float nearestDistance = M_LARGE_VALUE;
        for (unsigned j = 0; j < areas_.Size(); j++)
        {
            NavArea* area = areas_[j].Get();
            if (area && area->IsEnabledEffective())
            {
                BoundingBox bb = area->GetWorldBoundingBox();
                if (bb.IsInside(pt.position_) == INSIDE)
                {
                    Vector3 areaWorldCenter = area->GetNode()->GetWorldPosition();
                    float distance = (areaWorldCenter - pt.position_).LengthSquared();
                    if (distance < nearestDistance)
                    {
                        nearestDistance = distance;
                        nearestNavAreaID = area->GetAreaID();
                    }
                }
            }
        }
        pt.areaID_ = (unsigned char)nearestNavAreaID;

        dest.Push(pt);
    }
}
Vector<WorldCoordinates>* PathFinderManager::findPathFromWorldToWorld(const WorldCoordinates& pointA, Vector<WorldCoordinates>& endPoints, Zone* zone, bool allowPartial) {
	Vector<WorldCoordinates>* finalpath = new Vector<WorldCoordinates>();
	float finalLengthSq = FLT_MAX;

#ifdef PROFILE_PATHING
	Timer t;
	t.start();
#endif

	for (const WorldCoordinates& pointB : endPoints) {
		const Vector3& startTemp = pointA.getPoint();
		const Vector3& targetTemp = pointB.getPoint();

		SortedVector<ManagedReference<NavArea*> > areas;

		Vector3 mid = startTemp + ((targetTemp-startTemp) * 0.5f);

		zone->getInRangeNavMeshes(mid.getX(), mid.getY(), &areas, true);

		SortedVector<NavCollision*> collisions;
		getNavMeshCollisions(&collisions, &areas, pointA.getWorldPosition(), pointB.getWorldPosition());
		// Collisions are sorted by distance from the start of the line. This is done so that we can chain our path from
		// one navmesh to another if a path spans multiple meshes.
		Vector<WorldCoordinates> *path = new Vector<WorldCoordinates>();
		float len = 0.0f;

		try {
			int collisionSize = collisions.size();

			if (collisionSize == 1) { // we're entering/exiting a navmesh
				NavCollision* collision = collisions.get(0);
				NavArea *area = collision->getNavArea();
				Vector3 position = collision->getPosition();
				position.setZ(CollisionManager::getWorldFloorCollision(position.getX(), position.getY(), zone, true));

				if (area->containsPoint(startTemp.getX(), startTemp.getY())) {
					if (!getRecastPath(startTemp, position, area, path, len, allowPartial)) { // exiting navmesh
						delete collision;
						if (path != NULL) delete path;
						continue;
					}

					path->add(pointB);

				} else {
					path->add(pointA);

					if (!getRecastPath(position, targetTemp, area, path, len, allowPartial)) { // entering navmesh
						delete collision;
						if (path != NULL) delete path;
						continue;
					}
				}

				if (len > 0 && len < finalLengthSq) {
					if (finalpath)
						delete finalpath;

					finalLengthSq = len;
					finalpath = path;
					path = NULL;
				}
			} else if (collisionSize == 0) { // we're already inside a navmesh (or there are no navmeshes around)
				for (int i = 0; i < areas.size(); i++) {
					if (!getRecastPath(startTemp, targetTemp, areas.get(i), path, len, allowPartial)) {
						continue;
					}

					if (len > 0 && len < finalLengthSq) {
						if (finalpath)
							delete finalpath;

						finalLengthSq = len;
						finalpath = path;
						path = new Vector<WorldCoordinates>();
					}
				}
			} else if (collisionSize == 2) { // we're crossing over a mesh or dealing with multiple meshes
				NavCollision* collision1 = collisions.get(0);
				NavArea *area1 = collision1->getNavArea();
				NavCollision* collision2 = collisions.get(1);
				NavArea *area2 = collision2->getNavArea();

				if (area1 == area2) { // crossing same mesh
					path->add(pointA);

					Vector3 position1 = collision1->getPosition();
					position1.setZ(CollisionManager::getWorldFloorCollision(position1.getX(), position1.getY(), zone, false));
					Vector3 position2 = collision2->getPosition();
					position2.setZ(CollisionManager::getWorldFloorCollision(position2.getX(), position2.getY(), zone, false));

					if (!getRecastPath(position1, position2, area1, path, len, allowPartial)) {
						delete collision1;
						delete collision2;
						if (path != NULL) delete path;
						continue;
					}

					path->add(pointB);

					if (len > 0 && len < finalLengthSq) {
						if (finalpath)
							delete finalpath;

						finalLengthSq = len;
						finalpath = path;
						path = NULL;
					}
				} else { // TODO: handle multiple meshes

				}
			} else { // TODO: handle multiple meshes

			}
		} catch (...) {
			error("Unhandled pathing exception");

			for (int i=collisions.size()-1; i>=0; i--) {
				NavCollision *collision = collisions.remove(i);
				delete collision;
			}

			delete path;
			path = NULL;
		}

		for (int i=collisions.size()-1; i>=0; i--) {
			NavCollision *collision = collisions.remove(i);
			delete collision;
		}

		if (path != NULL)
			delete path;
	}

	if (finalpath && finalpath->size() < 2) { // path could not be evaluated, just return the start/end position
		finalpath->removeAll();
		finalpath->add(pointA);
		finalpath->add(endPoints.get(0));
	}

#ifdef PROFILE_PATHING
	t.stop();
	totalTime.add(t.getElapsedTime());
	info("Spent " + String::valueOf(totalTime.get()) + " in recast", true);
#endif
	return finalpath;
}