Refactored and cleaned up CrowdManager and moved it to DetourCrowd. Update tool UI layout, context sensitive help is now rendered as overlay.
This commit is contained in:
Mikko Mononen
parent
da00ac2b91
commit
4a81213b3b
@ -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.
|
||||
|
||||
@ -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]; }
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
|
||||
185
DetourCrowd/Include/DetourCrowd.h
Normal file
185
DetourCrowd/Include/DetourCrowd.h
Normal file
@ -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
|
||||
55
DetourCrowd/Include/DetourLocalBoundary.h
Normal file
55
DetourCrowd/Include/DetourLocalBoundary.h
Normal file
@ -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
|
||||
74
DetourCrowd/Include/DetourPathCorridor.h
Normal file
74
DetourCrowd/Include/DetourPathCorridor.h
Normal file
@ -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
|
||||
75
DetourCrowd/Include/DetourPathQueue.h
Normal file
75
DetourCrowd/Include/DetourPathQueue.h
Normal file
@ -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
|
||||
70
DetourCrowd/Include/DetourProximityGrid.h
Normal file
70
DetourCrowd/Include/DetourProximityGrid.h
Normal file
@ -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
|
||||
|
||||
944
DetourCrowd/Source/DetourCrowd.cpp
Normal file
944
DetourCrowd/Source/DetourCrowd.cpp
Normal file
@ -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 <math.h>
|
||||
#include <string.h>
|
||||
#include <float.h>
|
||||
#include <new>
|
||||
#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());
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
121
DetourCrowd/Source/DetourLocalBoundary.cpp
Normal file
121
DetourCrowd/Source/DetourLocalBoundary.cpp
Normal file
@ -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 <float.h>
|
||||
#include <string.h>
|
||||
#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);
|
||||
}
|
||||
}
|
||||
}
|
||||
342
DetourCrowd/Source/DetourPathCorridor.cpp
Normal file
342
DetourCrowd/Source/DetourPathCorridor.cpp
Normal file
@ -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 <string.h>
|
||||
#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;
|
||||
}
|
||||
197
DetourCrowd/Source/DetourPathQueue.cpp
Normal file
197
DetourCrowd/Source/DetourPathQueue.cpp
Normal file
@ -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 <string.h>
|
||||
#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;
|
||||
}
|
||||
194
DetourCrowd/Source/DetourProximityGrid.cpp
Normal file
194
DetourCrowd/Source/DetourProximityGrid.cpp
Normal file
@ -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 <math.h>
|
||||
#include <string.h>
|
||||
#include <new>
|
||||
#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;
|
||||
}
|
||||
Binary file not shown.
@ -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 = "<group>";
|
||||
};
|
||||
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 = "<group>";
|
||||
};
|
||||
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 = "<group>";
|
||||
};
|
||||
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;
|
||||
};
|
||||
|
||||
@ -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
|
||||
@ -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();
|
||||
|
||||
@ -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));
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@ -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));
|
||||
|
||||
}
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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));
|
||||
}
|
||||
}
|
||||
|
||||
@ -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));
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
@ -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));
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
@ -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();
|
||||
}
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user