Scarfski/recastnavigation
1
0
Fork 0
mirror of https://github.com/recastnavigation/recastnavigation.git synced 2026-09-29 05:26:12 +00:00
Code Issues Packages Projects Releases Wiki Activity

Cleaned up and simplified Detour obst. avoidance. Simplified path corridor, spinned off LocalBoundary to manage edge segs.

Browse Source
This commit is contained in:
Mikko Mononen
2010-10-20 17:13:47 +00:00
parent 35df0bfdcb
commit 7f84699bfe
8 changed files with 686 additions and 377 deletions
Download Patch File Download Diff File Expand all files Collapse all files

23
Detour/Include/DetourObstacleAvoidance.h
View File

@@ -26,14 +26,12 @@ struct dtObstacleCircle
float dvel[3]; // Velocity of the obstacle
float rad; // Radius of the obstacle
float dp[3], np[3]; // Use for side selection during sampling.
float dist;
};
struct dtObstacleSegment
{
float p[3], q[3]; // End points of the obstacle segment
bool touch;
float dist;
};
static const int RVO_SAMPLE_RAD = 15;
@@ -88,13 +86,10 @@ public:
void reset();
void addCircle(const float* pos, const float rad,
const float* vel, const float* dvel,
const float dist);
const float* vel, const float* dvel);
void addSegment(const float* p, const float* q, const float dist);
void addSegment(const float* p, const float* q);
inline void setSamplingGridSize(int n) { m_gridSize = n; }
inline void setSamplingGridDepth(int n) { m_gridDepth = n; }
inline void setVelocitySelectionBias(float v) { m_velBias = v; }
inline void setDesiredVelocityWeight(float w) { m_weightDesVel = w; }
inline void setCurrentVelocityWeight(float w) { m_weightCurVel = w; }
@@ -102,14 +97,14 @@ public:
inline void setCollisionTimeWeight(float w) { m_weightToi = w; }
inline void setTimeHorizon(float t) { m_horizTime = t; }
void sampleVelocity(const float* pos, const float rad, const float vmax,
const float* vel, const float* dvel,
float* nvel,
dtObstacleAvoidanceDebugData* debug = 0);
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);
void sampleVelocityAdaptive(const float* pos, const float rad, const float vmax,
const float* vel, const float* dvel,
float* nvel,
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; }
@@ -130,8 +125,6 @@ private:
dtObstacleCircle* insertCircle(const float dist);
dtObstacleSegment* insertSegment(const float dist);
int m_gridSize;
int m_gridDepth;
float m_velBias;
float m_weightDesVel;
float m_weightCurVel;

146
Detour/Source/DetourObstacleAvoidance.cpp
View File

