Added proximity grid for agents.

This commit is contained in:
Mikko Mononen 2010-10-01 08:21:43 +00:00
parent 4cf5ab4d7b
commit c11a3a950e
8 changed files with 1128 additions and 588 deletions

File diff suppressed because it is too large Load Diff

View File

@ -284,13 +284,14 @@
<key>PBXSmartGroupTreeModuleOutlineStateSelectionKey</key>
<array>
<array>
<integer>88</integer>
<integer>53</integer>
<integer>51</integer>
<integer>1</integer>
<integer>0</integer>
</array>
</array>
<key>PBXSmartGroupTreeModuleOutlineStateVisibleRectKey</key>
<string>{{0, 1041}, {264, 607}}</string>
<string>{{0, 799}, {264, 607}}</string>
</dict>
<key>PBXTopSmartGroupGIDs</key>
<array/>
@ -325,7 +326,7 @@
<key>PBXProjectModuleGUID</key>
<string>6B8632A30F78115100E2684A</string>
<key>PBXProjectModuleLabel</key>
<string>CrowdManager.cpp</string>
<string>CrowdTool.cpp</string>
<key>PBXSplitModuleInNavigatorKey</key>
<dict>
<key>Split0</key>
@ -333,11 +334,11 @@
<key>PBXProjectModuleGUID</key>
<string>6B8632A40F78115100E2684A</string>
<key>PBXProjectModuleLabel</key>
<string>CrowdManager.cpp</string>
<string>CrowdTool.cpp</string>
<key>_historyCapacity</key>
<integer>0</integer>
<key>bookmark</key>
<string>6BBB035912473E1800533229</string>
<string>6BA8CF451255D17A00272A3B</string>
<key>history</key>
<array>
<string>6BBB4AA5115B4F3400CF791D</string>
@ -370,7 +371,6 @@
<string>6BD402121224336600995864</string>
<string>6BD402611224387200995864</string>
<string>6BD402621224387200995864</string>
<string>6BD402641224387200995864</string>
<string>6BD402811224393000995864</string>
<string>6BD4028C1224399300995864</string>
<string>6BD4029B12243A8000995864</string>
@ -400,12 +400,10 @@
<string>6B8477FF122D2E2A00ADF63D</string>
<string>6BD667FF123D2D230021A7A4</string>
<string>6BD6681812434B790021A7A4</string>
<string>6BD6683512434D9D0021A7A4</string>
<string>6BD6684E124350B80021A7A4</string>
<string>6BD66855124350F80021A7A4</string>
<string>6BD6685E124351D70021A7A4</string>
<string>6BD6685F124351D70021A7A4</string>
<string>6BD6686C124356CE0021A7A4</string>
<string>6BD6686E124356CE0021A7A4</string>
<string>6BD668A3124361EB0021A7A4</string>
<string>6BD668D6124367E30021A7A4</string>
@ -415,16 +413,23 @@
<string>6BD66934124375DF0021A7A4</string>
<string>6BD66937124376780021A7A4</string>
<string>6BD66938124376780021A7A4</string>
<string>6BD66939124376780021A7A4</string>
<string>6BD669501243778E0021A7A4</string>
<string>6BD669511243778E0021A7A4</string>
<string>6BD6695C124377D40021A7A4</string>
<string>6BD669661243806F0021A7A4</string>
<string>6BD6698912438A3B0021A7A4</string>
<string>6BD66A131243AA4A0021A7A4</string>
<string>6BBB02EA124731A400533229</string>
<string>6BBB035612473DA500533229</string>
<string>6BBB035712473DA500533229</string>
<string>6BBB0361124E242E00533229</string>
<string>6BBB0363124E242E00533229</string>
<string>6BBB0365124E242E00533229</string>
<string>6BBB0366124E242E00533229</string>
<string>6BA8CEC01255BCE600272A3B</string>
<string>6BA8CECB1255C1A400272A3B</string>
<string>6BA8CECD1255C1A400272A3B</string>
<string>6BA8CEEF1255C4B700272A3B</string>
<string>6BA8CF221255CCD500272A3B</string>
<string>6BA8CF411255D16D00272A3B</string>
<string>6BA8CF421255D16D00272A3B</string>
<string>6BA8CF431255D16D00272A3B</string>
</array>
</dict>
<key>SplitCount</key>
@ -438,18 +443,18 @@
<key>GeometryConfiguration</key>
<dict>
<key>Frame</key>
<string>{{0, 0}, {994, 487}}</string>
<string>{{0, 0}, {994, 516}}</string>
<key>RubberWindowFrame</key>
<string>0 112 1280 666 0 0 1280 778 </string>
</dict>
<key>Module</key>
<string>PBXNavigatorGroup</string>
<key>Proportion</key>
<string>487pt</string>
<string>516pt</string>
</dict>
<dict>
<key>Proportion</key>
<string>133pt</string>
<string>104pt</string>
<key>Tabs</key>
<array>
<dict>
@ -463,7 +468,7 @@
<key>GeometryConfiguration</key>
<dict>
<key>Frame</key>
<string>{{10, 27}, {994, 66}}</string>
<string>{{10, 27}, {994, 53}}</string>
</dict>
<key>Module</key>
<string>XCDetailModule</string>
@ -517,7 +522,7 @@
<key>GeometryConfiguration</key>
<dict>
<key>Frame</key>
<string>{{10, 27}, {994, 106}}</string>
<string>{{10, 27}, {994, 77}}</string>
<key>RubberWindowFrame</key>
<string>0 112 1280 666 0 0 1280 778 </string>
</dict>
@ -547,11 +552,11 @@
</array>
<key>TableOfContents</key>
<array>
<string>6BBB02B71247291B00533229</string>
<string>6BA8CEC41255BCE600272A3B</string>
<string>1CA23ED40692098700951B8B</string>
<string>6BBB02B81247291B00533229</string>
<string>6BA8CEC51255BCE600272A3B</string>
<string>6B8632A30F78115100E2684A</string>
<string>6BBB02B91247291B00533229</string>
<string>6BA8CEC61255BCE600272A3B</string>
<string>1CA23EDF0692099D00951B8B</string>
<string>1CA23EE00692099D00951B8B</string>
<string>1CA23EE10692099D00951B8B</string>
@ -699,14 +704,14 @@
</array>
<key>TableOfContents</key>
<array>
<string>6BBB02BA1247291B00533229</string>
<string>6BA8CF151255CC4F00272A3B</string>
<string>1CCC7628064C1048000F2A68</string>
<string>1CCC7629064C1048000F2A68</string>
<string>6BBB02BB1247291B00533229</string>
<string>6BBB02BC1247291B00533229</string>
<string>6BBB02BD1247291B00533229</string>
<string>6BBB02BE1247291B00533229</string>
<string>6BBB02BF1247291B00533229</string>
<string>6BA8CF161255CC4F00272A3B</string>
<string>6BA8CF171255CC4F00272A3B</string>
<string>6BA8CF181255CC4F00272A3B</string>
<string>6BA8CF191255CC4F00272A3B</string>
<string>6BA8CF1A1255CC4F00272A3B</string>
</array>
<key>ToolbarConfigUserDefaultsMinorVersion</key>
<string>2</string>
@ -738,8 +743,8 @@
<integer>5</integer>
<key>WindowOrderList</key>
<array>
<string>6BBB02EF124731A400533229</string>
<string>6BBB02F0124731A400533229</string>
<string>6BA8CF1C1255CC4F00272A3B</string>
<string>6BA8CF1D1255CC4F00272A3B</string>
<string>/Users/memon/Code/recastnavigation/RecastDemo/Build/Xcode/Recast.xcodeproj</string>
</array>
<key>WindowString</key>

