path: root/Source/AI/tilemeshtestertool.cpp

//-----------------------------------------------------------------------------
//           Name: tilemeshtestertool.cpp
//      Developer: External
//         Author:
//    Description: This is a utility file from the Recast project which has been
//                 extracted and modified by Wolfire Games LLC
//        License: Read below
//-----------------------------------------------------------------------------

//
// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org
//
// This software is provided 'as-is', without any express or implied
// warranty.  In no event will the authors be held liable for any damages
// arising from the use of this software.
// Permission is granted to anyone to use this software for any purpose,
// including commercial applications, and to alter it and redistribute it
// freely, subject to the following restrictions:
// 1. The origin of this software must not be misrepresented; you must not
//    claim that you wrote the original software. If you use this software
//    in a product, an acknowledgment in the product documentation would be
//    appreciated but is not required.
// 2. Altered source versions must be plainly marked as such, and must not be
//    misrepresented as being the original software.
// 3. This notice may not be removed or altered from any source distribution.
//

#define _USE_MATH_DEFINES
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "Recast.h"
#include "DetourNavMesh.h"
#include "DetourNavMeshBuilder.h"
#include "DetourCommon.h"

#include "tilemeshtestertool.h"

#ifdef WIN32
#define snprintf _snprintf
#endif

// Uncomment this to dump all the requests in stdout.
#define DUMP_REQS

/*
// Returns a random number [0..1)
static float frand()
{
//	return ((float)(rand() & 0xffff)/(float)0xffff);
        return (float)rand()/(float)RAND_MAX;
}
*/

inline bool inRange(const float* v1, const float* v2, const float r, const float h) {
    const float dx = v2[0] - v1[0];
    const float dy = v2[1] - v1[1];
    const float dz = v2[2] - v1[2];
    return (dx * dx + dz * dz) < r * r && fabsf(dy) < h;
}

static int fixupCorridor(dtPolyRef* path, const int npath, const int maxPath,
                         const dtPolyRef* visited, const int nvisited) {
    int furthestPath = -1;
    int furthestVisited = -1;

    // Find furthest common polygon.
    for (int i = npath - 1; i >= 0; --i) {
        bool found = false;
        for (int j = nvisited - 1; j >= 0; --j) {
            if (path[i] == visited[j]) {
                furthestPath = i;
                furthestVisited = j;
                found = true;
            }
        }
        if (found)
            break;
    }

    // If no intersection found just return current path.
    if (furthestPath == -1 || furthestVisited == -1)
        return npath;

    // Concatenate paths.

    // Adjust beginning of the buffer to include the visited.
    const int req = nvisited - furthestVisited;
    const int orig = rcMin(furthestPath + 1, npath);
    int size = rcMax(0, npath - orig);
    if (req + size > maxPath)
        size = maxPath - req;
    if (size)
        memmove(path + req, path + orig, size * sizeof(dtPolyRef));

    // Store visited
    for (int i = 0; i < req; ++i)
        path[i] = visited[(nvisited - 1) - i];

    return req + size;
}

static bool getSteerTarget(dtNavMeshQuery* navQuery, const float* startPos, const float* endPos,
                           const float minTargetDist,
                           const dtPolyRef* path, const int pathSize,
                           float* steerPos, unsigned char& steerPosFlag, dtPolyRef& steerPosRef,
                           float* outPoints = 0, int* outPointCount = 0) {
    // Find steer target.
    static const int MAX_STEER_POINTS = 3;
    float steerPath[MAX_STEER_POINTS * 3];
    unsigned char steerPathFlags[MAX_STEER_POINTS];
    dtPolyRef steerPathPolys[MAX_STEER_POINTS];
    int nsteerPath = 0;
    navQuery->findStraightPath(startPos, endPos, path, pathSize,
                               steerPath, steerPathFlags, steerPathPolys, &nsteerPath, MAX_STEER_POINTS);
    if (!nsteerPath)
        return false;

    if (outPoints && outPointCount) {
        *outPointCount = nsteerPath;
        for (int i = 0; i < nsteerPath; ++i)
            dtVcopy(&outPoints[i * 3], &steerPath[i * 3]);
    }

    // Find vertex far enough to steer to.
    int ns = 0;
    while (ns < nsteerPath) {
        // Stop at Off-Mesh link or when point is further than slop away.
        if ((steerPathFlags[ns] & DT_STRAIGHTPATH_OFFMESH_CONNECTION) ||
            !inRange(&steerPath[ns * 3], startPos, minTargetDist, 1000.0f))
            break;
        ns++;
    }
    // Failed to find good point to steer to.
    if (ns >= nsteerPath)
        return false;

    dtVcopy(steerPos, &steerPath[ns * 3]);
    steerPos[1] = startPos[1];
    steerPosFlag = steerPathFlags[ns];
    steerPosRef = steerPathPolys[ns];

    return true;
}

