diff --git a/Detour/Include/DetourNavMeshQuery.h b/Detour/Include/DetourNavMeshQuery.h index 105e493..7a91888 100644 --- a/Detour/Include/DetourNavMeshQuery.h +++ b/Detour/Include/DetourNavMeshQuery.h @@ -167,9 +167,10 @@ public: // Updates sliced path find query. // Params: - // maxIter - (in) max number of iterations to update. + // maxIter - (in) Max number of iterations to update. + // doneIters - (out,opt) Number of iterations done during the update. // Returns: Path query state. - dtStatus updateSlicedFindPath(const int maxIter); + dtStatus updateSlicedFindPath(const int maxIter, int* doneIters); // Finalizes sliced path find query and returns found path. // path - (out) array holding the search result. diff --git a/Detour/Include/DetourObstacleAvoidance.h b/Detour/Include/DetourObstacleAvoidance.h index 4a7187a..4e17064 100644 --- a/Detour/Include/DetourObstacleAvoidance.h +++ b/Detour/Include/DetourObstacleAvoidance.h @@ -97,15 +97,15 @@ public: inline void setCollisionTimeWeight(float w) { m_weightToi = w; } inline void setTimeHorizon(float t) { m_horizTime = t; } - void sampleVelocityGrid(const float* pos, const float rad, const float vmax, - const float* vel, const float* dvel, float* nvel, - const int gsize, - dtObstacleAvoidanceDebugData* debug = 0); + int sampleVelocityGrid(const float* pos, const float rad, const float vmax, + const float* vel, const float* dvel, float* nvel, + const int gsize, + dtObstacleAvoidanceDebugData* debug = 0); - void sampleVelocityAdaptive(const float* pos, const float rad, const float vmax, - const float* vel, const float* dvel, float* nvel, - const int ndivs, const int nrings, const int depth, - dtObstacleAvoidanceDebugData* debug = 0); + int sampleVelocityAdaptive(const float* pos, const float rad, const float vmax, + const float* vel, const float* dvel, float* nvel, + const int ndivs, const int nrings, const int depth, + dtObstacleAvoidanceDebugData* debug = 0); inline int getObstacleCircleCount() const { return m_ncircles; } const dtObstacleCircle* getObstacleCircle(const int i) { return &m_circles[i]; } diff --git a/Detour/Source/DetourNavMeshQuery.cpp b/Detour/Source/DetourNavMeshQuery.cpp index 2a2ebfd..545773b 100644 --- a/Detour/Source/DetourNavMeshQuery.cpp +++ b/Detour/Source/DetourNavMeshQuery.cpp @@ -813,7 +813,7 @@ dtStatus dtNavMeshQuery::initSlicedFindPath(dtPolyRef startRef, dtPolyRef endRef return m_query.status; } -dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter) +dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter, int* doneIters) { if (!dtStatusInProgress(m_query.status)) return m_query.status; @@ -841,6 +841,8 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter) m_query.lastBestNode = bestNode; const dtStatus details = m_query.status & DT_STATUS_DETAIL_MASK; m_query.status = DT_SUCCESS | details; + if (doneIters) + *doneIters = iter; return m_query.status; } @@ -853,6 +855,8 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter) { // The polygon has disappeared during the sliced query, fail. m_query.status = DT_FAILURE; + if (doneIters) + *doneIters = iter; return m_query.status; } @@ -868,6 +872,8 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter) { // The polygon has disappeared during the sliced query, fail. m_query.status = DT_FAILURE; + if (doneIters) + *doneIters = iter; return m_query.status; } } @@ -979,6 +985,9 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter) m_query.status = DT_SUCCESS | details; } + if (doneIters) + *doneIters = iter; + return m_query.status; } diff --git a/Detour/Source/DetourObstacleAvoidance.cpp b/Detour/Source/DetourObstacleAvoidance.cpp index a255c9b..ac6e0d1 100644 --- a/Detour/Source/DetourObstacleAvoidance.cpp +++ b/Detour/Source/DetourObstacleAvoidance.cpp @@ -410,10 +410,10 @@ float dtObstacleAvoidanceQuery::processSample(const float* vcand, const float cs return penalty; } -void dtObstacleAvoidanceQuery::sampleVelocityGrid(const float* pos, const float rad, const float vmax, - const float* vel, const float* dvel, - float* nvel, const int gsize, - dtObstacleAvoidanceDebugData* debug) +int dtObstacleAvoidanceQuery::sampleVelocityGrid(const float* pos, const float rad, const float vmax, + const float* vel, const float* dvel, + float* nvel, const int gsize, + dtObstacleAvoidanceDebugData* debug) { prepare(pos, dvel); @@ -428,6 +428,7 @@ void dtObstacleAvoidanceQuery::sampleVelocityGrid(const float* pos, const float const float half = (gsize-1)*cs*0.5f; float minPenalty = FLT_MAX; + int ns = 0; for (int y = 0; y < gsize; ++y) { @@ -441,6 +442,7 @@ void dtObstacleAvoidanceQuery::sampleVelocityGrid(const float* pos, const float if (dtSqr(vcand[0])+dtSqr(vcand[2]) > dtSqr(vmax+cs/2)) continue; const float penalty = processSample(vcand, cs, pos,rad,vmax,vel,dvel, debug); + ns++; if (penalty < minPenalty) { minPenalty = penalty; @@ -448,15 +450,17 @@ void dtObstacleAvoidanceQuery::sampleVelocityGrid(const float* pos, const float } } } + + return ns; } static const float DT_PI = 3.14159265f; -void dtObstacleAvoidanceQuery::sampleVelocityAdaptive(const float* pos, const float rad, const float vmax, - const float* vel, const float* dvel, float* nvel, - const int ndivs, const int nrings, const int depth, - dtObstacleAvoidanceDebugData* debug) +int dtObstacleAvoidanceQuery::sampleVelocityAdaptive(const float* pos, const float rad, const float vmax, + const float* vel, const float* dvel, float* nvel, + const int ndivs, const int nrings, const int depth, + dtObstacleAvoidanceDebugData* debug) { prepare(pos, dvel); @@ -498,6 +502,7 @@ void dtObstacleAvoidanceQuery::sampleVelocityAdaptive(const float* pos, const fl float cr = vmax * (1.0f-m_velBias); float res[3]; dtVset(res, dvel[0] * m_velBias, 0, dvel[2] * m_velBias); + int ns = 0; for (int k = 0; k < depth; ++k) { @@ -515,6 +520,7 @@ void dtObstacleAvoidanceQuery::sampleVelocityAdaptive(const float* pos, const fl if (dtSqr(vcand[0])+dtSqr(vcand[2]) > dtSqr(vmax+0.001f)) continue; const float penalty = processSample(vcand,cr/10, pos,rad,vmax,vel,dvel, debug); + ns++; if (penalty < minPenalty) { minPenalty = penalty; @@ -528,5 +534,7 @@ void dtObstacleAvoidanceQuery::sampleVelocityAdaptive(const float* pos, const fl } dtVcopy(nvel, res); + + return ns; } diff --git a/DetourCrowd/Include/DetourCrowd.h b/DetourCrowd/Include/DetourCrowd.h new file mode 100644 index 0000000..b17957d --- /dev/null +++ b/DetourCrowd/Include/DetourCrowd.h @@ -0,0 +1,185 @@ +// +// 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. +// + +#ifndef DETOURCROWD_H +#define DETOURCROWD_H + +#include "DetourNavMeshQuery.h" +#include "DetourObstacleAvoidance.h" +#include "DetourLocalBoundary.h" +#include "DetourPathCorridor.h" +#include "DetourProximityGrid.h" +#include "DetourPathQueue.h" + + +static const int DT_CROWDAGENT_MAX_NEIGHBOURS = 6; +static const int DT_CROWDAGENT_MAX_CORNERS = 4; + +struct dtCrowdNeighbour +{ + int idx; + float dist; +}; + +struct dtCrowdAgent +{ + unsigned char active; + + dtPathCorridor corridor; + dtLocalBoundary boundary; + + float t; + float var; + + float topologyOptTime; + + dtCrowdNeighbour neis[DT_CROWDAGENT_MAX_NEIGHBOURS]; + int nneis; + + float radius, height; + float maxAcceleration; + float maxSpeed; + float collisionQueryRange; + float pathOptimizationRange; + + float desiredSpeed; + + float npos[3]; + float disp[3]; + float dvel[3]; + float nvel[3]; + float vel[3]; + + float cornerVerts[DT_CROWDAGENT_MAX_CORNERS*3]; + unsigned char cornerFlags[DT_CROWDAGENT_MAX_CORNERS]; + dtPolyRef cornerPolys[DT_CROWDAGENT_MAX_CORNERS]; + int ncorners; +}; + + +enum UpdateFlags +{ + DT_CROWD_ANTICIPATE_TURNS = 1, + DT_CROWD_USE_VO = 2, +// DT_CROWD_DRUNK = 4, + DT_CROWD_OPTIMIZE_VIS = 8, + DT_CROWD_OPTIMIZE_TOPO = 16, +}; + +struct dtCrowdAgentParams +{ + float radius; + float height; + float maxAcceleration; + float maxSpeed; + float collisionQueryRange; + float pathOptimizationRange; +}; + +struct dtCrowdAgentDebugInfo +{ + int idx; + float optStart[3], optEnd[3]; + dtObstacleAvoidanceDebugData* vod; +}; + +class dtCrowd +{ + int m_maxAgents; + dtCrowdAgent* m_agents; + dtCrowdAgent** m_activeAgents; + + dtPathQueue m_pathq; + + dtObstacleAvoidanceQuery* m_obstacleQuery; + dtProximityGrid* m_grid; + + dtPolyRef* m_pathResult; + int m_maxPathResult; + + float m_ext[3]; + dtQueryFilter m_filter; + + float m_maxAgentRadius; + + int m_velocitySampleCount; + + enum MoveRequestState + { + MR_TARGET_FAILED, + MR_TARGET_VALID, + MR_TARGET_REQUESTING, + MR_TARGET_WAITING_FOR_PATH, + MR_TARGET_ADJUST, + }; + + static const int MAX_TEMP_PATH = 32; + + struct MoveRequest + { + unsigned char state; // State of the request + int idx; // Agent index + dtPolyRef ref; // Goal ref + float pos[3]; // Goal position + dtPathQueueRef pathqRef; // Path find query ref + dtPolyRef aref; // Goal adjustment ref + float apos[3]; // Goal adjustment pos + dtPolyRef temp[MAX_TEMP_PATH]; // Adjusted path to the goal + int ntemp; + }; + MoveRequest* m_moveRequests; + int m_moveRequestCount; + + dtNavMeshQuery* m_navquery; + + int getNeighbours(const float* pos, const float height, const float range, + const dtCrowdAgent* skip, dtCrowdNeighbour* result, const int maxResult); + void updateTopologyOptimization(dtCrowdAgent** agents, const int nagents, const float dt); + void updateMoveRequest(const float dt); + + void purge(); + +public: + dtCrowd(); + ~dtCrowd(); + + bool init(const int maxAgents, const float maxAgentRadius, dtNavMesh* nav); + + const dtCrowdAgent* getAgent(const int idx); + const int getAgentCount() const; + + int addAgent(const float* pos, const dtCrowdAgentParams* params); + void removeAgent(const int idx); + + bool requestMoveTarget(const int idx, dtPolyRef ref, const float* pos); + bool adjustMoveTarget(const int idx, dtPolyRef ref, const float* pos); + + int getActiveAgents(dtCrowdAgent** agents, const int maxAgents); + void update(const float dt, unsigned int flags, dtCrowdAgentDebugInfo* debug); + + const dtQueryFilter* getFilter() const { return &m_filter; } + const float* getQueryExtents() const { return m_ext; } + + inline int getVelocitySampleCount() const { return m_velocitySampleCount; } + + const dtProximityGrid* getGrid() const { return m_grid; } + const dtPathQueue* getPathQueue() const { return &m_pathq; } +}; + + +#endif // CROWDMANAGER_H \ No newline at end of file diff --git a/DetourCrowd/Include/DetourLocalBoundary.h b/DetourCrowd/Include/DetourLocalBoundary.h new file mode 100644 index 0000000..149659e --- /dev/null +++ b/DetourCrowd/Include/DetourLocalBoundary.h @@ -0,0 +1,55 @@ +// +// 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. +// + +#ifndef DETOURLOCALBOUNDARY_H +#define DETOURLOCALBOUNDARY_H + +#include "DetourNavMeshQuery.h" + + +class dtLocalBoundary +{ + static const int MAX_SEGS = 8; + + struct Segment + { + float s[6]; // Segment start/end + float d; // Distance for pruning. + }; + + float m_center[3]; + Segment m_segs[MAX_SEGS]; + int m_nsegs; + + void addSegment(const float dist, const float* seg); + +public: + dtLocalBoundary(); + ~dtLocalBoundary(); + + void reset(); + + void update(dtPolyRef ref, const float* pos, const float collisionQueryRange, + dtNavMeshQuery* navquery, const dtQueryFilter* filter); + + inline const float* getCenter() const { return m_center; } + inline int getSegmentCount() const { return m_nsegs; } + inline const float* getSegment(int i) const { return m_segs[i].s; } +}; + +#endif // DETOURLOCALBOUNDARY_H \ No newline at end of file diff --git a/DetourCrowd/Include/DetourPathCorridor.h b/DetourCrowd/Include/DetourPathCorridor.h new file mode 100644 index 0000000..bbf1d5e --- /dev/null +++ b/DetourCrowd/Include/DetourPathCorridor.h @@ -0,0 +1,74 @@ +// +// 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. +// + +#ifndef DETOUTPATHCORRIDOR_H +#define DETOUTPATHCORRIDOR_H + +#include "DetourNavMeshQuery.h" + + +class dtPathCorridor +{ + float m_pos[3]; + float m_target[3]; + + dtPolyRef* m_path; + int m_npath; + int m_maxPath; + +public: + dtPathCorridor(); + ~dtPathCorridor(); + + bool init(const int maxPath); + + void reset(dtPolyRef ref, const float* pos); + + int findCorners(float* cornerVerts, unsigned char* cornerFlags, + dtPolyRef* cornerPolys, const int maxCorners, + dtNavMeshQuery* navquery, const dtQueryFilter* filter); + + void optimizePathVisibility(const float* next, const float pathOptimizationRange, + dtNavMeshQuery* navquery, const dtQueryFilter* filter); + + bool optimizePathTopology(dtNavMeshQuery* navquery, const dtQueryFilter* filter); + + void movePosition(const float* npos, dtNavMeshQuery* navquery, const dtQueryFilter* filter); + void moveTargetPosition(const float* npos, dtNavMeshQuery* navquery, const dtQueryFilter* filter); + + void setCorridor(const float* target, const dtPolyRef* polys, const int npolys); + + inline const float* getPos() const { return m_pos; } + inline const float* getTarget() const { return m_target; } + + inline dtPolyRef getFirstPoly() const { return m_npath ? m_path[0] : 0; } + + inline const dtPolyRef* getPath() const { return m_path; } + inline int getPathCount() const { return m_npath; } +}; + +int dtMergeCorridorStartMoved(dtPolyRef* path, const int npath, const int maxPath, + const dtPolyRef* visited, const int nvisited); + +int dtMergeCorridorEndMoved(dtPolyRef* path, const int npath, const int maxPath, + const dtPolyRef* visited, const int nvisited); + +int dtMergeCorridorStartShortcut(dtPolyRef* path, const int npath, const int maxPath, + const dtPolyRef* visited, const int nvisited); + +#endif // DETOUTPATHCORRIDOR_H diff --git a/DetourCrowd/Include/DetourPathQueue.h b/DetourCrowd/Include/DetourPathQueue.h new file mode 100644 index 0000000..cc144cb --- /dev/null +++ b/DetourCrowd/Include/DetourPathQueue.h @@ -0,0 +1,75 @@ +// +// 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. +// + +#ifndef DETOURPATHQUEUE_H +#define DETOURPATHQUEUE_H + +#include "DetourNavMesh.h" +#include "DetourNavMeshQuery.h" + +static const unsigned int DT_PATHQ_INVALID = 0; + +typedef unsigned int dtPathQueueRef; + +class dtPathQueue +{ + struct PathQuery + { + dtPathQueueRef ref; + // Path find start and end location. + float startPos[3], endPos[3]; + dtPolyRef startRef, endRef; + // Result. + dtPolyRef* path; + int npath; + // State. + dtStatus status; + int keepAlive; + const dtQueryFilter* filter; // TODO: This is potentially dangerous! + }; + + static const int MAX_QUEUE = 8; + PathQuery m_queue[MAX_QUEUE]; + dtPathQueueRef m_nextHandle; + int m_maxPathSize; + int m_queueHead; + dtNavMeshQuery* m_navquery; + + void purge(); + +public: + dtPathQueue(); + ~dtPathQueue(); + + bool init(const int maxPathSize, const int maxSearchNodeCount, dtNavMesh* nav); + + void update(const int maxIters); + + dtPathQueueRef request(dtPolyRef startRef, dtPolyRef endRef, + const float* startPos, const float* endPos, + const dtQueryFilter* filter); + + dtStatus getRequestStatus(dtPathQueueRef ref) const; + + dtStatus getPathResult(dtPathQueueRef ref, dtPolyRef* path, int* pathSize, const int maxPath); + + inline const dtNavMeshQuery* getNavQuery() const { return m_navquery; } + +}; + +#endif // DETOURPATHQUEUE_H diff --git a/DetourCrowd/Include/DetourProximityGrid.h b/DetourCrowd/Include/DetourProximityGrid.h new file mode 100644 index 0000000..b098261 --- /dev/null +++ b/DetourCrowd/Include/DetourProximityGrid.h @@ -0,0 +1,70 @@ +// +// 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. +// + +#ifndef DETOURPROXIMITYGRID_H +#define DETOURPROXIMITYGRID_H + +class dtProximityGrid +{ + int m_maxItems; + float m_cellSize; + float m_invCellSize; + + struct Item + { + unsigned short id; + short x,y; + unsigned short next; + }; + Item* m_pool; + int m_poolHead; + int m_poolSize; + + unsigned short* m_buckets; + int m_bucketsSize; + + int m_bounds[4]; + +public: + dtProximityGrid(); + ~dtProximityGrid(); + + bool init(const int maxItems, const float cellSize); + + void clear(); + + void addItem(const unsigned short id, + const float minx, const float miny, + const float maxx, const float maxy); + + int queryItems(const float minx, const float miny, + const float maxx, const float maxy, + unsigned short* ids, const int maxIds) const; + + int getItemCountAt(const int x, const int y) const; + + inline const int* getBounds() const { return m_bounds; } + inline const float getCellSize() const { return m_cellSize; } +}; + +dtProximityGrid* dtAllocProximityGrid(); +void dtFreeProximityGrid(dtProximityGrid* ptr); + + +#endif // DETOURPROXIMITYGRID_H + diff --git a/DetourCrowd/Source/DetourCrowd.cpp b/DetourCrowd/Source/DetourCrowd.cpp new file mode 100644 index 0000000..f90e8e6 --- /dev/null +++ b/DetourCrowd/Source/DetourCrowd.cpp @@ -0,0 +1,944 @@ +// +// 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 +#include +#include +#include +#include "DetourCrowd.h" +#include "DetourNavMesh.h" +#include "DetourNavMeshQuery.h" +#include "DetourObstacleAvoidance.h" +#include "DetourCommon.h" +#include "DetourAssert.h" +#include "DetourAlloc.h" + +static const int VO_ADAPTIVE_DIVS = 7; +static const int VO_ADAPTIVE_RINGS = 2; +static const int VO_ADAPTIVE_DEPTH = 5; + +static const int VO_GRID_SIZE = 33; + +static const int MAX_ITERS_PER_UPDATE = 10; + +static const int MAX_PATHQUEUE_NODES = 4096; +static const int MAX_COMMON_NODES = 512; + + +static void integrate(dtCrowdAgent* ag, const float dt) +{ + // Fake dynamic constraint. + const float maxDelta = ag->maxAcceleration * dt; + float dv[3]; + dtVsub(dv, ag->nvel, ag->vel); + float ds = dtVlen(dv); + if (ds > maxDelta) + dtVscale(dv, dv, maxDelta/ds); + dtVadd(ag->vel, ag->vel, dv); + + // Integrate + if (dtVlen(ag->vel) > 0.0001f) + dtVmad(ag->npos, ag->npos, ag->vel, dt); + else + dtVset(ag->vel,0,0,0); +} + +static float getDistanceToGoal(const dtCrowdAgent* ag, const float range) +{ + if (!ag->ncorners) + return range; + + const bool endOfPath = (ag->cornerFlags[ag->ncorners-1] & DT_STRAIGHTPATH_END) ? true : false; + const bool offMeshConnection = (ag->cornerFlags[ag->ncorners-1] & DT_STRAIGHTPATH_OFFMESH_CONNECTION) ? true : false; + if (endOfPath || offMeshConnection) + return dtMin(dtVdist2D(ag->npos, &ag->cornerVerts[(ag->ncorners-1)*3]), range); + + return range; +} + +static void calcSmoothSteerDirection(const dtCrowdAgent* ag, float* dir) +{ + if (!ag->ncorners) + { + dtVset(dir, 0,0,0); + return; + } + + const int ip0 = 0; + const int ip1 = dtMin(1, ag->ncorners-1); + const float* p0 = &ag->cornerVerts[ip0*3]; + const float* p1 = &ag->cornerVerts[ip1*3]; + + float dir0[3], dir1[3]; + dtVsub(dir0, p0, ag->npos); + dtVsub(dir1, p1, ag->npos); + dir0[1] = 0; + dir1[1] = 0; + + float len0 = dtVlen(dir0); + float len1 = dtVlen(dir1); + if (len1 > 0.001f) + dtVscale(dir1,dir1,1.0f/len1); + + dir[0] = dir0[0] - dir1[0]*len0*0.5f; + dir[1] = 0; + dir[2] = dir0[2] - dir1[2]*len0*0.5f; + + dtVnormalize(dir); +} + +static void calcStraightSteerDirection(const dtCrowdAgent* ag, float* dir) +{ + if (!ag->ncorners) + { + dtVset(dir, 0,0,0); + return; + } + dtVsub(dir, &ag->cornerVerts[0], ag->npos); + dir[1] = 0; + dtVnormalize(dir); +} + +static int addNeighbour(const int idx, const float dist, + dtCrowdNeighbour* neis, const int nneis, const int maxNeis) +{ + // Insert neighbour based on the distance. + dtCrowdNeighbour* nei = 0; + if (!nneis) + { + nei = &neis[nneis]; + } + else if (dist >= neis[nneis-1].dist) + { + if (nneis >= maxNeis) + return nneis; + nei = &neis[nneis]; + } + else + { + int i; + for (i = 0; i < nneis; ++i) + if (dist <= neis[i].dist) + break; + + const int tgt = i+1; + const int n = dtMin(nneis-i, maxNeis-tgt); + + dtAssert(tgt+n <= maxNeis); + + if (n > 0) + memmove(&neis[tgt], &neis[i], sizeof(dtCrowdNeighbour)*n); + nei = &neis[i]; + } + + memset(nei, 0, sizeof(dtCrowdNeighbour)); + + nei->idx = idx; + nei->dist = dist; + + return dtMin(nneis+1, maxNeis); +} + + + +dtCrowd::dtCrowd() : + m_maxAgents(0), + m_agents(0), + m_activeAgents(0), + m_obstacleQuery(0), + m_grid(0), + m_pathResult(0), + m_maxPathResult(0), + m_maxAgentRadius(0), + m_velocitySampleCount(0), + m_moveRequests(0), + m_moveRequestCount(0), + m_navquery(0) +{ +} + +dtCrowd::~dtCrowd() +{ + purge(); +} + +void dtCrowd::purge() +{ + for (int i = 0; i < m_maxAgents; ++i) + m_agents[i].~dtCrowdAgent(); + dtFree(m_agents); + m_agents = 0; + m_maxAgents = 0; + + dtFree(m_activeAgents); + m_activeAgents = 0; + + dtFree(m_pathResult); + m_pathResult = 0; + + dtFree(m_moveRequests); + m_moveRequests = 0; + + dtFreeProximityGrid(m_grid); + m_grid = 0; + + dtFreeObstacleAvoidanceQuery(m_obstacleQuery); + m_obstacleQuery = 0; + + dtFreeNavMeshQuery(m_navquery); + m_navquery = 0; +} + +bool dtCrowd::init(const int maxAgents, const float maxAgentRadius, dtNavMesh* nav) +{ + m_maxAgents = maxAgents; + m_maxAgentRadius = maxAgentRadius; + + dtVset(m_ext, m_maxAgentRadius*2.0f,m_maxAgentRadius*1.5f,m_maxAgentRadius*2.0f); + + m_grid = dtAllocProximityGrid(); + if (!m_grid) + return false; + if (!m_grid->init(m_maxAgents*4, maxAgentRadius*3)) + return false; + + m_obstacleQuery = dtAllocObstacleAvoidanceQuery(); + if (!m_obstacleQuery) + return false; + m_obstacleQuery->init(6, 8); + + m_obstacleQuery->setDesiredVelocityWeight(2.0f); + m_obstacleQuery->setCurrentVelocityWeight(0.75f); + m_obstacleQuery->setPreferredSideWeight(0.75f); + m_obstacleQuery->setCollisionTimeWeight(2.5f); + m_obstacleQuery->setTimeHorizon(2.5f); + m_obstacleQuery->setVelocitySelectionBias(0.4f); + + // Allocate temp buffer for merging paths. + m_maxPathResult = 256; + m_pathResult = (dtPolyRef*)dtAlloc(sizeof(dtPolyRef)*m_maxPathResult, DT_ALLOC_PERM); + if (!m_pathResult) + return false; + + m_moveRequests = (MoveRequest*)dtAlloc(sizeof(MoveRequest)*m_maxAgents, DT_ALLOC_PERM); + if (!m_moveRequests) + return false; + m_moveRequestCount = 0; + + if (!m_pathq.init(m_maxPathResult, MAX_PATHQUEUE_NODES, nav)) + return false; + + m_agents = (dtCrowdAgent*)dtAlloc(sizeof(dtCrowdAgent)*m_maxAgents, DT_ALLOC_PERM); + if (!m_agents) + return false; + + m_activeAgents = (dtCrowdAgent**)dtAlloc(sizeof(dtCrowdAgent*)*m_maxAgents, DT_ALLOC_PERM); + if (!m_activeAgents) + return false; + + for (int i = 0; i < m_maxAgents; ++i) + { + new(&m_agents[i]) dtCrowdAgent(); + m_agents[i].active = 0; + if (!m_agents[i].corridor.init(m_maxPathResult)) + return false; + } + + // The navquery is mostly used for local searches, no need for large node pool. + m_navquery = dtAllocNavMeshQuery(); + if (!m_navquery) + return false; + if (dtStatusFailed(m_navquery->init(nav, MAX_COMMON_NODES))) + return false; + + return true; +} + +const int dtCrowd::getAgentCount() const +{ + return m_maxAgents; +} + +const dtCrowdAgent* dtCrowd::getAgent(const int idx) +{ + return &m_agents[idx]; +} + +int dtCrowd::addAgent(const float* pos, const dtCrowdAgentParams* params) +{ + // Find empty slot. + int idx = -1; + for (int i = 0; i < m_maxAgents; ++i) + { + if (!m_agents[i].active) + { + idx = i; + break; + } + } + if (idx == -1) + return -1; + + dtCrowdAgent* ag = &m_agents[idx]; + + // Find nearest position on navmesh and place the agent there. + float nearest[3]; + dtPolyRef ref; + m_navquery->findNearestPoly(pos, m_ext, &m_filter, &ref, nearest); + if (!ref) + { + // Could not find a location on navmesh. + return -1; + } + + ag->corridor.reset(ref, nearest); + ag->boundary.reset(); + + ag->radius = params->radius; + ag->height = params->height; + ag->maxAcceleration = params->maxAcceleration; + ag->maxSpeed = params->maxSpeed; + ag->collisionQueryRange = params->collisionQueryRange; + ag->pathOptimizationRange = params->pathOptimizationRange; + + ag->topologyOptTime = 0; + ag->nneis = 0; + + dtVset(ag->dvel, 0,0,0); + dtVset(ag->nvel, 0,0,0); + dtVset(ag->vel, 0,0,0); + dtVcopy(ag->npos, nearest); + + ag->desiredSpeed = 0; + ag->t = 0; + ag->var = (rand() % 10) / 9.0f; + + ag->active = 1; + + return idx; +} + +void dtCrowd::removeAgent(const int idx) +{ + if (idx >= 0 && idx < m_maxAgents) + { + m_agents[idx].active = 0; + } +} + +bool dtCrowd::requestMoveTarget(const int idx, dtPolyRef ref, const float* pos) +{ + if (idx < 0 || idx > m_maxAgents) + return false; + if (!ref) + return false; + + MoveRequest* req = 0; + // Check if there is existing request and update that instead. + for (int i = 0; i < m_moveRequestCount; ++i) + { + if (m_moveRequests[i].idx == idx) + { + req = &m_moveRequests[i]; + break; + } + } + if (!req) + { + if (m_moveRequestCount >= m_maxAgents) + return false; + req = &m_moveRequests[m_moveRequestCount++]; + memset(req, 0, sizeof(MoveRequest)); + } + + // Initialize request. + req->idx = idx; + req->ref = ref; + dtVcopy(req->pos, pos); + req->pathqRef = DT_PATHQ_INVALID; + req->state = MR_TARGET_REQUESTING; + + req->temp[0] = ref; + req->ntemp = 1; + + return true; +} + +bool dtCrowd::adjustMoveTarget(const int idx, dtPolyRef ref, const float* pos) +{ + if (idx < 0 || idx > m_maxAgents) + return false; + if (!ref) + return false; + + MoveRequest* req = 0; + // Check if there is existing request and update that instead. + for (int i = 0; i < m_moveRequestCount; ++i) + { + if (m_moveRequests[i].idx == idx) + { + req = &m_moveRequests[i]; + break; + } + } + if (!req) + { + if (m_moveRequestCount >= m_maxAgents) + return false; + req = &m_moveRequests[m_moveRequestCount++]; + memset(req, 0, sizeof(MoveRequest)); + + // New adjust request + req->state = MR_TARGET_ADJUST; + req->idx = idx; + } + + // Set adjustment request. + req->aref = ref; + dtVcopy(req->apos, pos); + + return true; +} + +int dtCrowd::getActiveAgents(dtCrowdAgent** agents, const int maxAgents) +{ + int n = 0; + for (int i = 0; i < m_maxAgents; ++i) + { + if (!m_agents[i].active) continue; + if (n < maxAgents) + agents[n++] = &m_agents[i]; + } + return n; +} + + + +int dtCrowd::getNeighbours(const float* pos, const float height, const float range, + const dtCrowdAgent* skip, dtCrowdNeighbour* result, const int maxResult) +{ + int n = 0; + + static const int MAX_NEIS = 32; + unsigned short ids[MAX_NEIS]; + int nids = m_grid->queryItems(pos[0]-range, pos[2]-range, + pos[0]+range, pos[2]+range, + ids, MAX_NEIS); + + for (int i = 0; i < nids; ++i) + { + dtCrowdAgent* ag = &m_agents[ids[i]]; + + if (ag == skip) continue; + + // Check for overlap. + float diff[3]; + dtVsub(diff, pos, ag->npos); + if (fabsf(diff[1]) >= (height+ag->height)/2.0f) + continue; + diff[1] = 0; + const float distSqr = dtVlenSqr(diff); + if (distSqr > dtSqr(range)) + continue; + + n = addNeighbour(ids[i], distSqr, result, n, maxResult); + } + return n; +} + +void dtCrowd::updateMoveRequest(const float dt) +{ + // Fire off new requests. + for (int i = 0; i < m_moveRequestCount; ++i) + { + MoveRequest* req = &m_moveRequests[i]; + dtCrowdAgent* ag = &m_agents[req->idx]; + + // Agent not active anymore, kill request. + if (!ag->active) + req->state = MR_TARGET_FAILED; + + // Adjust target + if (req->aref) + { + if (req->state == MR_TARGET_ADJUST) + { + // Adjust existing path. + ag->corridor.moveTargetPosition(req->apos, m_navquery, &m_filter); + req->state = MR_TARGET_VALID; + } + else + { + // Adjust on the flight request. + float result[3]; + static const int MAX_VISITED = 16; + dtPolyRef visited[MAX_VISITED]; + int nvisited = 0; + m_navquery->moveAlongSurface(req->temp[req->ntemp-1], req->pos, req->apos, &m_filter, + result, visited, &nvisited, MAX_VISITED); + req->ntemp = dtMergeCorridorEndMoved(req->temp, req->ntemp, MAX_TEMP_PATH, visited, nvisited); + dtVcopy(req->pos, result); + + // Reset adjustment. + dtVset(req->apos, 0,0,0); + req->aref = 0; + } + } + + + if (req->state == MR_TARGET_REQUESTING) + { + // Calculate request position. + // If there is a lot of latency between requests, it is possible to + // project the current position ahead and use raycast to find the actual + // location and path. + const dtPolyRef* path = ag->corridor.getPath(); + const int npath = ag->corridor.getPathCount(); + dtAssert(npath); + + // Here we take the simple approach and set the path to be just the current location. + float reqPos[3]; + dtVcopy(reqPos, ag->corridor.getPos()); // The location of the request + dtPolyRef reqPath[8]; // The path to the request location + reqPath[0] = path[0]; + int reqPathCount = 1; + + req->pathqRef = m_pathq.request(reqPath[reqPathCount-1], req->ref, reqPos, req->pos, &m_filter); + if (req->pathqRef != DT_PATHQ_INVALID) + { + ag->corridor.setCorridor(reqPos, reqPath, reqPathCount); + req->state = MR_TARGET_WAITING_FOR_PATH; + } + } + } + + + // Update requests. + m_pathq.update(MAX_ITERS_PER_UPDATE); + + // Process path results. + for (int i = 0; i < m_moveRequestCount; ++i) + { + MoveRequest* req = &m_moveRequests[i]; + dtCrowdAgent* ag = &m_agents[req->idx]; + + if (req->state == MR_TARGET_WAITING_FOR_PATH) + { + // Poll path queue. + dtStatus status = m_pathq.getRequestStatus(req->pathqRef); + if (dtStatusFailed(status)) + { + req->pathqRef = DT_PATHQ_INVALID; + req->state = MR_TARGET_FAILED; + } + else if (dtStatusSucceed(status)) + { + const dtPolyRef* path = ag->corridor.getPath(); + const int npath = ag->corridor.getPathCount(); + dtAssert(npath); + + // Apply results. + float targetPos[3]; + dtVcopy(targetPos, req->pos); + + dtPolyRef* res = m_pathResult; + bool valid = true; + int nres = 0; + dtStatus status = m_pathq.getPathResult(req->pathqRef, res, &nres, m_maxPathResult); + if (dtStatusFailed(status) || !nres) + valid = false; + + // Merge with any target adjustment that happened during the search. + if (req->ntemp > 1) + { + nres = dtMergeCorridorEndMoved(res, nres, m_maxPathResult, req->temp, req->ntemp); + } + + // Merge result and existing path. + // The agent might have moved whilst the request is + // being processed, so the path may have changed. + // We assume that the end of the path is at the same location + // where the request was issued. + + // The last ref in the old path should be the same as + // the location where the request was issued.. + if (valid && path[npath-1] != res[0]) + valid = false; + + if (valid) + { + // Put the old path infront of the old path. + if (npath > 1) + { + // Make space for the old path. + if ((npath-1)+nres > m_maxPathResult) + nres = m_maxPathResult - (npath-1); + memmove(res+npath-1, res, sizeof(dtPolyRef)*nres); + // Copy old path in the beginning. + memcpy(res, path, sizeof(dtPolyRef)*(npath-1)); + nres += npath-1; + } + + // Check for partial path. + if (res[nres-1] != req->ref) + { + // Partial path, constrain target position inside the last polygon. + float nearest[3]; + if (m_navquery->closestPointOnPoly(res[nres-1], targetPos, nearest) == DT_SUCCESS) + dtVcopy(targetPos, nearest); + else + valid = false; + } + } + + if (valid) + { + ag->corridor.setCorridor(targetPos, res, nres); + req->state = MR_TARGET_VALID; + } + else + { + // Something went wrong. + req->state = MR_TARGET_FAILED; + } + } + } + + // Remove request when done with it. + if (req->state == MR_TARGET_VALID || req->state == MR_TARGET_FAILED) + { + m_moveRequestCount--; + if (i != m_moveRequestCount) + memcpy(&m_moveRequests[i], &m_moveRequests[m_moveRequestCount], sizeof(MoveRequest)); + --i; + } + } + +} + + + +static int addToOptQueue(dtCrowdAgent* newag, dtCrowdAgent** agents, const int nagents, const int maxAgents) +{ + // Insert neighbour based on greatest time. + int slot = 0; + if (!nagents) + { + slot = nagents; + } + else if (newag->topologyOptTime <= agents[nagents-1]->topologyOptTime) + { + if (nagents >= maxAgents) + return nagents; + slot = nagents; + } + else + { + int i; + for (i = 0; i < nagents; ++i) + if (newag->topologyOptTime >= agents[i]->topologyOptTime) + break; + + const int tgt = i+1; + const int n = dtMin(nagents-i, maxAgents-tgt); + + dtAssert(tgt+n <= maxAgents); + + if (n > 0) + memmove(&agents[tgt], &agents[i], sizeof(dtCrowdAgent*)*n); + slot = i; + } + + agents[slot] = newag; + + return dtMin(nagents+1, maxAgents); +} + +void dtCrowd::updateTopologyOptimization(dtCrowdAgent** agents, const int nagents, const float dt) +{ + if (!nagents) + return; + + const float OPT_TIME_THR = 0.5f; // seconds + const int OPT_MAX_AGENTS = 1; + dtCrowdAgent* queue[OPT_MAX_AGENTS]; + int nqueue = 0; + + for (int i = 0; i < nagents; ++i) + { + dtCrowdAgent* ag = agents[i]; + ag->topologyOptTime += dt; + if (ag->topologyOptTime >= OPT_TIME_THR) + nqueue = addToOptQueue(ag, queue, nqueue, OPT_MAX_AGENTS); + } + + for (int i = 0; i < nqueue; ++i) + { + dtCrowdAgent* ag = queue[i]; + ag->corridor.optimizePathTopology(m_navquery, &m_filter); + ag->topologyOptTime = 0; + } + +} + +void dtCrowd::update(const float dt, unsigned int flags, dtCrowdAgentDebugInfo* debug) +{ + m_velocitySampleCount = 0; + + const int debugIdx = debug ? debug->idx : -1; + + dtCrowdAgent** agents = m_activeAgents; + int nagents = getActiveAgents(agents, m_maxAgents); + + // Update async move request and path finder. + updateMoveRequest(dt); + + // Optimize path topology. + if (flags & DT_CROWD_OPTIMIZE_TOPO) + updateTopologyOptimization(agents, nagents, dt); + + // Register agents to proximity grid. + m_grid->clear(); + for (int i = 0; i < nagents; ++i) + { + dtCrowdAgent* ag = agents[i]; + const float* p = ag->npos; + const float r = ag->radius; + m_grid->addItem((unsigned short)i, p[0]-r, p[2]-r, p[0]+r, p[2]+r); + } + + // Get nearby navmesh segments and agents to collide with. + for (int i = 0; i < nagents; ++i) + { + dtCrowdAgent* ag = agents[i]; + // Only update the collision boundary after certain distance has been passed. + if (dtVdist2DSqr(ag->npos, ag->boundary.getCenter()) > dtSqr(ag->collisionQueryRange*0.25f)) + ag->boundary.update(ag->corridor.getFirstPoly(), ag->npos, ag->collisionQueryRange, m_navquery, &m_filter); + // Query neighbour agents + ag->nneis = getNeighbours(ag->npos, ag->height, ag->collisionQueryRange, ag, ag->neis, DT_CROWDAGENT_MAX_NEIGHBOURS); + } + + // Find next corner to steer to. + for (int i = 0; i < nagents; ++i) + { + dtCrowdAgent* ag = agents[i]; + + // Find corners for steering + ag->ncorners = ag->corridor.findCorners(ag->cornerVerts, ag->cornerFlags, ag->cornerPolys, + DT_CROWDAGENT_MAX_CORNERS, m_navquery, &m_filter); + + // Check to see if the corner after the next corner is directly visible, + // and short cut to there. + if ((flags & DT_CROWD_OPTIMIZE_VIS) && ag->ncorners > 0) + { + const float* target = &ag->cornerVerts[dtMin(1,ag->ncorners-1)*3]; + ag->corridor.optimizePathVisibility(target, ag->pathOptimizationRange, m_navquery, &m_filter); + + // Copy data for debug purposes. + if (debugIdx == i) + { + dtVcopy(debug->optStart, ag->corridor.getPos()); + dtVcopy(debug->optEnd, target); + } + + } + else + { + // Copy data for debug purposes. + if (debugIdx == i) + { + dtVset(debug->optStart, 0,0,0); + dtVset(debug->optEnd, 0,0,0); + } + } + } + + // Calculate steering. + for (int i = 0; i < nagents; ++i) + { + dtCrowdAgent* ag = agents[i]; + + float dvel[3] = {0,0,0}; + + // Calculate steering direction. + if (flags & DT_CROWD_ANTICIPATE_TURNS) + calcSmoothSteerDirection(ag, dvel); + else + calcStraightSteerDirection(ag, dvel); + + // Calculate speed scale, which tells the agent to slowdown at the end of the path. + const float slowDownRadius = ag->radius*2; // TODO: make less hacky. + const float speedScale = getDistanceToGoal(ag, slowDownRadius) / slowDownRadius; + + // Apply style. + // TODO: find way to express custom movement styles. +/* if (flags & DT_CROWD_DRUNK) + { + // Drunken steering + + // Pulsating speed. + ag->t += dt * (1.0f - ag->var*0.25f); + ag->desiredSpeed = ag->maxSpeed * (1 + dtSqr(cosf(ag->t*2.0f))*0.3f); + + dtVscale(dvel, dvel, ag->desiredSpeed * speedScale); + + // Slightly wandering steering. + const float amp = cosf(ag->var*13.69f+ag->t*3.123f) * 0.2f; + const float nx = -dvel[2]; + const float nz = dvel[0]; + dvel[0] += nx*amp; + dvel[2] += nz*amp; + } + else*/ + { + // Normal steering. + ag->desiredSpeed = ag->maxSpeed; + dtVscale(dvel, dvel, ag->desiredSpeed * speedScale); + } + + // Set the desired velocity. + dtVcopy(ag->dvel, dvel); + } + + // Velocity planning. + for (int i = 0; i < nagents; ++i) + { + dtCrowdAgent* ag = agents[i]; + + if (flags & DT_CROWD_USE_VO) + { + m_obstacleQuery->reset(); + + // Add neighbours as obstacles. + for (int j = 0; j < ag->nneis; ++j) + { + const dtCrowdAgent* nei = &m_agents[ag->neis[j].idx]; + m_obstacleQuery->addCircle(nei->npos, nei->radius, nei->vel, nei->dvel); + } + + // Append neighbour segments as obstacles. + for (int j = 0; j < ag->boundary.getSegmentCount(); ++j) + { + const float* s = ag->boundary.getSegment(j); + if (dtTriArea2D(ag->npos, s, s+3) < 0.0f) + continue; + m_obstacleQuery->addSegment(s, s+3); + } + + dtObstacleAvoidanceDebugData* vod = 0; + if (debugIdx == i) + vod = debug->vod; + + // Sample new safe velocity. + bool adaptive = true; + int ns = 0; + + if (adaptive) + { + ns = m_obstacleQuery->sampleVelocityAdaptive(ag->npos, ag->radius, ag->desiredSpeed, + ag->vel, ag->dvel, ag->nvel, + VO_ADAPTIVE_DIVS, VO_ADAPTIVE_RINGS, VO_ADAPTIVE_DEPTH, + vod); + } + else + { + ns = m_obstacleQuery->sampleVelocityGrid(ag->npos, ag->radius, ag->desiredSpeed, + ag->vel, ag->dvel, ag->nvel, + VO_GRID_SIZE, vod); + } + m_velocitySampleCount += ns; + } + else + { + // If not using velocity planning, new velocity is directly the desired velocity. + dtVcopy(ag->nvel, ag->dvel); + } + } + + // Integrate. + for (int i = 0; i < nagents; ++i) + { + dtCrowdAgent* ag = agents[i]; + integrate(ag, dt); + } + + // Handle collisions. + static const float COLLISION_RESOLVE_FACTOR = 0.7f; + + for (int iter = 0; iter < 4; ++iter) + { + for (int i = 0; i < nagents; ++i) + { + dtCrowdAgent* ag = agents[i]; + + dtVset(ag->disp, 0,0,0); + + float w = 0; + + for (int j = 0; j < ag->nneis; ++j) + { + const dtCrowdAgent* nei = &m_agents[ag->neis[j].idx]; + + float diff[3]; + dtVsub(diff, ag->npos, nei->npos); + + if (fabsf(diff[1]) >= (ag->height+ nei->height)/2.0f) + continue; + + diff[1] = 0; + + float dist = dtVlenSqr(diff); + if (dist > dtSqr(ag->radius + nei->radius)) + continue; + dist = sqrtf(dist); + float pen = (ag->radius + nei->radius) - dist; + if (dist > 0.0001f) + pen = (1.0f/dist) * (pen*0.5f) * COLLISION_RESOLVE_FACTOR; + + dtVmad(ag->disp, ag->disp, diff, pen); + + w += 1.0f; + } + + if (w > 0.0001f) + { + const float iw = 1.0f / w; + dtVscale(ag->disp, ag->disp, iw); + } + } + + for (int i = 0; i < nagents; ++i) + { + dtCrowdAgent* ag = agents[i]; + dtVadd(ag->npos, ag->npos, ag->disp); + } + } + + for (int i = 0; i < nagents; ++i) + { + dtCrowdAgent* ag = agents[i]; + // Move along navmesh. + ag->corridor.movePosition(ag->npos, m_navquery, &m_filter); + // Get valid constrained position back. + dtVcopy(ag->npos, ag->corridor.getPos()); + } + +} + + diff --git a/DetourCrowd/Source/DetourLocalBoundary.cpp b/DetourCrowd/Source/DetourLocalBoundary.cpp new file mode 100644 index 0000000..e3fdbcc --- /dev/null +++ b/DetourCrowd/Source/DetourLocalBoundary.cpp @@ -0,0 +1,121 @@ +// +// 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. +// + +#include +#include +#include "DetourLocalBoundary.h" +#include "DetourNavMeshQuery.h" +#include "DetourCommon.h" +#include "DetourAssert.h" + + +dtLocalBoundary::dtLocalBoundary() : + m_nsegs(0) +{ + dtVset(m_center, FLT_MAX,FLT_MAX,FLT_MAX); +} + +dtLocalBoundary::~dtLocalBoundary() +{ +} + +void dtLocalBoundary::reset() +{ + dtVset(m_center, FLT_MAX,FLT_MAX,FLT_MAX); + m_nsegs = 0; +} + +void dtLocalBoundary::addSegment(const float dist, const float* s) +{ + // Insert neighbour based on the distance. + Segment* seg = 0; + if (!m_nsegs) + { + // First, trivial accept. + seg = &m_segs[0]; + } + else if (dist >= m_segs[m_nsegs-1].d) + { + // Further than the last segment, skip. + if (m_nsegs >= MAX_SEGS) + return; + // Last, trivial accept. + seg = &m_segs[m_nsegs]; + } + else + { + // Insert inbetween. + int i; + for (i = 0; i < m_nsegs; ++i) + if (dist <= m_segs[i].d) + break; + const int tgt = i+1; + const int n = dtMin(m_nsegs-i, MAX_SEGS-tgt); + dtAssert(tgt+n <= MAX_SEGS); + if (n > 0) + memmove(&m_segs[tgt], &m_segs[i], sizeof(Segment)*n); + seg = &m_segs[i]; + } + + seg->d = dist; + memcpy(seg->s, s, sizeof(float)*6); + + if (m_nsegs < MAX_SEGS) + m_nsegs++; +} + +void dtLocalBoundary::update(dtPolyRef ref, const float* pos, const float collisionQueryRange, + dtNavMeshQuery* navquery, const dtQueryFilter* filter) +{ + static const int MAX_LOCAL_POLYS = 16; + static const int MAX_SEGS_PER_POLY = DT_VERTS_PER_POLYGON*3; + + if (!ref) + { + dtVset(m_center, FLT_MAX,FLT_MAX,FLT_MAX); + m_nsegs = 0; + return; + } + + dtVcopy(m_center, pos); + + // First query non-overlapping polygons. + dtPolyRef locals[MAX_LOCAL_POLYS]; + int nlocals = 0; + navquery->findLocalNeighbourhood(ref, pos, collisionQueryRange, + filter, locals, 0, &nlocals, MAX_LOCAL_POLYS); + + // Secondly, store all polygon edges. + m_nsegs = 0; + float segs[MAX_SEGS_PER_POLY*6]; + int nsegs = 0; + for (int j = 0; j < nlocals; ++j) + { + navquery->getPolyWallSegments(locals[j], filter, segs, &nsegs, MAX_SEGS_PER_POLY); + for (int k = 0; k < nsegs; ++k) + { + const float* s = &segs[k*6]; + // Skip too distant segments. + float tseg; + const float distSqr = dtDistancePtSegSqr2D(pos, s, s+3, tseg); + if (distSqr > dtSqr(collisionQueryRange)) + continue; + addSegment(distSqr, s); + } + } +} diff --git a/DetourCrowd/Source/DetourPathCorridor.cpp b/DetourCrowd/Source/DetourPathCorridor.cpp new file mode 100644 index 0000000..28641fb --- /dev/null +++ b/DetourCrowd/Source/DetourPathCorridor.cpp @@ -0,0 +1,342 @@ +// +// 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. +// + +#include +#include "DetourPathCorridor.h" +#include "DetourNavMeshQuery.h" +#include "DetourCommon.h" +#include "DetourAssert.h" +#include "DetourAlloc.h" + + +int dtMergeCorridorStartMoved(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 = dtMin(furthestPath+1, npath); + int size = dtMax(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; +} + +int dtMergeCorridorEndMoved(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 = 0; i < npath; ++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. + const int ppos = furthestPath+1; + const int vpos = furthestVisited+1; + const int count = dtMin(nvisited-vpos, maxPath-ppos); + dtAssert(ppos+count <= maxPath); + if (count) + memcpy(path+ppos, visited+vpos, sizeof(dtPolyRef)*count); + + return ppos+count; +} + +int dtMergeCorridorStartShortcut(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 = furthestVisited; + if (req <= 0) + return npath; + + const int orig = furthestPath; + int size = dtMax(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[i]; + + return req+size; +} + +dtPathCorridor::dtPathCorridor() : + m_path(0), + m_npath(0), + m_maxPath(0) +{ +} + +dtPathCorridor::~dtPathCorridor() +{ + dtFree(m_path); +} + +bool dtPathCorridor::init(const int maxPath) +{ + dtAssert(!m_path); + m_path = (dtPolyRef*)dtAlloc(sizeof(dtPolyRef)*maxPath, DT_ALLOC_PERM); + if (!m_path) + return false; + m_npath = 0; + m_maxPath = maxPath; + return true; +} + +void dtPathCorridor::reset(dtPolyRef ref, const float* pos) +{ + dtAssert(m_path); + dtVcopy(m_pos, pos); + dtVcopy(m_target, pos); + m_path[0] = ref; + m_npath = 1; +} + +int dtPathCorridor::findCorners(float* cornerVerts, unsigned char* cornerFlags, + dtPolyRef* cornerPolys, const int maxCorners, + dtNavMeshQuery* navquery, const dtQueryFilter* filter) +{ + dtAssert(m_path); + dtAssert(m_npath); + + static const float MIN_TARGET_DIST = 0.01f; + + int ncorners = 0; + navquery->findStraightPath(m_pos, m_target, m_path, m_npath, + cornerVerts, cornerFlags, cornerPolys, &ncorners, maxCorners); + + // Prune points in the beginning of the path which are too close. + while (ncorners) + { + if ((cornerFlags[0] & DT_STRAIGHTPATH_OFFMESH_CONNECTION) || + dtVdist2DSqr(&cornerVerts[0], m_pos) > dtSqr(MIN_TARGET_DIST)) + break; + ncorners--; + if (ncorners) + { + memmove(cornerFlags, cornerFlags+1, sizeof(unsigned char)*ncorners); + memmove(cornerPolys, cornerPolys+1, sizeof(dtPolyRef)*ncorners); + memmove(cornerVerts, cornerVerts+3, sizeof(float)*3*ncorners); + } + } + + // Prune points after an off-mesh connection. + for (int i = 0; i < ncorners; ++i) + { + if (cornerFlags[i] & DT_STRAIGHTPATH_OFFMESH_CONNECTION) + { + ncorners = i+1; + break; + } + } + + return ncorners; +} + +void dtPathCorridor::optimizePathVisibility(const float* next, const float pathOptimizationRange, + dtNavMeshQuery* navquery, const dtQueryFilter* filter) +{ + dtAssert(m_path); + + // Clamp the ray to max distance. + float goal[3]; + dtVcopy(goal, next); + float dist = dtVdist2D(m_pos, goal); + + // If too close to the goal, do not try to optimize. + if (dist < 0.01f) + return; + + // Overshoot a little. This helps to optimize open fields in tiled meshes. + dist = dtMin(dist+0.01f, pathOptimizationRange); + + // Adjust ray length. + float delta[3]; + dtVsub(delta, goal, m_pos); + dtVmad(goal, m_pos, delta, pathOptimizationRange/dist); + + static const int MAX_RES = 32; + dtPolyRef res[MAX_RES]; + float t, norm[3]; + int nres = 0; + navquery->raycast(m_path[0], m_pos, goal, filter, &t, norm, res, &nres, MAX_RES); + if (nres > 1 && t > 0.99f) + { + m_npath = dtMergeCorridorStartShortcut(m_path, m_npath, m_maxPath, res, nres); + } +} + +bool dtPathCorridor::optimizePathTopology(dtNavMeshQuery* navquery, const dtQueryFilter* filter) +{ + dtAssert(m_path); + + if (m_npath < 3) + return false; + + static const int MAX_ITER = 32; + static const int MAX_RES = 32; + + dtPolyRef res[MAX_RES]; + int nres = 0; + navquery->initSlicedFindPath(m_path[0], m_path[m_npath-1], m_pos, m_target, filter); + navquery->updateSlicedFindPath(MAX_ITER, 0); + dtStatus status = navquery->finalizeSlicedFindPathPartial(m_path, m_npath, res, &nres, MAX_RES); + + if (status == DT_SUCCESS && nres > 0) + { + m_npath = dtMergeCorridorStartShortcut(m_path, m_npath, m_maxPath, res, nres); + return true; + } + + return false; +} + +void dtPathCorridor::movePosition(const float* npos, dtNavMeshQuery* navquery, const dtQueryFilter* filter) +{ + dtAssert(m_path); + dtAssert(m_npath); + + // Move along navmesh and update new position. + float result[3]; + static const int MAX_VISITED = 16; + dtPolyRef visited[MAX_VISITED]; + int nvisited = 0; + navquery->moveAlongSurface(m_path[0], m_pos, npos, filter, + result, visited, &nvisited, MAX_VISITED); + m_npath = dtMergeCorridorStartMoved(m_path, m_npath, m_maxPath, visited, nvisited); + + // Adjust the position to stay on top of the navmesh. + float h = m_pos[1]; + navquery->getPolyHeight(m_path[0], result, &h); + result[1] = h; + dtVcopy(m_pos, result); +} + +void dtPathCorridor::moveTargetPosition(const float* npos, dtNavMeshQuery* navquery, const dtQueryFilter* filter) +{ + dtAssert(m_path); + dtAssert(m_npath); + + // Move along navmesh and update new position. + float result[3]; + static const int MAX_VISITED = 16; + dtPolyRef visited[MAX_VISITED]; + int nvisited = 0; + navquery->moveAlongSurface(m_path[m_npath-1], m_target, npos, filter, + result, visited, &nvisited, MAX_VISITED); + m_npath = dtMergeCorridorEndMoved(m_path, m_npath, m_maxPath, visited, nvisited); + + // TODO: should we do that? + // Adjust the position to stay on top of the navmesh. + /* float h = m_target[1]; + navquery->getPolyHeight(m_path[m_npath-1], result, &h); + result[1] = h;*/ + + dtVcopy(m_target, result); +} + +void dtPathCorridor::setCorridor(const float* target, const dtPolyRef* path, const int npath) +{ + dtAssert(m_path); + dtAssert(npath > 0); + dtAssert(npath < m_maxPath); + + dtVcopy(m_target, target); + memcpy(m_path, path, sizeof(dtPolyRef)*npath); + m_npath = npath; +} diff --git a/DetourCrowd/Source/DetourPathQueue.cpp b/DetourCrowd/Source/DetourPathQueue.cpp new file mode 100644 index 0000000..f7c4c48 --- /dev/null +++ b/DetourCrowd/Source/DetourPathQueue.cpp @@ -0,0 +1,197 @@ +// +// 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. +// + +#include +#include "DetourPathQueue.h" +#include "DetourNavMesh.h" +#include "DetourNavMeshQuery.h" +#include "DetourAlloc.h" +#include "DetourCommon.h" + + +dtPathQueue::dtPathQueue() : + m_nextHandle(1), + m_maxPathSize(0), + m_queueHead(0), + m_navquery(0) +{ +} + +dtPathQueue::~dtPathQueue() +{ + purge(); +} + +void dtPathQueue::purge() +{ + dtFreeNavMeshQuery(m_navquery); + m_navquery = 0; + for (int i = 0; i < MAX_QUEUE; ++i) + { + dtFree(m_queue[i].path); + m_queue[i].path = 0; + } +} + +bool dtPathQueue::init(const int maxPathSize, const int maxSearchNodeCount, dtNavMesh* nav) +{ + purge(); + + m_navquery = dtAllocNavMeshQuery(); + if (!m_navquery) + return false; + if (dtStatusFailed(m_navquery->init(nav, maxSearchNodeCount))) + return false; + + m_maxPathSize = maxPathSize; + for (int i = 0; i < MAX_QUEUE; ++i) + { + m_queue[i].ref = DT_PATHQ_INVALID; + m_queue[i].path = (dtPolyRef*)dtAlloc(sizeof(dtPolyRef)*m_maxPathSize, DT_ALLOC_PERM); + if (!m_queue[i].path) + return false; + } + + m_queueHead = 0; + + return true; +} + +void dtPathQueue::update(const int maxIters) +{ + static const int MAX_KEEP_ALIVE = 2; // in update ticks. + + // Update path request until there is nothing to update + // or upto maxIters pathfinder iterations has been consumed. + int iterCount = maxIters; + + for (int i = 0; i < MAX_QUEUE; ++i) + { + PathQuery& q = m_queue[m_queueHead % MAX_QUEUE]; + + // Skip inactive requests. + if (q.ref == DT_PATHQ_INVALID) + { + m_queueHead++; + continue; + } + + // Handle completed request. + if (dtStatusSucceed(q.status) || dtStatusFailed(q.status)) + { + // If the path result has not been read in few frames, free the slot. + q.keepAlive++; + if (q.keepAlive > MAX_KEEP_ALIVE) + { + q.ref = DT_PATHQ_INVALID; + q.status = 0; + } + + m_queueHead++; + continue; + } + + // Handle query start. + if (q.status == 0) + { + q.status = m_navquery->initSlicedFindPath(q.startRef, q.endRef, q.startPos, q.endPos, q.filter); + } + // Handle query in progress. + if (dtStatusInProgress(q.status)) + { + int iters = 0; + q.status = m_navquery->updateSlicedFindPath(iterCount, &iters); + iterCount -= iters; + } + if (dtStatusSucceed(q.status)) + { + q.status = m_navquery->finalizeSlicedFindPath(q.path, &q.npath, m_maxPathSize); + } + + if (iterCount <= 0) + break; + + m_queueHead++; + } +} + +dtPathQueueRef dtPathQueue::request(dtPolyRef startRef, dtPolyRef endRef, + const float* startPos, const float* endPos, + const dtQueryFilter* filter) +{ + // Find empty slot + int slot = -1; + for (int i = 0; i < MAX_QUEUE; ++i) + { + if (m_queue[i].ref == DT_PATHQ_INVALID) + { + slot = i; + break; + } + } + // Could not find slot. + if (slot == -1) + return DT_PATHQ_INVALID; + + dtPathQueueRef ref = m_nextHandle++; + if (m_nextHandle == DT_PATHQ_INVALID) m_nextHandle++; + + PathQuery& q = m_queue[slot]; + q.ref = ref; + dtVcopy(q.startPos, startPos); + q.startRef = startRef; + dtVcopy(q.endPos, endPos); + q.endRef = endRef; + + q.status = 0; + q.npath = 0; + q.filter = filter; + q.keepAlive = 0; + + return ref; +} + +dtStatus dtPathQueue::getRequestStatus(dtPathQueueRef ref) const +{ + for (int i = 0; i < MAX_QUEUE; ++i) + { + if (m_queue[i].ref == ref) + return m_queue[i].status; + } + return DT_FAILURE; +} + +dtStatus dtPathQueue::getPathResult(dtPathQueueRef ref, dtPolyRef* path, int* pathSize, const int maxPath) +{ + for (int i = 0; i < MAX_QUEUE; ++i) + { + if (m_queue[i].ref == ref) + { + PathQuery& q = m_queue[i]; + // Free request for reuse. + q.ref = DT_PATHQ_INVALID; + q.status = 0; + // Copy path + int n = dtMin(q.npath, maxPath); + memcpy(path, q.path, sizeof(dtPolyRef)*n); + *pathSize = n; + return DT_SUCCESS; + } + } + return DT_FAILURE; +} diff --git a/DetourCrowd/Source/DetourProximityGrid.cpp b/DetourCrowd/Source/DetourProximityGrid.cpp new file mode 100644 index 0000000..d8226a4 --- /dev/null +++ b/DetourCrowd/Source/DetourProximityGrid.cpp @@ -0,0 +1,194 @@ +// +// 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. +// + +#include +#include +#include +#include "DetourProximityGrid.h" +#include "DetourCommon.h" +#include "DetourAlloc.h" +#include "DetourAssert.h" + + +dtProximityGrid* dtAllocProximityGrid() +{ + void* mem = dtAlloc(sizeof(dtProximityGrid), DT_ALLOC_PERM); + if (!mem) return 0; + return new(mem) dtProximityGrid; +} + +void dtFreeProximityGrid(dtProximityGrid* ptr) +{ + if (!ptr) return; + ptr->~dtProximityGrid(); + dtFree(ptr); +} + + +inline int hashPos2(int x, int y, int n) +{ + return ((x*73856093) ^ (y*19349663)) & (n-1); +} + + +dtProximityGrid::dtProximityGrid() : + m_maxItems(0), + m_cellSize(0), + m_pool(0), + m_poolHead(0), + m_poolSize(0), + m_buckets(0), + m_bucketsSize(0) +{ +} + +dtProximityGrid::~dtProximityGrid() +{ + dtFree(m_buckets); + dtFree(m_pool); +} + +bool dtProximityGrid::init(const int poolSize, const float cellSize) +{ + dtAssert(poolSize > 0); + dtAssert(cellSize > 0.0f); + + m_cellSize = cellSize; + m_invCellSize = 1.0f / m_cellSize; + + // Allocate hashs buckets + m_bucketsSize = dtNextPow2(poolSize); + m_buckets = (unsigned short*)dtAlloc(sizeof(unsigned short)*m_bucketsSize, DT_ALLOC_PERM); + if (!m_buckets) + return false; + + // Allocate pool of items. + m_poolSize = poolSize; + m_poolHead = 0; + m_pool = (Item*)dtAlloc(sizeof(Item)*m_poolSize, DT_ALLOC_PERM); + if (!m_pool) + return false; + + clear(); + + return true; +} + +void dtProximityGrid::clear() +{ + memset(m_buckets, 0xff, sizeof(unsigned short)*m_bucketsSize); + m_poolHead = 0; + m_bounds[0] = 0xffff; + m_bounds[1] = 0xffff; + m_bounds[2] = -0xffff; + m_bounds[3] = -0xffff; +} + +void dtProximityGrid::addItem(const unsigned short id, + const float minx, const float miny, + const float maxx, const float maxy) +{ + const int iminx = (int)floorf(minx * m_invCellSize); + const int iminy = (int)floorf(miny * m_invCellSize); + const int imaxx = (int)floorf(maxx * m_invCellSize); + const int imaxy = (int)floorf(maxy * m_invCellSize); + + m_bounds[0] = dtMin(m_bounds[0], iminx); + m_bounds[1] = dtMin(m_bounds[1], iminy); + m_bounds[2] = dtMax(m_bounds[2], imaxx); + m_bounds[3] = dtMax(m_bounds[3], imaxy); + + for (int y = iminy; y <= imaxy; ++y) + { + for (int x = iminx; x <= imaxx; ++x) + { + if (m_poolHead < m_poolSize) + { + const int h = hashPos2(x, y, m_bucketsSize); + const unsigned short idx = (unsigned short)m_poolHead; + m_poolHead++; + Item& item = m_pool[idx]; + item.x = (short)x; + item.y = (short)y; + item.id = id; + item.next = m_buckets[h]; + m_buckets[h] = idx; + } + } + } +} + +int dtProximityGrid::queryItems(const float minx, const float miny, + const float maxx, const float maxy, + unsigned short* ids, const int maxIds) const +{ + const int iminx = (int)floorf(minx * m_invCellSize); + const int iminy = (int)floorf(miny * m_invCellSize); + const int imaxx = (int)floorf(maxx * m_invCellSize); + const int imaxy = (int)floorf(maxy * m_invCellSize); + + int n = 0; + + for (int y = iminy; y <= imaxy; ++y) + { + for (int x = iminx; x <= imaxx; ++x) + { + const int h = hashPos2(x, y, m_bucketsSize); + unsigned short idx = m_buckets[h]; + while (idx != 0xffff) + { + Item& item = m_pool[idx]; + if ((int)item.x == x && (int)item.y == y) + { + // Check if the id exists already. + const unsigned short* end = ids + n; + unsigned short* i = ids; + while (i != end && *i != item.id) + ++i; + // Item not found, add it. + if (i == end) + { + if (n >= maxIds) + return n; + ids[n++] = item.id; + } + } + idx = item.next; + } + } + } + + return n; +} + +int dtProximityGrid::getItemCountAt(const int x, const int y) const +{ + int n = 0; + + const int h = hashPos2(x, y, m_bucketsSize); + unsigned short idx = m_buckets[h]; + while (idx != 0xffff) + { + Item& item = m_pool[idx]; + if ((int)item.x == x && (int)item.y == y) + n++; + idx = item.next; + } + + return n; +} diff --git a/RecastDemo/Bin/Recast.app/Contents/MacOS/Recast b/RecastDemo/Bin/Recast.app/Contents/MacOS/Recast index 9271ed0..331eb7e 100755 Binary files a/RecastDemo/Bin/Recast.app/Contents/MacOS/Recast and b/RecastDemo/Bin/Recast.app/Contents/MacOS/Recast differ diff --git a/RecastDemo/Build/Xcode/Recast.xcodeproj/project.pbxproj b/RecastDemo/Build/Xcode/Recast.xcodeproj/project.pbxproj index f67cd29..2798cbb 100644 --- a/RecastDemo/Build/Xcode/Recast.xcodeproj/project.pbxproj +++ b/RecastDemo/Build/Xcode/Recast.xcodeproj/project.pbxproj @@ -43,6 +43,11 @@ 6BAF3C591211663A008CFCDF /* CrowdTool.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 6BAF3C581211663A008CFCDF /* CrowdTool.cpp */; }; 6BAF40DB12196A3D008CFCDF /* DetourNavMeshQuery.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 6BAF40DA12196A3D008CFCDF /* DetourNavMeshQuery.cpp */; }; 6BAF4442121C3D26008CFCDF /* SampleInterfaces.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 6BAF4441121C3D26008CFCDF /* SampleInterfaces.cpp */; }; + 6BB5013612F458CB001B1957 /* DetourCrowd.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 6BB5013112F458CB001B1957 /* DetourCrowd.cpp */; }; + 6BB5013712F458CB001B1957 /* DetourLocalBoundary.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 6BB5013212F458CB001B1957 /* DetourLocalBoundary.cpp */; }; + 6BB5013812F458CB001B1957 /* DetourPathCorridor.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 6BB5013312F458CB001B1957 /* DetourPathCorridor.cpp */; }; + 6BB5013912F458CB001B1957 /* DetourPathQueue.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 6BB5013412F458CB001B1957 /* DetourPathQueue.cpp */; }; + 6BB5013A12F458CB001B1957 /* DetourProximityGrid.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 6BB5013512F458CB001B1957 /* DetourProximityGrid.cpp */; }; 6BB788170FC0472B003C24DB /* ChunkyTriMesh.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 6BB788160FC0472B003C24DB /* ChunkyTriMesh.cpp */; }; 6BB7FC0B10EBB6AA006DA0A6 /* NavMeshTesterTool.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 6BB7FC0A10EBB6AA006DA0A6 /* NavMeshTesterTool.cpp */; }; 6BB7FDA510F36F0E006DA0A6 /* InputGeom.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 6BB7FDA410F36F0E006DA0A6 /* InputGeom.cpp */; }; @@ -52,7 +57,6 @@ 6BB93CF610CFEC4500F74F2B /* RecastDump.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 6BB93CF510CFEC4500F74F2B /* RecastDump.cpp */; }; 6BCF32361104CD05009445BF /* OffMeshConnectionTool.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 6BCF32351104CD05009445BF /* OffMeshConnectionTool.cpp */; }; 6BD402011224279400995864 /* PerfTimer.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 6BD402001224279400995864 /* PerfTimer.cpp */; }; - 6BD667DA123D28100021A7A4 /* CrowdManager.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 6BD667D9123D28100021A7A4 /* CrowdManager.cpp */; }; 6BF5F23A11747606000502A6 /* Filelist.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 6BF5F23911747606000502A6 /* Filelist.cpp */; }; 6BF5F2401174763B000502A6 /* SlideShow.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 6BF5F23F1174763B000502A6 /* SlideShow.cpp */; }; 6BF7C1401111953A002B3F46 /* TestCase.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 6BF7C13F1111953A002B3F46 /* TestCase.cpp */; }; @@ -132,6 +136,16 @@ 6BAF4440121C3D0A008CFCDF /* SampleInterfaces.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; name = SampleInterfaces.h; path = ../../Include/SampleInterfaces.h; sourceTree = SOURCE_ROOT; }; 6BAF4441121C3D26008CFCDF /* SampleInterfaces.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; name = SampleInterfaces.cpp; path = ../../Source/SampleInterfaces.cpp; sourceTree = SOURCE_ROOT; }; 6BAF4561121D173A008CFCDF /* RecastAssert.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; name = RecastAssert.h; path = ../../../Recast/Include/RecastAssert.h; sourceTree = SOURCE_ROOT; }; + 6BB5012C12F458CB001B1957 /* DetourCrowd.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; name = DetourCrowd.h; path = ../../../DetourCrowd/Include/DetourCrowd.h; sourceTree = SOURCE_ROOT; }; + 6BB5012D12F458CB001B1957 /* DetourLocalBoundary.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; name = DetourLocalBoundary.h; path = ../../../DetourCrowd/Include/DetourLocalBoundary.h; sourceTree = SOURCE_ROOT; }; + 6BB5012E12F458CB001B1957 /* DetourPathCorridor.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; name = DetourPathCorridor.h; path = ../../../DetourCrowd/Include/DetourPathCorridor.h; sourceTree = SOURCE_ROOT; }; + 6BB5012F12F458CB001B1957 /* DetourPathQueue.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; name = DetourPathQueue.h; path = ../../../DetourCrowd/Include/DetourPathQueue.h; sourceTree = SOURCE_ROOT; }; + 6BB5013012F458CB001B1957 /* DetourProximityGrid.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; name = DetourProximityGrid.h; path = ../../../DetourCrowd/Include/DetourProximityGrid.h; sourceTree = SOURCE_ROOT; }; + 6BB5013112F458CB001B1957 /* DetourCrowd.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; name = DetourCrowd.cpp; path = ../../../DetourCrowd/Source/DetourCrowd.cpp; sourceTree = SOURCE_ROOT; }; + 6BB5013212F458CB001B1957 /* DetourLocalBoundary.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; name = DetourLocalBoundary.cpp; path = ../../../DetourCrowd/Source/DetourLocalBoundary.cpp; sourceTree = SOURCE_ROOT; }; + 6BB5013312F458CB001B1957 /* DetourPathCorridor.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; name = DetourPathCorridor.cpp; path = ../../../DetourCrowd/Source/DetourPathCorridor.cpp; sourceTree = SOURCE_ROOT; }; + 6BB5013412F458CB001B1957 /* DetourPathQueue.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; name = DetourPathQueue.cpp; path = ../../../DetourCrowd/Source/DetourPathQueue.cpp; sourceTree = SOURCE_ROOT; }; + 6BB5013512F458CB001B1957 /* DetourProximityGrid.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; name = DetourProximityGrid.cpp; path = ../../../DetourCrowd/Source/DetourProximityGrid.cpp; sourceTree = SOURCE_ROOT; }; 6BB788160FC0472B003C24DB /* ChunkyTriMesh.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; name = ChunkyTriMesh.cpp; path = ../../Source/ChunkyTriMesh.cpp; sourceTree = SOURCE_ROOT; }; 6BB788180FC04753003C24DB /* ChunkyTriMesh.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; name = ChunkyTriMesh.h; path = ../../Include/ChunkyTriMesh.h; sourceTree = SOURCE_ROOT; }; 6BB7FC0910EBB6AA006DA0A6 /* NavMeshTesterTool.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; name = NavMeshTesterTool.h; path = ../../Include/NavMeshTesterTool.h; sourceTree = SOURCE_ROOT; }; @@ -150,8 +164,6 @@ 6BCF32351104CD05009445BF /* OffMeshConnectionTool.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; name = OffMeshConnectionTool.cpp; path = ../../Source/OffMeshConnectionTool.cpp; sourceTree = SOURCE_ROOT; }; 6BD401FF1224278800995864 /* PerfTimer.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; name = PerfTimer.h; path = ../../Include/PerfTimer.h; sourceTree = SOURCE_ROOT; }; 6BD402001224279400995864 /* PerfTimer.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; name = PerfTimer.cpp; path = ../../Source/PerfTimer.cpp; sourceTree = SOURCE_ROOT; }; - 6BD667D8123D27EC0021A7A4 /* CrowdManager.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; name = CrowdManager.h; path = ../../Include/CrowdManager.h; sourceTree = SOURCE_ROOT; }; - 6BD667D9123D28100021A7A4 /* CrowdManager.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; name = CrowdManager.cpp; path = ../../Source/CrowdManager.cpp; sourceTree = SOURCE_ROOT; }; 6BF5F23911747606000502A6 /* Filelist.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; name = Filelist.cpp; path = ../../Source/Filelist.cpp; sourceTree = SOURCE_ROOT; }; 6BF5F23C11747614000502A6 /* Filelist.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; name = Filelist.h; path = ../../Include/Filelist.h; sourceTree = SOURCE_ROOT; }; 6BF5F23E1174763B000502A6 /* SlideShow.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; name = SlideShow.h; path = ../../Include/SlideShow.h; sourceTree = SOURCE_ROOT; }; @@ -181,26 +193,20 @@ 080E96DDFE201D6D7F000001 /* Classes */ = { isa = PBXGroup; children = ( - 6B56847412DA05F2000B9960 /* lzf.h */, - 6B8BA93912DAE5CC00EE6EFF /* lzfP.h */, - 6B8BA92612DAE1DF00EE6EFF /* lzf_c.c */, - 6B8BA92712DAE1DF00EE6EFF /* lzf_d.c */, + 6BB5012B12F458AE001B1957 /* DetourCrowd */, + 6BB5012A12F45891001B1957 /* Contrib */, 6BB93C7610CFE1BD00F74F2B /* DebugUtils */, 6BDD9E030F91110C00904EEF /* Detour */, 6B137C7D0F7FCBE800459200 /* Recast */, 6B555DF5100B25FC00247EA3 /* Samples */, 6BB7FE8E10F4A175006DA0A6 /* Tools */, 6B25B6180FFA62BE004F1BC4 /* main.cpp */, - 6BD667D8123D27EC0021A7A4 /* CrowdManager.h */, - 6BD667D9123D28100021A7A4 /* CrowdManager.cpp */, 6BAF4440121C3D0A008CFCDF /* SampleInterfaces.h */, 6BAF4441121C3D26008CFCDF /* SampleInterfaces.cpp */, - 6BF5F2C511747E9F000502A6 /* stb_image.h */, 6BF5F23E1174763B000502A6 /* SlideShow.h */, 6BF5F23F1174763B000502A6 /* SlideShow.cpp */, 6BF5F23C11747614000502A6 /* Filelist.h */, 6BF5F23911747606000502A6 /* Filelist.cpp */, - 6B555DF6100B273500247EA3 /* stb_truetype.h */, 6B137C7A0F7FCBE400459200 /* imgui.h */, 6B137C6C0F7FCBBB00459200 /* imgui.cpp */, 6B555DAE100B211D00247EA3 /* imguiRenderGL.h */, @@ -329,6 +335,36 @@ name = Samples; sourceTree = ""; }; + 6BB5012A12F45891001B1957 /* Contrib */ = { + isa = PBXGroup; + children = ( + 6B555DF6100B273500247EA3 /* stb_truetype.h */, + 6BF5F2C511747E9F000502A6 /* stb_image.h */, + 6B56847412DA05F2000B9960 /* lzf.h */, + 6B8BA93912DAE5CC00EE6EFF /* lzfP.h */, + 6B8BA92612DAE1DF00EE6EFF /* lzf_c.c */, + 6B8BA92712DAE1DF00EE6EFF /* lzf_d.c */, + ); + name = Contrib; + sourceTree = ""; + }; + 6BB5012B12F458AE001B1957 /* DetourCrowd */ = { + isa = PBXGroup; + children = ( + 6BB5012C12F458CB001B1957 /* DetourCrowd.h */, + 6BB5013112F458CB001B1957 /* DetourCrowd.cpp */, + 6BB5012D12F458CB001B1957 /* DetourLocalBoundary.h */, + 6BB5013212F458CB001B1957 /* DetourLocalBoundary.cpp */, + 6BB5012E12F458CB001B1957 /* DetourPathCorridor.h */, + 6BB5013312F458CB001B1957 /* DetourPathCorridor.cpp */, + 6BB5012F12F458CB001B1957 /* DetourPathQueue.h */, + 6BB5013412F458CB001B1957 /* DetourPathQueue.cpp */, + 6BB5013012F458CB001B1957 /* DetourProximityGrid.h */, + 6BB5013512F458CB001B1957 /* DetourProximityGrid.cpp */, + ); + name = DetourCrowd; + sourceTree = ""; + }; 6BB7FE8E10F4A175006DA0A6 /* Tools */ = { isa = PBXGroup; children = ( @@ -479,10 +515,14 @@ 6BD402011224279400995864 /* PerfTimer.cpp in Sources */, 6B9EFF0912281C3E00535FF1 /* DetourObstacleAvoidance.cpp in Sources */, 6B847777122D221D00ADF63D /* ValueHistory.cpp in Sources */, - 6BD667DA123D28100021A7A4 /* CrowdManager.cpp in Sources */, 6B5683B812D9E7D3000B9960 /* Sample_TempObstacles.cpp in Sources */, 6B8BA92812DAE1DF00EE6EFF /* lzf_c.c in Sources */, 6B8BA92912DAE1DF00EE6EFF /* lzf_d.c in Sources */, + 6BB5013612F458CB001B1957 /* DetourCrowd.cpp in Sources */, + 6BB5013712F458CB001B1957 /* DetourLocalBoundary.cpp in Sources */, + 6BB5013812F458CB001B1957 /* DetourPathCorridor.cpp in Sources */, + 6BB5013912F458CB001B1957 /* DetourPathQueue.cpp in Sources */, + 6BB5013A12F458CB001B1957 /* DetourProximityGrid.cpp in Sources */, ); runOnlyForDeploymentPostprocessing = 0; }; diff --git a/RecastDemo/Include/CrowdManager.h b/RecastDemo/Include/CrowdManager.h deleted file mode 100644 index faa6423..0000000 --- a/RecastDemo/Include/CrowdManager.h +++ /dev/null @@ -1,335 +0,0 @@ -// -// 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. -// - -#ifndef CROWDMANAGER_H -#define CROWDMANAGER_H - -#include "DetourNavMeshQuery.h" -#include "DetourObstacleAvoidance.h" -#include "ValueHistory.h" - - -class ProximityGrid -{ - int m_maxItems; - float m_cellSize; - float m_invCellSize; - - struct Item - { - unsigned short id; - short x,y; - unsigned short next; - }; - Item* m_pool; - int m_poolHead; - int m_poolSize; - - unsigned short* m_buckets; - int m_bucketsSize; - - int m_bounds[4]; - -public: - ProximityGrid(); - ~ProximityGrid(); - - bool init(const int maxItems, const float cellSize); - - void clear(); - - void addItem(const unsigned short id, - const float minx, const float miny, - const float maxx, const float maxy); - - int queryItems(const float minx, const float miny, - const float maxx, const float maxy, - unsigned short* ids, const int maxIds) const; - - int getItemCountAt(const int x, const int y) const; - const int* getBounds() const { return m_bounds; } - const float getCellSize() const { return m_cellSize; } -}; - - - - -static const unsigned int PATHQ_INVALID = 0; - -enum PathQueueRequestState -{ - PATHQ_STATE_INVALID, - PATHQ_STATE_WORKING, - PATHQ_STATE_READY, -}; - -typedef unsigned int PathQueueRef; - -class PathQueue -{ - static const int PQ_MAX_PATH = 256; - - struct PathQuery - { - // Path find start and end location. - float startPos[3], endPos[3]; - dtPolyRef startRef, endRef; - // Result. - dtPolyRef path[PQ_MAX_PATH]; - bool ready; - int npath; - PathQueueRef ref; - const dtQueryFilter* filter; // TODO: This is potentially dangerous! - int keepalive; - }; - - static const int MAX_QUEUE = 8; - PathQuery m_queue[MAX_QUEUE]; - PathQueueRef m_nextHandle; - - int m_delay; - -public: - PathQueue(); - ~PathQueue(); - - void update(dtNavMeshQuery* navquery); - PathQueueRef request(dtPolyRef startRef, dtPolyRef endRef, - const float* startPos, const float* endPos, - const dtQueryFilter* filter); - int getRequestState(PathQueueRef ref); - int getPathResult(PathQueueRef ref, dtPolyRef* path, const int maxPath); -}; - - -class PathCorridor -{ - float m_pos[3]; - float m_target[3]; - - dtPolyRef* m_path; - int m_npath; - int m_maxPath; - -public: - PathCorridor(); - ~PathCorridor(); - - bool init(const int maxPath); - - void reset(dtPolyRef ref, const float* pos); - - int findCorners(float* cornerVerts, unsigned char* cornerFlags, - dtPolyRef* cornerPolys, const int maxCorners, - dtNavMeshQuery* navquery, const dtQueryFilter* filter); - - void optimizePathVisibility(const float* next, const float pathOptimizationRange, - dtNavMeshQuery* navquery, const dtQueryFilter* filter); - - bool optimizePathTopology(dtNavMeshQuery* navquery, const dtQueryFilter* filter); - - void movePosition(const float* npos, dtNavMeshQuery* navquery, const dtQueryFilter* filter); - void moveTargetPosition(const float* npos, dtNavMeshQuery* navquery, const dtQueryFilter* filter); - - void setCorridor(const float* target, const dtPolyRef* polys, const int npolys); - - inline const float* getPos() const { return m_pos; } - inline const float* getTarget() const { return m_target; } - - inline dtPolyRef getFirstPoly() const { return m_npath ? m_path[0] : 0; } - - inline const dtPolyRef* getPath() const { return m_path; } - inline int getPathCount() const { return m_npath; } -}; - - -class LocalBoundary -{ - static const int MAX_SEGS = 8; - - struct Segment - { - float s[6]; // Segment start/end - float d; // Distance for pruning. - }; - - float m_center[3]; - Segment m_segs[MAX_SEGS]; - int m_nsegs; - - void addSegment(const float dist, const float* seg); - -public: - LocalBoundary(); - ~LocalBoundary(); - - void reset(); - - void update(dtPolyRef ref, const float* pos, const float collisionQueryRange, - dtNavMeshQuery* navquery, const dtQueryFilter* filter); - - inline const float* getCenter() const { return m_center; } - inline int getSegmentCount() const { return m_nsegs; } - inline const float* getSegment(int i) const { return m_segs[i].s; } -}; - - -static const int AGENT_MAX_NEIGHBOURS = 6; -static const int AGENT_MAX_CORNERS = 4; -static const int AGENT_MAX_TRAIL = 64; - -struct Neighbour -{ - int idx; - float dist; -}; - -struct Agent -{ - void integrate(const float maxAcc, const float dt); - void calcSmoothSteerDirection(float* dir); - void calcStraightSteerDirection(float* dir); - float getDistanceToGoal(const float range) const; - - unsigned char active; - - PathCorridor corridor; - LocalBoundary boundary; - - float maxspeed; - float t; - float var; - - float collisionQueryRange; - float pathOptimizationRange; - - float topologyOptTime; - - Neighbour neis[AGENT_MAX_NEIGHBOURS]; - int nneis; - - float radius, height; - float npos[3]; - float disp[3]; - float dvel[3]; - float nvel[3]; - float vel[3]; - - float cornerVerts[AGENT_MAX_CORNERS*3]; - unsigned char cornerFlags[AGENT_MAX_CORNERS]; - dtPolyRef cornerPolys[AGENT_MAX_CORNERS]; - int ncorners; - - float opts[3], opte[3]; - - float trail[AGENT_MAX_TRAIL*3]; - int htrail; -}; - - -enum UpdateFlags -{ - CROWDMAN_ANTICIPATE_TURNS = 1, - CROWDMAN_USE_VO = 2, - CROWDMAN_DRUNK = 4, - CROWDMAN_OPTIMIZE_VIS = 8, - CROWDMAN_OPTIMIZE_TOPO = 16, -}; - - -class CrowdManager -{ - static const int MAX_AGENTS = 128; - Agent m_agents[MAX_AGENTS]; - dtObstacleAvoidanceDebugData* m_vodebug[MAX_AGENTS]; - - dtObstacleAvoidanceQuery* m_obstacleQuery; - PathQueue m_pathq; - ProximityGrid m_grid; - - dtPolyRef* m_pathResult; - int m_maxPathResult; - - float m_ext[3]; - dtQueryFilter m_filter; - - int m_totalTime; - int m_rvoTime; - int m_sampleCount; - - enum MoveRequestState - { - MR_TARGET_FAILED, - MR_TARGET_VALID, - MR_TARGET_REQUESTING, - MR_TARGET_WAITING_FOR_PATH, - MR_TARGET_ADJUST, - }; - - static const int MAX_TEMP_PATH = 32; - - struct MoveRequest - { - unsigned char state; // State of the request - int idx; // Agent index - dtPolyRef ref; // Goal ref - float pos[3]; // Goal position - PathQueueRef pathqRef; // Path find query ref - dtPolyRef aref; // Goal adjustment ref - float apos[3]; // Goal adjustment pos - dtPolyRef temp[MAX_TEMP_PATH]; // Adjusted path to the goal - int ntemp; - }; - MoveRequest m_moveRequests[MAX_AGENTS]; - int m_moveRequestCount; - - int getNeighbours(const float* pos, const float height, const float range, - const Agent* skip, Neighbour* result, const int maxResult); - - void updateTopologyOptimization(const float dt, dtNavMeshQuery* navquery, const dtQueryFilter* filter); - void updateMoveRequest(const float dt, dtNavMeshQuery* navquery, const dtQueryFilter* filter); - -public: - CrowdManager(); - ~CrowdManager(); - - void reset(); - const Agent* getAgent(const int idx); - const int getAgentCount() const; - int addAgent(const float* pos, const float radius, const float height, dtNavMeshQuery* navquery); - void removeAgent(const int idx); - - bool requestMoveTarget(const int idx, dtPolyRef ref, const float* pos); - bool adjustMoveTarget(const int idx, dtPolyRef ref, const float* pos); - - int getActiveAgents(Agent** agents, const int maxAgents); - void update(const float dt, unsigned int flags, dtNavMeshQuery* navquery); - - const dtQueryFilter* getFilter() const { return &m_filter; } - const float* getQueryExtents() const { return m_ext; } - - const dtObstacleAvoidanceDebugData* getVODebugData(const int idx) const { return m_vodebug[idx]; } - inline int getTotalTime() const { return m_totalTime; } - inline int getRVOTime() const { return m_rvoTime; } - inline int getSampleCount() const { return m_sampleCount; } - const ProximityGrid* getGrid() const { return &m_grid; } - -}; - - -#endif // CROWDMANAGER_H \ No newline at end of file diff --git a/RecastDemo/Include/CrowdTool.h b/RecastDemo/Include/CrowdTool.h index 1c69f6e..2796cca 100644 --- a/RecastDemo/Include/CrowdTool.h +++ b/RecastDemo/Include/CrowdTool.h @@ -23,7 +23,7 @@ #include "DetourNavMesh.h" #include "DetourObstacleAvoidance.h" #include "ValueHistory.h" -#include "CrowdManager.h" +#include "DetourCrowd.h" // Tool to create crowds. @@ -34,15 +34,17 @@ class CrowdTool : public SampleTool float m_targetPos[3]; dtPolyRef m_targetRef; - - bool m_expandDebugDraw; - bool m_showLabels; + + + bool m_expandSelectedDebugDraw; bool m_showCorners; - bool m_showTargets; bool m_showCollisionSegments; bool m_showPath; bool m_showVO; bool m_showOpt; + + bool m_expandDebugDraw; + bool m_showLabels; bool m_showGrid; bool m_showNodes; bool m_showPerfGraph; @@ -55,20 +57,34 @@ class CrowdTool : public SampleTool bool m_drunkMove; bool m_run; + + dtCrowdAgentDebugInfo m_agentDebug; + dtObstacleAvoidanceDebugData* m_vod; - CrowdManager m_crowd; + static const int AGENT_MAX_TRAIL = 64; + static const int MAX_AGENTS = 128; + struct AgentTrail + { + float trail[AGENT_MAX_TRAIL*3]; + int htrail; + }; + AgentTrail m_trails[MAX_AGENTS]; + + dtCrowd m_crowd; ValueHistory m_crowdTotalTime; - ValueHistory m_crowdRvoTime; ValueHistory m_crowdSampleCount; enum ToolMode { TOOLMODE_CREATE, TOOLMODE_MOVE_TARGET, + TOOLMODE_SELECT, }; ToolMode m_mode; + void updateTick(const float dt); + public: CrowdTool(); ~CrowdTool(); diff --git a/RecastDemo/Source/ConvexVolumeTool.cpp b/RecastDemo/Source/ConvexVolumeTool.cpp index 00c9e09..a401384 100644 --- a/RecastDemo/Source/ConvexVolumeTool.cpp +++ b/RecastDemo/Source/ConvexVolumeTool.cpp @@ -149,16 +149,6 @@ void ConvexVolumeTool::handleMenu() m_npts = 0; m_nhull = 0; } - - imguiSeparator(); - - imguiValue("Click to create points."); - imguiValue("The shape is convex hull"); - imguiValue("of all the create points."); - imguiValue("Click on highlited point"); - imguiValue("to finish the shape."); - - imguiSeparator(); } void ConvexVolumeTool::handleClick(const float* /*s*/, const float* p, bool shift) @@ -278,6 +268,18 @@ void ConvexVolumeTool::handleRender() dd.end(); } -void ConvexVolumeTool::handleRenderOverlay(double* /*proj*/, double* /*model*/, int* /*view*/) +void ConvexVolumeTool::handleRenderOverlay(double* /*proj*/, double* /*model*/, int* view) { + // Tool help + const int h = view[3]; + if (!m_npts) + { + imguiDrawText(280, h-40, IMGUI_ALIGN_LEFT, "LMB: Create new shape. SHIFT+LMB: Delete existing shape (click inside a shape).", imguiRGBA(255,255,255,192)); + } + else + { + imguiDrawText(280, h-40, IMGUI_ALIGN_LEFT, "Click LMB to add new points. Click on the red point to finish the shape.", imguiRGBA(255,255,255,192)); + imguiDrawText(280, h-60, IMGUI_ALIGN_LEFT, "The shape will be convex hull of all added points.", imguiRGBA(255,255,255,192)); + } + } diff --git a/RecastDemo/Source/CrowdManager.cpp b/RecastDemo/Source/CrowdManager.cpp deleted file mode 100644 index f3eb94c..0000000 --- a/RecastDemo/Source/CrowdManager.cpp +++ /dev/null @@ -1,1593 +0,0 @@ -// -// 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 -#include -#include -#include -#include -#include "DetourNavMesh.h" -#include "DetourNavMeshQuery.h" -#include "DetourObstacleAvoidance.h" -#include "DetourCommon.h" -#include "CrowdManager.h" -#include "SampleInterfaces.h" // For timer -#include "DetourAssert.h" -#include "DetourAlloc.h" - -static const int VO_ADAPTIVE_DIVS = 7; -static const int VO_ADAPTIVE_RINGS = 2; -static const int VO_ADAPTIVE_DEPTH = 5; - -static const int VO_GRID_SIZE = 33; - - -inline int hashPos2(int x, int y, int n) -{ - return ((x*73856093) ^ (y*19349663)) & (n-1); -} - -ProximityGrid::ProximityGrid() : - m_maxItems(0), - m_cellSize(0), - m_pool(0), - m_poolHead(0), - m_poolSize(0), - m_buckets(0), - m_bucketsSize(0) -{ -} - -ProximityGrid::~ProximityGrid() -{ - dtFree(m_buckets); - dtFree(m_pool); -} - -bool ProximityGrid::init(const int maxItems, const float cellSize) -{ - dtAssert(maxItems > 0); - dtAssert(cellSize > 0.0f); - - m_cellSize = cellSize; - m_invCellSize = 1.0f / m_cellSize; - - // Allocate hashs buckets - m_bucketsSize = dtNextPow2(maxItems); - m_buckets = (unsigned short*)dtAlloc(sizeof(unsigned short)*m_bucketsSize, DT_ALLOC_PERM); - if (!m_buckets) - return false; - - // Allocate pool of items. - m_poolSize = maxItems*4; - m_poolHead = 0; - m_pool = (Item*)dtAlloc(sizeof(Item)*m_poolSize, DT_ALLOC_PERM); - if (!m_pool) - return false; - - clear(); - - return true; -} - -void ProximityGrid::clear() -{ - memset(m_buckets, 0xff, sizeof(unsigned short)*m_bucketsSize); - m_poolHead = 0; - m_bounds[0] = 0xffff; - m_bounds[1] = 0xffff; - m_bounds[2] = -0xffff; - m_bounds[3] = -0xffff; -} - -void ProximityGrid::addItem(const unsigned short id, - const float minx, const float miny, - const float maxx, const float maxy) -{ - const int iminx = (int)floorf(minx * m_invCellSize); - const int iminy = (int)floorf(miny * m_invCellSize); - const int imaxx = (int)floorf(maxx * m_invCellSize); - const int imaxy = (int)floorf(maxy * m_invCellSize); - - m_bounds[0] = dtMin(m_bounds[0], iminx); - m_bounds[1] = dtMin(m_bounds[1], iminy); - m_bounds[2] = dtMax(m_bounds[2], imaxx); - m_bounds[3] = dtMax(m_bounds[3], imaxy); - - for (int y = iminy; y <= imaxy; ++y) - { - for (int x = iminx; x <= imaxx; ++x) - { - if (m_poolHead < m_poolSize) - { - const int h = hashPos2(x, y, m_bucketsSize); - const unsigned short idx = (unsigned short)m_poolHead; - m_poolHead++; - Item& item = m_pool[idx]; - item.x = (short)x; - item.y = (short)y; - item.id = id; - item.next = m_buckets[h]; - m_buckets[h] = idx; - } - } - } -} - -int ProximityGrid::queryItems(const float minx, const float miny, - const float maxx, const float maxy, - unsigned short* ids, const int maxIds) const -{ - const int iminx = (int)floorf(minx * m_invCellSize); - const int iminy = (int)floorf(miny * m_invCellSize); - const int imaxx = (int)floorf(maxx * m_invCellSize); - const int imaxy = (int)floorf(maxy * m_invCellSize); - - int n = 0; - - for (int y = iminy; y <= imaxy; ++y) - { - for (int x = iminx; x <= imaxx; ++x) - { - const int h = hashPos2(x, y, m_bucketsSize); - unsigned short idx = m_buckets[h]; - while (idx != 0xffff) - { - Item& item = m_pool[idx]; - if ((int)item.x == x && (int)item.y == y) - { - // Check if the id exists already. - const unsigned short* end = ids + n; - unsigned short* i = ids; - while (i != end && *i != item.id) - ++i; - // Item not found, add it. - if (i == end) - { - if (n >= maxIds) - return n; - ids[n++] = item.id; - } - } - idx = item.next; - } - } - } - - return n; -} - -int ProximityGrid::getItemCountAt(const int x, const int y) const -{ - int n = 0; - - const int h = hashPos2(x, y, m_bucketsSize); - unsigned short idx = m_buckets[h]; - while (idx != 0xffff) - { - Item& item = m_pool[idx]; - if ((int)item.x == x && (int)item.y == y) - n++; - idx = item.next; - } - - return n; -} - -PathQueue::PathQueue() : - m_nextHandle(1), - m_delay(0) -{ - for (int i = 0; i < MAX_QUEUE; ++i) - m_queue[i].ref = PATHQ_INVALID; -} - -PathQueue::~PathQueue() -{ -} - -void PathQueue::update(dtNavMeshQuery* navquery) -{ - // Artificial delay to test the code better, - // update only one request too. - - // TODO: Use sliced pathfinder. - m_delay++; - if ((m_delay % 4) == 0) - { - for (int i = 0; i < MAX_QUEUE; ++i) - { - PathQuery& q = m_queue[i]; - if (q.ref == PATHQ_INVALID) - continue; - navquery->findPath(q.startRef, q.endRef, q.startPos, q.endPos, - q.filter, q.path, &q.npath, PQ_MAX_PATH); - q.ready = true; - break; - } - } - - // Kill forgotten request. - for (int i = 0; i < MAX_QUEUE; ++i) - { - PathQuery& q = m_queue[i]; - if (q.ref != PATHQ_INVALID && q.ready) - { - q.keepalive++; - if (q.keepalive > 2) - q.ref = PATHQ_INVALID; - } - } -} - -PathQueueRef PathQueue::request(dtPolyRef startRef, dtPolyRef endRef, - const float* startPos, const float* endPos, - const dtQueryFilter* filter) -{ - // Find empty slot - int slot = -1; - for (int i = 0; i < MAX_QUEUE; ++i) - { - if (m_queue[i].ref == PATHQ_INVALID) - { - slot = i; - break; - } - } - // Could not find slot. - if (slot == -1) - return PATHQ_INVALID; - - PathQueueRef ref = m_nextHandle++; - if (m_nextHandle == PATHQ_INVALID) m_nextHandle++; - - PathQuery& q = m_queue[slot]; - q.ref = ref; - dtVcopy(q.startPos, startPos); - q.startRef = startRef; - dtVcopy(q.endPos, endPos); - q.endRef = endRef; - q.ready = false; - q.npath = 0; - q.filter = filter; // TODO: This is potentially dangerous! - q.keepalive = 0; - - return ref; -} - -int PathQueue::getRequestState(PathQueueRef ref) -{ - for (int i = 0; i < MAX_QUEUE; ++i) - { - if (m_queue[i].ref == ref) - return m_queue[i].ready ? PATHQ_STATE_READY : PATHQ_STATE_WORKING; - } - - return PATHQ_STATE_INVALID; -} - -int PathQueue::getPathResult(PathQueueRef ref, dtPolyRef* path, const int maxPath) -{ - for (int i = 0; i < MAX_QUEUE; ++i) - { - if (m_queue[i].ref == ref) - { - PathQuery& q = m_queue[i]; - // Allow to reuse the request. - q.ref = PATHQ_INVALID; - int n = 0; - for (int j = 0; j < q.npath && j < maxPath; ++j) - path[n++] = q.path[j]; - return n; - } - } - return 0; -} - - -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 = dtMin(furthestPath+1, npath); - int size = dtMax(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 int fixupCorridorEnd(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 = 0; i < npath; ++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. - const int ppos = furthestPath+1; - const int vpos = furthestVisited+1; - const int count = dtMin(nvisited-vpos, maxPath-ppos); - dtAssert(ppos+count <= maxPath); - if (count) - memcpy(path+ppos, visited+vpos, sizeof(dtPolyRef)*count); - - return ppos+count; -} - -static int mergeCorridor(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 = furthestVisited; - if (req <= 0) - return npath; - - const int orig = furthestPath; - int size = dtMax(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[i]; - - return req+size; -} - -PathCorridor::PathCorridor() : - m_path(0), - m_npath(0), - m_maxPath(0) -{ - -} - -PathCorridor::~PathCorridor() -{ - dtFree(m_path); -} - -bool PathCorridor::init(const int maxPath) -{ - dtAssert(!m_path); - m_path = (dtPolyRef*)dtAlloc(sizeof(dtPolyRef)*maxPath, DT_ALLOC_PERM); - if (!m_path) - return false; - m_npath = 0; - m_maxPath = maxPath; - return true; -} - -void PathCorridor::reset(dtPolyRef ref, const float* pos) -{ - dtAssert(m_path); - dtVcopy(m_pos, pos); - dtVcopy(m_target, pos); - m_path[0] = ref; - m_npath = 1; -} - -int PathCorridor::findCorners(float* cornerVerts, unsigned char* cornerFlags, - dtPolyRef* cornerPolys, const int maxCorners, - dtNavMeshQuery* navquery, const dtQueryFilter* filter) -{ - dtAssert(m_path); - dtAssert(m_npath); - - static const float MIN_TARGET_DIST = 0.01f; - - int ncorners = 0; - navquery->findStraightPath(m_pos, m_target, m_path, m_npath, - cornerVerts, cornerFlags, cornerPolys, &ncorners, maxCorners); - - // Prune points in the beginning of the path which are too close. - while (ncorners) - { - if ((cornerFlags[0] & DT_STRAIGHTPATH_OFFMESH_CONNECTION) || - dtVdist2DSqr(&cornerVerts[0], m_pos) > dtSqr(MIN_TARGET_DIST)) - break; - ncorners--; - if (ncorners) - { - memmove(cornerFlags, cornerFlags+1, sizeof(unsigned char)*ncorners); - memmove(cornerPolys, cornerPolys+1, sizeof(dtPolyRef)*ncorners); - memmove(cornerVerts, cornerVerts+3, sizeof(float)*3*ncorners); - } - } - - // Prune points after an off-mesh connection. - for (int i = 0; i < ncorners; ++i) - { - if (cornerFlags[i] & DT_STRAIGHTPATH_OFFMESH_CONNECTION) - { - ncorners = i+1; - break; - } - } - - return ncorners; -} - -void PathCorridor::optimizePathVisibility(const float* next, const float pathOptimizationRange, - dtNavMeshQuery* navquery, const dtQueryFilter* filter) -{ - dtAssert(m_path); - - // Clamp the ray to max distance. - float goal[3]; - dtVcopy(goal, next); - float dist = dtVdist2D(m_pos, goal); - - // If too close to the goal, do not try to optimize. - if (dist < 0.01f) - return; - - // Overshoot a little. This helps to optimize open fields in tiled meshes. - dist = dtMin(dist+0.01f, pathOptimizationRange); - - // Adjust ray length. - float delta[3]; - dtVsub(delta, goal, m_pos); - dtVmad(goal, m_pos, delta, pathOptimizationRange/dist); - - static const int MAX_RES = 32; - dtPolyRef res[MAX_RES]; - float t, norm[3]; - int nres = 0; - navquery->raycast(m_path[0], m_pos, goal, filter, &t, norm, res, &nres, MAX_RES); - if (nres > 1 && t > 0.99f) - { - m_npath = mergeCorridor(m_path, m_npath, m_maxPath, res, nres); - } -} - -bool PathCorridor::optimizePathTopology(dtNavMeshQuery* navquery, const dtQueryFilter* filter) -{ - dtAssert(m_path); - - if (m_npath < 3) - return false; - - static const int MAX_ITER = 32; - static const int MAX_RES = 32; - - dtPolyRef res[MAX_RES]; - int nres = 0; - navquery->initSlicedFindPath(m_path[0], m_path[m_npath-1], m_pos, m_target, filter); - navquery->updateSlicedFindPath(MAX_ITER); - dtStatus status = navquery->finalizeSlicedFindPathPartial(m_path, m_npath, res, &nres, MAX_RES); - - if (status == DT_SUCCESS && nres > 0) - { - m_npath = mergeCorridor(m_path, m_npath, m_maxPath, res, nres); - return true; - } - - return false; -} - -void PathCorridor::movePosition(const float* npos, dtNavMeshQuery* navquery, const dtQueryFilter* filter) -{ - dtAssert(m_path); - dtAssert(m_npath); - - // Move along navmesh and update new position. - float result[3]; - static const int MAX_VISITED = 16; - dtPolyRef visited[MAX_VISITED]; - int nvisited = 0; - navquery->moveAlongSurface(m_path[0], m_pos, npos, filter, - result, visited, &nvisited, MAX_VISITED); - m_npath = fixupCorridor(m_path, m_npath, m_maxPath, visited, nvisited); - - // Adjust the position to stay on top of the navmesh. - float h = m_pos[1]; - navquery->getPolyHeight(m_path[0], result, &h); - result[1] = h; - dtVcopy(m_pos, result); -} - -void PathCorridor::moveTargetPosition(const float* npos, dtNavMeshQuery* navquery, const dtQueryFilter* filter) -{ - dtAssert(m_path); - dtAssert(m_npath); - - // Move along navmesh and update new position. - float result[3]; - static const int MAX_VISITED = 16; - dtPolyRef visited[MAX_VISITED]; - int nvisited = 0; - navquery->moveAlongSurface(m_path[m_npath-1], m_target, npos, filter, - result, visited, &nvisited, MAX_VISITED); - m_npath = fixupCorridorEnd(m_path, m_npath, m_maxPath, visited, nvisited); - - // TODO: should we do that? - // Adjust the position to stay on top of the navmesh. -/* float h = m_target[1]; - navquery->getPolyHeight(m_path[m_npath-1], result, &h); - result[1] = h;*/ - - dtVcopy(m_target, result); -} - -void PathCorridor::setCorridor(const float* target, const dtPolyRef* path, const int npath) -{ - dtAssert(m_path); - dtAssert(npath > 0); - dtAssert(npath < m_maxPath); - - dtVcopy(m_target, target); - memcpy(m_path, path, sizeof(dtPolyRef)*npath); - m_npath = npath; -} - - - - -void Agent::integrate(const float maxAcc, const float dt) -{ - // Fake dynamic constraint. - const float maxDelta = maxAcc * dt; - float dv[3]; - dtVsub(dv, nvel, vel); - float ds = dtVlen(dv); - if (ds > maxDelta) - dtVscale(dv, dv, maxDelta/ds); - dtVadd(vel, vel, dv); - - // Integrate - if (dtVlen(vel) > 0.0001f) - dtVmad(npos, npos, vel, dt); - else - dtVset(vel,0,0,0); -} - -float Agent::getDistanceToGoal(const float range) const -{ - if (!ncorners) - return range; - - const bool endOfPath = (cornerFlags[ncorners-1] & DT_STRAIGHTPATH_END) ? true : false; - const bool offMeshConnection = (cornerFlags[ncorners-1] & DT_STRAIGHTPATH_OFFMESH_CONNECTION) ? true : false; - if (endOfPath || offMeshConnection) - return dtMin(dtVdist2D(npos, &cornerVerts[(ncorners-1)*3]), range); - - return range; -} - -void Agent::calcSmoothSteerDirection(float* dir) -{ - if (!ncorners) - { - dtVset(dir, 0,0,0); - return; - } - - const int ip0 = 0; - const int ip1 = dtMin(1, ncorners-1); - const float* p0 = &cornerVerts[ip0*3]; - const float* p1 = &cornerVerts[ip1*3]; - - float dir0[3], dir1[3]; - dtVsub(dir0, p0, npos); - dtVsub(dir1, p1, npos); - dir0[1] = 0; - dir1[1] = 0; - - float len0 = dtVlen(dir0); - float len1 = dtVlen(dir1); - if (len1 > 0.001f) - dtVscale(dir1,dir1,1.0f/len1); - - dir[0] = dir0[0] - dir1[0]*len0*0.5f; - dir[1] = 0; - dir[2] = dir0[2] - dir1[2]*len0*0.5f; - - dtVnormalize(dir); -} - -void Agent::calcStraightSteerDirection(float* dir) -{ - if (!ncorners) - { - dtVset(dir, 0,0,0); - return; - } - dtVsub(dir, &cornerVerts[0], npos); - dir[1] = 0; - dtVnormalize(dir); -} - - - -LocalBoundary::LocalBoundary() : - m_nsegs(0) -{ - dtVset(m_center, FLT_MAX,FLT_MAX,FLT_MAX); -} - -LocalBoundary::~LocalBoundary() -{ -} - -void LocalBoundary::reset() -{ - dtVset(m_center, FLT_MAX,FLT_MAX,FLT_MAX); - m_nsegs = 0; -} - -void LocalBoundary::addSegment(const float dist, const float* s) -{ - // Insert neighbour based on the distance. - Segment* seg = 0; - if (!m_nsegs) - { - // First, trivial accept. - seg = &m_segs[0]; - } - else if (dist >= m_segs[m_nsegs-1].d) - { - // Further than the last segment, skip. - if (m_nsegs >= MAX_SEGS) - return; - // Last, trivial accept. - seg = &m_segs[m_nsegs]; - } - else - { - // Insert inbetween. - int i; - for (i = 0; i < m_nsegs; ++i) - if (dist <= m_segs[i].d) - break; - const int tgt = i+1; - const int n = dtMin(m_nsegs-i, MAX_SEGS-tgt); - dtAssert(tgt+n <= MAX_SEGS); - if (n > 0) - memmove(&m_segs[tgt], &m_segs[i], sizeof(Segment)*n); - seg = &m_segs[i]; - } - - seg->d = dist; - memcpy(seg->s, s, sizeof(float)*6); - - if (m_nsegs < MAX_SEGS) - m_nsegs++; -} - -void LocalBoundary::update(dtPolyRef ref, const float* pos, const float collisionQueryRange, - dtNavMeshQuery* navquery, const dtQueryFilter* filter) -{ - static const int MAX_LOCAL_POLYS = 16; - static const int MAX_SEGS_PER_POLY = DT_VERTS_PER_POLYGON*2; - - if (!ref) - { - dtVset(m_center, FLT_MAX,FLT_MAX,FLT_MAX); - m_nsegs = 0; - return; - } - - dtVcopy(m_center, pos); - - // First query non-overlapping polygons. - dtPolyRef locals[MAX_LOCAL_POLYS]; - int nlocals = 0; - navquery->findLocalNeighbourhood(ref, pos, collisionQueryRange, - filter, locals, 0, &nlocals, MAX_LOCAL_POLYS); - - // Secondly, store all polygon edges. - m_nsegs = 0; - float segs[MAX_SEGS_PER_POLY*6]; - int nsegs = 0; - for (int j = 0; j < nlocals; ++j) - { - navquery->getPolyWallSegments(locals[j], filter, segs, &nsegs, MAX_SEGS_PER_POLY); - for (int k = 0; k < nsegs; ++k) - { - const float* s = &segs[k*6]; - // Skip too distant segments. - float tseg; - const float distSqr = dtDistancePtSegSqr2D(pos, s, s+3, tseg); - if (distSqr > dtSqr(collisionQueryRange)) - continue; - addSegment(distSqr, s); - } - } -} - - -CrowdManager::CrowdManager() : - m_obstacleQuery(0), - m_pathResult(0), - m_maxPathResult(0), - m_totalTime(0), - m_rvoTime(0), - m_sampleCount(0), - m_moveRequestCount(0) -{ - dtVset(m_ext, 2,4,2); - - m_obstacleQuery = dtAllocObstacleAvoidanceQuery(); - m_obstacleQuery->init(6, 8); - - m_obstacleQuery->setDesiredVelocityWeight(2.0f); - m_obstacleQuery->setCurrentVelocityWeight(0.75f); - m_obstacleQuery->setPreferredSideWeight(0.75f); - m_obstacleQuery->setCollisionTimeWeight(2.5f); - m_obstacleQuery->setTimeHorizon(2.5f); - m_obstacleQuery->setVelocitySelectionBias(0.4f); - - memset(m_vodebug, 0, sizeof(m_vodebug)); - const int maxAdaptiveSamples = (VO_ADAPTIVE_DIVS*VO_ADAPTIVE_RINGS+1)*VO_ADAPTIVE_DEPTH; - const int maxGridSamples = VO_GRID_SIZE*VO_GRID_SIZE; - const int sampleCount = dtMax(maxAdaptiveSamples, maxGridSamples); - for (int i = 0; i < MAX_AGENTS; ++i) - { - m_vodebug[i] = dtAllocObstacleAvoidanceDebugData(); - m_vodebug[i]->init(sampleCount); - } - - // Allocate temp buffer for merging paths. - m_maxPathResult = 256; - m_pathResult = (dtPolyRef*)dtAlloc(sizeof(dtPolyRef)*m_maxPathResult, DT_ALLOC_PERM); - - // Alloca corridors. - for (int i = 0; i < MAX_AGENTS; ++i) - { - m_agents[i].corridor.init(m_maxPathResult); - } - - // TODO: the radius should be related to the agent radius used to create the navmesh! - m_grid.init(MAX_AGENTS, 1.0f); - - reset(); -} - -CrowdManager::~CrowdManager() -{ - delete [] m_pathResult; - - for (int i = 0; i < MAX_AGENTS; ++i) - dtFreeObstacleAvoidanceDebugData(m_vodebug[i]); - dtFreeObstacleAvoidanceQuery(m_obstacleQuery); -} - -void CrowdManager::reset() -{ - for (int i = 0; i < MAX_AGENTS; ++i) - m_agents[i].active = 0; -} - -const int CrowdManager::getAgentCount() const -{ - return MAX_AGENTS; -} - -const Agent* CrowdManager::getAgent(const int idx) -{ - return &m_agents[idx]; -} - -int CrowdManager::addAgent(const float* pos, const float radius, const float height, dtNavMeshQuery* navquery) -{ - // Find empty slot. - int idx = -1; - for (int i = 0; i < MAX_AGENTS; ++i) - { - if (!m_agents[i].active) - { - idx = i; - break; - } - } - if (idx == -1) - return -1; - - Agent* ag = &m_agents[idx]; - - // Find nearest position on navmesh and place the agent there. - float nearest[3]; - dtPolyRef ref; - navquery->findNearestPoly(pos, m_ext, &m_filter, &ref, nearest); - if (!ref) - { - // Could not find a location on navmesh. - return -1; - } - - ag->corridor.reset(ref, nearest); - ag->boundary.reset(); - - ag->radius = radius; - ag->height = height; - ag->collisionQueryRange = radius * 8; - ag->pathOptimizationRange = radius * 30; - ag->topologyOptTime = 0; - ag->nneis = 0; - - dtVset(ag->dvel, 0,0,0); - dtVset(ag->nvel, 0,0,0); - dtVset(ag->vel, 0,0,0); - dtVcopy(ag->npos, nearest); - - ag->maxspeed = 0; - ag->t = 0; - dtVset(ag->opts, 0,0,0); - dtVset(ag->opte, 0,0,0); - ag->active = 1; - ag->var = (rand() % 10) / 9.0f; - - // Init trail - for (int i = 0; i < AGENT_MAX_TRAIL; ++i) - dtVcopy(&ag->trail[i*3], ag->corridor.getPos()); - ag->htrail = 0; - - return idx; -} - -void CrowdManager::removeAgent(const int idx) -{ - if (idx >= 0 && idx < MAX_AGENTS) - { - m_agents[idx].active = 0; - } -} - -bool CrowdManager::requestMoveTarget(const int idx, dtPolyRef ref, const float* pos) -{ - if (idx < 0 || idx > MAX_AGENTS) - return false; - if (!ref) - return false; - - MoveRequest* req = 0; - // Check if there is existing request and update that instead. - for (int i = 0; i < m_moveRequestCount; ++i) - { - if (m_moveRequests[i].idx == idx) - { - req = &m_moveRequests[i]; - break; - } - } - if (!req) - { - if (m_moveRequestCount >= MAX_AGENTS) - return false; - req = &m_moveRequests[m_moveRequestCount++]; - memset(req, 0, sizeof(MoveRequest)); - } - - // Initialize request. - req->idx = idx; - req->ref = ref; - dtVcopy(req->pos, pos); - req->pathqRef = PATHQ_INVALID; - req->state = MR_TARGET_REQUESTING; - - req->temp[0] = ref; - req->ntemp = 1; - - return true; -} - -bool CrowdManager::adjustMoveTarget(const int idx, dtPolyRef ref, const float* pos) -{ - if (idx < 0 || idx > MAX_AGENTS) - return false; - if (!ref) - return false; - - MoveRequest* req = 0; - // Check if there is existing request and update that instead. - for (int i = 0; i < m_moveRequestCount; ++i) - { - if (m_moveRequests[i].idx == idx) - { - req = &m_moveRequests[i]; - break; - } - } - if (!req) - { - if (m_moveRequestCount >= MAX_AGENTS) - return false; - req = &m_moveRequests[m_moveRequestCount++]; - memset(req, 0, sizeof(MoveRequest)); - - // New adjust request - req->state = MR_TARGET_ADJUST; - req->idx = idx; - } - - // Set adjustment request. - req->aref = ref; - dtVcopy(req->apos, pos); - - return true; -} - -int CrowdManager::getActiveAgents(Agent** agents, const int maxAgents) -{ - int n = 0; - for (int i = 0; i < MAX_AGENTS; ++i) - { - if (!m_agents[i].active) continue; - if (n < maxAgents) - agents[n++] = &m_agents[i]; - } - return n; -} - - -static int addNeighbour(const int idx, const float dist, - Neighbour* neis, const int nneis, const int maxNeis) -{ - // Insert neighbour based on the distance. - Neighbour* nei = 0; - if (!nneis) - { - nei = &neis[nneis]; - } - else if (dist >= neis[nneis-1].dist) - { - if (nneis >= maxNeis) - return nneis; - nei = &neis[nneis]; - } - else - { - int i; - for (i = 0; i < nneis; ++i) - if (dist <= neis[i].dist) - break; - - const int tgt = i+1; - const int n = dtMin(nneis-i, maxNeis-tgt); - - dtAssert(tgt+n <= maxNeis); - - if (n > 0) - memmove(&neis[tgt], &neis[i], sizeof(Neighbour)*n); - nei = &neis[i]; - } - - memset(nei, 0, sizeof(Neighbour)); - - nei->idx = idx; - nei->dist = dist; - - return dtMin(nneis+1, maxNeis); -} - -int CrowdManager::getNeighbours(const float* pos, const float height, const float range, - const Agent* skip, Neighbour* result, const int maxResult) -{ - int n = 0; - - unsigned short ids[MAX_AGENTS]; - int nids = m_grid.queryItems(pos[0]-range, pos[2]-range, - pos[0]+range, pos[2]+range, - ids, MAX_AGENTS); - - for (int i = 0; i < nids; ++i) - { - Agent* ag = &m_agents[ids[i]]; - - if (ag == skip) continue; - - // Check for overlap. - float diff[3]; - dtVsub(diff, pos, ag->npos); - if (fabsf(diff[1]) >= (height+ag->height)/2.0f) - continue; - diff[1] = 0; - const float distSqr = dtVlenSqr(diff); - if (distSqr > dtSqr(range)) - continue; - - n = addNeighbour(ids[i], distSqr, result, n, maxResult); - } - return n; -} - -void CrowdManager::updateMoveRequest(const float dt, dtNavMeshQuery* navquery, const dtQueryFilter* filter) -{ - // Fire off new requests. - for (int i = 0; i < m_moveRequestCount; ++i) - { - MoveRequest* req = &m_moveRequests[i]; - Agent* ag = &m_agents[req->idx]; - - // Agent not active anymore, kill request. - if (!ag->active) - req->state = MR_TARGET_FAILED; - - // Adjust target - if (req->aref) - { - if (req->state == MR_TARGET_ADJUST) - { - // Adjust existing path. - ag->corridor.moveTargetPosition(req->apos, navquery, filter); - req->state = MR_TARGET_VALID; - } - else - { - // Adjust on the flight request. - float result[3]; - static const int MAX_VISITED = 16; - dtPolyRef visited[MAX_VISITED]; - int nvisited = 0; - navquery->moveAlongSurface(req->temp[req->ntemp-1], req->pos, req->apos, filter, - result, visited, &nvisited, MAX_VISITED); - req->ntemp = fixupCorridorEnd(req->temp, req->ntemp, MAX_TEMP_PATH, visited, nvisited); - dtVcopy(req->pos, result); - - // Reset adjustment. - dtVset(req->apos, 0,0,0); - req->aref = 0; - } - } - - - if (req->state == MR_TARGET_REQUESTING) - { - // Calculate request position. - // If there is a lot of latency between requests, it is possible to - // project the current position ahead and use raycast to find the actual - // location and path. - const dtPolyRef* path = ag->corridor.getPath(); - const int npath = ag->corridor.getPathCount(); - dtAssert(npath); - - // Here we take the simple approach and set the path to be just the current location. - float reqPos[3]; - dtVcopy(reqPos, ag->corridor.getPos()); // The location of the request - dtPolyRef reqPath[8]; // The path to the request location - reqPath[0] = path[0]; - int reqPathCount = 1; - - req->pathqRef = m_pathq.request(reqPath[reqPathCount-1], req->ref, reqPos, req->pos, &m_filter); - if (req->pathqRef != PATHQ_INVALID) - { - ag->corridor.setCorridor(reqPos, reqPath, reqPathCount); - req->state = MR_TARGET_WAITING_FOR_PATH; - } - } - } - - // Update requests. - m_pathq.update(navquery); - - - // Process path results. - for (int i = 0; i < m_moveRequestCount; ++i) - { - MoveRequest* req = &m_moveRequests[i]; - Agent* ag = &m_agents[req->idx]; - - if (req->state == MR_TARGET_WAITING_FOR_PATH) - { - // Poll path queue. - int state = m_pathq.getRequestState(req->pathqRef); - if (state == PATHQ_STATE_INVALID) - { - req->pathqRef = PATHQ_INVALID; - req->state = MR_TARGET_FAILED; - } - else if (state == PATHQ_STATE_READY) - { - const dtPolyRef* path = ag->corridor.getPath(); - const int npath = ag->corridor.getPathCount(); - dtAssert(npath); - - // Apply results. - float targetPos[3]; - dtVcopy(targetPos, req->pos); - - dtPolyRef* res = m_pathResult; - bool valid = true; - int nres = m_pathq.getPathResult(req->pathqRef, res, m_maxPathResult); - if (!nres) - valid = false; - - // Merge with any target adjustment that happened during the search. - if (req->ntemp > 1) - { - nres = fixupCorridorEnd(res, nres, m_maxPathResult, req->temp, req->ntemp); - } - - // Merge result and existing path. - // The agent might have moved whilst the request is - // being processed, so the path may have changed. - // We assume that the end of the path is at the same location - // where the request was issued. - - // The last ref in the old path should be the same as - // the location where the request was issued.. - if (valid && path[npath-1] != res[0]) - valid = false; - - if (valid) - { - // Put the old path infront of the old path. - if (npath > 1) - { - // Make space for the old path. - if ((npath-1)+nres > m_maxPathResult) - nres = m_maxPathResult - (npath-1); - memmove(res+npath-1, res, sizeof(dtPolyRef)*nres); - // Copy old path in the beginning. - memcpy(res, path, sizeof(dtPolyRef)*(npath-1)); - nres += npath-1; - } - - // Check for partial path. - if (res[nres-1] != req->ref) - { - // Partial path, constrain target position inside the last polygon. - float nearest[3]; - if (navquery->closestPointOnPoly(res[nres-1], targetPos, nearest) == DT_SUCCESS) - dtVcopy(targetPos, nearest); - else - valid = false; - } - } - - if (valid) - { - ag->corridor.setCorridor(targetPos, res, nres); - req->state = MR_TARGET_VALID; - } - else - { - // Something went wrong. - req->state = MR_TARGET_FAILED; - } - } - } - - // Remove request when done with it. - if (req->state == MR_TARGET_VALID || req->state == MR_TARGET_FAILED) - { - m_moveRequestCount--; - if (i != m_moveRequestCount) - memcpy(&m_moveRequests[i], &m_moveRequests[m_moveRequestCount], sizeof(MoveRequest)); - --i; - } - } - -} - - - -static int addToOptQueue(Agent* newag, Agent** agents, const int nagents, const int maxAgents) -{ - // Insert neighbour based on greatest time. - int slot = 0; - if (!nagents) - { - slot = nagents; - } - else if (newag->topologyOptTime <= agents[nagents-1]->topologyOptTime) - { - if (nagents >= maxAgents) - return nagents; - slot = nagents; - } - else - { - int i; - for (i = 0; i < nagents; ++i) - if (newag->topologyOptTime >= agents[i]->topologyOptTime) - break; - - const int tgt = i+1; - const int n = dtMin(nagents-i, maxAgents-tgt); - - dtAssert(tgt+n <= maxAgents); - - if (n > 0) - memmove(&agents[tgt], &agents[i], sizeof(Agent*)*n); - slot = i; - } - - agents[slot] = newag; - - return dtMin(nagents+1, maxAgents); -} - -void CrowdManager::updateTopologyOptimization(const float dt, dtNavMeshQuery* navquery, const dtQueryFilter* filter) -{ - Agent* agents[MAX_AGENTS]; - int nagents = getActiveAgents(agents, MAX_AGENTS); - if (!nagents) - return; - - const float OPT_TIME_THR = 0.5f; // seconds - const int OPT_MAX_AGENTS = 1; - - Agent* queue[OPT_MAX_AGENTS]; - int nqueue = 0; - - for (int i = 0; i < nagents; ++i) - { - Agent* ag = agents[i]; - ag->topologyOptTime += dt; - if (ag->topologyOptTime >= OPT_TIME_THR) - { - nqueue = addToOptQueue(ag, queue, nqueue, OPT_MAX_AGENTS); - } - } - - for (int i = 0; i < nqueue; ++i) - { - Agent* ag = queue[i]; - ag->corridor.optimizePathTopology(navquery, filter); - ag->topologyOptTime = 0; - } - -} - -void CrowdManager::update(const float dt, unsigned int flags, dtNavMeshQuery* navquery) -{ - m_sampleCount = 0; - m_totalTime = 0; - m_rvoTime = 0; - - if (!navquery) - return; - - TimeVal startTime = getPerfTime(); - - Agent* agents[MAX_AGENTS]; - int nagents = getActiveAgents(agents, MAX_AGENTS); - - static const float MAX_ACC = 8.0f; - static const float MAX_SPEED = 3.5f; - - // Update async move request and path finder. - updateMoveRequest(dt, navquery, &m_filter); - - // Optimize path topology. - if (flags & CROWDMAN_OPTIMIZE_TOPO) - updateTopologyOptimization(dt, navquery, &m_filter); - - // Register agents to proximity grid. - m_grid.clear(); - for (int i = 0; i < nagents; ++i) - { - Agent* ag = agents[i]; - const float* p = ag->npos; - const float r = ag->radius; - m_grid.addItem((unsigned short)i, p[0]-r, p[2]-r, p[0]+r, p[2]+r); - } - - // Get nearby navmesh segments and agents to collide with. - for (int i = 0; i < nagents; ++i) - { - Agent* ag = agents[i]; - // Only update the collision boundary after certain distance has been passed. - if (dtVdist2DSqr(ag->npos, ag->boundary.getCenter()) > dtSqr(ag->collisionQueryRange*0.25f)) - ag->boundary.update(ag->corridor.getFirstPoly(), ag->npos, ag->collisionQueryRange, navquery, &m_filter); - // Query neighbour agents - ag->nneis = getNeighbours(ag->npos, ag->height, ag->collisionQueryRange, ag, ag->neis, AGENT_MAX_NEIGHBOURS); - } - - // Find next corner to steer to. - for (int i = 0; i < nagents; ++i) - { - Agent* ag = agents[i]; - - // Find corners for steering - ag->ncorners = ag->corridor.findCorners(ag->cornerVerts, ag->cornerFlags, ag->cornerPolys, - AGENT_MAX_CORNERS, navquery, &m_filter); - - // Check to see if the corner after the next corner is directly visible, - // and short cut to there. - if ((flags & CROWDMAN_OPTIMIZE_VIS) && ag->ncorners > 0) - { - const float* target = &ag->cornerVerts[dtMin(1,ag->ncorners-1)*3]; - dtVcopy(ag->opts, ag->corridor.getPos()); - dtVcopy(ag->opte, target); - ag->corridor.optimizePathVisibility(target, ag->pathOptimizationRange, navquery, &m_filter); - } - else - { - dtVset(ag->opts, 0,0,0); - dtVset(ag->opte, 0,0,0); - } - - // Copy data for debug purposes. - } - - // Calculate steering. - for (int i = 0; i < nagents; ++i) - { - Agent* ag = agents[i]; - - float dvel[3] = {0,0,0}; - - // Calculate steering direction. - if (flags & CROWDMAN_ANTICIPATE_TURNS) - ag->calcSmoothSteerDirection(dvel); - else - ag->calcStraightSteerDirection(dvel); - - // Calculate speed scale, which tells the agent to slowdown at the end of the path. - const float slowDownRadius = ag->radius*2; // TODO: make less hacky. - const float speedScale = ag->getDistanceToGoal(slowDownRadius) / slowDownRadius; - - // Apply style. - if (flags & CROWDMAN_DRUNK) - { - // Drunken steering - - // Pulsating speed. - ag->t += dt * (1.0f - ag->var*0.25f); - ag->maxspeed = MAX_SPEED*(1 + dtSqr(cosf(ag->t*2.0f))*0.3f); - - dtVscale(dvel, dvel, ag->maxspeed * speedScale); - - // Slightly wandering steering. - const float amp = cosf(ag->var*13.69f+ag->t*3.123f) * 0.2f; - const float nx = -dvel[2]; - const float nz = dvel[0]; - dvel[0] += nx*amp; - dvel[2] += nz*amp; - } - else - { - // Normal steering. - ag->maxspeed = MAX_SPEED; - dtVscale(dvel, dvel, ag->maxspeed * speedScale); - } - - // Set the desired velocity. - dtVcopy(ag->dvel, dvel); - } - - // Velocity planning. - TimeVal rvoStartTime = getPerfTime(); - - for (int i = 0; i < nagents; ++i) - { - Agent* ag = agents[i]; - - if (flags & CROWDMAN_USE_VO) - { - m_obstacleQuery->reset(); - - // Add neighbours as obstacles. - for (int j = 0; j < ag->nneis; ++j) - { - const Agent* nei = &m_agents[ag->neis[j].idx]; - m_obstacleQuery->addCircle(nei->npos, nei->radius, nei->vel, nei->dvel); - } - - // Append neighbour segments as obstacles. - for (int j = 0; j < ag->boundary.getSegmentCount(); ++j) - { - const float* s = ag->boundary.getSegment(j); - if (dtTriArea2D(ag->npos, s, s+3) < 0.0f) - continue; - m_obstacleQuery->addSegment(s, s+3); - } - - // Sample new safe velocity. - bool adaptive = true; - - if (adaptive) - { - m_obstacleQuery->sampleVelocityAdaptive(ag->npos, ag->radius, ag->maxspeed, - ag->vel, ag->dvel, ag->nvel, - VO_ADAPTIVE_DIVS, VO_ADAPTIVE_RINGS, VO_ADAPTIVE_DEPTH, - m_vodebug[i]); - } - else - { - m_obstacleQuery->sampleVelocityGrid(ag->npos, ag->radius, ag->maxspeed, - ag->vel, ag->dvel, ag->nvel, - VO_GRID_SIZE, m_vodebug[i]); - } - } - else - { - // If not using velocity planning, new velocity is directly the desired velocity. - dtVcopy(ag->nvel, ag->dvel); - } - } - TimeVal rvoEndTime = getPerfTime(); - - // Integrate. - for (int i = 0; i < nagents; ++i) - { - Agent* ag = agents[i]; - ag->integrate(MAX_ACC, dt); - } - - // Handle collisions. - for (int iter = 0; iter < 4; ++iter) - { - for (int i = 0; i < nagents; ++i) - { - Agent* ag = agents[i]; - - dtVset(ag->disp, 0,0,0); - - float w = 0; - - for (int j = 0; j < ag->nneis; ++j) - { - const Agent* nei = &m_agents[ag->neis[j].idx]; - - float diff[3]; - dtVsub(diff, ag->npos, nei->npos); - - if (fabsf(diff[1]) >= (ag->height+ nei->height)/2.0f) - continue; - - diff[1] = 0; - - float dist = dtVlenSqr(diff); - if (dist > dtSqr(ag->radius + nei->radius)) - continue; - dist = sqrtf(dist); - float pen = (ag->radius + nei->radius) - dist; - if (dist > 0.0001f) - pen = (1.0f/dist) * (pen*0.5f) * 0.7f; - - dtVmad(ag->disp, ag->disp, diff, pen); - - w += 1.0f; - } - - if (w > 0.0001f) - { - const float iw = 1.0f / w; - dtVscale(ag->disp, ag->disp, iw); - } - } - - for (int i = 0; i < nagents; ++i) - { - Agent* ag = agents[i]; - dtVadd(ag->npos, ag->npos, ag->disp); - } - } - - for (int i = 0; i < nagents; ++i) - { - Agent* ag = agents[i]; - // Move along navmesh. - ag->corridor.movePosition(ag->npos, navquery, &m_filter); - // Get valid constrained position back. - dtVcopy(ag->npos, ag->corridor.getPos()); - } - - TimeVal endTime = getPerfTime(); - - // Debug/demo book keeping - int ns = 0; - for (int i = 0; i < nagents; ++i) - { - Agent* ag = agents[i]; - - if (flags & CROWDMAN_USE_VO) - { - // Normalize samples for debug draw - m_vodebug[i]->normalizeSamples(); - ns += m_vodebug[i]->getSampleCount(); - } - - // Update agent movement trail. - ag->htrail = (ag->htrail + 1) % AGENT_MAX_TRAIL; - dtVcopy(&ag->trail[ag->htrail*3], ag->npos); - } - - m_sampleCount = ns; - m_rvoTime = getPerfDeltaTimeUsec(rvoStartTime, rvoEndTime); - m_totalTime = getPerfDeltaTimeUsec(startTime, endTime); -} - - diff --git a/RecastDemo/Source/CrowdTool.cpp b/RecastDemo/Source/CrowdTool.cpp index 9a739d6..63b1bd5 100644 --- a/RecastDemo/Source/CrowdTool.cpp +++ b/RecastDemo/Source/CrowdTool.cpp @@ -27,11 +27,11 @@ #include "CrowdTool.h" #include "InputGeom.h" #include "Sample.h" +#include "DetourCrowd.h" #include "DetourDebugDraw.h" #include "DetourObstacleAvoidance.h" #include "DetourCommon.h" #include "SampleInterfaces.h" -#include "CrowdManager.h" #ifdef WIN32 # define snprintf _snprintf @@ -77,7 +77,7 @@ static bool isectSegAABB(const float* sp, const float* sq, return true; } -static void getAgentBounds(const Agent* ag, float* bmin, float* bmax) +static void getAgentBounds(const dtCrowdAgent* ag, float* bmin, float* bmax) { const float* p = ag->npos; const float r = ag->radius; @@ -94,26 +94,34 @@ CrowdTool::CrowdTool() : m_sample(0), m_oldFlags(0), m_targetRef(0), - m_expandDebugDraw(false), - m_showLabels(false), + m_expandSelectedDebugDraw(true), m_showCorners(false), - m_showTargets(false), m_showCollisionSegments(false), m_showPath(false), m_showVO(false), m_showOpt(false), + m_expandDebugDraw(false), + m_showLabels(false), m_showGrid(false), m_showNodes(false), m_showPerfGraph(false), + m_expandOptions(true), m_anticipateTurns(true), m_optimizeVis(true), m_optimizeTopo(true), m_useVO(true), - m_drunkMove(false), m_run(true), m_mode(TOOLMODE_CREATE) { + memset(m_trails, 0, sizeof(m_trails)); + + m_vod = dtAllocObstacleAvoidanceDebugData(); + m_vod->init(2048); + + memset(&m_agentDebug, 0, sizeof(m_agentDebug)); + m_agentDebug.idx = -1; + m_agentDebug.vod = m_vod; } CrowdTool::~CrowdTool() @@ -122,6 +130,8 @@ CrowdTool::~CrowdTool() { m_sample->setNavMeshDrawFlags(m_oldFlags); } + + dtFreeObstacleAvoidanceDebugData(m_vod); } void CrowdTool::init(Sample* sample) @@ -132,6 +142,11 @@ void CrowdTool::init(Sample* sample) m_oldFlags = m_sample->getNavMeshDrawFlags(); m_sample->setNavMeshDrawFlags(m_oldFlags & ~DU_DRAWNAVMESH_CLOSEDLIST); } + + dtNavMesh* nav = m_sample->getNavMesh(); + if (nav) + m_crowd.init(MAX_AGENTS, m_sample->getAgentRadius(), nav); + } void CrowdTool::reset() @@ -146,26 +161,11 @@ void CrowdTool::handleMenu() m_mode = TOOLMODE_CREATE; if (imguiCheck("Move Target", m_mode == TOOLMODE_MOVE_TARGET)) m_mode = TOOLMODE_MOVE_TARGET; + if (imguiCheck("Select Agent", m_mode == TOOLMODE_SELECT)) + m_mode = TOOLMODE_SELECT; - imguiSeparator(); - - if (m_mode == TOOLMODE_CREATE) - { - imguiValue("Click to add agents."); - imguiValue("Shift+Click to remove."); - } - else if (m_mode == TOOLMODE_MOVE_TARGET) - { - imguiValue("Click to set move target."); - imguiValue("Shift+Click to adjust target."); - imguiValue("Adjusting uses special pathfinder"); - imguiValue("which is really fast to change the"); - imguiValue("target in small increments."); - } - - imguiSeparator(); - imguiSeparator(); - + imguiSeparatorLine(); + if (imguiCollapse("Options", 0, m_expandOptions)) m_expandOptions = !m_expandOptions; @@ -180,23 +180,17 @@ void CrowdTool::handleMenu() m_anticipateTurns = !m_anticipateTurns; if (imguiCheck("Use VO", m_useVO)) m_useVO = !m_useVO; - if (imguiCheck("Drunk Move", m_drunkMove)) - m_drunkMove = !m_drunkMove; imguiUnindent(); } - if (imguiCollapse("Debug Draw", 0, m_expandDebugDraw)) - m_expandDebugDraw = !m_expandDebugDraw; + if (imguiCollapse("Selected Debug Draw", 0, m_expandSelectedDebugDraw)) + m_expandSelectedDebugDraw = !m_expandSelectedDebugDraw; - if (m_expandDebugDraw) + if (m_expandSelectedDebugDraw) { imguiIndent(); - if (imguiCheck("Show Labels", m_showLabels)) - m_showLabels = !m_showLabels; if (imguiCheck("Show Corners", m_showCorners)) m_showCorners = !m_showCorners; - if (imguiCheck("Show Targets", m_showTargets)) - m_showTargets = !m_showTargets; if (imguiCheck("Show Collision Segs", m_showCollisionSegments)) m_showCollisionSegments = !m_showCollisionSegments; if (imguiCheck("Show Path", m_showPath)) @@ -205,6 +199,17 @@ void CrowdTool::handleMenu() m_showVO = !m_showVO; if (imguiCheck("Show Path Optimization", m_showOpt)) m_showOpt = !m_showOpt; + imguiUnindent(); + } + + if (imguiCollapse("Debug Draw", 0, m_expandDebugDraw)) + m_expandDebugDraw = !m_expandDebugDraw; + + if (m_expandDebugDraw) + { + imguiIndent(); + if (imguiCheck("Show Labels", m_showLabels)) + m_showLabels = !m_showLabels; if (imguiCheck("Show Prox Grid", m_showGrid)) m_showGrid = !m_showGrid; if (imguiCheck("Show Nodes", m_showNodes)) @@ -231,7 +236,7 @@ void CrowdTool::handleClick(const float* s, const float* p, bool shift) for (int i = 0; i < m_crowd.getAgentCount(); ++i) { - const Agent* ag = m_crowd.getAgent(i); + const dtCrowdAgent* ag = m_crowd.getAgent(i); if (!ag->active) continue; float bmin[3], bmax[3]; getAgentBounds(ag, bmin, bmax); @@ -253,10 +258,26 @@ void CrowdTool::handleClick(const float* s, const float* p, bool shift) else { // Add - dtNavMeshQuery* navquery = m_sample->getNavMeshQuery(); - int idx = m_crowd.addAgent(p, m_sample->getAgentRadius(), m_sample->getAgentHeight(), navquery); - if (idx != -1 && m_targetRef) - m_crowd.requestMoveTarget(idx, m_targetRef, m_targetPos); + dtCrowdAgentParams ap; + ap.radius = m_sample->getAgentRadius(); + ap.height = m_sample->getAgentHeight(); + ap.maxAcceleration = 8.0f; + ap.maxSpeed = 3.5f; + ap.collisionQueryRange = ap.radius * 8.0f; + ap.pathOptimizationRange = ap.radius * 30.0f; + + int idx = m_crowd.addAgent(p, &ap); + if (idx != -1) + { + if (m_targetRef) + m_crowd.requestMoveTarget(idx, m_targetRef, m_targetPos); + + // Init trail + AgentTrail* trail = &m_trails[idx]; + for (int i = 0; i < AGENT_MAX_TRAIL; ++i) + dtVcopy(&trail->trail[i*3], p); + trail->htrail = 0; + } } } else if (m_mode == TOOLMODE_MOVE_TARGET) @@ -273,7 +294,7 @@ void CrowdTool::handleClick(const float* s, const float* p, bool shift) // Adjust target using tiny local search. for (int i = 0; i < m_crowd.getAgentCount(); ++i) { - const Agent* ag = m_crowd.getAgent(i); + const dtCrowdAgent* ag = m_crowd.getAgent(i); if (!ag->active) continue; m_crowd.adjustMoveTarget(i, m_targetRef, m_targetPos); } @@ -283,16 +304,83 @@ void CrowdTool::handleClick(const float* s, const float* p, bool shift) // Move target using paht finder for (int i = 0; i < m_crowd.getAgentCount(); ++i) { - const Agent* ag = m_crowd.getAgent(i); + const dtCrowdAgent* ag = m_crowd.getAgent(i); if (!ag->active) continue; m_crowd.requestMoveTarget(i, m_targetRef, m_targetPos); } } } + else if (m_mode == TOOLMODE_SELECT) + { + // Highlight + m_agentDebug.idx = -1; + + float tsel = FLT_MAX; + for (int i = 0; i < m_crowd.getAgentCount(); ++i) + { + const dtCrowdAgent* ag = m_crowd.getAgent(i); + if (!ag->active) continue; + float bmin[3], bmax[3]; + getAgentBounds(ag, bmin, bmax); + float tmin, tmax; + if (isectSegAABB(s, p, bmin,bmax, tmin, tmax)) + { + if (tmin > 0 && tmin < tsel) + { + m_agentDebug.idx = i; + tsel = tmin; + } + } + } + } +} + +void CrowdTool::updateTick(const float dt) +{ + dtNavMesh* nav = m_sample->getNavMesh(); + if (!nav) + return; + + unsigned int flags = 0; + + if (m_anticipateTurns) + flags |= DT_CROWD_ANTICIPATE_TURNS; + if (m_useVO) + flags |= DT_CROWD_USE_VO; + if (m_optimizeVis) + flags |= DT_CROWD_OPTIMIZE_VIS; + if (m_optimizeTopo) + flags |= DT_CROWD_OPTIMIZE_TOPO; + + TimeVal startTime = getPerfTime(); + + m_crowd.update(dt, flags, &m_agentDebug); + + TimeVal endTime = getPerfTime(); + + // Update agent trails + for (int i = 0; i < m_crowd.getAgentCount(); ++i) + { + const dtCrowdAgent* ag = m_crowd.getAgent(i); + AgentTrail* trail = &m_trails[i]; + if (!ag->active) + continue; + // Update agent movement trail. + trail->htrail = (trail->htrail + 1) % AGENT_MAX_TRAIL; + dtVcopy(&trail->trail[trail->htrail*3], ag->npos); + } + + m_agentDebug.vod->normalizeSamples(); + + m_crowdSampleCount.addSample((float)m_crowd.getVelocitySampleCount()); + m_crowdTotalTime.addSample(getPerfDeltaTimeUsec(startTime, endTime) / 1000.0f); } void CrowdTool::handleStep() { + const float dt = 1.0f/20.0f; + updateTick(dt); + m_run = false; } void CrowdTool::handleToggle() @@ -303,28 +391,8 @@ void CrowdTool::handleToggle() void CrowdTool::handleUpdate(const float dt) { if (!m_sample) return; - if (!m_sample->getNavMesh()) return; if (m_run) - { - unsigned int flags = 0; - - if (m_anticipateTurns) - flags |= CROWDMAN_ANTICIPATE_TURNS; - if (m_useVO) - flags |= CROWDMAN_USE_VO; - if (m_drunkMove) - flags |= CROWDMAN_DRUNK; - if (m_optimizeVis) - flags |= CROWDMAN_OPTIMIZE_VIS; - if (m_optimizeTopo) - flags |= CROWDMAN_OPTIMIZE_TOPO; - - m_crowd.update(dt, flags, m_sample->getNavMeshQuery()); - - m_crowdSampleCount.addSample((float)m_crowd.getSampleCount()); - m_crowdTotalTime.addSample(m_crowd.getTotalTime() / 1000.0f); - m_crowdRvoTime.addSample(m_crowd.getRVOTime() / 1000.0f); - } + updateTick(dt); } void CrowdTool::handleRender() @@ -336,10 +404,9 @@ void CrowdTool::handleRender() if (!nmesh) return; - dtNavMeshQuery* navquery = m_sample->getNavMeshQuery(); - - if (m_showNodes) + if (m_showNodes && m_crowd.getPathQueue()) { + const dtNavMeshQuery* navquery = m_crowd.getPathQueue()->getNavQuery(); if (navquery) duDebugDrawNavMeshNodes(&dd, *navquery); } @@ -349,15 +416,17 @@ void CrowdTool::handleRender() // Draw paths if (m_showPath) { - for (int i = 0; i < m_crowd.getAgentCount(); ++i) + if (m_agentDebug.idx != -1) { - const Agent* ag = m_crowd.getAgent(i); - if (!ag->active) continue; - - const dtPolyRef* path = ag->corridor.getPath(); - const int npath = ag->corridor.getPathCount(); - for (int i = 0; i < npath; ++i) - duDebugDrawNavMeshPoly(&dd, *nmesh, path[i], duRGBA(0,0,0,32)); + const dtCrowdAgent* ag = m_crowd.getAgent(m_agentDebug.idx); + if (ag->active) + { + + const dtPolyRef* path = ag->corridor.getPath(); + const int npath = ag->corridor.getPathCount(); + for (int i = 0; i < npath; ++i) + duDebugDrawNavMeshPoly(&dd, *nmesh, path[i], duRGBA(0,0,0,16)); + } } } @@ -370,7 +439,7 @@ void CrowdTool::handleRender() float gridy = -FLT_MAX; for (int i = 0; i < m_crowd.getAgentCount(); ++i) { - const Agent* ag = m_crowd.getAgent(i); + const dtCrowdAgent* ag = m_crowd.getAgent(i); if (!ag->active) continue; const float* pos = ag->corridor.getPos(); gridy = dtMax(gridy, pos[1]); @@ -378,7 +447,7 @@ void CrowdTool::handleRender() gridy += 1.0f; dd.begin(DU_DRAW_QUADS); - const ProximityGrid* grid = m_crowd.getGrid(); + const dtProximityGrid* grid = m_crowd.getGrid(); const int* bounds = grid->getBounds(); const float cs = grid->getCellSize(); for (int y = bounds[1]; y <= bounds[3]; ++y) @@ -400,9 +469,10 @@ void CrowdTool::handleRender() // Trail for (int i = 0; i < m_crowd.getAgentCount(); ++i) { - const Agent* ag = m_crowd.getAgent(i); + const dtCrowdAgent* ag = m_crowd.getAgent(i); if (!ag->active) continue; - + + const AgentTrail* trail = &m_trails[i]; const float* pos = ag->npos; dd.begin(DU_DRAW_LINES,3.0f); @@ -410,8 +480,8 @@ void CrowdTool::handleRender() dtVcopy(prev, pos); for (int j = 0; j < AGENT_MAX_TRAIL-1; ++j) { - const int idx = (ag->htrail + AGENT_MAX_TRAIL-j) % AGENT_MAX_TRAIL; - const float* v = &ag->trail[idx*3]; + const int idx = (trail->htrail + AGENT_MAX_TRAIL-j) % AGENT_MAX_TRAIL; + const float* v = &trail->trail[idx*3]; float a = 1 - j/(float)AGENT_MAX_TRAIL; dd.vertex(prev[0],prev[1]+0.1f,prev[2], duRGBA(0,0,0,(int)(128*preva))); dd.vertex(v[0],v[1]+0.1f,v[2], duRGBA(0,0,0,(int)(128*a))); @@ -423,86 +493,88 @@ void CrowdTool::handleRender() } // Corners & co - for (int i = 0; i < m_crowd.getAgentCount(); ++i) + if (m_agentDebug.idx != -1) { - const Agent* ag = m_crowd.getAgent(i); - if (!ag->active) continue; - - const float radius = ag->radius; - const float* pos = ag->npos; - - if (m_showCorners) + const dtCrowdAgent* ag = m_crowd.getAgent(m_agentDebug.idx); + if (ag->active) { - if (ag->ncorners) + + const float radius = ag->radius; + const float* pos = ag->npos; + + if (m_showCorners) { - dd.begin(DU_DRAW_LINES, 2.0f); - for (int j = 0; j < ag->ncorners; ++j) + if (ag->ncorners) { - const float* va = j == 0 ? pos : &ag->cornerVerts[(j-1)*3]; - const float* vb = &ag->cornerVerts[j*3]; - dd.vertex(va[0],va[1]+radius,va[2], duRGBA(128,0,0,192)); - dd.vertex(vb[0],vb[1]+radius,vb[2], duRGBA(128,0,0,192)); + dd.begin(DU_DRAW_LINES, 2.0f); + for (int j = 0; j < ag->ncorners; ++j) + { + const float* va = j == 0 ? pos : &ag->cornerVerts[(j-1)*3]; + const float* vb = &ag->cornerVerts[j*3]; + dd.vertex(va[0],va[1]+radius,va[2], duRGBA(128,0,0,192)); + dd.vertex(vb[0],vb[1]+radius,vb[2], duRGBA(128,0,0,192)); + } + dd.end(); + + if (m_anticipateTurns) + { + /* float dvel[3], pos[3]; + calcSmoothSteerDirection(ag->pos, ag->cornerVerts, ag->ncorners, dvel); + pos[0] = ag->pos[0] + dvel[0]; + pos[1] = ag->pos[1] + dvel[1]; + pos[2] = ag->pos[2] + dvel[2]; + + const float off = ag->radius+0.1f; + const float* tgt = &ag->cornerVerts[0]; + const float y = ag->pos[1]+off; + + dd.begin(DU_DRAW_LINES, 2.0f); + + dd.vertex(ag->pos[0],y,ag->pos[2], duRGBA(255,0,0,192)); + dd.vertex(pos[0],y,pos[2], duRGBA(255,0,0,192)); + + dd.vertex(pos[0],y,pos[2], duRGBA(255,0,0,192)); + dd.vertex(tgt[0],y,tgt[2], duRGBA(255,0,0,192)); + + dd.end();*/ + } + } + } + + if (m_showCollisionSegments) + { + const float* center = ag->boundary.getCenter(); + duDebugDrawCross(&dd, center[0],center[1]+radius,center[2], 0.2f, duRGBA(192,0,128,255), 2.0f); + duDebugDrawCircle(&dd, center[0],center[1]+radius,center[2], ag->collisionQueryRange, + duRGBA(192,0,128,128), 2.0f); + + dd.begin(DU_DRAW_LINES, 3.0f); + for (int j = 0; j < ag->boundary.getSegmentCount(); ++j) + { + const float* s = ag->boundary.getSegment(j); + unsigned int col = duRGBA(192,0,128,192); + if (dtTriArea2D(pos, s, s+3) < 0.0f) + col = duDarkenCol(col); + + duAppendArrow(&dd, s[0],s[1]+0.2f,s[2], s[3],s[4]+0.2f,s[5], 0.0f, 0.3f, col); } dd.end(); - - if (m_anticipateTurns) - { - /* float dvel[3], pos[3]; - calcSmoothSteerDirection(ag->pos, ag->cornerVerts, ag->ncorners, dvel); - pos[0] = ag->pos[0] + dvel[0]; - pos[1] = ag->pos[1] + dvel[1]; - pos[2] = ag->pos[2] + dvel[2]; - - const float off = ag->radius+0.1f; - const float* tgt = &ag->cornerVerts[0]; - const float y = ag->pos[1]+off; - - dd.begin(DU_DRAW_LINES, 2.0f); - - dd.vertex(ag->pos[0],y,ag->pos[2], duRGBA(255,0,0,192)); - dd.vertex(pos[0],y,pos[2], duRGBA(255,0,0,192)); - - dd.vertex(pos[0],y,pos[2], duRGBA(255,0,0,192)); - dd.vertex(tgt[0],y,tgt[2], duRGBA(255,0,0,192)); - - dd.end();*/ - } } - } - - if (m_showCollisionSegments) - { - const float* center = ag->boundary.getCenter(); - duDebugDrawCross(&dd, center[0],center[1]+radius,center[2], 0.2f, duRGBA(192,0,128,255), 2.0f); - duDebugDrawCircle(&dd, center[0],center[1]+radius,center[2], ag->collisionQueryRange, - duRGBA(192,0,128,128), 2.0f); - dd.begin(DU_DRAW_LINES, 3.0f); - for (int j = 0; j < ag->boundary.getSegmentCount(); ++j) + if (m_showOpt) { - const float* s = ag->boundary.getSegment(j); - unsigned int col = duRGBA(192,0,128,192); - if (dtTriArea2D(pos, s, s+3) < 0.0f) - col = duDarkenCol(col); - - duAppendArrow(&dd, s[0],s[1]+0.2f,s[2], s[3],s[4]+0.2f,s[5], 0.0f, 0.3f, col); + dd.begin(DU_DRAW_LINES, 2.0f); + dd.vertex(m_agentDebug.optStart[0],m_agentDebug.optStart[1]+0.3f,m_agentDebug.optStart[2], duRGBA(0,128,0,192)); + dd.vertex(m_agentDebug.optEnd[0],m_agentDebug.optEnd[1]+0.3f,m_agentDebug.optEnd[2], duRGBA(0,128,0,192)); + dd.end(); } - dd.end(); - } - - if (m_showOpt) - { - dd.begin(DU_DRAW_LINES, 2.0f); - dd.vertex(ag->opts[0],ag->opts[1]+0.3f,ag->opts[2], duRGBA(0,128,0,192)); - dd.vertex(ag->opte[0],ag->opte[1]+0.3f,ag->opte[2], duRGBA(0,128,0,192)); - dd.end(); } } // Agent cylinders. for (int i = 0; i < m_crowd.getAgentCount(); ++i) { - const Agent* ag = m_crowd.getAgent(i); + const dtCrowdAgent* ag = m_crowd.getAgent(i); if (!ag->active) continue; const float radius = ag->radius; @@ -513,23 +585,26 @@ void CrowdTool::handleRender() for (int i = 0; i < m_crowd.getAgentCount(); ++i) { - const Agent* ag = m_crowd.getAgent(i); + const dtCrowdAgent* ag = m_crowd.getAgent(i); if (!ag->active) continue; const float height = ag->height; const float radius = ag->radius; const float* pos = ag->npos; + unsigned int col = duRGBA(220,220,220,128); + if (m_agentDebug.idx == i) + col = duRGBA(255,192,0,128); + duDebugDrawCylinder(&dd, pos[0]-radius, pos[1]+radius*0.1f, pos[2]-radius, - pos[0]+radius, pos[1]+height, pos[2]+radius, - duRGBA(220,220,220,128)); + pos[0]+radius, pos[1]+height, pos[2]+radius, col); } // Velocity stuff. for (int i = 0; i < m_crowd.getAgentCount(); ++i) { - const Agent* ag = m_crowd.getAgent(i); + const dtCrowdAgent* ag = m_crowd.getAgent(i); if (!ag->active) continue; const float radius = ag->radius; @@ -538,34 +613,11 @@ void CrowdTool::handleRender() const float* vel = ag->vel; const float* dvel = ag->dvel; - duDebugDrawCircle(&dd, pos[0], pos[1]+height, pos[2], radius, duRGBA(220,220,220,192), 2.0f); - - if (m_showVO) - { - // Draw detail about agent sela - const dtObstacleAvoidanceDebugData* debug = m_crowd.getVODebugData(i); - - const float dx = pos[0]; - const float dy = pos[1]+height; - const float dz = pos[2]; - - dd.begin(DU_DRAW_QUADS); - for (int i = 0; i < debug->getSampleCount(); ++i) - { - const float* p = debug->getSampleVelocity(i); - const float sr = debug->getSampleSize(i); - const float pen = debug->getSamplePenalty(i); - const float pen2 = debug->getSamplePreferredSidePenalty(i); - unsigned int col = duLerpCol(duRGBA(255,255,255,220), duRGBA(128,96,0,220), (int)(pen*255)); - col = duLerpCol(col, duRGBA(128,0,0,220), (int)(pen2*128)); - dd.vertex(dx+p[0]-sr, dy, dz+p[2]-sr, col); - dd.vertex(dx+p[0]-sr, dy, dz+p[2]+sr, col); - dd.vertex(dx+p[0]+sr, dy, dz+p[2]+sr, col); - dd.vertex(dx+p[0]+sr, dy, dz+p[2]-sr, col); - } - dd.end(); - - } + unsigned int col = duRGBA(220,220,220,192); + if (m_agentDebug.idx == i) + col = duRGBA(255,192,0,192); + + duDebugDrawCircle(&dd, pos[0], pos[1]+height, pos[2], radius, col, 2.0f); duDebugDrawArrow(&dd, pos[0],pos[1]+height,pos[2], pos[0]+dvel[0],pos[1]+height+dvel[1],pos[2]+dvel[2], @@ -576,19 +628,36 @@ void CrowdTool::handleRender() 0.0f, 0.4f, duRGBA(0,0,0,192), 2.0f); } - // Targets - for (int i = 0; i < m_crowd.getAgentCount(); ++i) + if (m_agentDebug.idx != -1) { - const Agent* ag = m_crowd.getAgent(i); - if (!ag->active) continue; - - const float* pos = ag->npos; - const float* target = ag->corridor.getTarget(); - - if (m_showTargets) + const dtCrowdAgent* ag = m_crowd.getAgent(m_agentDebug.idx); + if (ag->active) { - duDebugDrawArc(&dd, pos[0], pos[1], pos[2], target[0], target[1], target[2], - 0.25f, 0, 0.4f, duRGBA(0,0,0,128), 1.0f); + if (m_showVO) + { + // Draw detail about agent sela + const dtObstacleAvoidanceDebugData* vod = m_agentDebug.vod; + + const float dx = ag->npos[0]; + const float dy = ag->npos[1]+ag->height; + const float dz = ag->npos[2]; + + dd.begin(DU_DRAW_QUADS); + for (int i = 0; i < vod->getSampleCount(); ++i) + { + const float* p = vod->getSampleVelocity(i); + const float sr = vod->getSampleSize(i); + const float pen = vod->getSamplePenalty(i); + const float pen2 = vod->getSamplePreferredSidePenalty(i); + unsigned int col = duLerpCol(duRGBA(255,255,255,220), duRGBA(128,96,0,220), (int)(pen*255)); + col = duLerpCol(col, duRGBA(128,0,0,220), (int)(pen2*128)); + dd.vertex(dx+p[0]-sr, dy, dz+p[2]-sr, col); + dd.vertex(dx+p[0]-sr, dy, dz+p[2]+sr, col); + dd.vertex(dx+p[0]+sr, dy, dz+p[2]+sr, col); + dd.vertex(dx+p[0]+sr, dy, dz+p[2]-sr, col); + } + dd.end(); + } } } @@ -611,7 +680,7 @@ void CrowdTool::handleRenderOverlay(double* proj, double* model, int* view) char label[32]; for (int i = 0; i < m_crowd.getAgentCount(); ++i) { - const Agent* ag = m_crowd.getAgent(i); + const dtCrowdAgent* ag = m_crowd.getAgent(i); if (!ag->active) continue; const float* pos = ag->npos; const float h = ag->height; @@ -632,11 +701,32 @@ void CrowdTool::handleRenderOverlay(double* proj, double* model, int* view) gp.setValueRange(0.0f, 2.0f, 4, "ms"); drawGraphBackground(&gp); - drawGraph(&gp, &m_crowdRvoTime, 0, "RVO Sampling", duRGBA(255,0,128,255)); - drawGraph(&gp, &m_crowdTotalTime, 1, "Total", duRGBA(128,255,0,255)); + drawGraph(&gp, &m_crowdTotalTime, 1, "Total", duRGBA(255,128,0,255)); gp.setRect(300, 10, 500, 50, 8); - gp.setValueRange(0.0f, 2000.0f, 1, "0"); - drawGraph(&gp, &m_crowdSampleCount, 0, "Sample Count", duRGBA(255,255,255,255)); + gp.setValueRange(0.0f, 2000.0f, 1, ""); + drawGraph(&gp, &m_crowdSampleCount, 0, "Sample Count", duRGBA(96,96,96,128)); } + + // Tool help + const int h = view[3]; + int ty = h-40; + + if (m_mode == TOOLMODE_CREATE) + { + imguiDrawText(280, ty, IMGUI_ALIGN_LEFT, "LMB: add agent. Shift+LMB: remove agent.", imguiRGBA(255,255,255,192)); + } + else if (m_mode == TOOLMODE_MOVE_TARGET) + { + imguiDrawText(280, ty, IMGUI_ALIGN_LEFT, "LMB: set move target. Shift+LMB: adjust target.", imguiRGBA(255,255,255,192)); + ty -= 20; + imguiDrawText(280, ty, IMGUI_ALIGN_LEFT, "Adjusting allows to change the target location in short range without pathfinder.", imguiRGBA(255,255,255,192)); + } + else if (m_mode == TOOLMODE_SELECT) + { + imguiDrawText(280, ty, IMGUI_ALIGN_LEFT, "LMB: select agent.", imguiRGBA(255,255,255,192)); + } + ty -= 20; + imguiDrawText(280, ty, IMGUI_ALIGN_LEFT, "SPACE: Run/Pause simulation. 1: Step simulation.", imguiRGBA(255,255,255,192)); + } diff --git a/RecastDemo/Source/NavMeshTesterTool.cpp b/RecastDemo/Source/NavMeshTesterTool.cpp index 95bfac6..4581354 100644 --- a/RecastDemo/Source/NavMeshTesterTool.cpp +++ b/RecastDemo/Source/NavMeshTesterTool.cpp @@ -479,7 +479,7 @@ void NavMeshTesterTool::handleUpdate(const float /*dt*/) { if (dtStatusInProgress(m_pathFindStatus)) { - m_pathFindStatus = m_navQuery->updateSlicedFindPath(1); + m_pathFindStatus = m_navQuery->updateSlicedFindPath(1,0); } if (dtStatusSucceed(m_pathFindStatus)) { @@ -1191,6 +1191,10 @@ void NavMeshTesterTool::handleRenderOverlay(double* proj, double* model, int* vi { imguiDrawText((int)x, (int)(y-25), IMGUI_ALIGN_CENTER, "End", imguiRGBA(0,0,0,220)); } + + // Tool help + const int h = view[3]; + imguiDrawText(280, h-40, IMGUI_ALIGN_LEFT, "LMB+SHIFT: Set start location LMB: Set end location", imguiRGBA(255,255,255,192)); } void NavMeshTesterTool::drawAgent(const float* pos, float r, float h, float c, const unsigned int col) diff --git a/RecastDemo/Source/OffMeshConnectionTool.cpp b/RecastDemo/Source/OffMeshConnectionTool.cpp index ec192a6..d258979 100644 --- a/RecastDemo/Source/OffMeshConnectionTool.cpp +++ b/RecastDemo/Source/OffMeshConnectionTool.cpp @@ -72,15 +72,6 @@ void OffMeshConnectionTool::handleMenu() m_bidir = false; if (imguiCheck("Bidirectional", m_bidir)) m_bidir = true; - - if (!m_hitPosSet) - { - imguiValue("Click to set connection start."); - } - else - { - imguiValue("Click to set connection end."); - } } void OffMeshConnectionTool::handleClick(const float* /*s*/, const float* p, bool shift) @@ -167,4 +158,15 @@ void OffMeshConnectionTool::handleRenderOverlay(double* proj, double* model, int { imguiDrawText((int)x, (int)(y-25), IMGUI_ALIGN_CENTER, "Start", imguiRGBA(0,0,0,220)); } + + // Tool help + const int h = view[3]; + if (!m_hitPosSet) + { + imguiDrawText(280, h-40, IMGUI_ALIGN_LEFT, "LMB: Create new connection. SHIFT+LMB: Delete existing connection, click close to start or end point.", imguiRGBA(255,255,255,192)); + } + else + { + imguiDrawText(280, h-40, IMGUI_ALIGN_LEFT, "LMB: Set connection end point and finish.", imguiRGBA(255,255,255,192)); + } } diff --git a/RecastDemo/Source/Sample_SoloMeshTiled.cpp b/RecastDemo/Source/Sample_SoloMeshTiled.cpp index b6d04a8..773f8b4 100644 --- a/RecastDemo/Source/Sample_SoloMeshTiled.cpp +++ b/RecastDemo/Source/Sample_SoloMeshTiled.cpp @@ -75,7 +75,6 @@ public: virtual void handleMenu() { - imguiValue("Click LMB to highlight a tile."); } virtual void handleClick(const float* /*s*/, const float* p, bool /*shift*/) @@ -127,6 +126,9 @@ public: imguiDrawText((int)x, (int)y-25, IMGUI_ALIGN_CENTER, text, imguiRGBA(0,0,0,220)); } + // Tool help + const int h = view[3]; + imguiDrawText(280, h-40, IMGUI_ALIGN_LEFT, "LMB: Highlight hit.", imguiRGBA(255,255,255,192)); } }; diff --git a/RecastDemo/Source/Sample_TileMesh.cpp b/RecastDemo/Source/Sample_TileMesh.cpp index 9b4ae7e..da7e043 100644 --- a/RecastDemo/Source/Sample_TileMesh.cpp +++ b/RecastDemo/Source/Sample_TileMesh.cpp @@ -108,8 +108,6 @@ public: if (m_sample) m_sample->removeAllTiles(); } - imguiValue("Click LMB to create a tile."); - imguiValue("Shift+LMB to remove a tile."); } virtual void handleClick(const float* /*s*/, const float* p, bool shift) @@ -163,6 +161,9 @@ public: imguiDrawText((int)x, (int)y-25, IMGUI_ALIGN_CENTER, text, imguiRGBA(0,0,0,220)); } + // Tool help + const int h = view[3]; + imguiDrawText(280, h-40, IMGUI_ALIGN_LEFT, "LMB: Rebuild hit tile. Shift+LMB: Clear hit tile.", imguiRGBA(255,255,255,192)); } }; diff --git a/RecastDemo/Source/main.cpp b/RecastDemo/Source/main.cpp index 5f2a074..c2d8206 100644 --- a/RecastDemo/Source/main.cpp +++ b/RecastDemo/Source/main.cpp @@ -132,7 +132,6 @@ int main(int /*argc*/, char** /*argv*/) bool mouseOverMenu = false; bool showMenu = !presentationMode; bool showLog = false; - bool showDebugMode = true; bool showTools = true; bool showLevels = false; bool showSample = false; @@ -141,7 +140,6 @@ int main(int /*argc*/, char** /*argv*/) int propScroll = 0; int logScroll = 0; int toolsScroll = 0; - int debugScroll = 0; char sampleName[64] = "Choose Sample..."; @@ -511,24 +509,19 @@ int main(int /*argc*/, char** /*argv*/) // Help text. if (showMenu) { - const char msg[] = "W/S/A/D: Move RMB: Rotate LMB+SHIFT: Place Start LMB: Place End"; - imguiDrawText(width/2, height-20, IMGUI_ALIGN_CENTER, msg, imguiRGBA(255,255,255,128)); + const char msg[] = "W/S/A/D: Move RMB: Rotate"; + imguiDrawText(280, height-20, IMGUI_ALIGN_LEFT, msg, imguiRGBA(255,255,255,128)); } if (showMenu) { - int propDiv = showDebugMode ? (int)(height*0.6f) : height; - - if (imguiBeginScrollArea("Properties", - width-250-10, 10+height-propDiv, 250, propDiv-20, &propScroll)) + if (imguiBeginScrollArea("Properties", width-250-10, 10, 250, height-20, &propScroll)) mouseOverMenu = true; if (imguiCheck("Show Log", showLog)) showLog = !showLog; if (imguiCheck("Show Tools", showTools)) showTools = !showTools; - if (imguiCheck("Show Debug Mode", showDebugMode)) - showDebugMode = !showDebugMode; imguiSeparator(); imguiLabel("Sample"); @@ -571,9 +564,11 @@ int main(int /*argc*/, char** /*argv*/) imguiValue(text); } imguiSeparator(); - + if (geom && sample) { + imguiSeparatorLine(); + sample->handleSettings(); if (imguiButton("Build")) @@ -594,20 +589,13 @@ int main(int /*argc*/, char** /*argv*/) imguiSeparator(); } - imguiEndScrollArea(); - - if (showDebugMode) + if (sample) { - if (imguiBeginScrollArea("Debug Mode", - width-250-10, 10, - 250, height-propDiv-10, &debugScroll)) - mouseOverMenu = true; - - if (sample) - sample->handleDebugMode(); - - imguiEndScrollArea(); + imguiSeparatorLine(); + sample->handleDebugMode(); } + + imguiEndScrollArea(); } // Sample selection dialog. @@ -873,7 +861,7 @@ int main(int /*argc*/, char** /*argv*/) // Log if (showLog && showMenu) { - if (imguiBeginScrollArea("Log", 10, 10, width - 300, 200, &logScroll)) + if (imguiBeginScrollArea("Log", 250+20, 10, width - 300 - 250, 200, &logScroll)) mouseOverMenu = true; for (int i = 0; i < ctx.getLogCount(); ++i) imguiLabel(ctx.getLogText(i)); @@ -881,12 +869,13 @@ int main(int /*argc*/, char** /*argv*/) } // Tools - if (!showTestCases && showTools && showMenu && geom && sample) + if (!showTestCases && showTools && showMenu) // && geom && sample) { - if (imguiBeginScrollArea("Tools", 10, height - 10 - 350, 250, 350, &toolsScroll)) + if (imguiBeginScrollArea("Tools", 10, 10, 250, height-20, &toolsScroll)) mouseOverMenu = true; - sample->handleTools(); + if (sample) + sample->handleTools(); imguiEndScrollArea(); }