View File

@ -178,6 +178,8 @@
6B555DF5100B25FC00247EA3 /* Samples */,
6BB7FE8E10F4A175006DA0A6 /* Tools */,
6B25B6180FFA62BE004F1BC4 /* main.cpp */,
6BD667D8123D27EC0021A7A4 /* CrowdManager.h */,
6BD667D9123D28100021A7A4 /* CrowdManager.cpp */,
6BAF4440121C3D0A008CFCDF /* SampleInterfaces.h */,
6BAF4441121C3D26008CFCDF /* SampleInterfaces.cpp */,
6BF5F2C511747E9F000502A6 /* stb_image.h */,
@ -204,8 +206,6 @@
6BD402001224279400995864 /* PerfTimer.cpp */,
6B847774122D220D00ADF63D /* ValueHistory.h */,
6B847776122D221C00ADF63D /* ValueHistory.cpp */,
6BD667D8123D27EC0021A7A4 /* CrowdManager.h */,
6BD667D9123D28100021A7A4 /* CrowdManager.cpp */,
);
name = Classes;
sourceTree = "<group>";

View File

@ -34,6 +34,50 @@
AGENT_TARGET_FAILED = 5,
};*/
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 int AGENT_MAX_PATH = 256;
static const int AGENT_MAX_CORNERS = 4;
static const int AGENT_MAX_TRAIL = 64;
@ -202,6 +246,7 @@ class CrowdManager
dtObstacleAvoidanceDebugData* m_vodebug[MAX_AGENTS];
dtObstacleAvoidanceQuery* m_obstacleQuery;
PathQueue m_pathq;
ProximityGrid m_grid;
int m_totalTime;
int m_rvoTime;
@ -229,6 +274,8 @@ public:
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; }
};

