327 lines
7.9 KiB
C++
327 lines
7.9 KiB
C++
//
|
|
// 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 optimizePath(const float* next, const float pathOptimizationRange,
|
|
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;
|
|
|
|
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,
|
|
};
|
|
|
|
|
|
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);
|
|
|
|
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 updateMoveRequest(const float dt, dtNavMeshQuery* navquery, const dtQueryFilter* filter);
|
|
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
|