server_common/recastnavigation_v1.6.0
5
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Hasty before flight check in. Some adjustments to VO code (renaming, optimize segment sampling). Crowd sim performance graph.

Browse Source
This commit is contained in:
Mikko Mononen 2010-08-22 20:15:25 +00:00
parent 0e4aa30e85
commit 07ec6dfa08
7 changed files with 1844 additions and 217 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

Binary file not shown.

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

File diff suppressed because it is too large Load Diff

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

@ -283,14 +283,13 @@
<key>PBXSmartGroupTreeModuleOutlineStateSelectionKey</key> <key>PBXSmartGroupTreeModuleOutlineStateSelectionKey</key>
<array> <array>
<array> <array>
<integer>17</integer> <integer>59</integer>
<integer>11</integer>
<integer>1</integer> <integer>1</integer>
<integer>0</integer> <integer>0</integer>
</array> </array>
</array> </array>
<key>PBXSmartGroupTreeModuleOutlineStateVisibleRectKey</key> <key>PBXSmartGroupTreeModuleOutlineStateVisibleRectKey</key>
<string>{{0, 0}, {264, 660}}</string> <string>{{0, 646}, {264, 660}}</string>
</dict> </dict>
<key>PBXTopSmartGroupGIDs</key> <key>PBXTopSmartGroupGIDs</key>
<array/> <array/>
@ -325,7 +324,7 @@
<key>PBXProjectModuleGUID</key> <key>PBXProjectModuleGUID</key>
<string>6B8632A30F78115100E2684A</string> <string>6B8632A30F78115100E2684A</string>
<key>PBXProjectModuleLabel</key> <key>PBXProjectModuleLabel</key>
<string>DetourNavMeshQuery.cpp</string> <string>CrowdTool.cpp</string>
<key>PBXSplitModuleInNavigatorKey</key> <key>PBXSplitModuleInNavigatorKey</key>
<dict> <dict>
<key>Split0</key> <key>Split0</key>
@ -333,11 +332,11 @@
<key>PBXProjectModuleGUID</key> <key>PBXProjectModuleGUID</key>
<string>6B8632A40F78115100E2684A</string> <string>6B8632A40F78115100E2684A</string>
<key>PBXProjectModuleLabel</key> <key>PBXProjectModuleLabel</key>
<string>DetourNavMeshQuery.cpp</string> <string>CrowdTool.cpp</string>
<key>_historyCapacity</key> <key>_historyCapacity</key>
<integer>0</integer> <integer>0</integer>
<key>bookmark</key> <key>bookmark</key>
<string>6B1C8DAF121E821F0048697F</string> <string>6B1C8E6F1221BC910048697F</string>
<key>history</key> <key>history</key>
<array> <array>
<string>6BBB4AA5115B4F3400CF791D</string> <string>6BBB4AA5115B4F3400CF791D</string>
@ -356,7 +355,6 @@
<string>6BAF3CB012116AD9008CFCDF</string> <string>6BAF3CB012116AD9008CFCDF</string>
<string>6BAF3CB112116AD9008CFCDF</string> <string>6BAF3CB112116AD9008CFCDF</string>
<string>6BAF3CB412116AD9008CFCDF</string> <string>6BAF3CB412116AD9008CFCDF</string>
<string>6BAF3DFC1211AEB2008CFCDF</string>
<string>6BAF3E781212869F008CFCDF</string> <string>6BAF3E781212869F008CFCDF</string>
<string>6BAF404F12140B4E008CFCDF</string> <string>6BAF404F12140B4E008CFCDF</string>
<string>6BAF405112140B4E008CFCDF</string> <string>6BAF405112140B4E008CFCDF</string>
@ -376,7 +374,6 @@
<string>6BAF4524121D1723008CFCDF</string> <string>6BAF4524121D1723008CFCDF</string>
<string>6BAF4525121D1723008CFCDF</string> <string>6BAF4525121D1723008CFCDF</string>
<string>6BAF4562121D1849008CFCDF</string> <string>6BAF4562121D1849008CFCDF</string>
<string>6BAF457C121D19CB008CFCDF</string>
<string>6BAF457D121D19CB008CFCDF</string> <string>6BAF457D121D19CB008CFCDF</string>
<string>6BAF457E121D19CB008CFCDF</string> <string>6BAF457E121D19CB008CFCDF</string>
<string>6BAF457F121D19CB008CFCDF</string> <string>6BAF457F121D19CB008CFCDF</string>
@ -384,18 +381,13 @@
<string>6BAF4581121D19CB008CFCDF</string> <string>6BAF4581121D19CB008CFCDF</string>
<string>6BAF4582121D19CB008CFCDF</string> <string>6BAF4582121D19CB008CFCDF</string>
<string>6BAF4593121D1B18008CFCDF</string> <string>6BAF4593121D1B18008CFCDF</string>
<string>6BAF45AC121D2C03008CFCDF</string>
<string>6BAF45B8121D2F37008CFCDF</string> <string>6BAF45B8121D2F37008CFCDF</string>
<string>6BAF45E4121D7277008CFCDF</string> <string>6BAF45E4121D7277008CFCDF</string>
<string>6BAF4604121D7388008CFCDF</string> <string>6BAF4604121D7388008CFCDF</string>
<string>6BAF4605121D7388008CFCDF</string> <string>6BAF4605121D7388008CFCDF</string>
<string>6BAF4607121D7388008CFCDF</string>
<string>6BAF461E121D746C008CFCDF</string>
<string>6BAF4637121D74D3008CFCDF</string> <string>6BAF4637121D74D3008CFCDF</string>
<string>6BAF46A0121D8B41008CFCDF</string>
<string>6BAF46A1121D8B41008CFCDF</string> <string>6BAF46A1121D8B41008CFCDF</string>
<string>6BAF46A2121D8B41008CFCDF</string> <string>6BAF46A2121D8B41008CFCDF</string>
<string>6BAF46A3121D8B41008CFCDF</string>
<string>6BAF46D3121D8FF1008CFCDF</string> <string>6BAF46D3121D8FF1008CFCDF</string>
<string>6BAF4739121D9FBE008CFCDF</string> <string>6BAF4739121D9FBE008CFCDF</string>
<string>6BAF473A121D9FBE008CFCDF</string> <string>6BAF473A121D9FBE008CFCDF</string>
@ -403,17 +395,25 @@
<string>6BAF475A121DA31D008CFCDF</string> <string>6BAF475A121DA31D008CFCDF</string>
<string>6BAF475B121DA31D008CFCDF</string> <string>6BAF475B121DA31D008CFCDF</string>
<string>6BAF475C121DA31D008CFCDF</string> <string>6BAF475C121DA31D008CFCDF</string>
<string>6BAF475D121DA31D008CFCDF</string>
<string>6BAF475E121DA31D008CFCDF</string>
<string>6BAF475F121DA31D008CFCDF</string>
<string>6BAF4760121DA31D008CFCDF</string>
<string>6BAF4778121DCAF9008CFCDF</string> <string>6BAF4778121DCAF9008CFCDF</string>
<string>6BAF478E121DD150008CFCDF</string>
<string>6B1C8D82121E80950048697F</string> <string>6B1C8D82121E80950048697F</string>
<string>6B1C8DA2121E813D0048697F</string> <string>6B1C8DA2121E813D0048697F</string>
<string>6B1C8DA3121E813D0048697F</string> <string>6B1C8DA3121E813D0048697F</string>
<string>6B1C8DAD121E821F0048697F</string> <string>6B1C8DB3121E8D970048697F</string>
<string>6B1C8DA8121E81C40048697F</string> <string>6B1C8DBF121E8F610048697F</string>
<string>6B1C8DC0121E8F610048697F</string>
<string>6B1C8DC5121E902F0048697F</string>
<string>6B1C8DD8121EA3470048697F</string>
<string>6B1C8E08121EB4FF0048697F</string>
<string>6B1C8E09121EB4FF0048697F</string>
<string>6B1C8E0A121EB4FF0048697F</string>
<string>6B1C8E0C121EB4FF0048697F</string>
<string>6B1C8E24121EB6D30048697F</string>
<string>6B1C8E27121EB6D30048697F</string>
<string>6B1C8E30121EB9310048697F</string>
<string>6B1C8E691221BC910048697F</string>
<string>6B1C8E6A1221BC910048697F</string>
<string>6B1C8E6B1221BC910048697F</string>
</array> </array>
<key>prevStack</key> <key>prevStack</key>
<array> <array>
@ -458,11 +458,7 @@
<string>6BAF3B1512112F65008CFCDF</string> <string>6BAF3B1512112F65008CFCDF</string>
<string>6BAF3DAA1211882E008CFCDF</string> <string>6BAF3DAA1211882E008CFCDF</string>
<string>6BAF3E631211B713008CFCDF</string> <string>6BAF3E631211B713008CFCDF</string>
<string>6BAF3F5F1213E45B008CFCDF</string>
<string>6BBB4AE2115B4F3400CF791D</string> <string>6BBB4AE2115B4F3400CF791D</string>
<string>6BBB4AF7115B4F3400CF791D</string>
<string>6BAF405912140B4E008CFCDF</string>
<string>6BAF40A612142254008CFCDF</string>
<string>6BAF411712197F3D008CFCDF</string> <string>6BAF411712197F3D008CFCDF</string>
<string>6BAF41801219811E008CFCDF</string> <string>6BAF41801219811E008CFCDF</string>
<string>6BAF41DA121A5D13008CFCDF</string> <string>6BAF41DA121A5D13008CFCDF</string>
@ -481,37 +477,43 @@
<string>6BAF410212197F3D008CFCDF</string> <string>6BAF410212197F3D008CFCDF</string>
<string>6BAF44FA121C5713008CFCDF</string> <string>6BAF44FA121C5713008CFCDF</string>
<string>6BAF45E9121D7277008CFCDF</string> <string>6BAF45E9121D7277008CFCDF</string>
<string>6BAF4455121C40AC008CFCDF</string>
<string>6BAF447E121C4895008CFCDF</string> <string>6BAF447E121C4895008CFCDF</string>
<string>6BAF473D121D9FBE008CFCDF</string> <string>6BAF473D121D9FBE008CFCDF</string>
<string>6BAF4749121D9FED008CFCDF</string> <string>6BAF4749121D9FED008CFCDF</string>
<string>6BAF4762121DA31D008CFCDF</string> <string>6BAF4762121DA31D008CFCDF</string>
<string>6BAF4767121DA31D008CFCDF</string> <string>6BAF4767121DA31D008CFCDF</string>
<string>6BAF4768121DA31D008CFCDF</string> <string>6BAF4768121DA31D008CFCDF</string>
<string>6BAF4797121DD34D008CFCDF</string>
<string>6B1C8D84121E80950048697F</string>
<string>6B1C8D85121E80950048697F</string> <string>6B1C8D85121E80950048697F</string>
<string>6B1C8D86121E80950048697F</string>
<string>6B1C8D87121E80950048697F</string> <string>6B1C8D87121E80950048697F</string>
<string>6B1C8D88121E80950048697F</string>
<string>6B1C8D89121E80950048697F</string> <string>6B1C8D89121E80950048697F</string>
<string>6B1C8D8A121E80950048697F</string>
<string>6B1C8D8B121E80950048697F</string> <string>6B1C8D8B121E80950048697F</string>
<string>6B1C8D8C121E80950048697F</string>
<string>6B1C8D8D121E80950048697F</string> <string>6B1C8D8D121E80950048697F</string>
<string>6B1C8D8E121E80950048697F</string>
<string>6B1C8D8F121E80950048697F</string> <string>6B1C8D8F121E80950048697F</string>
<string>6B1C8D90121E80950048697F</string>
<string>6B1C8D91121E80950048697F</string> <string>6B1C8D91121E80950048697F</string>
<string>6B1C8D92121E80950048697F</string>
<string>6B1C8D93121E80950048697F</string> <string>6B1C8D93121E80950048697F</string>
<string>6B1C8D94121E80950048697F</string>
<string>6B1C8D95121E80950048697F</string> <string>6B1C8D95121E80950048697F</string>
<string>6B1C8D96121E80950048697F</string> <string>6B1C8D96121E80950048697F</string>
<string>6B1C8DA5121E813D0048697F</string> <string>6B1C8DA5121E813D0048697F</string>
<string>6B1C8DA6121E813D0048697F</string> <string>6B1C8DA6121E813D0048697F</string>
<string>6B1C8DAA121E81C40048697F</string>
<string>6B1C8DAE121E821F0048697F</string> <string>6B1C8DAE121E821F0048697F</string>
<string>6B1C8DB6121E8D970048697F</string>
<string>6B1C8DB7121E8D970048697F</string>
<string>6B1C8DDB121EA3470048697F</string>
<string>6B1C8E11121EB4FF0048697F</string>
<string>6B1C8E12121EB4FF0048697F</string>
<string>6B1C8E13121EB4FF0048697F</string>
<string>6B1C8E14121EB4FF0048697F</string>
<string>6B1C8E1B121EB4FF0048697F</string>
<string>6B1C8E2A121EB6D30048697F</string>
<string>6B1C8E2B121EB6D30048697F</string>
<string>6B1C8E2D121EB6D30048697F</string>
<string>6B1C8E2E121EB6D30048697F</string>
<string>6B1C8E58121EBEBF0048697F</string>
<string>6B1C8E59121EBEBF0048697F</string>
<string>6B1C8E661221BC080048697F</string>
<string>6B1C8E6C1221BC910048697F</string>
<string>6B1C8E6D1221BC910048697F</string>
<string>6B1C8E6E1221BC910048697F</string>
</array> </array>
</dict> </dict>
<key>SplitCount</key> <key>SplitCount</key>
@ -525,18 +527,18 @@
<key>GeometryConfiguration</key> <key>GeometryConfiguration</key>
<dict> <dict>
<key>Frame</key> <key>Frame</key>
<string>{{0, 0}, {994, 606}}</string> <string>{{0, 0}, {994, 505}}</string>
<key>RubberWindowFrame</key> <key>RubberWindowFrame</key>
<string>0 59 1280 719 0 0 1280 778 </string> <string>0 59 1280 719 0 0 1280 778 </string>
</dict> </dict>
<key>Module</key> <key>Module</key>
<string>PBXNavigatorGroup</string> <string>PBXNavigatorGroup</string>
<key>Proportion</key> <key>Proportion</key>
<string>606pt</string> <string>505pt</string>
</dict> </dict>
<dict> <dict>
<key>Proportion</key> <key>Proportion</key>
<string>67pt</string> <string>168pt</string>
<key>Tabs</key> <key>Tabs</key>
<array> <array>
<dict> <dict>
@ -604,7 +606,7 @@
<key>GeometryConfiguration</key> <key>GeometryConfiguration</key>
<dict> <dict>
<key>Frame</key> <key>Frame</key>
<string>{{10, 27}, {994, 40}}</string> <string>{{10, 27}, {994, 141}}</string>
<key>RubberWindowFrame</key> <key>RubberWindowFrame</key>
<string>0 59 1280 719 0 0 1280 778 </string> <string>0 59 1280 719 0 0 1280 778 </string>
</dict> </dict>

