diff --git a/Detour/Include/DetourObstacleAvoidance.h b/Detour/Include/DetourObstacleAvoidance.h
index ec66cac8..4a7187a7 100644
--- a/Detour/Include/DetourObstacleAvoidance.h
+++ b/Detour/Include/DetourObstacleAvoidance.h
@@ -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;
diff --git a/Detour/Source/DetourObstacleAvoidance.cpp b/Detour/Source/DetourObstacleAvoidance.cpp
index 808bc225..ea48a89b 100644
--- a/Detour/Source/DetourObstacleAvoidance.cpp
+++ b/Detour/Source/DetourObstacleAvoidance.cpp
@@ -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];
diff --git a/RecastDemo/Bin/Recast.app/Contents/MacOS/Recast b/RecastDemo/Bin/Recast.app/Contents/MacOS/Recast
index 1c77393e..e4927bde 100755
Binary files a/RecastDemo/Bin/Recast.app/Contents/MacOS/Recast and b/RecastDemo/Bin/Recast.app/Contents/MacOS/Recast differ
diff --git a/RecastDemo/Build/Xcode/Recast.xcodeproj/memon.pbxuser b/RecastDemo/Build/Xcode/Recast.xcodeproj/memon.pbxuser
index 29327e7e..8fb07e57 100644
--- a/RecastDemo/Build/Xcode/Recast.xcodeproj/memon.pbxuser
+++ b/RecastDemo/Build/Xcode/Recast.xcodeproj/memon.pbxuser
@@ -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;
};
diff --git a/RecastDemo/Build/Xcode/Recast.xcodeproj/memon.perspectivev3 b/RecastDemo/Build/Xcode/Recast.xcodeproj/memon.perspectivev3
index e1cdf21a..370c415a 100644
--- a/RecastDemo/Build/Xcode/Recast.xcodeproj/memon.perspectivev3
+++ b/RecastDemo/Build/Xcode/Recast.xcodeproj/memon.perspectivev3
@@ -284,14 +284,14 @@
PBXSmartGroupTreeModuleOutlineStateSelectionKey
- 15
+ 26
11
1
0
PBXSmartGroupTreeModuleOutlineStateVisibleRectKey
- {{0, 178}, {264, 660}}
+ {{0, 430}, {264, 660}}
PBXTopSmartGroupGIDs
@@ -326,7 +326,7 @@
PBXProjectModuleGUID
6B8632A30F78115100E2684A
PBXProjectModuleLabel
- DetourNavMesh.h
+ DetourObstacleAvoidance.cpp
PBXSplitModuleInNavigatorKey
Split0
@@ -334,11 +334,11 @@
PBXProjectModuleGUID
6B8632A40F78115100E2684A
PBXProjectModuleLabel
- DetourNavMesh.h
+ DetourObstacleAvoidance.cpp
_historyCapacity
0
bookmark
- 6BB9C1E4126C265200B97C1C
+ 6BB9C262126F562C00B97C1C
history
6BBB4C34115B7A3D00CF791D
@@ -423,14 +423,14 @@
6B1635D3126887C80083FC15
6B1635D4126887C80083FC15
6B1635E812688D1B0083FC15
- 6B163608126891A40083FC15
6B16360A126891A40083FC15
- 6B16360B126891A40083FC15
- 6B16360C126891A40083FC15
- 6B163611126892060083FC15
- 6BB9C1B6126B55F200B97C1C
- 6BB9C1E1126C24C300B97C1C
- 6BB9C1E2126C24C300B97C1C
+ 6BB9C228126F4A9100B97C1C
+ 6BB9C229126F4A9100B97C1C
+ 6BB9C22A126F4A9100B97C1C
+ 6BB9C22C126F4A9100B97C1C
+ 6BB9C253126F555F00B97C1C
+ 6BB9C254126F555F00B97C1C
+ 6BB9C255126F555F00B97C1C
SplitCount
@@ -444,18 +444,18 @@
GeometryConfiguration
Frame
- {{0, 0}, {992, 673}}
+ {{0, 0}, {992, 530}}
RubberWindowFrame
0 59 1278 719 0 0 1280 778
Module
PBXNavigatorGroup
Proportion
- 673pt
+ 530pt
Proportion
- 0pt
+ 143pt
Tabs
@@ -470,8 +470,6 @@
Frame
{{10, 27}, {992, -27}}
- RubberWindowFrame
- 0 59 1278 719 0 0 1280 778
Module
XCDetailModule
@@ -525,7 +523,9 @@
GeometryConfiguration
Frame
- {{0, 0}, {568, 405}}
+ {{10, 27}, {992, 116}}
+ RubberWindowFrame
+ 0 59 1278 719 0 0 1280 778
Module
PBXBuildResultsModule
@@ -744,6 +744,7 @@
5
WindowOrderList
+ 6BB9C263126F562C00B97C1C
6BB9C1C9126B562300B97C1C
6BB9C1CA126B562300B97C1C
/Users/memon/Code/recastnavigation/RecastDemo/Build/Xcode/Recast.xcodeproj
diff --git a/RecastDemo/Include/CrowdManager.h b/RecastDemo/Include/CrowdManager.h
index 65fc4308..ed017904 100644
--- a/RecastDemo/Include/CrowdManager.h
+++ b/RecastDemo/Include/CrowdManager.h
@@ -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];
diff --git a/RecastDemo/Source/CrowdManager.cpp b/RecastDemo/Source/CrowdManager.cpp
index ec46d10b..ae094c06 100644
--- a/RecastDemo/Source/CrowdManager.cpp
+++ b/RecastDemo/Source/CrowdManager.cpp
@@ -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
diff --git a/RecastDemo/Source/CrowdTool.cpp b/RecastDemo/Source/CrowdTool.cpp
index 2392a484..45d76d0c 100644
--- a/RecastDemo/Source/CrowdTool.cpp
+++ b/RecastDemo/Source/CrowdTool.cpp
@@ -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);