@@ -206,8 +206,6 @@ void dtFreeObstacleAvoidanceQuery(dtObstacleAvoidanceQuery* ptr)
dtObstacleAvoidanceQuery::dtObstacleAvoidanceQuery() :
m_gridSize(0),
m_gridDepth(0),
m_velBias(0.0f),
m_weightDesVel(0.0f),
m_weightCurVel(0.0f),
@@ -255,85 +253,24 @@ void dtObstacleAvoidanceQuery::reset()
}
void dtObstacleAvoidanceQuery::addCircle(const float* pos, const float rad,
const float* vel, const float* dvel,
const float dist)
const float* vel, const float* dvel)
{
// Find location for the circle.
dtObstacleCircle* cir = 0;
if (!m_ncircles)
{
cir = &m_circles[m_ncircles];
}
else if (dist >= m_circles[m_ncircles-1].dist)
{
if (m_ncircles >= m_maxCircles)
return;
cir = &m_circles[m_ncircles];
}
else
{
int i;
for (i = 0; i < m_ncircles; ++i)
if (dist <= m_circles[i].dist)
break;
if (m_ncircles >= m_maxCircles)
return;
const int tgt = i+1;
const int n = dtMin(m_ncircles-i, m_maxCircles-tgt);
dtAssert(tgt+n <= m_maxCircles);
if (n > 0)
memmove(&m_circles[tgt], &m_circles[i], sizeof(dtObstacleCircle)*n);
cir = &m_circles[i];
}
memset(cir, 0, sizeof(dtObstacleCircle));
if (m_ncircles < m_maxCircles)
m_ncircles++;
dtObstacleCircle* cir = &m_circles[m_ncircles++];
dtVcopy(cir->p, pos);
cir->rad = rad;
dtVcopy(cir->vel, vel);
dtVcopy(cir->dvel, dvel);
cir->dist = dist;
}
void dtObstacleAvoidanceQuery::addSegment(const float* p, const float* q, const float dist)
void dtObstacleAvoidanceQuery::addSegment(const float* p, const float* q)
{
// Find location for the segment.
dtObstacleSegment* seg = 0;
if (!m_nsegments)
{
seg = &m_segments[m_nsegments];
}
else if (dist >= m_segments[m_nsegments-1].dist)
{
if (m_nsegments >= m_maxSegments)
return;
seg = &m_segments[m_nsegments];
}
else
{
int i;
for (i = 0; i < m_nsegments; ++i)
if (dist <= m_segments[i].dist)
break;
const int tgt = i+1;
const int n = dtMin(m_nsegments-i, m_maxSegments-tgt);
dtAssert(tgt+n <= m_maxSegments);
if (n > 0)
memmove(&m_segments[tgt], &m_segments[i], sizeof(dtObstacleSegment)*n);
seg = &m_segments[i];
}
if (m_nsegments > m_maxSegments)
return;
memset(seg, 0, sizeof(dtObstacleSegment));
if (m_nsegments < m_maxSegments)
m_nsegments++;
seg->dist = dist;
dtObstacleSegment* seg = &m_segments[m_nsegments++];
dtVcopy(seg->p, p);
dtVcopy(seg->q, q);
}
@@ -473,10 +410,10 @@ float dtObstacleAvoidanceQuery::processSample(const float* vcand, const float cs
return penalty;
}
void dtObstacleAvoidanceQuery::sampleVelocity(const float* pos, const float rad, const float vmax,
const float* vel, const float* dvel,
float* nvel,
dtObstacleAvoidanceDebugData* debug)
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)
{
prepare(pos, dvel);
@@ -487,14 +424,14 @@ void dtObstacleAvoidanceQuery::sampleVelocity(const float* pos, const float rad,
const float cvx = dvel[0] * m_velBias;
const float cvz = dvel[2] * m_velBias;
const float cs = vmax * 2 * (1 - m_velBias) / (float)(m_gridSize-1);
const float half = (m_gridSize-1)*cs*0.5f;
const float cs = vmax * 2 * (1 - m_velBias) / (float)(gsize-1);
const float half = (gsize-1)*cs*0.5f;
float minPenalty = FLT_MAX;
for (int y = 0; y < m_gridSize; ++y)
for (int y = 0; y < gsize; ++y)
{
for (int x = 0; x < m_gridSize; ++x)
for (int x = 0; x < gsize; ++x)
{
float vcand[3];
vcand[0] = cvx + x*cs - half;
@@ -517,8 +454,8 @@ void dtObstacleAvoidanceQuery::sampleVelocity(const float* pos, const float rad,
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 float* vel, const float* dvel, float* nvel,
const int ndivs, const int nrings, const int depth,
dtObstacleAvoidanceDebugData* debug)
{
prepare(pos, dvel);
@@ -528,43 +465,42 @@ void dtObstacleAvoidanceQuery::sampleVelocityAdaptive(const float* pos, const fl
if (debug)
debug->reset();
// First sample location.
float res[3];
dtVset(res, dvel[0] * m_velBias, 0, dvel[2] * m_velBias);
// Build sampling pattern aligned to desired velocity.
static const int MAX_PATTERN_SIZE = 32;
float pat[MAX_PATTERN_SIZE*2];
static const int MAX_PATTERN_DIVS = 32;
static const int MAX_PATTERN_RINGS = 4;
float pat[(MAX_PATTERN_DIVS*MAX_PATTERN_RINGS+1)*2];
int npat = 0;
const int nd = dtClamp(ndivs, 1, MAX_PATTERN_DIVS);
const int nr = dtClamp(nrings, 1, MAX_PATTERN_RINGS);
const float da = (1.0f/nd) * DT_PI*2;
const float dang = atan2f(dvel[2], dvel[0]);
// Always add sample at zero
pat[npat*2+0] = 0;
pat[npat*2+1] = 0;
npat++;
const int nring = dtClamp(m_gridSize, 1, MAX_PATTERN_SIZE);
const float sring = (1.0f/nring) * DT_PI*2;
const float dang = atan2f(dvel[2], dvel[0]);
for (int i = 0; i < nring; ++i)
for (int j = 0; j < nr; ++j)
{
const float a = dang + (float)(i+0.5f)*sring;
pat[npat*2+0] = cosf(a)*0.5f;
pat[npat*2+1] = sinf(a)*0.5f;
npat++;
}
for (int i = 0; i < nring; ++i)
{
const float a = dang + (float)i * sring;
pat[npat*2+0] = cosf(a);
pat[npat*2+1] = sinf(a);
npat++;
const float rad = (float)(nr-j)/(float)nr;
float a = dang + (j&1)*0.5f*da;
for (int i = 0; i < nd; ++i)
{
pat[npat*2+0] = cosf(a)*rad;
pat[npat*2+1] = sinf(a)*rad;
npat++;
a += da;
}
}
// Start sampling.
float cr = vmax * (1.0f-m_velBias);
for (int k = 0; k < m_gridDepth; ++k)
float res[3];
dtVset(res, dvel[0] * m_velBias, 0, dvel[2] * m_velBias);
for (int k = 0; k < depth; ++k)
{
float minPenalty = FLT_MAX;
float bvel[3];

BIN
RecastDemo/Bin/Recast.app/Contents/MacOS/Recast
View File

Binary file not shown.

357
RecastDemo/Build/Xcode/Recast.xcodeproj/memon.pbxuser
View File

@@ -9,7 +9,7 @@
};
};
29B97313FDCFA39411CA2CEA /* Project object */ = {
activeBuildConfigurationName = Release;
activeBuildConfigurationName = Debug;
activeExecutable = 6B8632970F78114600E2684A /* Recast */;
activeTarget = 8D1107260486CEB800E47090 /* Recast */;
addToTargets = (
@@ -25,6 +25,7 @@
6B920A141225B1CF00D5B5AD /* DetourHashLookup.cpp:131 */,
6BD66851124350F50021A7A4 /* NavMeshTesterTool.cpp:480 */,
6BA8CF601255D4C500272A3B /* Sample.cpp:45 */,
6BB9C251126F555D00B97C1C /* DetourObstacleAvoidance.cpp:465 */,
);
codeSenseManager = 6B8632AA0F78115100E2684A /* Code sense */;
executables = (
@@ -221,6 +222,29 @@
6BB9C1E2126C24C300B97C1C /* PBXTextBookmark */ = 6BB9C1E2126C24C300B97C1C /* PBXTextBookmark */;
6BB9C1E3126C24C300B97C1C /* PBXTextBookmark */ = 6BB9C1E3126C24C300B97C1C /* PBXTextBookmark */;
6BB9C1E4126C265200B97C1C /* PBXTextBookmark */ = 6BB9C1E4126C265200B97C1C /* PBXTextBookmark */;
6BB9C1E5126C273100B97C1C /* PBXTextBookmark */ = 6BB9C1E5126C273100B97C1C /* PBXTextBookmark */;
6BB9C228126F4A9100B97C1C /* PBXTextBookmark */ = 6BB9C228126F4A9100B97C1C /* PBXTextBookmark */;
6BB9C229126F4A9100B97C1C /* PBXTextBookmark */ = 6BB9C229126F4A9100B97C1C /* PBXTextBookmark */;
6BB9C22A126F4A9100B97C1C /* PBXTextBookmark */ = 6BB9C22A126F4A9100B97C1C /* PBXTextBookmark */;
6BB9C22B126F4A9100B97C1C /* PBXTextBookmark */ = 6BB9C22B126F4A9100B97C1C /* PBXTextBookmark */;
6BB9C22C126F4A9100B97C1C /* PBXTextBookmark */ = 6BB9C22C126F4A9100B97C1C /* PBXTextBookmark */;
6BB9C22D126F4A9100B97C1C /* PBXTextBookmark */ = 6BB9C22D126F4A9100B97C1C /* PBXTextBookmark */;
6BB9C22E126F4A9100B97C1C /* PBXTextBookmark */ = 6BB9C22E126F4A9100B97C1C /* PBXTextBookmark */;
6BB9C22F126F4A9100B97C1C /* PBXTextBookmark */ = 6BB9C22F126F4A9100B97C1C /* PBXTextBookmark */;
6BB9C23E126F4DB200B97C1C /* PBXTextBookmark */ = 6BB9C23E126F4DB200B97C1C /* PBXTextBookmark */;
6BB9C23F126F4DB200B97C1C /* XCBuildMessageTextBookmark */ = 6BB9C23F126F4DB200B97C1C /* XCBuildMessageTextBookmark */;
6BB9C240126F4DB200B97C1C /* PBXTextBookmark */ = 6BB9C240126F4DB200B97C1C /* PBXTextBookmark */;
6BB9C243126F549B00B97C1C /* PBXTextBookmark */ = 6BB9C243126F549B00B97C1C /* PBXTextBookmark */;
6BB9C244126F549B00B97C1C /* PBXTextBookmark */ = 6BB9C244126F549B00B97C1C /* PBXTextBookmark */;
6BB9C245126F549B00B97C1C /* PBXTextBookmark */ = 6BB9C245126F549B00B97C1C /* PBXTextBookmark */;
6BB9C246126F549B00B97C1C /* PBXTextBookmark */ = 6BB9C246126F549B00B97C1C /* PBXTextBookmark */;
6BB9C24B126F54C800B97C1C /* PBXTextBookmark */ = 6BB9C24B126F54C800B97C1C /* PBXTextBookmark */;
6BB9C253126F555F00B97C1C /* PBXTextBookmark */ = 6BB9C253126F555F00B97C1C /* PBXTextBookmark */;
6BB9C254126F555F00B97C1C /* PBXTextBookmark */ = 6BB9C254126F555F00B97C1C /* PBXTextBookmark */;
6BB9C255126F555F00B97C1C /* PBXTextBookmark */ = 6BB9C255126F555F00B97C1C /* PBXTextBookmark */;
6BB9C256126F555F00B97C1C /* PBXTextBookmark */ = 6BB9C256126F555F00B97C1C /* PBXTextBookmark */;
6BB9C25C126F55D600B97C1C /* PBXTextBookmark */ = 6BB9C25C126F55D600B97C1C /* PBXTextBookmark */;
6BB9C262126F562C00B97C1C /* PBXTextBookmark */ = 6BB9C262126F562C00B97C1C /* PBXTextBookmark */;
6BBB0361124E242E00533229 = 6BBB0361124E242E00533229 /* PBXTextBookmark */;
6BBB0363124E242E00533229 = 6BBB0363124E242E00533229 /* PBXTextBookmark */;
6BBB4C34115B7A3D00CF791D = 6BBB4C34115B7A3D00CF791D /* PBXTextBookmark */;
@@ -272,7 +296,8 @@
hitCount = 0;
ignoreCount = 0;
lineNumber = 1324;
modificationTime = 308826279.221625;
location = Recast;
modificationTime = 309286258.801484;
originalNumberOfMultipleMatches = 1;
state = 1;
};
@@ -573,8 +598,8 @@
isa = PBXTextBookmark;
fRef = 6B9EFF02122819E200535FF1 /* DetourObstacleAvoidance.h */;
name = "DetourObstacleAvoidance.h: 96";
rLen = 10;
rLoc = 3207;
rLen = 0;
rLoc = 3096;
rType = 0;
vrLen = 1819;
vrLoc = 3088;
@@ -604,7 +629,7 @@
fRef = 6BD667D8123D27EC0021A7A4 /* CrowdManager.h */;
name = "CrowdManager.h: 232";
rLen = 0;
rLoc = 5681;
rLoc = 6163;
rType = 0;
vrLen = 908;
vrLoc = 5232;
@@ -614,7 +639,7 @@
fRef = 6BD667D9123D28100021A7A4 /* CrowdManager.cpp */;
name = "CrowdManager.cpp: 35";
rLen = 0;
rLoc = 1417;
rLoc = 1407;
rType = 0;
vrLen = 1286;
vrLoc = 446;
@@ -624,7 +649,7 @@
fRef = 6B9EFF0812281C3E00535FF1 /* DetourObstacleAvoidance.cpp */;
name = "DetourObstacleAvoidance.cpp: 568";
rLen = 0;
rLoc = 13659;
rLoc = 12450;
rType = 0;
vrLen = 905;
vrLoc = 13302;
@@ -634,7 +659,7 @@
fRef = 6B9EFF0812281C3E00535FF1 /* DetourObstacleAvoidance.cpp */;
name = "DetourObstacleAvoidance.cpp: 568";
rLen = 0;
rLoc = 13659;
rLoc = 12450;
rType = 0;
vrLen = 938;
vrLoc = 13269;
@@ -665,9 +690,9 @@
};
6B25B6180FFA62BE004F1BC4 /* main.cpp */ = {
uiCtxt = {
sepNavIntBoundsRect = "{{0, 0}, {931, 11895}}";
sepNavSelRange = "{1942, 0}";
sepNavVisRange = "{1842, 1045}";
sepNavIntBoundsRect = "{{0, 0}, {931, 11869}}";
sepNavSelRange = "{2461, 0}";
sepNavVisRange = "{1585, 1213}";
sepNavWindowFrame = "{{15, 51}, {1214, 722}}";
};
};
@@ -724,7 +749,8 @@
hitCount = 0;
ignoreCount = 0;
lineNumber = 362;
modificationTime = 308826279.220464;
location = Recast;
modificationTime = 309286240.760318;
originalNumberOfMultipleMatches = 1;
state = 1;
};
@@ -925,9 +951,9 @@
};
6B8DE88B10B69E4C00DF20FB /* DetourNavMesh.h */ = {
uiCtxt = {
sepNavIntBoundsRect = "{{0, 0}, {931, 5356}}";
sepNavSelRange = "{12977, 0}";
sepNavVisRange = "{11723, 1652}";
sepNavIntBoundsRect = "{{0, 0}, {931, 5278}}";
sepNavSelRange = "{14566, 0}";
sepNavVisRange = "{13725, 2039}";
};
};
6B8DE88C10B69E4C00DF20FB /* DetourNavMeshBuilder.h */ = {
@@ -961,7 +987,7 @@
ignoreCount = 0;
lineNumber = 78;
location = Recast;
modificationTime = 308826245.414741;
modificationTime = 309286237.392774;
originalNumberOfMultipleMatches = 0;
state = 2;
};
@@ -978,9 +1004,9 @@
hitCount = 0;
ignoreCount = 0;
lineNumber = 131;
modificationTime = 308826245.41512;
modificationTime = 309286258.80118;
originalNumberOfMultipleMatches = 1;
state = 0;
state = 1;
};
6B920A521225C0AC00D5B5AD /* PBXTextBookmark */ = {
isa = PBXTextBookmark;
@@ -1067,16 +1093,16 @@
};
6B9EFF02122819E200535FF1 /* DetourObstacleAvoidance.h */ = {
uiCtxt = {
sepNavIntBoundsRect = "{{0, 0}, {931, 1963}}";
sepNavSelRange = "{3207, 10}";
sepNavVisRange = "{3088, 1819}";
sepNavIntBoundsRect = "{{0, 0}, {931, 1846}}";
sepNavSelRange = "{3711, 22}";
sepNavVisRange = "{3162, 1535}";
};
};
6B9EFF0812281C3E00535FF1 /* DetourObstacleAvoidance.cpp */ = {
uiCtxt = {
sepNavIntBoundsRect = "{{0, 0}, {931, 7813}}";
sepNavSelRange = "{13659, 0}";
sepNavVisRange = "{13267, 940}";
sepNavIntBoundsRect = "{{0, 0}, {931, 7059}}";
sepNavSelRange = "{12152, 0}";
sepNavVisRange = "{11613, 838}";
};
};
6BA1E88810C7BFC9008007F6 /* Sample_SoloMeshSimple.cpp */ = {
@@ -1133,7 +1159,8 @@
hitCount = 0;
ignoreCount = 0;
lineNumber = 137;
modificationTime = 308826279.220855;
location = Recast;
modificationTime = 309286240.769202;
originalNumberOfMultipleMatches = 1;
state = 1;
};
@@ -1254,7 +1281,8 @@
hitCount = 0;
ignoreCount = 0;
lineNumber = 45;
modificationTime = 308826279.219005;
location = Recast;
modificationTime = 309286240.789448;
originalNumberOfMultipleMatches = 1;
state = 1;
};
@@ -1297,9 +1325,9 @@
};
6BAF3C581211663A008CFCDF /* CrowdTool.cpp */ = {
uiCtxt = {
sepNavIntBoundsRect = "{{0, 0}, {931, 8008}}";
sepNavSelRange = "{1351, 1095}";
sepNavVisRange = "{1742, 1225}";
sepNavIntBoundsRect = "{{0, 0}, {931, 8151}}";
sepNavSelRange = "{8689, 0}";
sepNavVisRange = "{8537, 1412}";
sepNavWindowFrame = "{{15, 51}, {1214, 722}}";
};
};
@@ -1625,7 +1653,7 @@
fRef = 6B9EFF0812281C3E00535FF1 /* DetourObstacleAvoidance.cpp */;
name = "DetourObstacleAvoidance.cpp: 568";
rLen = 0;
rLoc = 13659;
rLoc = 12450;
rType = 0;
vrLen = 940;
vrLoc = 13267;
@@ -1850,6 +1878,253 @@
vrLen = 1652;
vrLoc = 11723;
};
6BB9C1E5126C273100B97C1C /* PBXTextBookmark */ = {
isa = PBXTextBookmark;
fRef = 6B8DE88B10B69E4C00DF20FB /* DetourNavMesh.h */;
name = "DetourNavMesh.h: 385";
rLen = 0;
rLoc = 15347;
rType = 0;
vrLen = 2039;
vrLoc = 13725;
};
6BB9C228126F4A9100B97C1C /* PBXTextBookmark */ = {
isa = PBXTextBookmark;
fRef = 6B8DE88B10B69E4C00DF20FB /* DetourNavMesh.h */;
name = "DetourNavMesh.h: 370";
rLen = 0;
rLoc = 14566;
rType = 0;
vrLen = 2039;
vrLoc = 13725;
};
6BB9C229126F4A9100B97C1C /* PBXTextBookmark */ = {
isa = PBXTextBookmark;
fRef = 6B25B6180FFA62BE004F1BC4 /* main.cpp */;
name = "main.cpp: 82";
rLen = 0;
rLoc = 2461;
rType = 0;
vrLen = 1213;
vrLoc = 1585;
};
6BB9C22A126F4A9100B97C1C /* PBXTextBookmark */ = {
isa = PBXTextBookmark;
fRef = 6BD667D8123D27EC0021A7A4 /* CrowdManager.h */;
name = "CrowdManager.h: 140";
rLen = 0;
rLoc = 3459;
rType = 0;
vrLen = 1135;
vrLoc = 3180;
};
6BB9C22B126F4A9100B97C1C /* PBXTextBookmark */ = {
isa = PBXTextBookmark;
fRef = 6BD667D9123D28100021A7A4 /* CrowdManager.cpp */;
name = "CrowdManager.cpp: 472";
rLen = 0;
rLoc = 10993;
rType = 0;
vrLen = 945;
vrLoc = 10373;
};
6BB9C22C126F4A9100B97C1C /* PBXTextBookmark */ = {
isa = PBXTextBookmark;
fRef = 6BAF3C581211663A008CFCDF /* CrowdTool.cpp */;
name = "CrowdTool.cpp: 338";
rLen = 0;
rLoc = 8689;
rType = 0;
vrLen = 1412;
vrLoc = 8537;
};
6BB9C22D126F4A9100B97C1C /* PBXTextBookmark */ = {
isa = PBXTextBookmark;
fRef = 6B9EFF02122819E200535FF1 /* DetourObstacleAvoidance.h */;
name = "DetourObstacleAvoidance.h: 107";
rLen = 51;
rLoc = 3853;
rType = 0;
vrLen = 1776;
vrLoc = 3037;
};
6BB9C22E126F4A9100B97C1C /* PBXTextBookmark */ = {
isa = PBXTextBookmark;
fRef = 6B9EFF0812281C3E00535FF1 /* DetourObstacleAvoidance.cpp */;
name = "DetourObstacleAvoidance.cpp: 460";
rLen = 0;
rLoc = 11421;
rType = 0;
vrLen = 1064;
vrLoc = 10440;
};
6BB9C22F126F4A9100B97C1C /* PBXTextBookmark */ = {
isa = PBXTextBookmark;
fRef = 6B9EFF0812281C3E00535FF1 /* DetourObstacleAvoidance.cpp */;
name = "DetourObstacleAvoidance.cpp: 491";
rLen = 0;
rLoc = 12183;
rType = 0;
vrLen = 991;
vrLoc = 11498;
};
6BB9C23E126F4DB200B97C1C /* PBXTextBookmark */ = {
isa = PBXTextBookmark;
fRef = 6B9EFF0812281C3E00535FF1 /* DetourObstacleAvoidance.cpp */;
name = "DetourObstacleAvoidance.cpp: 231";
rLen = 0;
rLoc = 5750;
rType = 0;
vrLen = 795;
vrLoc = 5496;
};
6BB9C23F126F4DB200B97C1C /* XCBuildMessageTextBookmark */ = {
isa = PBXTextBookmark;
comments = "'class dtObstacleAvoidanceQuery' has no member named 'sampleVelocity'";
fRef = 6BD667D9123D28100021A7A4 /* CrowdManager.cpp */;
fallbackIsa = XCBuildMessageTextBookmark;
rLen = 1;
rLoc = 1229;
rType = 1;
};
6BB9C240126F4DB200B97C1C /* PBXTextBookmark */ = {
isa = PBXTextBookmark;
fRef = 6BD667D9123D28100021A7A4 /* CrowdManager.cpp */;
name = "CrowdManager.cpp: 1225";
rLen = 0;
rLoc = 29474;
rType = 0;
vrLen = 824;
vrLoc = 29149;
};
6BB9C243126F549B00B97C1C /* PBXTextBookmark */ = {
isa = PBXTextBookmark;
fRef = 6B9EFF0812281C3E00535FF1 /* DetourObstacleAvoidance.cpp */;
name = "DetourObstacleAvoidance.cpp: 458";
rLen = 50;
rLoc = 11370;
rType = 0;
vrLen = 890;
vrLoc = 10955;
};
6BB9C244126F549B00B97C1C /* PBXTextBookmark */ = {
isa = PBXTextBookmark;
fRef = 6B9EFF02122819E200535FF1 /* DetourObstacleAvoidance.h */;
name = "DetourObstacleAvoidance.h: 105";
rLen = 22;
rLoc = 3711;
rType = 0;
vrLen = 1535;
vrLoc = 3162;
};
6BB9C245126F549B00B97C1C /* PBXTextBookmark */ = {
isa = PBXTextBookmark;
fRef = 6BD667D9123D28100021A7A4 /* CrowdManager.cpp */;
name = "CrowdManager.cpp: 1228";
rLen = 0;
rLoc = 29571;
rType = 0;
vrLen = 931;
vrLoc = 29100;
};
6BB9C246126F549B00B97C1C /* PBXTextBookmark */ = {
isa = PBXTextBookmark;
fRef = 6BD667D9123D28100021A7A4 /* CrowdManager.cpp */;
name = "CrowdManager.cpp: 35";
rLen = 0;
rLoc = 1361;
rType = 0;
vrLen = 822;
vrLoc = 842;
};
6BB9C24B126F54C800B97C1C /* PBXTextBookmark */ = {
isa = PBXTextBookmark;
fRef = 6BD667D9123D28100021A7A4 /* CrowdManager.cpp */;
name = "CrowdManager.cpp: 1226";
rLen = 0;
rLoc = 29558;
rType = 0;
vrLen = 898;
vrLoc = 29100;
};
6BB9C251126F555D00B97C1C /* DetourObstacleAvoidance.cpp:465 */ = {
isa = PBXFileBreakpoint;
actions = (
);
breakpointStyle = 0;
continueAfterActions = 0;
countType = 0;
delayBeforeContinue = 0;
fileReference = 6B9EFF0812281C3E00535FF1 /* DetourObstacleAvoidance.cpp */;
functionName = "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)";
hitCount = 1;
ignoreCount = 0;
lineNumber = 465;
location = Recast;
modificationTime = 309286240.829786;
originalNumberOfMultipleMatches = 1;
state = 1;
};
6BB9C253126F555F00B97C1C /* PBXTextBookmark */ = {
isa = PBXTextBookmark;
fRef = 6B9EFF02122819E200535FF1 /* DetourObstacleAvoidance.h */;
name = "DetourObstacleAvoidance.h: 105";
rLen = 22;
rLoc = 3711;
rType = 0;
vrLen = 1535;
vrLoc = 3162;
};
6BB9C254126F555F00B97C1C /* PBXTextBookmark */ = {
isa = PBXTextBookmark;
fRef = 6BD667D9123D28100021A7A4 /* CrowdManager.cpp */;
name = "CrowdManager.cpp: 1224";
rLen = 0;
rLoc = 29429;
rType = 0;
vrLen = 931;
vrLoc = 29100;
};
6BB9C255126F555F00B97C1C /* PBXTextBookmark */ = {
isa = PBXTextBookmark;
fRef = 6B9EFF0812281C3E00535FF1 /* DetourObstacleAvoidance.cpp */;
name = "DetourObstacleAvoidance.cpp: 480";
rLen = 0;
rLoc = 11817;
rType = 0;
vrLen = 1022;
vrLoc = 11183;
};
6BB9C256126F555F00B97C1C /* PBXTextBookmark */ = {
isa = PBXTextBookmark;
fRef = 6B9EFF0812281C3E00535FF1 /* DetourObstacleAvoidance.cpp */;
name = "DetourObstacleAvoidance.cpp: 466";
rLen = 0;
rLoc = 11552;
rType = 0;
vrLen = 1022;
vrLoc = 11183;
};
6BB9C25C126F55D600B97C1C /* PBXTextBookmark */ = {
isa = PBXTextBookmark;
fRef = 6B9EFF0812281C3E00535FF1 /* DetourObstacleAvoidance.cpp */;
name = "DetourObstacleAvoidance.cpp: 488";
rLen = 0;
rLoc = 12152;
rType = 0;
vrLen = 859;
vrLoc = 11555;
};
6BB9C262126F562C00B97C1C /* PBXTextBookmark */ = {
isa = PBXTextBookmark;
fRef = 6B9EFF0812281C3E00535FF1 /* DetourObstacleAvoidance.cpp */;
name = "DetourObstacleAvoidance.cpp: 488";
rLen = 0;
rLoc = 12152;
rType = 0;
vrLen = 838;
vrLoc = 11613;
};
6BBB0361124E242E00533229 /* PBXTextBookmark */ = {
isa = PBXTextBookmark;
fRef = 6BBB0362124E242E00533229 /* Matrix4.h */;
@@ -1914,7 +2189,8 @@
hitCount = 0;
ignoreCount = 0;
lineNumber = 279;
modificationTime = 308826279.219623;
location = Recast;
modificationTime = 309286240.655807;
originalNumberOfMultipleMatches = 1;
state = 1;
};
@@ -2029,7 +2305,8 @@
hitCount = 0;
ignoreCount = 0;
lineNumber = 541;
modificationTime = 308826279.220026;
location = Recast;
modificationTime = 309286240.691116;
originalNumberOfMultipleMatches = 1;
state = 1;
};
@@ -2043,16 +2320,17 @@
};
6BD667D8123D27EC0021A7A4 /* CrowdManager.h */ = {
uiCtxt = {
sepNavIntBoundsRect = "{{0, 0}, {1041, 3718}}";
sepNavSelRange = "{5681, 0}";
sepNavVisRange = "{5232, 908}";
sepNavIntBoundsRect = "{{0, 0}, {931, 4147}}";
sepNavSelRange = "{3459, 0}";
sepNavVisRange = "{3180, 1135}";
};
};
6BD667D9123D28100021A7A4 /* CrowdManager.cpp */ = {
uiCtxt = {
sepNavIntBoundsRect = "{{0, 0}, {931, 16445}}";
sepNavSelRange = "{1417, 0}";
sepNavVisRange = "{446, 1286}";
sepNavIntBoundsRect = "{{0, 0}, {931, 16900}}";
sepNavSelRange = "{29429, 0}";
sepNavVisRange = "{29100, 931}";
sepNavWindowFrame = "{{15, 134}, {1120, 639}}";
};
};
6BD6681812434B790021A7A4 /* PBXTextBookmark */ = {
@@ -2078,7 +2356,8 @@
hitCount = 0;
ignoreCount = 0;
lineNumber = 480;
modificationTime = 308826279.221242;
location = Recast;
modificationTime = 309286240.704872;
originalNumberOfMultipleMatches = 1;
state = 1;
};