76
RecastDemo/Include/CrowdTool.h
View File

@ -24,19 +24,20 @@
// Tool to create crowds. // Tool to create crowds.
enum BodyType enum BodyType
{ {
BODY_CIRCLE = 0, BODY_CIRCLE = 0,
BODY_CAPSULE = 1, BODY_SEGMENT = 1,
}; };
struct Body struct ObstacleBody
{ {
float p[3], q[3]; // Position of the obstacle float p[3], q[3]; // Position of the obstacle
float vel[3]; // Velocity of the obstacle float vel[3]; // Velocity of the obstacle
float dvel[3]; // Velocity of the obstacle float dvel[3]; // Velocity of the obstacle
float rad; // Radius of the obstacle float rad; // Radius of the obstacle
float dp[3], np[3]; // Use for side selection during sampling.
bool touch;
int type; // Type of the obstacle (see ObstacleType) int type; // Type of the obstacle (see ObstacleType)
}; };
@ -44,9 +45,9 @@ struct Body
static const int RVO_SAMPLE_RAD = 15; static const int RVO_SAMPLE_RAD = 15;
static const int MAX_RVO_SAMPLES = (RVO_SAMPLE_RAD*2+1)*(RVO_SAMPLE_RAD*2+1) + 100; static const int MAX_RVO_SAMPLES = (RVO_SAMPLE_RAD*2+1)*(RVO_SAMPLE_RAD*2+1) + 100;
struct RVO struct RVODebugData
{ {
inline RVO() : ns(0) {} inline RVODebugData() : ns(0) {}
float spos[MAX_RVO_SAMPLES*3]; float spos[MAX_RVO_SAMPLES*3];
float scs[MAX_RVO_SAMPLES]; float scs[MAX_RVO_SAMPLES];
float spen[MAX_RVO_SAMPLES]; float spen[MAX_RVO_SAMPLES];
@ -90,11 +91,13 @@ struct Agent
float npos[3]; float npos[3];
float disp[3]; float disp[3];
float opts[3], opte[3];
float maxspeed; float maxspeed;
float t; float t;
float var; float var;
RVO rvo; RVODebugData rvo;
float colradius; float colradius;
float colcenter[3]; float colcenter[3];
@ -116,6 +119,59 @@ struct Agent
unsigned char active; unsigned char active;
}; };
class SampleGraph
{
static const int MAX_SAMPLES = 512;
float m_samples[MAX_SAMPLES];
int m_hsamples;
public:
SampleGraph() :
m_hsamples(0)
{
for (int i = 0; i < MAX_SAMPLES; ++i)
m_samples[i] = 0;
}
~SampleGraph()
{
}
inline void addSample(const float val)
{
m_hsamples = (m_hsamples+MAX_SAMPLES-1) % MAX_SAMPLES;
m_samples[m_hsamples] = val;
}
inline int getSampleCount() const
{
return MAX_SAMPLES;
}
inline float getSample(const int i) const
{
return m_samples[(m_hsamples+i) % MAX_SAMPLES];
}
inline float getSampleMin() const
{
float val = m_samples[0];
for (int i = 1; i < MAX_SAMPLES; ++i)
if (m_samples[i] < val)
val = m_samples[i];
return val;
}
inline float getSampleMax() const
{
float val = m_samples[0];
for (int i = 1; i < MAX_SAMPLES; ++i)
if (m_samples[i] > val)
val = m_samples[i];
return val;
}
};
enum UpdateFlags enum UpdateFlags
{ {
CROWDMAN_ANTICIPATE_TURNS = 1, CROWDMAN_ANTICIPATE_TURNS = 1,
@ -128,6 +184,10 @@ class CrowdManager
static const int MAX_AGENTS = 32; static const int MAX_AGENTS = 32;
Agent m_agents[MAX_AGENTS]; Agent m_agents[MAX_AGENTS];
int m_shortcutIter; int m_shortcutIter;
SampleGraph m_totalTime;
SampleGraph m_rvoTime;
public: public:
CrowdManager(); CrowdManager();
~CrowdManager(); ~CrowdManager();
@ -140,6 +200,9 @@ public:
void setMoveTarget(const int idx, const float* pos); void setMoveTarget(const int idx, const float* pos);
void update(const float dt, unsigned int flags, dtNavMeshQuery* navquery); void update(const float dt, unsigned int flags, dtNavMeshQuery* navquery);
const SampleGraph* getTotalTimeGraph() const { return &m_totalTime; }
const SampleGraph* getRVOTimeGraph() const { return &m_rvoTime; }
}; };
class CrowdTool : public SampleTool class CrowdTool : public SampleTool
@ -155,6 +218,7 @@ class CrowdTool : public SampleTool
bool m_showCollisionSegments; bool m_showCollisionSegments;
bool m_showPath; bool m_showPath;
bool m_showVO; bool m_showVO;
bool m_showOpt;
bool m_expandOptions; bool m_expandOptions;
bool m_anticipateTurns; bool m_anticipateTurns;

