diff options
Diffstat (limited to 'Source/AI/tilemeshtestertool.cpp')
| -rw-r--r-- | Source/AI/tilemeshtestertool.cpp | 802 |
1 files changed, 802 insertions, 0 deletions
diff --git a/Source/AI/tilemeshtestertool.cpp b/Source/AI/tilemeshtestertool.cpp new file mode 100644 index 00000000..f6c5bfc7 --- /dev/null +++ b/Source/AI/tilemeshtestertool.cpp @@ -0,0 +1,802 @@ +//----------------------------------------------------------------------------- +// 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 "SDL.h" +#include "opengl.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) +{ +} |