37
RecastDemo/Build/Xcode/Recast.xcodeproj/memon.perspectivev3
View File

@@ -284,14 +284,14 @@
<key>PBXSmartGroupTreeModuleOutlineStateSelectionKey</key>
<array>
<array>
<integer>15</integer>
<integer>26</integer>
<integer>11</integer>
<integer>1</integer>
<integer>0</integer>
</array>
</array>
<key>PBXSmartGroupTreeModuleOutlineStateVisibleRectKey</key>
<string>{{0, 178}, {264, 660}}</string>
<string>{{0, 430}, {264, 660}}</string>
</dict>
<key>PBXTopSmartGroupGIDs</key>
<array/>
@@ -326,7 +326,7 @@
<key>PBXProjectModuleGUID</key>
<string>6B8632A30F78115100E2684A</string>
<key>PBXProjectModuleLabel</key>
<string>DetourNavMesh.h</string>
<string>DetourObstacleAvoidance.cpp</string>
<key>PBXSplitModuleInNavigatorKey</key>
<dict>
<key>Split0</key>
@@ -334,11 +334,11 @@
<key>PBXProjectModuleGUID</key>
<string>6B8632A40F78115100E2684A</string>
<key>PBXProjectModuleLabel</key>
<string>DetourNavMesh.h</string>
<string>DetourObstacleAvoidance.cpp</string>
<key>_historyCapacity</key>
<integer>0</integer>
<key>bookmark</key>
<string>6BB9C1E4126C265200B97C1C</string>
<string>6BB9C262126F562C00B97C1C</string>
<key>history</key>
<array>
<string>6BBB4C34115B7A3D00CF791D</string>
@@ -423,14 +423,14 @@
<string>6B1635D3126887C80083FC15</string>
<string>6B1635D4126887C80083FC15</string>
<string>6B1635E812688D1B0083FC15</string>
<string>6B163608126891A40083FC15</string>
<string>6B16360A126891A40083FC15</string>
<string>6B16360B126891A40083FC15</string>
<string>6B16360C126891A40083FC15</string>
<string>6B163611126892060083FC15</string>
<string>6BB9C1B6126B55F200B97C1C</string>
<string>6BB9C1E1126C24C300B97C1C</string>
<string>6BB9C1E2126C24C300B97C1C</string>
<string>6BB9C228126F4A9100B97C1C</string>
<string>6BB9C229126F4A9100B97C1C</string>
<string>6BB9C22A126F4A9100B97C1C</string>
<string>6BB9C22C126F4A9100B97C1C</string>
<string>6BB9C253126F555F00B97C1C</string>
<string>6BB9C254126F555F00B97C1C</string>
<string>6BB9C255126F555F00B97C1C</string>
</array>
</dict>
<key>SplitCount</key>
@@ -444,18 +444,18 @@
<key>GeometryConfiguration</key>
<dict>
<key>Frame</key>
<string>{{0, 0}, {992, 673}}</string>
<string>{{0, 0}, {992, 530}}</string>
<key>RubberWindowFrame</key>
<string>0 59 1278 719 0 0 1280 778 </string>
</dict>
<key>Module</key>
<string>PBXNavigatorGroup</string>
<key>Proportion</key>
<string>673pt</string>
<string>530pt</string>
</dict>
<dict>
<key>Proportion</key>
<string>0pt</string>
<string>143pt</string>
<key>Tabs</key>
<array>
<dict>
@@ -470,8 +470,6 @@
<dict>
<key>Frame</key>
<string>{{10, 27}, {992, -27}}</string>
<key>RubberWindowFrame</key>
<string>0 59 1278 719 0 0 1280 778 </string>
</dict>
<key>Module</key>
<string>XCDetailModule</string>
@@ -525,7 +523,9 @@
<key>GeometryConfiguration</key>
<dict>
<key>Frame</key>
<string>{{0, 0}, {568, 405}}</string>
<string>{{10, 27}, {992, 116}}</string>
<key>RubberWindowFrame</key>
<string>0 59 1278 719 0 0 1280 778 </string>
</dict>
<key>Module</key>
<string>PBXBuildResultsModule</string>
@@ -744,6 +744,7 @@
<integer>5</integer>
<key>WindowOrderList</key>
<array>
<string>6BB9C263126F562C00B97C1C</string>
<string>6BB9C1C9126B562300B97C1C</string>
<string>6BB9C1CA126B562300B97C1C</string>
<string>/Users/memon/Code/recastnavigation/RecastDemo/Build/Xcode/Recast.xcodeproj</string>