12
RecastDemo/Include/SampleInterfaces.h
View File

@ -16,6 +16,9 @@
// 3. This notice may not be removed or altered from any source distribution. // 3. This notice may not be removed or altered from any source distribution.
// //
#ifndef SAMPLEINTERFACES_H
#define SAMPLEINTERFACES_H
#include "DebugDraw.h" #include "DebugDraw.h"
#include "Recast.h" #include "Recast.h"
#include "RecastDump.h" #include "RecastDump.h"
@ -62,6 +65,12 @@ public:
virtual int getBuildTime(const rcBuilTimeLabel label); virtual int getBuildTime(const rcBuilTimeLabel label);
}; };
// TODO: Find better solution. This is useful for timing in some cases when the full build interface would be overkill.
rcTimeVal getPerfTime();
int getPerfDeltaTimeUsec(const rcTimeVal start, const rcTimeVal end);
// OpenGL debug draw implementation. // OpenGL debug draw implementation.
class DebugDrawGL : public duDebugDraw class DebugDrawGL : public duDebugDraw
{ {
@ -88,3 +97,6 @@ public:
virtual bool write(const void* ptr, const size_t size); virtual bool write(const void* ptr, const size_t size);
virtual bool read(void* ptr, const size_t size); virtual bool read(void* ptr, const size_t size);
}; };
#endif // SAMPLEINTERFACES_H