View File

@ -68,6 +68,7 @@ class CrowdTool : public SampleTool
bool m_showPath;
bool m_showVO;
bool m_showOpt;
bool m_showGrid;
bool m_expandOptions;
bool m_anticipateTurns;

View File

@ -29,12 +29,165 @@
#include "CrowdManager.h"
#include "SampleInterfaces.h" // For timer
#include "DetourAssert.h"
#include "DetourAlloc.h"
static const int VO_ADAPTIVE_GRID_SIZE = 4;
static const int VO_ADAPTIVE_GRID_DEPTH = 5;
static const int VO_GRID_SIZE = 33;
inline int hashPos2(int x, int y, int n)
{
return ((x*73856093) ^ (y*19349663)) & (n-1);
}
ProximityGrid::ProximityGrid() :
m_maxItems(0),
m_cellSize(0),
m_pool(0),
m_poolHead(0),
m_poolSize(0),
m_buckets(0),
m_bucketsSize(0)
{
}
ProximityGrid::~ProximityGrid()
{
dtFree(m_buckets);
dtFree(m_pool);
}
bool ProximityGrid::init(const int maxItems, const float cellSize)
{
dtAssert(maxItems > 0);
dtAssert(cellSize > 0.0f);
m_cellSize = cellSize;
m_invCellSize = 1.0f / m_cellSize;
// Allocate hashs buckets
m_bucketsSize = dtNextPow2(maxItems);
m_buckets = (unsigned short*)dtAlloc(sizeof(unsigned short)*m_bucketsSize, DT_ALLOC_PERM);
if (!m_buckets)
return false;
// Allocate pool of items.
m_poolSize = maxItems*4;
m_poolHead = 0;
m_pool = (Item*)dtAlloc(sizeof(Item)*m_poolSize, DT_ALLOC_PERM);
if (!m_pool)
return false;
clear();
return true;
}
void ProximityGrid::clear()
{
memset(m_buckets, 0xff, sizeof(unsigned short)*m_bucketsSize);
m_poolHead = 0;
m_bounds[0] = 0xffff;
m_bounds[1] = 0xffff;
m_bounds[2] = -0xffff;
m_bounds[3] = -0xffff;
}
void ProximityGrid::addItem(const unsigned short id,
const float minx, const float miny,
const float maxx, const float maxy)
{
const int iminx = (int)floorf(minx * m_invCellSize);
const int iminy = (int)floorf(miny * m_invCellSize);
const int imaxx = (int)floorf(maxx * m_invCellSize);
const int imaxy = (int)floorf(maxy * m_invCellSize);
m_bounds[0] = dtMin(m_bounds[0], iminx);
m_bounds[1] = dtMin(m_bounds[1], iminy);
m_bounds[2] = dtMax(m_bounds[2], imaxx);
m_bounds[3] = dtMax(m_bounds[3], imaxy);
for (int y = iminy; y <= imaxy; ++y)
{
for (int x = iminx; x <= imaxx; ++x)
{
if (m_poolHead < m_poolSize)
{
const int h = hashPos2(x, y, m_bucketsSize);
const unsigned short idx = (unsigned short)m_poolHead;
m_poolHead++;
Item& item = m_pool[idx];
item.x = (short)x;
item.y = (short)y;
item.id = id;
item.next = m_buckets[h];
m_buckets[h] = idx;
}
}
}
}
int ProximityGrid::queryItems(const float minx, const float miny,
const float maxx, const float maxy,
unsigned short* ids, const int maxIds) const
{
const int iminx = (int)floorf(minx * m_invCellSize);
const int iminy = (int)floorf(miny * m_invCellSize);
const int imaxx = (int)floorf(maxx * m_invCellSize);
const int imaxy = (int)floorf(maxy * m_invCellSize);
int n = 0;
for (int y = iminy; y <= imaxy; ++y)
{
for (int x = iminx; x <= imaxx; ++x)
{
const int h = hashPos2(x, y, m_bucketsSize);
unsigned short idx = m_buckets[h];
while (idx != 0xffff)
{
Item& item = m_pool[idx];
if ((int)item.x == x && (int)item.y == y)
{
// Check if the id exists already.
const unsigned short* end = ids + n;
unsigned short* i = ids;
while (i != end && *i != item.id)
++i;
// Item not found, add it.
if (i == end)
{
if (n >= maxIds)
return n;
ids[n++] = item.id;
}
}
idx = item.next;
}
}
}
return n;
}
int ProximityGrid::getItemCountAt(const int x, const int y) const
{
int n = 0;
const int h = hashPos2(x, y, m_bucketsSize);
unsigned short idx = m_buckets[h];
while (idx != 0xffff)
{
Item& item = m_pool[idx];
if ((int)item.x == x && (int)item.y == y)
n++;
idx = item.next;
}
return n;
}
PathQueue::PathQueue() :
m_nextHandle(1),
@ -673,6 +826,9 @@ CrowdManager::CrowdManager() :
m_vodebug[i] = dtAllocObstacleAvoidanceDebugData();
m_vodebug[i]->init(sampleCount);
}
// TODO: the radius should be related to the agent radius used to create the navmesh!
m_grid.init(100, 1.0f);
reset();
}
@ -767,10 +923,14 @@ int CrowdManager::getNeighbours(const float* pos, const float height, const floa
{
int n = 0;
for (int i = 0; i < MAX_AGENTS; ++i)
unsigned short ids[MAX_AGENTS];
int nids = m_grid.queryItems(pos[0]-range, pos[2]-range,
pos[0]+range, pos[2]+range,
ids, MAX_AGENTS);
for (int i = 0; i < nids; ++i)
{
if (!m_agents[i].active) continue;
Agent* ag = &m_agents[i];
Agent* ag = &m_agents[ids[i]];
if (ag == skip) continue;
@ -812,6 +972,21 @@ void CrowdManager::update(const float dt, unsigned int flags, dtNavMeshQuery* na
m_pathq.update(navquery);
m_grid.clear();
for (int i = 0; i < nagents; ++i)
{
Agent* ag = agents[i];
const float* p = ag->mover.m_pos;
const float r = ag->mover.m_radius;
const float minx = p[0] - r;
const float miny = p[2] - r;
const float maxx = p[0] + r;
const float maxy = p[2] + r;
m_grid.addItem((unsigned short)i, minx, miny, maxx, maxy);
}
// Update target and agent navigation state.
// TODO: add queue for path queries.
for (int i = 0; i < nagents; ++i)

View File

@ -182,6 +182,7 @@ CrowdTool::CrowdTool() :
m_showPath(false),
m_showVO(false),
m_showOpt(false),
m_showGrid(false),
m_expandOptions(true),
m_anticipateTurns(true),
m_useVO(true),
@ -262,8 +263,10 @@ void CrowdTool::handleMenu()
m_showPath = !m_showPath;
if (imguiCheck("Show VO", m_showVO))
m_showVO = !m_showVO;
if (imguiCheck("Show Opt", m_showOpt))
if (imguiCheck("Show Path Optimization", m_showOpt))
m_showOpt = !m_showOpt;
if (imguiCheck("Show Prox Grid", m_showGrid))
m_showGrid = !m_showGrid;
imguiUnindent();
}
}
@ -591,6 +594,40 @@ void CrowdTool::handleRender()
dd.depthMask(true);
}
if (m_showGrid)
{
float gridy = -FLT_MAX;
for (int i = 0; i < m_crowd.getAgentCount(); ++i)
{
const Agent* ag = m_crowd.getAgent(i);
if (!ag->active) continue;
const float* pos = ag->mover.m_pos;
gridy = dtMax(gridy, pos[1]);
}
gridy += 1.0f;
dd.depthMask(false);
dd.begin(DU_DRAW_QUADS);
const ProximityGrid* grid = m_crowd.getGrid();
const int* bounds = grid->getBounds();
const float cs = grid->getCellSize();
for (int y = bounds[1]; y <= bounds[3]; ++y)
{
for (int x = bounds[0]; x <= bounds[2]; ++x)
{
const int count = grid->getItemCountAt(x,y);
if (!count) continue;
unsigned int col = duRGBA(128,0,0,dtMin(count*40,255));
dd.vertex(x*cs, gridy, y*cs, col);
dd.vertex(x*cs, gridy, y*cs+cs, col);
dd.vertex(x*cs+cs, gridy, y*cs+cs, col);
dd.vertex(x*cs+cs, gridy, y*cs, col);
}
}
dd.end();
dd.depthMask(true);
}
/*
for (int i = 0; i < m_form.npolys; ++i)