TileMeshTesterTool::TileMeshTesterTool() : m_sample(0),
                                           m_navMesh(0),
                                           m_navQuery(0),
                                           m_pathFindStatus(DT_FAILURE),
                                           m_toolMode(TOOLMODE_PATHFIND_FOLLOW),
                                           m_startRef(0),
                                           m_endRef(0),
                                           m_npolys(0),
                                           m_nstraightPath(0),
                                           m_nsmoothPath(0),
                                           m_nrandPoints(0),
                                           m_randPointsInCircle(false),
                                           m_hitResult(false),
                                           m_distanceToWall(0),
                                           m_sposSet(false),
                                           m_eposSet(false),
                                           m_pathIterNum(0),
                                           m_steerPointCount(0) {
    m_filter.setIncludeFlags(SAMPLE_POLYFLAGS_ALL ^ SAMPLE_POLYFLAGS_DISABLED);
    m_filter.setExcludeFlags(0);

    m_polyPickExt[0] = 2;
    m_polyPickExt[1] = 4;
    m_polyPickExt[2] = 2;

    m_neighbourhoodRadius = 2.5f;
    m_randomRadius = 5.0f;
}

TileMeshTesterTool::~TileMeshTesterTool() {
}

void TileMeshTesterTool::init(Sample* sample) {
    m_sample = sample;
    m_navMesh = sample->getNavMesh();
    m_navQuery = sample->getNavMeshQuery();
    recalc();

    if (m_navQuery) {
        // Change costs.
        m_filter.setAreaCost(SAMPLE_POLYAREA_GROUND, 1.0f);
        m_filter.setAreaCost(SAMPLE_POLYAREA_WATER, 10.0f);
        m_filter.setAreaCost(SAMPLE_POLYAREA_ROAD, 1.0f);
        m_filter.setAreaCost(SAMPLE_POLYAREA_DOOR, 1.0f);
        m_filter.setAreaCost(SAMPLE_POLYAREA_GRASS, 2.0f);
        m_filter.setAreaCost(SAMPLE_POLYAREA_JUMP1, 1.5f);
        m_filter.setAreaCost(SAMPLE_POLYAREA_JUMP2, 1.5f);
        m_filter.setAreaCost(SAMPLE_POLYAREA_JUMP3, 1.5f);
        m_filter.setAreaCost(SAMPLE_POLYAREA_JUMP4, 1.5f);
        m_filter.setAreaCost(SAMPLE_POLYAREA_JUMP5, 1.5f);
    }

    m_neighbourhoodRadius = sample->getAgentRadius() * 20.0f;
    m_randomRadius = sample->getAgentRadius() * 30.0f;
}

void TileMeshTesterTool::handleMenu() {
}

void TileMeshTesterTool::handleClick(const float* /*s*/, const float* p, bool shift) {
    if (shift) {
        m_sposSet = true;
        dtVcopy(m_spos, p);
    } else {
        m_eposSet = true;
        dtVcopy(m_epos, p);
    }
    recalc();
}

void TileMeshTesterTool::handleStep() {
}