292
RecastDemo/Source/CrowdTool.cpp
View File

@ -29,6 +29,7 @@
#include "Sample.h" #include "Sample.h"
#include "DetourDebugDraw.h" #include "DetourDebugDraw.h"
#include "DetourCommon.h" #include "DetourCommon.h"
#include "SampleInterfaces.h"
#ifdef WIN32 #ifdef WIN32
# define snprintf _snprintf # define snprintf _snprintf
@ -197,7 +198,7 @@ static void normalizeArray(float* arr, const int n)
arr[i] = dtClamp((arr[i]-minPen)*s, 0.0f, 1.0f); arr[i] = dtClamp((arr[i]-minPen)*s, 0.0f, 1.0f);
} }
void normalizeSamples(RVO* rvo) void normalizeSamples(RVODebugData* rvo)
{ {
normalizeArray(rvo->spen, rvo->ns); normalizeArray(rvo->spen, rvo->ns);
normalizeArray(rvo->svpen, rvo->ns); normalizeArray(rvo->svpen, rvo->ns);
@ -207,7 +208,7 @@ void normalizeSamples(RVO* rvo)
} }
void setDynCircleBody(Body* b, const float* pos, const float rad, const float* vel, const float* dvel) void setDynCircleBody(ObstacleBody* b, const float* pos, const float rad, const float* vel, const float* dvel)
{ {
b->type = BODY_CIRCLE; b->type = BODY_CIRCLE;
dtVcopy(b->p, pos); dtVcopy(b->p, pos);
@ -216,7 +217,7 @@ void setDynCircleBody(Body* b, const float* pos, const float rad, const float* v
b->rad = rad; b->rad = rad;
} }
void setStatCircleBody(Body* b, const float* pos, const float rad) void setStatCircleBody(ObstacleBody* b, const float* pos, const float rad)
{ {
b->type = BODY_CIRCLE; b->type = BODY_CIRCLE;
dtVcopy(b->p, pos); dtVcopy(b->p, pos);
@ -225,14 +226,14 @@ void setStatCircleBody(Body* b, const float* pos, const float rad)
b->rad = rad; b->rad = rad;
} }
void setStatCapsuleBody(Body* b, const float* p, const float* q, const float rad) void setStatSegmentBody(ObstacleBody* b, const float* p, const float* q)
{ {
b->type = BODY_CAPSULE; b->type = BODY_SEGMENT;
dtVcopy(b->p, p); dtVcopy(b->p, p);
dtVcopy(b->q, q); dtVcopy(b->q, q);
dtVset(b->vel, 0,0,0); dtVset(b->vel, 0,0,0);
dtVset(b->dvel, 0,0,0); dtVset(b->dvel, 0,0,0);
b->rad = rad; b->rad = 0;
} }
static const float VEL_WEIGHT = 2.0f; static const float VEL_WEIGHT = 2.0f;
@ -261,84 +262,66 @@ static int sweepCircleCircle(const float* c0, const float r0, const float* v,
return 1; return 1;
} }
static int sweepCircleSegment(const float* c0, const float r0, const float* v, static void prepareBodies(const ObstacleBody* agent, ObstacleBody* obs, const int nobs)
const float* sa, const float* sb, const float sr,
float& tmin, float &tmax)
{ {
// equation parameters for (int i = 0; i < nobs; ++i)
float L[3], H[3];
dtVsub(L, sb, sa);
dtVsub(H, c0, sa);
const float radius = r0+sr;
const float l2 = dtVdot2D(L, L);
const float r2 = radius * radius;
const float dl = dtVperp2D(v, L);
const float hl = dtVperp2D(H, L);
const float a = dl * dl;
const float b = 2.0f * hl * dl;
const float c = hl * hl - (r2 * l2);
float d = (b*b) - (4.0f * a * c);
// infinite line missed by infinite ray.
if (d < 0.0f)
return 0;
const float i2a = 1.0f/(2*a);
d = dtSqrt(d);
tmin = (-b - d) * i2a;
tmax = (-b + d) * i2a;
// line missed by ray range.
/* if (tmax < 0.0f || tmin > 1.0f)
return 0;*/
// find what part of the ray was collided.
const float il2 = 1.0f / l2;
float Pedge[3];
dtVmad(Pedge, c0, v, tmin);
dtVsub(H, Pedge, sa);
const float e0 = dtVdot2D(H, L) * il2;
dtVmad(Pedge, c0, v, tmax);
dtVsub(H, Pedge, sa);
const float e1 = dtVdot2D(H, L) * il2;
if (e0 < 0.0f || e1 < 0.0f)
{ {
float ctmin, ctmax; ObstacleBody* ob = &obs[i];
if (sweepCircleCircle(c0, r0, v, sa, sr, ctmin, ctmax))
if (ob->type == BODY_CIRCLE)
{ {
if (e0 < 0.0f && ctmin > tmin) // Side
tmin = ctmin; const float* pa = agent->p;
if (e1 < 0.0f && ctmax < tmax) const float* pb = ob->p;
tmax = ctmax;
const float orig[3] = {0,0};
float dv[3];
dtVsub(ob->dp,pb,pa);
dtVnormalize(ob->dp);
dtVsub(dv, ob->dvel, agent->dvel);
const float a = dtTriArea2D(orig, ob->dp,dv);
if (a < 0.01f)
{
ob->np[0] = -ob->dp[2];
ob->np[2] = ob->dp[0];
} }
else else
{ {
return 0; ob->np[0] = ob->dp[2];
} ob->np[2] = -ob->dp[0];
}
if (e0 > 1.0f || e1 > 1.0f)
{
float ctmin, ctmax;
if (sweepCircleCircle(c0, r0, v, sb, sr, ctmin, ctmax))
{
if (e0 > 1.0f && ctmin > tmin)
tmin = ctmin;
if (e1 > 1.0f && ctmax < tmax)
tmax = ctmax;
}
else
{
return 0;
} }
} }
else if (ob->type == BODY_SEGMENT)
{
// Precalc if the agent is really close to the segment.
const float r = 0.01f;
float t;
ob->touch = dtDistancePtSegSqr2D(agent->p, ob->p, ob->q, t) < dtSqr(r);
}
}
}
static int isectRaySeg(const float* ap, const float* u,
const float* bp, const float* bq,
float& t)
{
float /*u[3]*/ v[3], w[3];
// dtVsub(u,aq,ap);
dtVsub(v,bq,bp);
dtVsub(w,ap,bp);
float d = dtVperp2D(u,v);
if (fabsf(d) < 1e-6f) return 0;
d = 1.0f/d;
t = dtVperp2D(v,w) * d;
if (t < 0 || t > 1) return 0;
float s = dtVperp2D(u,w) * d;
if (s < 0 || s > 1) return 0;
return 1; return 1;
} }
static void processSamples(Body* agent, const float vmax, static void processSamples(ObstacleBody* agent, const float vmax,
const Body* obs, const int nobs, RVO* rvo, const ObstacleBody* obs, const int nobs, RVODebugData* rvo,
const float* spos, const float cs, const int nspos, const float* spos, const float cs, const int nspos,
float* res) float* res)
{ {
@ -365,7 +348,7 @@ static void processSamples(Body* agent, const float vmax,
for (int i = 0; i < nobs; ++i) for (int i = 0; i < nobs; ++i)
{ {
const Body* ob = &obs[i]; const ObstacleBody* ob = &obs[i];
float htmin = 0, htmax = 0; float htmin = 0, htmax = 0;
if (ob->type == BODY_CIRCLE) if (ob->type == BODY_CIRCLE)
@ -378,30 +361,7 @@ static void processSamples(Body* agent, const float vmax,
dtVsub(vab, vab, ob->vel); dtVsub(vab, vab, ob->vel);
// Side // Side
// NOTE: dp, and dv are constant over the whole calculation, side += dtClamp(dtMin(dtVdot2D(ob->dp,vab)*2, dtVdot2D(ob->np,vab)*2), 0.0f, 1.0f);
// they can be precomputed per object.
const float* pa = agent->p;
const float* pb = ob->p;
const float orig[3] = {0,0};
float dp[3],dv[3],np[3];
dtVsub(dp,pb,pa);
dtVnormalize(dp);
dtVsub(dv, ob->dvel, agent->dvel);
const float a = dtTriArea2D(orig, dp,dv);
if (a < 0.01f)
{
np[0] = -dp[2];
np[2] = dp[0];
}
else
{
np[0] = dp[2];
np[2] = -dp[0];
}
side += dtClamp(dtMin(dtVdot2D(dp,vab)*2,dtVdot2D(np,vab)*2), 0.0f, 1.0f);
nside++; nside++;
if (!sweepCircleCircle(agent->p,agent->rad, vab, ob->p,ob->rad, htmin, htmax)) if (!sweepCircleCircle(agent->p,agent->rad, vab, ob->p,ob->rad, htmin, htmax))
@ -414,17 +374,11 @@ static void processSamples(Body* agent, const float vmax,
htmin = -htmin * 0.5f; htmin = -htmin * 0.5f;
} }
} }
else if (ob->type == BODY_CAPSULE) else if (ob->type == BODY_SEGMENT)
{ {
// NOTE: the segments are assumed to come from a navmesh which is shrunken by if (ob->touch)
// the agent radius, hence the use of really small radius.
// This can be handle more efficiently by using seg-seg test instead.
// If the whole segment is to be treated as obstacle, use agent->rad instead of 0.01f!
const float r = 0.01f; // agent->rad
float t;
if (dtDistancePtSegSqr2D(agent->p, ob->p, ob->q, t) < dtSqr(r+ob->rad))
{ {
// Special case when the agent is very close to the segment.
float sdir[3], snorm[3]; float sdir[3], snorm[3];
dtVsub(sdir, ob->q, ob->p); dtVsub(sdir, ob->q, ob->p);
snorm[0] = -sdir[2]; snorm[0] = -sdir[2];
@ -438,8 +392,9 @@ static void processSamples(Body* agent, const float vmax,
} }
else else
{ {
if (!sweepCircleSegment(agent->p, r, vcand, ob->p, ob->q, ob->rad, htmin, htmax)) if (!isectRaySeg(agent->p, vcand, ob->p, ob->q, htmin))
continue; continue;
htmax = maxToi;
} }
// Avoid less when facing walls. // Avoid less when facing walls.
@ -483,7 +438,8 @@ static void processSamples(Body* agent, const float vmax,
} }
void sampleRVO(Body* agent, const float vmax, const Body* obs, const int nobs, RVO* rvo, const float bias, float* nvel) void sampleRVO(ObstacleBody* agent, const float vmax, const ObstacleBody* obs,
const int nobs, RVODebugData* rvo, const float bias, float* nvel)
{ {
dtVset(nvel, 0,0,0); dtVset(nvel, 0,0,0);
@ -620,6 +576,8 @@ void CrowdManager::update(const float dt, unsigned int flags, dtNavMeshQuery* na
if (!navquery) if (!navquery)
return; return;
rcTimeVal startTime = getPerfTime();
const float ext[3] = {2,4,2}; const float ext[3] = {2,4,2};
dtQueryFilter filter; dtQueryFilter filter;
@ -802,8 +760,9 @@ void CrowdManager::update(const float dt, unsigned int flags, dtNavMeshQuery* na
} }
// Velocity planning. // Velocity planning.
rcTimeVal rvoStartTime = getPerfTime();
static const int MAX_BODIES = 32; static const int MAX_BODIES = 32;
Body bodies[MAX_BODIES]; ObstacleBody bodies[MAX_BODIES];
for (int i = 0; i < MAX_AGENTS; ++i) for (int i = 0; i < MAX_AGENTS; ++i)
{ {
if (!m_agents[i].active) continue; if (!m_agents[i].active) continue;
@ -847,14 +806,14 @@ void CrowdManager::update(const float dt, unsigned int flags, dtNavMeshQuery* na
if (nbodies < MAX_BODIES) if (nbodies < MAX_BODIES)
{ {
setStatCapsuleBody(&bodies[nbodies], s,s+3,0); setStatSegmentBody(&bodies[nbodies], s,s+3);
nbodies++; nbodies++;
} }
} }
Body agent; ObstacleBody agent;
setDynCircleBody(&agent, ag->pos, ag->radius, ag->vel, ag->dvel); setDynCircleBody(&agent, ag->pos, ag->radius, ag->vel, ag->dvel);
prepareBodies(&agent, bodies, nbodies);
sampleRVO(&agent, ag->maxspeed, bodies, nbodies, &ag->rvo, 0.4f, ag->nvel); sampleRVO(&agent, ag->maxspeed, bodies, nbodies, &ag->rvo, 0.4f, ag->nvel);
// Normalize samples for debug draw // Normalize samples for debug draw
@ -865,6 +824,8 @@ void CrowdManager::update(const float dt, unsigned int flags, dtNavMeshQuery* na
dtVcopy(ag->nvel, ag->dvel); dtVcopy(ag->nvel, ag->dvel);
} }
} }
rcTimeVal rvoEndTime = getPerfTime();
m_rvoTime.addSample(getPerfDeltaTimeUsec(rvoStartTime, rvoEndTime) / 1000.0f);
// Integrate and update perceived velocity. // Integrate and update perceived velocity.
for (int i = 0; i < MAX_AGENTS; ++i) for (int i = 0; i < MAX_AGENTS; ++i)
@ -967,13 +928,30 @@ void CrowdManager::update(const float dt, unsigned int flags, dtNavMeshQuery* na
{ {
if (!m_agents[i].active) continue; if (!m_agents[i].active) continue;
Agent* ag = &m_agents[i]; Agent* ag = &m_agents[i];
dtVset(ag->opts, 0,0,0);
dtVset(ag->opte, 0,0,0);
if (ag->npath && ag->ncorners > 1) if (ag->npath && ag->ncorners > 1)
{ {
// The target is the corner after the next corner to steer to. // The target is the corner after the next corner to steer to.
const float* tgt = &ag->corners[3]; float tgt[3];
dtVcopy(tgt, &ag->corners[3]);
const float distSqr = dtVdist2DSqr(ag->pos, tgt); const float distSqr = dtVdist2DSqr(ag->pos, tgt);
if (distSqr > dtSqr(0.01f)) // && distSqr < dtSqr(20.0f)) if (distSqr > dtSqr(0.01f))
{ {
// Clamp teh ray to max distance.
const float maxDist = ag->colradius*3;
if (distSqr > dtSqr(maxDist))
{
float delta[3];
dtVsub(delta, tgt, ag->pos);
dtVmad(tgt, ag->pos, delta, dtSqr(maxDist)/distSqr);
}
dtVcopy(ag->opts, ag->pos);
dtVcopy(ag->opte, tgt);
static const int MAX_RES = 32; static const int MAX_RES = 32;
dtPolyRef res[MAX_RES]; dtPolyRef res[MAX_RES];
float t, norm[3]; float t, norm[3];
@ -986,6 +964,9 @@ void CrowdManager::update(const float dt, unsigned int flags, dtNavMeshQuery* na
} }
} }
rcTimeVal endTime = getPerfTime();
m_totalTime.addSample(getPerfDeltaTimeUsec(startTime, endTime) / 1000.0f);
} }
@ -1000,6 +981,7 @@ CrowdTool::CrowdTool() :
m_showCollisionSegments(false), m_showCollisionSegments(false),
m_showPath(false), m_showPath(false),
m_showVO(false), m_showVO(false),
m_showOpt(false),
m_expandOptions(true), m_expandOptions(true),
m_anticipateTurns(true), m_anticipateTurns(true),
m_useVO(true), m_useVO(true),
@ -1079,6 +1061,8 @@ void CrowdTool::handleMenu()
m_showPath = !m_showPath; m_showPath = !m_showPath;
if (imguiCheck("Show VO", m_showVO)) if (imguiCheck("Show VO", m_showVO))
m_showVO = !m_showVO; m_showVO = !m_showVO;
if (imguiCheck("Show Opt", m_showOpt))
m_showOpt = !m_showOpt;
imguiUnindent(); imguiUnindent();
} }
} }
@ -1276,10 +1260,18 @@ void CrowdTool::handleRender()
dd.end(); dd.end();
} }
if (m_showOpt)
{
dd.begin(DU_DRAW_LINES, 2.0f);
dd.vertex(ag->opts[0],ag->opts[1]+0.3f,ag->opts[2], duRGBA(0,128,0,192));
dd.vertex(ag->opte[0],ag->opte[1]+0.3f,ag->opte[2], duRGBA(0,128,0,192));
dd.end();
}
if (m_showVO) if (m_showVO)
{ {
// Draw detail about agent sela // Draw detail about agent sela
const RVO* rvo = &ag->rvo; const RVODebugData* rvo = &ag->rvo;
const float dx = ag->pos[0]; const float dx = ag->pos[0];
const float dy = ag->pos[1]+ag->height; const float dy = ag->pos[1]+ag->height;
@ -1316,6 +1308,69 @@ void CrowdTool::handleRender()
} }
} }
static void drawGraphBackground(const int x, const int y, const int w, const int h,
const float vmin, const float vmax, const int ndiv,
const char* units)
{
// BG
DebugDrawGL dd;
dd.begin(DU_DRAW_QUADS);
dd.vertex(x,y,0, duRGBA(0,0,0,64));
dd.vertex(x+w,y,0, duRGBA(0,0,0,96));
dd.vertex(x+w,y+h,0, duRGBA(128,128,128,64));
dd.vertex(x,y+h,0, duRGBA(128,128,128,64));
dd.end();
const int pad = 10;
const float sy = (h-pad*2) / (vmax-vmin);
const float oy = y+pad-vmin*sy;
char text[64];
// Divider Lines
for (int i = 0; i < ndiv; ++i)
{
const float u = (float)i/(float)ndiv;
const float v = vmin + (vmax-vmin)*u;
snprintf(text, 64, "%.2f %s", v, units);
const float fy = oy + v*sy;
imguiDrawText(x+w-pad, (int)fy-4, IMGUI_ALIGN_RIGHT, text, imguiRGBA(0,0,0,255));
dd.begin(DU_DRAW_LINES, 1.0f);
dd.vertex(x,fy,0,duRGBA(0,0,0,64));
dd.vertex(x+w-pad-60,fy,0,duRGBA(0,0,0,64));
dd.end();
}
}
static void drawGraph(const SampleGraph* graph,
const int x, const int y, const int w, const int h,
const float vmin, const float vmax, const unsigned int col)
{
DebugDrawGL dd;
const int pad = 10;
const float sx = (w-pad*2) / (float)graph->getSampleCount();
const float sy = (h-pad*2) / (vmax-vmin);
const float ox = x+pad;
const float oy = y+pad-vmin*sy;
// Values
dd.begin(DU_DRAW_LINES, 2.0f);
for (int i = 0; i < graph->getSampleCount()-1; ++i)
{
const float fx0 = ox + i*sx;
const float fy0 = oy + graph->getSample(i)*sy;
const float fx1 = ox + (i+1)*sx;
const float fy1 = oy + graph->getSample(i+1)*sy;
dd.vertex(fx0,fy0,0,col);
dd.vertex(fx1,fy1,0,col);
}
dd.end();
}
void CrowdTool::handleRenderOverlay(double* proj, double* model, int* view) void CrowdTool::handleRenderOverlay(double* proj, double* model, int* view)
{ {
GLdouble x, y, z; GLdouble x, y, z;
@ -1344,4 +1399,11 @@ void CrowdTool::handleRenderOverlay(double* proj, double* model, int* view)
} }
} }
const int gx = 300, gy = 10, gw = 500, gh = 200;
const float vmin = 0.0f, vmax = 10.0f;
drawGraphBackground(gx,gy,gw,gh, vmin,vmax, 4, "ms");
drawGraph(m_crowd.getRVOTimeGraph(), gx,gy,gw,gh, vmin,vmax, duRGBA(255,0,0,128));
drawGraph(m_crowd.getTotalTimeGraph(), gx,gy,gw,gh, vmin,vmax, duRGBA(192,255,0,192));
} }