76
RecastDemo/Include/CrowdManager.h
View File

@@ -71,7 +71,6 @@ public:
static const int AGENT_MAX_PATH = 256;
static const int AGENT_MAX_CORNERS = 4;
static const int AGENT_MAX_TRAIL = 64;
static const int AGENT_MAX_LOCALSEGS = 32;
static const int AGENT_MAX_NEIS = 8;
static const unsigned int PATHQ_INVALID = 0;
@@ -125,45 +124,68 @@ class PathCorridor
float m_pos[3];
float m_target[3];
float m_localCenter[3];
float m_localSegs[AGENT_MAX_LOCALSEGS*6];
int m_localSegCount;
dtPolyRef m_path[AGENT_MAX_PATH];
int m_npath;
float m_cornerVerts[AGENT_MAX_CORNERS*3];
unsigned char m_cornerFlags[AGENT_MAX_CORNERS];
dtPolyRef m_cornerPolys[AGENT_MAX_CORNERS];
int m_ncorners;
public:
PathCorridor();
~PathCorridor();
void init(dtPolyRef ref, const float* pos);
void updateLocalNeighbourhood(const float collisionQueryRange, dtNavMeshQuery* navquery, const dtQueryFilter* filter);
void updateCorners(const float pathOptimizationRange, dtNavMeshQuery* navquery, const dtQueryFilter* filter, float* opts = 0, float* opte = 0);
void updatePosition(const float* npos, dtNavMeshQuery* navquery, const dtQueryFilter* filter);
float getDistanceToGoal(const float range) const;
void calcSmoothSteerDirection(float* dir);
void calcStraightSteerDirection(float* dir);
/* void updateCorners(const float pathOptimizationRange,
dtNavMeshQuery* navquery, const dtQueryFilter* filter,
float* opts = 0, float* opte = 0);*/
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 updatePosition(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;
inline int getCornerCount() const { return m_ncorners; }
inline const float* getCornerPos(int i) const { return &m_cornerVerts[i*3]; }
struct Segment
{
float s[6]; // Segment start/end
float d; // Distance for pruning.
};
inline const float* getLocalCenter() const { return m_localCenter; }
inline int getLocalSegmentCount() const { return m_localSegCount; }
inline const float* getLocalSegment(int i) const { return &m_localSegs[i*6]; }
float m_center[3];
Segment m_segs[MAX_SEGS];
int m_nsegs;
void addSegment(const float dist, const float* seg);
public:
LocalBoundary();
~LocalBoundary();
void init();
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 MAX_NEIGHBOURS = 6;
@@ -179,9 +201,14 @@ struct Agent
unsigned char active;
PathCorridor corridor;
LocalBoundary boundary;
void integrate(const float maxAcc, const float dt);
void calcSmoothSteerDirection(float* dir);
void calcStraightSteerDirection(float* dir);
float getDistanceToGoal(const float range) const;
float maxspeed;
float t;
float var;
@@ -198,6 +225,11 @@ struct Agent
float collisionQueryRange;
float pathOptimizationRange;
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];