void TileMeshTesterTool::handleToggle() {
    // TODO: merge separate to a path iterator. Use same code in recalc() too.
    if (m_toolMode != TOOLMODE_PATHFIND_FOLLOW)
        return;

    if (!m_sposSet || !m_eposSet || !m_startRef || !m_endRef)
        return;

    static const float STEP_SIZE = 0.5f;
    static const float SLOP = 0.01f;

    if (m_pathIterNum == 0) {
        m_navQuery->findPath(m_startRef, m_endRef, m_spos, m_epos, &m_filter, m_polys, &m_npolys, MAX_POLYS);
        m_nsmoothPath = 0;

        m_pathIterPolyCount = m_npolys;
        if (m_pathIterPolyCount)
            memcpy(m_pathIterPolys, m_polys, sizeof(dtPolyRef) * m_pathIterPolyCount);

        if (m_pathIterPolyCount) {
            // Iterate over the path to find smooth path on the detail mesh surface.

            m_navQuery->closestPointOnPolyBoundary(m_startRef, m_spos, m_iterPos);
            m_navQuery->closestPointOnPolyBoundary(m_pathIterPolys[m_pathIterPolyCount - 1], m_epos, m_targetPos);

            m_nsmoothPath = 0;

            dtVcopy(&m_smoothPath[m_nsmoothPath * 3], m_iterPos);
            m_nsmoothPath++;
        }
    }

    dtVcopy(m_prevIterPos, m_iterPos);

    m_pathIterNum++;

    if (!m_pathIterPolyCount)
        return;

    if (m_nsmoothPath >= MAX_SMOOTH)
        return;

    // Move towards target a small advancement at a time until target reached or
    // when ran out of memory to store the path.

    // Find location to steer towards.
    float steerPos[3];
    unsigned char steerPosFlag;
    dtPolyRef steerPosRef;

    if (!getSteerTarget(m_navQuery, m_iterPos, m_targetPos, SLOP,
                        m_pathIterPolys, m_pathIterPolyCount, steerPos, steerPosFlag, steerPosRef,
                        m_steerPoints, &m_steerPointCount))
        return;

    dtVcopy(m_steerPos, steerPos);

    bool endOfPath = (steerPosFlag & DT_STRAIGHTPATH_END) ? true : false;
    bool offMeshConnection = (steerPosFlag & DT_STRAIGHTPATH_OFFMESH_CONNECTION) ? true : false;

    // Find movement delta.
    float delta[3], len;
    dtVsub(delta, steerPos, m_iterPos);
    len = sqrtf(dtVdot(delta, delta));
    // If the steer target is end of path or off-mesh link, do not move past the location.
    if ((endOfPath || offMeshConnection) && len < STEP_SIZE)
        len = 1;
    else
        len = STEP_SIZE / len;
    float moveTgt[3];
    dtVmad(moveTgt, m_iterPos, delta, len);

    // Move
    float result[3];
    dtPolyRef visited[16];
    int nvisited = 0;
    m_navQuery->moveAlongSurface(m_pathIterPolys[0], m_iterPos, moveTgt, &m_filter,
                                 result, visited, &nvisited, 16);
    m_pathIterPolyCount = fixupCorridor(m_pathIterPolys, m_pathIterPolyCount, MAX_POLYS, visited, nvisited);
    float h = 0;
    m_navQuery->getPolyHeight(m_pathIterPolys[0], result, &h);
    result[1] = h;
    dtVcopy(m_iterPos, result);

    // Handle end of path and off-mesh links when close enough.
    if (endOfPath && inRange(m_iterPos, steerPos, SLOP, 1.0f)) {
        // Reached end of path.
        dtVcopy(m_iterPos, m_targetPos);
        if (m_nsmoothPath < MAX_SMOOTH) {
            dtVcopy(&m_smoothPath[m_nsmoothPath * 3], m_iterPos);
            m_nsmoothPath++;
        }
        return;
    } else if (offMeshConnection && inRange(m_iterPos, steerPos, SLOP, 1.0f)) {
        // Reached off-mesh connection.
        float startPos[3], endPos[3];

        // Advance the path up to and over the off-mesh connection.
        dtPolyRef prevRef = 0, polyRef = m_pathIterPolys[0];
        int npos = 0;
        while (npos < m_pathIterPolyCount && polyRef != steerPosRef) {
            prevRef = polyRef;
            polyRef = m_pathIterPolys[npos];
            npos++;
        }
        for (int i = npos; i < m_pathIterPolyCount; ++i)
            m_pathIterPolys[i - npos] = m_pathIterPolys[i];
        m_pathIterPolyCount -= npos;

        // Handle the connection.
        dtStatus status = m_navMesh->getOffMeshConnectionPolyEndPoints(prevRef, polyRef, startPos, endPos);
        if (dtStatusSucceed(status)) {
            if (m_nsmoothPath < MAX_SMOOTH) {
                dtVcopy(&m_smoothPath[m_nsmoothPath * 3], startPos);
                m_nsmoothPath++;
                // Hack to make the dotted path not visible during off-mesh connection.
                if (m_nsmoothPath & 1) {
                    dtVcopy(&m_smoothPath[m_nsmoothPath * 3], startPos);
                    m_nsmoothPath++;
                }
            }
            // Move position at the other side of the off-mesh link.
            dtVcopy(m_iterPos, endPos);
            float eh = 0.0f;
            m_navQuery->getPolyHeight(m_pathIterPolys[0], m_iterPos, &eh);
            m_iterPos[1] = eh;
        }
    }

    // Store results.
    if (m_nsmoothPath < MAX_SMOOTH) {
        dtVcopy(&m_smoothPath[m_nsmoothPath * 3], m_iterPos);
        m_nsmoothPath++;
    }
}