43
RecastDemo/Source/SampleInterfaces.cpp
View File

@ -174,6 +174,49 @@ int BuildContext::getBuildTime(const rcBuilTimeLabel label)
//////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////
#if defined(WIN32)
// Win32
#include <windows.h>
rcTimeVal getPerfTime()
{
__int64 count;
QueryPerformanceCounter((LARGE_INTEGER*)&count);
return count;
}
int getPerfDeltaTimeUsec(const rcTimeVal start, const rcTimeVal end)
{
static __int64 freq = 0;
if (freq == 0)
QueryPerformanceFrequency((LARGE_INTEGER*)&freq);
__int64 elapsed = end - start;
return (int)(elapsed*1000000 / freq);
}
#else
// Linux, BSD, OSX
#include <sys/time.h>
rcTimeVal getPerfTime()
{
timeval now;
gettimeofday(&now, 0);
return (rcTimeVal)now.tv_sec*1000000L + (rcTimeVal)now.tv_usec;
}
int getPerfDeltaTimeUsec(const rcTimeVal start, const rcTimeVal end)
{
return (int)(end - start);
}
#endif
////////////////////////////////////////////////////////////////////////////////////////////////////
void DebugDrawGL::depthMask(bool state) void DebugDrawGL::depthMask(bool state)
{ {
glDepthMask(state ? GL_TRUE : GL_FALSE); glDepthMask(state ? GL_TRUE : GL_FALSE);