408
RecastDemo/Source/CrowdManager.cpp
View File

@@ -31,8 +31,9 @@
#include "DetourAssert.h"
#include "DetourAlloc.h"
static const int VO_ADAPTIVE_GRID_SIZE = 7; // this resuts 1+n*2 samples per depth.
static const int VO_ADAPTIVE_GRID_DEPTH = 5;
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;
@@ -392,30 +393,46 @@ static int mergeCorridor(dtPolyRef* path, const int npath, const int maxPath,
return req+size;
}
// Finds straight path towards the goal and prunes it to contain only relevant vertices.
static int findCorners(const float* pos, const float* target,
const dtPolyRef* path, const int npath,
float* cornerVerts, unsigned char* cornerFlags,
dtPolyRef* cornerpath, const int maxCorners,
const dtNavMeshQuery* navquery)
PathCorridor::PathCorridor() :
m_npath(0)
{
}
PathCorridor::~PathCorridor()
{
}
void PathCorridor::init(dtPolyRef ref, const float* pos)
{
dtVcopy(m_pos, pos);
dtVcopy(m_target, pos);
m_path[0] = ref;
m_npath = 1;
}
int PathCorridor::findCorners(float* cornerVerts, unsigned char* cornerFlags,
dtPolyRef* cornerPolys, const int maxCorners,
dtNavMeshQuery* navquery, const dtQueryFilter* filter)
{
dtAssert(m_npath);
static const float MIN_TARGET_DIST = 0.01f;
int ncorners = navquery->findStraightPath(pos, target, path, npath,
cornerVerts, cornerFlags, cornerpath,
maxCorners);
int ncorners = navquery->findStraightPath(m_pos, m_target, m_path, m_npath,
cornerVerts, cornerFlags, cornerPolys, 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], pos) > dtSqr(MIN_TARGET_DIST))
dtVdist2DSqr(&cornerVerts[0], m_pos) > dtSqr(MIN_TARGET_DIST))
break;
ncorners--;
if (ncorners)
{
memmove(cornerFlags, cornerFlags+1, sizeof(unsigned char)*ncorners);
memmove(cornerpath, cornerpath+1, sizeof(dtPolyRef)*ncorners);
memmove(cornerPolys, cornerPolys+1, sizeof(dtPolyRef)*ncorners);
memmove(cornerVerts, cornerVerts+3, sizeof(float)*3*ncorners);
}
}
@@ -433,116 +450,37 @@ static int findCorners(const float* pos, const float* target,
return ncorners;
}
static int optimizePath(const float* pos, const float* next, const float maxLookAhead,
dtPolyRef* path, const int npath,
const dtNavMeshQuery* navquery, const dtQueryFilter* filter)
void PathCorridor::optimizePath(const float* next, const float pathOptimizationRange,
dtNavMeshQuery* navquery, const dtQueryFilter* filter)
{
// Clamp the ray to max distance.
float goal[3];
dtVcopy(goal, next);
const float distSqr = dtVdist2DSqr(pos, goal);
const float distSqr = dtVdist2DSqr(m_pos, goal);
// If too close to the goal, do not try to optimize.
if (distSqr < dtSqr(0.01f))
return npath;
return;
// If too far truncate ray length.
if (distSqr > dtSqr(maxLookAhead))
if (distSqr > dtSqr(pathOptimizationRange))
{
float delta[3];
dtVsub(delta, goal, pos);
dtVmad(goal, pos, delta, dtSqr(maxLookAhead)/distSqr);
dtVsub(delta, goal, m_pos);
dtVmad(goal, m_pos, delta, dtSqr(pathOptimizationRange)/distSqr);
}
static const int MAX_RES = 32;
dtPolyRef res[MAX_RES];
float t, norm[3];
const int nres = navquery->raycast(path[0], pos, goal, filter, t, norm, res, MAX_RES);
const int nres = navquery->raycast(m_path[0], m_pos, goal, filter, t, norm, res, MAX_RES);
if (nres > 1 && t > 0.99f)
{
return mergeCorridor(path, npath, AGENT_MAX_PATH, res, nres);
}
return npath;
}
PathCorridor::PathCorridor()
{
}
PathCorridor::~PathCorridor()
{
}
void PathCorridor::init(dtPolyRef ref, const float* pos)
{
dtVcopy(m_pos, pos);
dtVcopy(m_target, pos);
m_path[0] = ref;
m_npath = 1;
dtVset(m_localCenter, 0,0,0);
m_localSegCount = 0;
m_ncorners = 0;
}
void PathCorridor::updateLocalNeighbourhood(const float collisionQueryRange, dtNavMeshQuery* navquery, const dtQueryFilter* filter)
{
dtAssert(m_npath);
// Only update the neigbourhood after certain distance has been passed.
if (dtVdist2DSqr(m_pos, m_localCenter) < dtSqr(collisionQueryRange*0.25f))
return;
dtVcopy(m_localCenter, m_pos);
// First query non-overlapping polygons.
static const int MAX_LOCALS = 32;
dtPolyRef locals[MAX_LOCALS];
const int nlocals = navquery->findLocalNeighbourhood(m_path[0], m_pos, collisionQueryRange,
filter, locals, 0, MAX_LOCALS);
// Secondly, store all polygon edges.
m_localSegCount = 0;
for (int j = 0; j < nlocals; ++j)
{
static const int MAX_SEGS = DT_VERTS_PER_POLYGON*2;
float segs[MAX_SEGS*6];
const int nsegs = navquery->getPolyWallSegments(locals[j], filter, segs, MAX_SEGS);
for (int k = 0; k < nsegs; ++k)
{
const float* s = &segs[k*6];
// Skip too distant segments.
float tseg;
const float distSqr = dtDistancePtSegSqr2D(m_pos, s, s+3, tseg);
if (distSqr > dtSqr(collisionQueryRange))
continue;
if (m_localSegCount < AGENT_MAX_LOCALSEGS)
{
memcpy(&m_localSegs[m_localSegCount*6], s, sizeof(float)*6);
m_localSegCount++;
}
}
m_npath = mergeCorridor(m_path, m_npath, AGENT_MAX_PATH, res, nres);
}
}
float PathCorridor::getDistanceToGoal(const float range) const
{
if (!m_ncorners)
return range;
const bool endOfPath = (m_cornerFlags[m_ncorners-1] & DT_STRAIGHTPATH_END) ? true : false;
const bool offMeshConnection = (m_cornerFlags[m_ncorners-1] & DT_STRAIGHTPATH_OFFMESH_CONNECTION) ? true : false;
if (endOfPath || offMeshConnection)
return dtMin(dtVdist2D(m_pos, &m_cornerVerts[(m_ncorners-1)*3]), range);
return range;
}
void PathCorridor::updateCorners(const float pathOptimizationRange,
/*void PathCorridor::updateCorners(const float pathOptimizationRange,
dtNavMeshQuery* navquery, const dtQueryFilter* filter,
float* opts, float* opte)
{
@@ -554,7 +492,7 @@ void PathCorridor::updateCorners(const float pathOptimizationRange,
if (opte)
dtVset(opte, 0,0,0);
// Find nest couple of corners for steering.
// Find next couple of corners for steering.
m_ncorners = findCorners(m_pos, m_target, m_path, m_npath,
m_cornerVerts, m_cornerFlags, m_cornerPolys,
AGENT_MAX_CORNERS, navquery);
@@ -571,7 +509,7 @@ void PathCorridor::updateCorners(const float pathOptimizationRange,
m_npath = optimizePath(m_pos, m_cornerVerts+3, pathOptimizationRange,
m_path, m_npath, navquery, filter);
}
}
}*/
void PathCorridor::updatePosition(const float* npos, dtNavMeshQuery* navquery, const dtQueryFilter* filter)
{
@@ -592,49 +530,6 @@ void PathCorridor::updatePosition(const float* npos, dtNavMeshQuery* navquery, c
dtVcopy(m_pos, result);
}
void PathCorridor::calcSmoothSteerDirection(float* dir)
{
if (!m_ncorners)
{
dtVset(dir, 0,0,0);
return;
}
const int ip0 = 0;
const int ip1 = dtMin(1, m_ncorners-1);
const float* p0 = &m_cornerVerts[ip0*3];
const float* p1 = &m_cornerVerts[ip1*3];
float dir0[3], dir1[3];
dtVsub(dir0, p0, m_pos);
dtVsub(dir1, p1, m_pos);
dir0[1] = 0;
dir1[1] = 0;
float len0 = dtVlen(dir0);
float len1 = dtVlen(dir1);
if (len1 > 0.001f)
dtVscale(dir1,dir1,1.0f/len1);
dir[0] = dir0[0] - dir1[0]*len0*0.5f;
dir[1] = 0;
dir[2] = dir0[2] - dir1[2]*len0*0.5f;
dtVnormalize(dir);
}
void PathCorridor::calcStraightSteerDirection(float* dir)
{
if (!m_ncorners)
{
dtVset(dir, 0,0,0);
return;
}
dtVsub(dir, &m_cornerVerts[0], m_pos);
dir[1] = 0;
dtVnormalize(dir);
}
void PathCorridor::setCorridor(const float* target, const dtPolyRef* path, const int npath)
{
dtAssert(npath > 0);
@@ -645,6 +540,8 @@ void PathCorridor::setCorridor(const float* target, const dtPolyRef* path, const
}
void Agent::integrate(const float maxAcc, const float dt)
{
// Fake dynamic constraint.
@@ -663,6 +560,157 @@ void Agent::integrate(const float maxAcc, const float dt)
dtVset(vel,0,0,0);
}
float Agent::getDistanceToGoal(const float range) const
{
if (!ncorners)
return range;
const bool endOfPath = (cornerFlags[ncorners-1] & DT_STRAIGHTPATH_END) ? true : false;
const bool offMeshConnection = (cornerFlags[ncorners-1] & DT_STRAIGHTPATH_OFFMESH_CONNECTION) ? true : false;
if (endOfPath || offMeshConnection)
return dtMin(dtVdist2D(npos, &cornerVerts[(ncorners-1)*3]), range);
return range;
}
void Agent::calcSmoothSteerDirection(float* dir)
{
if (!ncorners)
{
dtVset(dir, 0,0,0);
return;
}
const int ip0 = 0;
const int ip1 = dtMin(1, ncorners-1);
const float* p0 = &cornerVerts[ip0*3];
const float* p1 = &cornerVerts[ip1*3];
float dir0[3], dir1[3];
dtVsub(dir0, p0, npos);
dtVsub(dir1, p1, npos);
dir0[1] = 0;
dir1[1] = 0;
float len0 = dtVlen(dir0);
float len1 = dtVlen(dir1);
if (len1 > 0.001f)
dtVscale(dir1,dir1,1.0f/len1);
dir[0] = dir0[0] - dir1[0]*len0*0.5f;
dir[1] = 0;
dir[2] = dir0[2] - dir1[2]*len0*0.5f;
dtVnormalize(dir);
}
void Agent::calcStraightSteerDirection(float* dir)
{
if (!ncorners)
{
dtVset(dir, 0,0,0);
return;
}
dtVsub(dir, &cornerVerts[0], npos);
dir[1] = 0;
dtVnormalize(dir);
}
LocalBoundary::LocalBoundary() :
m_nsegs(0)
{
dtVset(m_center, FLT_MAX,FLT_MAX,FLT_MAX);
}
LocalBoundary::~LocalBoundary()
{
}
void LocalBoundary::init()
{
dtVset(m_center, FLT_MAX,FLT_MAX,FLT_MAX);
m_nsegs = 0;
}
void LocalBoundary::addSegment(const float dist, const float* s)
{
// Insert neighbour based on the distance.
Segment* seg = 0;
if (!m_nsegs)
{
// First, trivial accept.
seg = &m_segs[0];
}
else if (dist >= m_segs[m_nsegs-1].d)
{
// Further than the last segment, skip.
if (m_nsegs >= MAX_SEGS)
return;
// Last, trivial accept.
seg = &m_segs[m_nsegs];
}
else
{
// Insert inbetween.
int i;
for (i = 0; i < m_nsegs; ++i)
if (dist <= m_segs[i].d)
break;
const int tgt = i+1;
const int n = dtMin(m_nsegs-i, MAX_SEGS-tgt);
dtAssert(tgt+n <= MAX_SEGS);
if (n > 0)
memmove(&m_segs[tgt], &m_segs[i], sizeof(Segment)*n);
seg = &m_segs[i];
}
seg->d = dist;
memcpy(seg->s, s, sizeof(float)*6);
if (m_nsegs < MAX_SEGS)
m_nsegs++;
}
void LocalBoundary::update(dtPolyRef ref, const float* pos, const float collisionQueryRange,
dtNavMeshQuery* navquery, const dtQueryFilter* filter)
{
static const int MAX_LOCAL_POLYS = 16;
static const int MAX_SEGS_PER_POLY = DT_VERTS_PER_POLYGON*2;
if (!ref)
{
dtVset(m_center, FLT_MAX,FLT_MAX,FLT_MAX);
m_nsegs = 0;
return;
}
dtVcopy(m_center, pos);
// First query non-overlapping polygons.
dtPolyRef locals[MAX_LOCAL_POLYS];
const int nlocals = navquery->findLocalNeighbourhood(ref, pos, collisionQueryRange,
filter, locals, 0, MAX_LOCAL_POLYS);
// Secondly, store all polygon edges.
m_nsegs = 0;
float segs[MAX_SEGS_PER_POLY*6];
for (int j = 0; j < nlocals; ++j)
{
const int nsegs = navquery->getPolyWallSegments(locals[j], filter, segs, MAX_SEGS_PER_POLY);
for (int k = 0; k < nsegs; ++k)
{
const float* s = &segs[k*6];
// Skip too distant segments.
float tseg;
const float distSqr = dtDistancePtSegSqr2D(pos, s, s+3, tseg);
if (distSqr > dtSqr(collisionQueryRange))
continue;
addSegment(distSqr, s);
}
}
}
CrowdManager::CrowdManager() :
@@ -675,7 +723,7 @@ CrowdManager::CrowdManager() :
dtVset(m_ext, 2,4,2);
m_obstacleQuery = dtAllocObstacleAvoidanceQuery();
m_obstacleQuery->init(6, 10);
m_obstacleQuery->init(6, 8);
m_obstacleQuery->setDesiredVelocityWeight(2.0f);
m_obstacleQuery->setCurrentVelocityWeight(0.75f);
@@ -685,7 +733,9 @@ CrowdManager::CrowdManager() :
m_obstacleQuery->setVelocitySelectionBias(0.4f);
memset(m_vodebug, 0, sizeof(m_vodebug));
const int sampleCount = dtMax(VO_GRID_SIZE*VO_GRID_SIZE, (VO_ADAPTIVE_GRID_SIZE*VO_ADAPTIVE_GRID_SIZE)*VO_ADAPTIVE_GRID_DEPTH);
const int maxAdaptiveSamples = (VO_ADAPTIVE_DIVS*VO_ADAPTIVE_RINGS+1)*VO_ADAPTIVE_DEPTH;
const int maxGridSamples = VO_GRID_SIZE*VO_GRID_SIZE;
const int sampleCount = dtMax(maxAdaptiveSamples, maxGridSamples);
for (int i = 0; i < MAX_AGENTS; ++i)
{
m_vodebug[i] = dtAllocObstacleAvoidanceDebugData();
@@ -748,6 +798,7 @@ int CrowdManager::addAgent(const float* pos, const float radius, const float hei
}
ag->corridor.init(ref, nearest);
ag->boundary.init();
ag->radius = radius;
ag->height = height;
@@ -1057,8 +1108,9 @@ void CrowdManager::update(const float dt, unsigned int flags, dtNavMeshQuery* na
for (int i = 0; i < nagents; ++i)
{
Agent* ag = agents[i];
// Update collision segments
ag->corridor.updateLocalNeighbourhood(ag->collisionQueryRange, navquery, &m_filter);
// Only update the collision boundary after certain distance has been passed.
if (dtVdist2DSqr(ag->npos, ag->boundary.getCenter()) > dtSqr(ag->collisionQueryRange*0.25f))
ag->boundary.update(ag->corridor.getFirstPoly(), ag->npos, ag->collisionQueryRange, navquery, &m_filter);
// Query neighbour agents
ag->nneis = getNeighbours(ag->npos, ag->height, ag->collisionQueryRange, ag, ag->neis, MAX_NEIGHBOURS);
}
@@ -1067,7 +1119,27 @@ void CrowdManager::update(const float dt, unsigned int flags, dtNavMeshQuery* na
for (int i = 0; i < nagents; ++i)
{
Agent* ag = agents[i];
ag->corridor.updateCorners(ag->pathOptimizationRange, navquery, &m_filter, ag->opts, ag->opte);
// Find corners for steering
ag->ncorners = ag->corridor.findCorners(ag->cornerVerts, ag->cornerFlags, ag->cornerPolys,
AGENT_MAX_CORNERS, navquery, &m_filter);
// Check to see if the corner after the next corner is directly visible,
// and short cut to there.
if (ag->ncorners > 1)
{
const float* target = ag->cornerVerts+3;
dtVcopy(ag->opts, ag->corridor.getPos());
dtVcopy(ag->opte, target);
ag->corridor.optimizePath(target, ag->pathOptimizationRange, navquery, &m_filter);
}
else
{
dtVset(ag->opts, 0,0,0);
dtVset(ag->opte, 0,0,0);
}
// ag->corridor.updateCorners(ag->pathOptimizationRange, navquery, &m_filter, ag->opts, ag->opte);
}
// Calculate steering.
@@ -1079,13 +1151,13 @@ void CrowdManager::update(const float dt, unsigned int flags, dtNavMeshQuery* na
// Calculate steering direction.
if (flags & CROWDMAN_ANTICIPATE_TURNS)
ag->corridor.calcSmoothSteerDirection(dvel);
ag->calcSmoothSteerDirection(dvel);
else
ag->corridor.calcStraightSteerDirection(dvel);
ag->calcStraightSteerDirection(dvel);
// Calculate speed scale, which tells the agent to slowdown at the end of the path.
const float slowDownRadius = ag->radius*2; // TODO: make less hacky.
const float speedScale = ag->corridor.getDistanceToGoal(slowDownRadius) / slowDownRadius;
const float speedScale = ag->getDistanceToGoal(slowDownRadius) / slowDownRadius;
// Apply style.
if (flags & CROWDMAN_DRUNK)
@@ -1131,19 +1203,16 @@ void CrowdManager::update(const float dt, unsigned int flags, dtNavMeshQuery* na
for (int j = 0; j < ag->nneis; ++j)
{
const Agent* nei = &m_agents[ag->neis[j].idx];
m_obstacleQuery->addCircle(nei->npos, nei->radius, nei->vel, nei->dvel,
dtVdist2DSqr(ag->npos, nei->npos));
m_obstacleQuery->addCircle(nei->npos, nei->radius, nei->vel, nei->dvel);
}
// Append neighbour segments as obstacles.
for (int j = 0; j < ag->corridor.getLocalSegmentCount(); ++j)
for (int j = 0; j < ag->boundary.getSegmentCount(); ++j)
{
const float* s = ag->corridor.getLocalSegment(j);
const float* s = ag->boundary.getSegment(j);
if (dtTriArea2D(ag->npos, s, s+3) < 0.0f)
continue;
float tseg;
const float distSqr = dtDistancePtSegSqr2D(ag->npos, s, s+3, tseg);
m_obstacleQuery->addSegment(s, s+3, distSqr);
m_obstacleQuery->addSegment(s, s+3);
}
// Sample new safe velocity.
@@ -1151,17 +1220,16 @@ void CrowdManager::update(const float dt, unsigned int flags, dtNavMeshQuery* na
if (adaptive)
{
m_obstacleQuery->setSamplingGridSize(VO_ADAPTIVE_GRID_SIZE);
m_obstacleQuery->setSamplingGridDepth(VO_ADAPTIVE_GRID_DEPTH);
m_obstacleQuery->sampleVelocityAdaptive(ag->npos, ag->radius, ag->maxspeed, ag->vel, ag->dvel,
ag->nvel, m_vodebug[i]);
m_obstacleQuery->sampleVelocityAdaptive(ag->npos, ag->radius, ag->maxspeed,
ag->vel, ag->dvel, ag->nvel,
VO_ADAPTIVE_DIVS, VO_ADAPTIVE_RINGS, VO_ADAPTIVE_DEPTH,
m_vodebug[i]);
}
else
{
m_obstacleQuery->setSamplingGridSize(VO_GRID_SIZE);
m_obstacleQuery->sampleVelocity(ag->npos, ag->radius, ag->maxspeed,
ag->vel, ag->dvel,
ag->nvel, m_vodebug[i]);
m_obstacleQuery->sampleVelocityGrid(ag->npos, ag->radius, ag->maxspeed,
ag->vel, ag->dvel, ag->nvel,
VO_GRID_SIZE, m_vodebug[i]);
}
}
else

16
RecastDemo/Source/CrowdTool.cpp
View File

@@ -348,13 +348,13 @@ void CrowdTool::handleRender()
if (m_showCorners)
{
if (ag->corridor.getCornerCount())
if (ag->ncorners)
{
dd.begin(DU_DRAW_LINES, 2.0f);
for (int j = 0; j < ag->corridor.getCornerCount(); ++j)
for (int j = 0; j < ag->ncorners; ++j)
{
const float* va = j == 0 ? pos : ag->corridor.getCornerPos(j-1);
const float* vb = ag->corridor.getCornerPos(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,64));
dd.vertex(vb[0],vb[1]+radius,vb[2], duRGBA(128,0,0,64));
}
@@ -387,15 +387,15 @@ void CrowdTool::handleRender()
if (m_showCollisionSegments)
{
const float* center = ag->corridor.getLocalCenter();
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->corridor.getLocalSegmentCount(); ++j)
for (int j = 0; j < ag->boundary.getSegmentCount(); ++j)
{
const float* s = ag->corridor.getLocalSegment(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);
@@ -429,7 +429,7 @@ void CrowdTool::handleRender()
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(0,96,128,220), (int)(pen*255));
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);