void TileMeshTesterTool::handleUpdate(const float /*dt*/) {
    if (m_toolMode == TOOLMODE_PATHFIND_SLICED) {
        if (dtStatusInProgress(m_pathFindStatus)) {
            m_pathFindStatus = m_navQuery->updateSlicedFindPath(1, 0);
        }
        if (dtStatusSucceed(m_pathFindStatus)) {
            m_navQuery->finalizeSlicedFindPath(m_polys, &m_npolys, MAX_POLYS);
            m_nstraightPath = 0;
            if (m_npolys) {
                // In case of partial path, make sure the end point is clamped to the last polygon.
                float epos[3];
                bool isOnPoly;
                dtVcopy(epos, m_epos);
                if (m_polys[m_npolys - 1] != m_endRef)
                    m_navQuery->closestPointOnPoly(m_polys[m_npolys - 1], m_epos, epos, &isOnPoly);

                m_navQuery->findStraightPath(m_spos, epos, m_polys, m_npolys,
                                             m_straightPath, m_straightPathFlags,
                                             m_straightPathPolys, &m_nstraightPath, MAX_POLYS);
            }

            m_pathFindStatus = DT_FAILURE;
        }
    }
}

void TileMeshTesterTool::reset() {
    m_startRef = 0;
    m_endRef = 0;
    m_npolys = 0;
    m_nstraightPath = 0;
    m_nsmoothPath = 0;
    memset(m_hitPos, 0, sizeof(m_hitPos));
    memset(m_hitNormal, 0, sizeof(m_hitNormal));
    m_distanceToWall = 0;
}

void TileMeshTesterTool::recalc() {
    if (!m_navMesh)
        return;

    if (m_sposSet)
        m_navQuery->findNearestPoly(m_spos, m_polyPickExt, &m_filter, &m_startRef, 0);
    else
        m_startRef = 0;

    if (m_eposSet)
        m_navQuery->findNearestPoly(m_epos, m_polyPickExt, &m_filter, &m_endRef, 0);
    else
        m_endRef = 0;

    m_pathFindStatus = DT_FAILURE;

    if (m_toolMode == TOOLMODE_PATHFIND_FOLLOW) {
        m_pathIterNum = 0;
        if (m_sposSet && m_eposSet && m_startRef && m_endRef) {
#ifdef DUMP_REQS
            printf("pi  %f %f %f  %f %f %f  0x%x 0x%x\n",
                   m_spos[0], m_spos[1], m_spos[2], m_epos[0], m_epos[1], m_epos[2],
                   m_filter.getIncludeFlags(), m_filter.getExcludeFlags());
#endif

            m_navQuery->findPath(m_startRef, m_endRef, m_spos, m_epos, &m_filter, m_polys, &m_npolys, MAX_POLYS);

            m_nsmoothPath = 0;

            if (m_npolys) {
                // Iterate over the path to find smooth path on the detail mesh surface.
                dtPolyRef polys[MAX_POLYS];
                memcpy(polys, m_polys, sizeof(dtPolyRef) * m_npolys);
                int npolys = m_npolys;

                float iterPos[3], targetPos[3];
                m_navQuery->closestPointOnPolyBoundary(m_startRef, m_spos, iterPos);
                m_navQuery->closestPointOnPolyBoundary(polys[npolys - 1], m_epos, targetPos);

                static const float STEP_SIZE = 0.5f;
                static const float SLOP = 0.01f;

                m_nsmoothPath = 0;

                dtVcopy(&m_smoothPath[m_nsmoothPath * 3], iterPos);
                m_nsmoothPath++;

                // Move towards target a small advancement at a time until target reached or
                // when ran out of memory to store the path.
                while (npolys && m_nsmoothPath < MAX_SMOOTH) {
                    // Find location to steer towards.
                    float steerPos[3];
                    unsigned char steerPosFlag;
                    dtPolyRef steerPosRef;

                    if (!getSteerTarget(m_navQuery, iterPos, targetPos, SLOP,
                                        polys, npolys, steerPos, steerPosFlag, steerPosRef))
                        break;

                    bool endOfPath = (steerPosFlag & DT_STRAIGHTPATH_END) ? true : false;
                    bool offMeshConnection = (steerPosFlag & DT_STRAIGHTPATH_OFFMESH_CONNECTION) ? true : false;

                    // Find movement delta.
                    float delta[3], len;
                    dtVsub(delta, steerPos, iterPos);
                    len = (float)sqrt(dtVdot(delta, delta));
                    // If the steer target is end of path or off-mesh link, do not move past the location.
                    if ((endOfPath || offMeshConnection) && len < STEP_SIZE)
                        len = 1;
                    else
                        len = STEP_SIZE / len;
                    float moveTgt[3];
                    dtVmad(moveTgt, iterPos, delta, len);

                    // Move
                    float result[3];
                    dtPolyRef visited[16];
                    int nvisited = 0;
                    m_navQuery->moveAlongSurface(polys[0], iterPos, moveTgt, &m_filter,
                                                 result, visited, &nvisited, 16);

                    npolys = fixupCorridor(polys, npolys, MAX_POLYS, visited, nvisited);
                    float h = 0;
                    m_navQuery->getPolyHeight(polys[0], result, &h);
                    result[1] = h;
                    dtVcopy(iterPos, result);

                    // Handle end of path and off-mesh links when close enough.
                    if (endOfPath && inRange(iterPos, steerPos, SLOP, 1.0f)) {
                        // Reached end of path.
                        dtVcopy(iterPos, targetPos);
                        if (m_nsmoothPath < MAX_SMOOTH) {
                            dtVcopy(&m_smoothPath[m_nsmoothPath * 3], iterPos);
                            m_nsmoothPath++;
                        }
                        break;
                    } else if (offMeshConnection && inRange(iterPos, steerPos, SLOP, 1.0f)) {
                        // Reached off-mesh connection.
                        float startPos[3], endPos[3];

                        // Advance the path up to and over the off-mesh connection.
                        dtPolyRef prevRef = 0, polyRef = polys[0];
                        int npos = 0;
                        while (npos < npolys && polyRef != steerPosRef) {
                            prevRef = polyRef;
                            polyRef = polys[npos];
                            npos++;
                        }
                        for (int i = npos; i < npolys; ++i)
                            polys[i - npos] = polys[i];
                        npolys -= npos;

                        // Handle the connection.
                        dtStatus status = m_navMesh->getOffMeshConnectionPolyEndPoints(prevRef, polyRef, startPos, endPos);
                        if (dtStatusSucceed(status)) {
                            if (m_nsmoothPath < MAX_SMOOTH) {
                                dtVcopy(&m_smoothPath[m_nsmoothPath * 3], startPos);
                                m_nsmoothPath++;
                                // Hack to make the dotted path not visible during off-mesh connection.
                                if (m_nsmoothPath & 1) {
                                    dtVcopy(&m_smoothPath[m_nsmoothPath * 3], startPos);
                                    m_nsmoothPath++;
                                }
                            }
                            // Move position at the other side of the off-mesh link.
                            dtVcopy(iterPos, endPos);
                            float eh = 0.0f;
                            m_navQuery->getPolyHeight(polys[0], iterPos, &eh);
                            iterPos[1] = eh;
                        }
                    }

                    // Store results.
                    if (m_nsmoothPath < MAX_SMOOTH) {
                        dtVcopy(&m_smoothPath[m_nsmoothPath * 3], iterPos);
                        m_nsmoothPath++;
                    }
                }
            }

        } else {
            m_npolys = 0;
            m_nsmoothPath = 0;
        }
    } else if (m_toolMode == TOOLMODE_PATHFIND_STRAIGHT) {
        if (m_sposSet && m_eposSet && m_startRef && m_endRef) {
#ifdef DUMP_REQS
            printf("ps  %f %f %f  %f %f %f  0x%x 0x%x\n",
                   m_spos[0], m_spos[1], m_spos[2], m_epos[0], m_epos[1], m_epos[2],
                   m_filter.getIncludeFlags(), m_filter.getExcludeFlags());
#endif
            m_navQuery->findPath(m_startRef, m_endRef, m_spos, m_epos, &m_filter, m_polys, &m_npolys, MAX_POLYS);
            m_nstraightPath = 0;
            if (m_npolys) {
                // In case of partial path, make sure the end point is clamped to the last polygon.
                float epos[3];
                dtVcopy(epos, m_epos);
                bool isOnPoly;
                if (m_polys[m_npolys - 1] != m_endRef)
                    m_navQuery->closestPointOnPoly(m_polys[m_npolys - 1], m_epos, epos, &isOnPoly);

                m_navQuery->findStraightPath(m_spos, epos, m_polys, m_npolys,
                                             m_straightPath, m_straightPathFlags,
                                             m_straightPathPolys, &m_nstraightPath, MAX_POLYS);
            }
        } else {
            m_npolys = 0;
            m_nstraightPath = 0;
        }
    } else if (m_toolMode == TOOLMODE_PATHFIND_SLICED) {
        if (m_sposSet && m_eposSet && m_startRef && m_endRef) {
#ifdef DUMP_REQS
            printf("ps  %f %f %f  %f %f %f  0x%x 0x%x\n",
                   m_spos[0], m_spos[1], m_spos[2], m_epos[0], m_epos[1], m_epos[2],
                   m_filter.getIncludeFlags(), m_filter.getExcludeFlags());
#endif
            m_npolys = 0;
            m_nstraightPath = 0;

            m_pathFindStatus = m_navQuery->initSlicedFindPath(m_startRef, m_endRef, m_spos, m_epos, &m_filter);
        } else {
            m_npolys = 0;
            m_nstraightPath = 0;
        }
    } else if (m_toolMode == TOOLMODE_RAYCAST) {
        m_nstraightPath = 0;
        if (m_sposSet && m_eposSet && m_startRef) {
#ifdef DUMP_REQS
            printf("rc  %f %f %f  %f %f %f  0x%x 0x%x\n",
                   m_spos[0], m_spos[1], m_spos[2], m_epos[0], m_epos[1], m_epos[2],
                   m_filter.getIncludeFlags(), m_filter.getExcludeFlags());
#endif
            float t = 0;
            m_npolys = 0;
            m_nstraightPath = 2;
            m_straightPath[0] = m_spos[0];
            m_straightPath[1] = m_spos[1];
            m_straightPath[2] = m_spos[2];
            m_navQuery->raycast(m_startRef, m_spos, m_epos, &m_filter, &t, m_hitNormal, m_polys, &m_npolys, MAX_POLYS);
            if (t > 1) {
                // No hit
                dtVcopy(m_hitPos, m_epos);
                m_hitResult = false;
            } else {
                // Hit
                m_hitPos[0] = m_spos[0] + (m_epos[0] - m_spos[0]) * t;
                m_hitPos[1] = m_spos[1] + (m_epos[1] - m_spos[1]) * t;
                m_hitPos[2] = m_spos[2] + (m_epos[2] - m_spos[2]) * t;
                if (m_npolys) {
                    float h = 0;
                    m_navQuery->getPolyHeight(m_polys[m_npolys - 1], m_hitPos, &h);
                    m_hitPos[1] = h;
                }
                m_hitResult = true;
            }
            dtVcopy(&m_straightPath[3], m_hitPos);
        }
    } else if (m_toolMode == TOOLMODE_DISTANCE_TO_WALL) {
        m_distanceToWall = 0;
        if (m_sposSet && m_startRef) {
#ifdef DUMP_REQS
            printf("dw  %f %f %f  %f  0x%x 0x%x\n",
                   m_spos[0], m_spos[1], m_spos[2], 100.0f,
                   m_filter.getIncludeFlags(), m_filter.getExcludeFlags());
#endif
            m_distanceToWall = 0.0f;
            m_navQuery->findDistanceToWall(m_startRef, m_spos, 100.0f, &m_filter, &m_distanceToWall, m_hitPos, m_hitNormal);
        }
    } else if (m_toolMode == TOOLMODE_FIND_POLYS_IN_CIRCLE) {
        if (m_sposSet && m_startRef && m_eposSet) {
            const float dx = m_epos[0] - m_spos[0];
            const float dz = m_epos[2] - m_spos[2];
            float dist = sqrtf(dx * dx + dz * dz);
#ifdef DUMP_REQS
            printf("fpc  %f %f %f  %f  0x%x 0x%x\n",
                   m_spos[0], m_spos[1], m_spos[2], dist,
                   m_filter.getIncludeFlags(), m_filter.getExcludeFlags());
#endif
            m_navQuery->findPolysAroundCircle(m_startRef, m_spos, dist, &m_filter,
                                              m_polys, m_parent, 0, &m_npolys, MAX_POLYS);
        }
    } else if (m_toolMode == TOOLMODE_FIND_POLYS_IN_SHAPE) {
        if (m_sposSet && m_startRef && m_eposSet) {
            const float nx = (m_epos[2] - m_spos[2]) * 0.25f;
            const float nz = -(m_epos[0] - m_spos[0]) * 0.25f;
            const float agentHeight = m_sample ? m_sample->getAgentHeight() : 0;

            m_queryPoly[0] = m_spos[0] + nx * 1.2f;
            m_queryPoly[1] = m_spos[1] + agentHeight / 2;
            m_queryPoly[2] = m_spos[2] + nz * 1.2f;

            m_queryPoly[3] = m_spos[0] - nx * 1.3f;
            m_queryPoly[4] = m_spos[1] + agentHeight / 2;
            m_queryPoly[5] = m_spos[2] - nz * 1.3f;

            m_queryPoly[6] = m_epos[0] - nx * 0.8f;
            m_queryPoly[7] = m_epos[1] + agentHeight / 2;
            m_queryPoly[8] = m_epos[2] - nz * 0.8f;

            m_queryPoly[9] = m_epos[0] + nx;
            m_queryPoly[10] = m_epos[1] + agentHeight / 2;
            m_queryPoly[11] = m_epos[2] + nz;

#ifdef DUMP_REQS
            printf("fpp  %f %f %f  %f %f %f  %f %f %f  %f %f %f  0x%x 0x%x\n",
                   m_queryPoly[0], m_queryPoly[1], m_queryPoly[2],
                   m_queryPoly[3], m_queryPoly[4], m_queryPoly[5],
                   m_queryPoly[6], m_queryPoly[7], m_queryPoly[8],
                   m_queryPoly[9], m_queryPoly[10], m_queryPoly[11],
                   m_filter.getIncludeFlags(), m_filter.getExcludeFlags());
#endif
            m_navQuery->findPolysAroundShape(m_startRef, m_queryPoly, 4, &m_filter,
                                             m_polys, m_parent, 0, &m_npolys, MAX_POLYS);
        }
    } else if (m_toolMode == TOOLMODE_FIND_LOCAL_NEIGHBOURHOOD) {
        if (m_sposSet && m_startRef) {
#ifdef DUMP_REQS
            printf("fln  %f %f %f  %f  0x%x 0x%x\n",
                   m_spos[0], m_spos[1], m_spos[2], m_neighbourhoodRadius,
                   m_filter.getIncludeFlags(), m_filter.getExcludeFlags());
#endif
            m_navQuery->findLocalNeighbourhood(m_startRef, m_spos, m_neighbourhoodRadius, &m_filter,
                                               m_polys, m_parent, &m_npolys, MAX_POLYS);
        }
    }
}
/*
static void getPolyCenter(dtNavMesh* navMesh, dtPolyRef ref, float* center)
{
        center[0] = 0;
        center[1] = 0;
        center[2] = 0;

        const dtMeshTile* tile = 0;
        const dtPoly* poly = 0;
        dtStatus status = navMesh->getTileAndPolyByRef(ref, &tile, &poly);
        if (dtStatusFailed(status))
                return;

        for (int i = 0; i < (int)poly->vertCount; ++i)
        {
                const float* v = &tile->verts[poly->verts[i]*3];
                center[0] += v[0];
                center[1] += v[1];
                center[2] += v[2];
        }
        const float s = 1.0f / poly->vertCount;
        center[0] *= s;
        center[1] *= s;
        center[2] *= s;
}
*/

void TileMeshTesterTool::handleRenderOverlay(double* proj, double* model, int* view) {
}

void TileMeshTesterTool::drawAgent(const float* pos, float r, float h, float c, const unsigned int col) {
}