mirror of
https://github.com/recastnavigation/recastnavigation.git
synced 2026-11-18 13:56:00 +00:00
Refactoring Mover. Moved path query handling to CrowdManager. Made mover a class and made member vars hidden.
This commit is contained in:
Binary file not shown.
File diff suppressed because it is too large
Load Diff
@@ -284,14 +284,13 @@
|
||||
<key>PBXSmartGroupTreeModuleOutlineStateSelectionKey</key>
|
||||
<array>
|
||||
<array>
|
||||
<integer>10</integer>
|
||||
<integer>2</integer>
|
||||
<integer>62</integer>
|
||||
<integer>1</integer>
|
||||
<integer>0</integer>
|
||||
</array>
|
||||
</array>
|
||||
<key>PBXSmartGroupTreeModuleOutlineStateVisibleRectKey</key>
|
||||
<string>{{0, 14}, {264, 607}}</string>
|
||||
<string>{{0, 603}, {264, 660}}</string>
|
||||
</dict>
|
||||
<key>PBXTopSmartGroupGIDs</key>
|
||||
<array/>
|
||||
@@ -301,14 +300,14 @@
|
||||
<key>GeometryConfiguration</key>
|
||||
<dict>
|
||||
<key>Frame</key>
|
||||
<string>{{0, 0}, {281, 625}}</string>
|
||||
<string>{{0, 0}, {281, 678}}</string>
|
||||
<key>GroupTreeTableConfiguration</key>
|
||||
<array>
|
||||
<string>MainColumn</string>
|
||||
<real>264</real>
|
||||
</array>
|
||||
<key>RubberWindowFrame</key>
|
||||
<string>0 112 1280 666 0 0 1280 778 </string>
|
||||
<string>0 59 1278 719 0 0 1280 778 </string>
|
||||
</dict>
|
||||
<key>Module</key>
|
||||
<string>PBXSmartGroupTreeModule</string>
|
||||
@@ -326,7 +325,7 @@
|
||||
<key>PBXProjectModuleGUID</key>
|
||||
<string>6B8632A30F78115100E2684A</string>
|
||||
<key>PBXProjectModuleLabel</key>
|
||||
<string>NavMeshTesterTool.cpp</string>
|
||||
<string>CrowdManager.h</string>
|
||||
<key>PBXSplitModuleInNavigatorKey</key>
|
||||
<dict>
|
||||
<key>Split0</key>
|
||||
@@ -334,11 +333,11 @@
|
||||
<key>PBXProjectModuleGUID</key>
|
||||
<string>6B8632A40F78115100E2684A</string>
|
||||
<key>PBXProjectModuleLabel</key>
|
||||
<string>NavMeshTesterTool.cpp</string>
|
||||
<string>CrowdManager.h</string>
|
||||
<key>_historyCapacity</key>
|
||||
<integer>0</integer>
|
||||
<key>bookmark</key>
|
||||
<string>6BA8CF7E1255D5FE00272A3B</string>
|
||||
<string>6BA8D06712560BD400272A3B</string>
|
||||
<key>history</key>
|
||||
<array>
|
||||
<string>6BBB4AA5115B4F3400CF791D</string>
|
||||
@@ -415,21 +414,21 @@
|
||||
<string>6BA8CECB1255C1A400272A3B</string>
|
||||
<string>6BA8CECD1255C1A400272A3B</string>
|
||||
<string>6BA8CEEF1255C4B700272A3B</string>
|
||||
<string>6BA8CF221255CCD500272A3B</string>
|
||||
<string>6BA8CF411255D16D00272A3B</string>
|
||||
<string>6BA8CF4A1255D44700272A3B</string>
|
||||
<string>6BA8CF4B1255D44700272A3B</string>
|
||||
<string>6BA8CF4C1255D44700272A3B</string>
|
||||
<string>6BA8CF4D1255D44700272A3B</string>
|
||||
<string>6BA8CF501255D44700272A3B</string>
|
||||
<string>6BA8CF511255D44700272A3B</string>
|
||||
<string>6BA8CF591255D49B00272A3B</string>
|
||||
<string>6BA8CF5B1255D49B00272A3B</string>
|
||||
<string>6BA8CF6F1255D50700272A3B</string>
|
||||
<string>6BA8CF7A1255D5FE00272A3B</string>
|
||||
<string>6BA8CF7B1255D5FE00272A3B</string>
|
||||
<string>6BA8CF7C1255D5FE00272A3B</string>
|
||||
<string>6BA8CF7D1255D5FE00272A3B</string>
|
||||
<string>6BA8CF941255D97400272A3B</string>
|
||||
<string>6BA8CF951255D97400272A3B</string>
|
||||
<string>6BA8CFA81255DC6500272A3B</string>
|
||||
<string>6BA8CFBE1255DE0500272A3B</string>
|
||||
<string>6BA8D05D1256059E00272A3B</string>
|
||||
<string>6BA8D0641256095800272A3B</string>
|
||||
<string>6BA8D0651256095800272A3B</string>
|
||||
</array>
|
||||
</dict>
|
||||
<key>SplitCount</key>
|
||||
@@ -443,18 +442,18 @@
|
||||
<key>GeometryConfiguration</key>
|
||||
<dict>
|
||||
<key>Frame</key>
|
||||
<string>{{0, 0}, {994, 408}}</string>
|
||||
<string>{{0, 0}, {992, 597}}</string>
|
||||
<key>RubberWindowFrame</key>
|
||||
<string>0 112 1280 666 0 0 1280 778 </string>
|
||||
<string>0 59 1278 719 0 0 1280 778 </string>
|
||||
</dict>
|
||||
<key>Module</key>
|
||||
<string>PBXNavigatorGroup</string>
|
||||
<key>Proportion</key>
|
||||
<string>408pt</string>
|
||||
<string>597pt</string>
|
||||
</dict>
|
||||
<dict>
|
||||
<key>Proportion</key>
|
||||
<string>212pt</string>
|
||||
<string>76pt</string>
|
||||
<key>Tabs</key>
|
||||
<array>
|
||||
<dict>
|
||||
@@ -484,9 +483,9 @@
|
||||
<key>GeometryConfiguration</key>
|
||||
<dict>
|
||||
<key>Frame</key>
|
||||
<string>{{10, 27}, {994, 185}}</string>
|
||||
<string>{{10, 27}, {992, 49}}</string>
|
||||
<key>RubberWindowFrame</key>
|
||||
<string>0 112 1280 666 0 0 1280 778 </string>
|
||||
<string>0 59 1278 719 0 0 1280 778 </string>
|
||||
</dict>
|
||||
<key>Module</key>
|
||||
<string>PBXProjectFindModule</string>
|
||||
@@ -524,7 +523,7 @@
|
||||
<key>GeometryConfiguration</key>
|
||||
<dict>
|
||||
<key>Frame</key>
|
||||
<string>{{10, 27}, {994, 77}}</string>
|
||||
<string>{{10, 27}, {992, 49}}</string>
|
||||
</dict>
|
||||
<key>Module</key>
|
||||
<string>PBXBuildResultsModule</string>
|
||||
@@ -533,7 +532,7 @@
|
||||
</dict>
|
||||
</array>
|
||||
<key>Proportion</key>
|
||||
<string>994pt</string>
|
||||
<string>992pt</string>
|
||||
</dict>
|
||||
</array>
|
||||
<key>Name</key>
|
||||
@@ -743,13 +742,12 @@
|
||||
<integer>5</integer>
|
||||
<key>WindowOrderList</key>
|
||||
<array>
|
||||
<string>6BA8CF7F1255D5FE00272A3B</string>
|
||||
<string>6BA8CF1C1255CC4F00272A3B</string>
|
||||
<string>6BA8CF1D1255CC4F00272A3B</string>
|
||||
<string>/Users/memon/Code/recastnavigation/RecastDemo/Build/Xcode/Recast.xcodeproj</string>
|
||||
</array>
|
||||
<key>WindowString</key>
|
||||
<string>0 112 1280 666 0 0 1280 778 </string>
|
||||
<string>0 59 1278 719 0 0 1280 778 </string>
|
||||
<key>WindowToolsV3</key>
|
||||
<array>
|
||||
<dict>
|
||||
|
||||
@@ -24,16 +24,6 @@
|
||||
#include "ValueHistory.h"
|
||||
|
||||
|
||||
/*enum AgentTargetState
|
||||
{
|
||||
AGENT_TARGET_NONE = 0,
|
||||
AGENT_TARGET_SET = 1,
|
||||
AGENT_TARGET_ACQUIRED = 2,
|
||||
AGENT_TARGET_WAITING_FOR_PATH = 3,
|
||||
AGENT_TARGET_PATH = 4,
|
||||
AGENT_TARGET_FAILED = 5,
|
||||
};*/
|
||||
|
||||
class ProximityGrid
|
||||
{
|
||||
int m_maxItems;
|
||||
@@ -129,23 +119,11 @@ public:
|
||||
int getPathResult(PathQueueRef ref, dtPolyRef* path, const int maxPath);
|
||||
};
|
||||
|
||||
enum MoverState
|
||||
{
|
||||
MOVER_INIT,
|
||||
MOVER_OK,
|
||||
MOVER_FAILED,
|
||||
};
|
||||
// TODO: Remove dynamics stuff from mover (velocity, new pos).
|
||||
// Mover should just store current location, target location and
|
||||
// path corridor, corners and collision segments.
|
||||
|
||||
enum MoverTargetState
|
||||
{
|
||||
MOVER_TARGET_NONE,
|
||||
MOVER_TARGET_REQUESTING,
|
||||
MOVER_TARGET_WAITING_FOR_PATH,
|
||||
MOVER_TARGET_VALID,
|
||||
MOVER_TARGET_FAILED,
|
||||
};
|
||||
|
||||
struct Mover
|
||||
class Mover
|
||||
{
|
||||
float m_pos[3];
|
||||
float m_target[3];
|
||||
@@ -154,24 +132,16 @@ struct Mover
|
||||
float m_dvel[3];
|
||||
float m_nvel[3];
|
||||
float m_vel[3];
|
||||
|
||||
float m_npos[3];
|
||||
float m_disp[3];
|
||||
|
||||
float m_pathOptimizationRange;
|
||||
|
||||
float m_colradius;
|
||||
float m_collisionQueryRange;
|
||||
|
||||
float m_localCenter[3];
|
||||
float m_localSegs[AGENT_MAX_LOCALSEGS*6];
|
||||
int m_localSegCount;
|
||||
|
||||
unsigned char m_state;
|
||||
|
||||
float m_reqTarget[3];
|
||||
unsigned char m_reqTargetState;
|
||||
dtPolyRef m_reqTargetRef;
|
||||
|
||||
PathQueueRef m_pathReqRef;
|
||||
dtPolyRef m_path[AGENT_MAX_PATH];
|
||||
int m_npath;
|
||||
|
||||
@@ -180,38 +150,45 @@ struct Mover
|
||||
dtPolyRef m_cornerPolys[AGENT_MAX_CORNERS];
|
||||
int m_ncorners;
|
||||
|
||||
void init(const float* p, const float r, const float h, const float cr, const float por);
|
||||
public:
|
||||
Mover();
|
||||
~Mover();
|
||||
|
||||
void requestMoveTarget(const float* pos);
|
||||
void init(dtPolyRef ref, const float* pos, const float radius, const float height,
|
||||
const float collisionQueryRange, const float pathOptimizationRange);
|
||||
|
||||
void updatePathState(dtNavMeshQuery* navquery, const dtQueryFilter* filter, const float* ext,
|
||||
PathQueue* pathq);
|
||||
|
||||
void updateLocalNeighbourhood(dtNavMeshQuery* navquery, const dtQueryFilter* filter);
|
||||
|
||||
void updateCorners(dtNavMeshQuery* navquery, const dtQueryFilter* filter, float* opts = 0, float* opte = 0);
|
||||
|
||||
void integrate(const float maxAcc, const float dt);
|
||||
|
||||
void updateLocation(dtNavMeshQuery* navquery, const dtQueryFilter* filter);
|
||||
|
||||
void updatePosition(dtNavMeshQuery* navquery, const dtQueryFilter* filter);
|
||||
float getDistanceToGoal(const float range) const;
|
||||
|
||||
void calcSmoothSteerDirection(float* dvel);
|
||||
void calcStraightSteerDirection(float* dvel);
|
||||
|
||||
void appendLocalCollisionSegments(dtObstacleAvoidanceQuery* obstacleQuery);
|
||||
|
||||
void getBounds(float* bmin, float* bmax) const
|
||||
{
|
||||
bmin[0] = m_pos[0] - m_radius;
|
||||
bmin[1] = m_pos[1];
|
||||
bmin[2] = m_pos[2] - m_radius;
|
||||
bmax[0] = m_pos[0] + m_radius;
|
||||
bmax[1] = m_pos[1] + m_height;
|
||||
bmax[2] = m_pos[2] + m_radius;
|
||||
}
|
||||
|
||||
void setDesiredVelocity(const float* dvel);
|
||||
void setNewVelocity(const float* nvel);
|
||||
void setNewPos(const float* npos);
|
||||
|
||||
inline int getCornerCount() const { return m_ncorners; }
|
||||
inline const float* getCornerPos(int i) const { return &m_cornerVerts[i*3]; }
|
||||
|
||||
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]; }
|
||||
|
||||
inline const float* getPos() const { return m_pos; }
|
||||
inline const float* getNewPos() const { return m_npos; }
|
||||
inline const float* getVelocity() const { return m_vel; }
|
||||
inline const float* getDesiredVelocity() const { return m_dvel; }
|
||||
inline float getRadius() const { return m_radius; }
|
||||
inline float getHeight() const { return m_height; }
|
||||
inline float getCollisionQueryRange() const { return m_collisionQueryRange; }
|
||||
|
||||
inline void setCorridor(const float* target, const dtPolyRef* path, int npath);
|
||||
inline const float* getCorridorTarget() const { return m_target; }
|
||||
inline const dtPolyRef* getCorridor() const { return m_path; }
|
||||
inline int getCorridorCount() const { return m_npath; }
|
||||
};
|
||||
|
||||
struct Agent
|
||||
@@ -225,6 +202,7 @@ struct Agent
|
||||
float var;
|
||||
|
||||
float opts[3], opte[3];
|
||||
float disp[3];
|
||||
|
||||
float trail[AGENT_MAX_TRAIL*3];
|
||||
int htrail;
|
||||
@@ -248,9 +226,31 @@ class CrowdManager
|
||||
PathQueue m_pathq;
|
||||
ProximityGrid m_grid;
|
||||
|
||||
float m_ext[3];
|
||||
dtQueryFilter m_filter;
|
||||
|
||||
int m_totalTime;
|
||||
int m_rvoTime;
|
||||
int m_sampleCount;
|
||||
|
||||
enum MoveRequestState
|
||||
{
|
||||
MR_TARGET_REQUESTING,
|
||||
MR_TARGET_WAITING_FOR_PATH,
|
||||
MR_TARGET_VALID,
|
||||
MR_TARGET_FAILED,
|
||||
};
|
||||
|
||||
struct MoveRequest
|
||||
{
|
||||
int idx;
|
||||
dtPolyRef ref;
|
||||
float pos[3];
|
||||
unsigned char state;
|
||||
PathQueueRef pathqRef;
|
||||
};
|
||||
MoveRequest m_moveRequests[MAX_AGENTS];
|
||||
int m_moveRequestCount;
|
||||
|
||||
int getNeighbours(const float* pos, const float height, const float range,
|
||||
const Agent* skip, Agent** result, const int maxResult);
|
||||
@@ -262,14 +262,17 @@ public:
|
||||
void reset();
|
||||
const Agent* getAgent(const int idx);
|
||||
const int getAgentCount() const;
|
||||
int addAgent(const float* pos, const float radius, const float height);
|
||||
int addAgent(const float* pos, const float radius, const float height, dtNavMeshQuery* navquery);
|
||||
void removeAgent(const int idx);
|
||||
void requestMoveTarget(const int idx, const float* pos);
|
||||
bool requestMoveTarget(const int idx, dtPolyRef ref, const float* pos);
|
||||
|
||||
int getActiveAgents(Agent** agents, const int maxAgents);
|
||||
|
||||
void update(const float dt, unsigned int flags, dtNavMeshQuery* navquery);
|
||||
|
||||
const dtQueryFilter* getFilter() const { return &m_filter; }
|
||||
const float* getQueryExtents() const { return m_ext; }
|
||||
|
||||
const dtObstacleAvoidanceDebugData* getVODebugData(const int idx) const { return m_vodebug[idx]; }
|
||||
inline int getTotalTime() const { return m_totalTime; }
|
||||
inline int getRVOTime() const { return m_rvoTime; }
|
||||
|
||||
@@ -27,40 +27,13 @@
|
||||
|
||||
// Tool to create crowds.
|
||||
|
||||
struct Isect
|
||||
{
|
||||
float u;
|
||||
int inside;
|
||||
};
|
||||
|
||||
static const int FORM_MAX_ISECT = 32;
|
||||
static const int FORM_MAX_SEGS = 16;
|
||||
static const int FORM_MAX_POLYS = 32;
|
||||
|
||||
struct FormationSeg
|
||||
{
|
||||
float p[3], q[3];
|
||||
Isect ints[FORM_MAX_ISECT];
|
||||
int nints;
|
||||
};
|
||||
|
||||
struct Formation
|
||||
{
|
||||
FormationSeg segs[FORM_MAX_SEGS];
|
||||
int nsegs;
|
||||
dtPolyRef polys[FORM_MAX_POLYS];
|
||||
int npolys;
|
||||
};
|
||||
|
||||
class CrowdTool : public SampleTool
|
||||
{
|
||||
Sample* m_sample;
|
||||
unsigned char m_oldFlags;
|
||||
|
||||
float m_targetPos[3];
|
||||
bool m_targetPosSet;
|
||||
|
||||
Formation m_form;
|
||||
dtPolyRef m_targetRef;
|
||||
|
||||
bool m_expandDebugDraw;
|
||||
bool m_showLabels;
|
||||
|
||||
+257
-201
@@ -465,179 +465,55 @@ static int optimizePath(const float* pos, const float* next, const float maxLook
|
||||
return npath;
|
||||
}
|
||||
|
||||
void Mover::init(const float* p, const float r, const float h, const float cr, const float por)
|
||||
|
||||
Mover::Mover()
|
||||
{
|
||||
dtVcopy(m_pos, p);
|
||||
dtVcopy(m_target, p);
|
||||
m_radius = r;
|
||||
m_height = h;
|
||||
}
|
||||
|
||||
Mover::~Mover()
|
||||
{
|
||||
}
|
||||
|
||||
void Mover::init(dtPolyRef ref, const float* pos, const float radius, const float height,
|
||||
const float collisionQueryRange, const float pathOptimizationRange)
|
||||
{
|
||||
dtVcopy(m_pos, pos);
|
||||
dtVcopy(m_target, pos);
|
||||
m_radius = radius;
|
||||
m_height = height;
|
||||
|
||||
m_path[0] = ref;
|
||||
m_npath = 1;
|
||||
|
||||
dtVset(m_dvel, 0,0,0);
|
||||
dtVset(m_nvel, 0,0,0);
|
||||
dtVset(m_vel, 0,0,0);
|
||||
dtVset(m_npos, 0,0,0);
|
||||
dtVset(m_disp, 0,0,0);
|
||||
|
||||
m_pathOptimizationRange = por;
|
||||
m_colradius = cr;
|
||||
m_pathOptimizationRange = pathOptimizationRange;
|
||||
m_collisionQueryRange = collisionQueryRange;
|
||||
|
||||
dtVset(m_localCenter, 0,0,0);
|
||||
m_localSegCount = 0;
|
||||
|
||||
m_state = MOVER_INIT;
|
||||
|
||||
m_reqTargetState = MOVER_TARGET_NONE;
|
||||
dtVset(m_reqTarget, 0,0,0);
|
||||
m_reqTargetRef = 0;
|
||||
|
||||
m_pathReqRef = PATHQ_INVALID;
|
||||
m_npath = 0;
|
||||
|
||||
m_ncorners = 0;
|
||||
}
|
||||
|
||||
void Mover::requestMoveTarget(const float* pos)
|
||||
{
|
||||
dtVcopy(m_reqTarget, pos);
|
||||
m_reqTargetRef = 0;
|
||||
m_reqTargetState = MOVER_TARGET_REQUESTING;
|
||||
m_pathReqRef = PATHQ_INVALID;
|
||||
}
|
||||
|
||||
void Mover::updatePathState(dtNavMeshQuery* navquery, const dtQueryFilter* filter, const float* ext,
|
||||
PathQueue* pathq)
|
||||
{
|
||||
// Make sure that the first path polygon corresponds to the current agent location.
|
||||
if (m_state == MOVER_INIT)
|
||||
{
|
||||
float nearest[3];
|
||||
dtPolyRef ref = navquery->findNearestPoly(m_pos, ext, filter, nearest);
|
||||
if (ref)
|
||||
{
|
||||
m_path[0] = ref;
|
||||
m_npath = 1;
|
||||
dtVcopy(m_pos, nearest);
|
||||
dtVcopy(m_target, nearest);
|
||||
m_state = MOVER_OK;
|
||||
}
|
||||
else
|
||||
{
|
||||
m_state = MOVER_FAILED;
|
||||
}
|
||||
}
|
||||
|
||||
if (m_state == MOVER_FAILED)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
if (m_reqTargetState == MOVER_TARGET_REQUESTING)
|
||||
{
|
||||
float nearest[3];
|
||||
m_reqTargetRef = navquery->findNearestPoly(m_reqTarget, ext, filter, nearest);
|
||||
if (m_reqTargetRef)
|
||||
{
|
||||
// Calculate request position.
|
||||
// If there is a lot of latency between requests, it is possible to
|
||||
// project the current position ahead and use raycast to find the actual
|
||||
// location and path.
|
||||
// Here we take the simple route and set the path to be just the current location.
|
||||
float reqPos[3];
|
||||
dtVcopy(reqPos, m_pos); // The location of the request
|
||||
dtPolyRef reqPath[8]; // The path to the request location
|
||||
reqPath[0] = m_path[0];
|
||||
int reqPathCount = 1;
|
||||
|
||||
dtVcopy(m_reqTarget, nearest);
|
||||
m_pathReqRef = pathq->request(reqPath[reqPathCount-1], m_reqTargetRef, reqPos, m_reqTarget, filter);
|
||||
if (m_pathReqRef != PATHQ_INVALID)
|
||||
{
|
||||
dtVcopy(m_target, reqPos);
|
||||
memcpy(m_path, reqPath, sizeof(dtPolyRef)*reqPathCount);
|
||||
m_npath = reqPathCount;
|
||||
|
||||
m_reqTargetState = MOVER_TARGET_WAITING_FOR_PATH;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
m_reqTargetState = MOVER_TARGET_FAILED;
|
||||
}
|
||||
}
|
||||
|
||||
if (m_reqTargetState == MOVER_TARGET_WAITING_FOR_PATH)
|
||||
{
|
||||
// Poll path queue.
|
||||
int state = pathq->getRequestState(m_pathReqRef);
|
||||
if (state == PATHQ_STATE_INVALID)
|
||||
{
|
||||
m_pathReqRef = PATHQ_INVALID;
|
||||
m_reqTargetState = MOVER_TARGET_FAILED;
|
||||
}
|
||||
else if (state == PATHQ_STATE_READY)
|
||||
{
|
||||
dtAssert(m_npath);
|
||||
|
||||
// Merge new results and current path.
|
||||
dtPolyRef res[AGENT_MAX_PATH];
|
||||
int nres = 0;
|
||||
nres = pathq->getPathResult(m_pathReqRef, res, AGENT_MAX_PATH);
|
||||
|
||||
if (!nres)
|
||||
{
|
||||
m_reqTargetState = MOVER_TARGET_FAILED;
|
||||
}
|
||||
else
|
||||
{
|
||||
// The last ref in the old path should be the same as the first ref of new path.
|
||||
if (m_path[m_npath-1] == res[0])
|
||||
{
|
||||
// Append new path
|
||||
if (m_npath-1+nres > AGENT_MAX_PATH)
|
||||
nres = AGENT_MAX_PATH-(m_npath-1);
|
||||
memcpy(&m_path[m_npath-1], res, sizeof(dtPolyRef)*nres);
|
||||
m_npath = m_npath-1 + nres;
|
||||
|
||||
dtVcopy(m_target, m_reqTarget);
|
||||
|
||||
// Check for partial path.
|
||||
if (m_path[m_npath-1] != m_reqTargetRef)
|
||||
{
|
||||
// Partial path, constrain target position inside the last polygon.
|
||||
float nearest[3];
|
||||
if (navquery->closestPointOnPoly(m_path[m_npath-1], m_target, nearest))
|
||||
dtVcopy(m_target, nearest);
|
||||
else
|
||||
m_reqTargetState = MOVER_TARGET_FAILED;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// Something went out of sync.
|
||||
m_reqTargetState = MOVER_TARGET_FAILED;
|
||||
}
|
||||
}
|
||||
|
||||
m_pathReqRef = PATHQ_INVALID;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void Mover::updateLocalNeighbourhood(dtNavMeshQuery* navquery, const dtQueryFilter* filter)
|
||||
{
|
||||
if (!m_npath)
|
||||
return;
|
||||
|
||||
// Only update the neigbourhood after certain distance has been passed.
|
||||
if (dtVdist2DSqr(m_pos, m_localCenter) < dtSqr(m_colradius*0.25f))
|
||||
if (dtVdist2DSqr(m_pos, m_localCenter) < dtSqr(m_collisionQueryRange*0.25f))
|
||||
return;
|
||||
|
||||
dtVcopy(m_localCenter, m_pos);
|
||||
static const int MAX_LOCALS = 32;
|
||||
dtPolyRef locals[MAX_LOCALS];
|
||||
|
||||
const int nlocals = navquery->findLocalNeighbourhood(m_path[0], m_pos, m_colradius, filter, locals, 0, MAX_LOCALS);
|
||||
const int nlocals = navquery->findLocalNeighbourhood(m_path[0], m_pos, m_collisionQueryRange,
|
||||
filter, locals, 0, MAX_LOCALS);
|
||||
|
||||
m_localSegCount = 0;
|
||||
for (int j = 0; j < nlocals; ++j)
|
||||
@@ -650,7 +526,7 @@ void Mover::updateLocalNeighbourhood(dtNavMeshQuery* navquery, const dtQueryFilt
|
||||
// Skip too distant segments.
|
||||
float tseg;
|
||||
const float distSqr = dtDistancePtSegSqr2D(m_pos, s, s+3, tseg);
|
||||
if (distSqr > dtSqr(m_colradius))
|
||||
if (distSqr > dtSqr(m_collisionQueryRange))
|
||||
continue;
|
||||
if (m_localSegCount < AGENT_MAX_LOCALSEGS)
|
||||
{
|
||||
@@ -704,16 +580,16 @@ void Mover::updateCorners(dtNavMeshQuery* navquery, const dtQueryFilter* filter,
|
||||
}
|
||||
}
|
||||
|
||||
void Mover::updateLocation(dtNavMeshQuery* navquery, const dtQueryFilter* filter)
|
||||
void Mover::updatePosition(dtNavMeshQuery* navquery, const dtQueryFilter* filter)
|
||||
{
|
||||
if (!m_npath)
|
||||
return;
|
||||
dtAssert(m_npath);
|
||||
|
||||
// Move along navmesh and update new position.
|
||||
float result[3];
|
||||
dtPolyRef visited[16];
|
||||
static const int MAX_VISITED = 16;
|
||||
dtPolyRef visited[MAX_VISITED];
|
||||
int nvisited = navquery->moveAlongSurface(m_path[0], m_pos, m_npos, filter,
|
||||
result, visited, 16);
|
||||
result, visited, MAX_VISITED);
|
||||
m_npath = fixupCorridor(m_path, m_npath, AGENT_MAX_PATH, visited, nvisited);
|
||||
|
||||
// Adjust agent height to stay on top of the navmesh.
|
||||
@@ -800,14 +676,39 @@ void Mover::appendLocalCollisionSegments(dtObstacleAvoidanceQuery* obstacleQuery
|
||||
}
|
||||
}
|
||||
|
||||
void Mover::setNewPos(const float* npos)
|
||||
{
|
||||
dtVcopy(m_npos, npos);
|
||||
}
|
||||
|
||||
void Mover::setDesiredVelocity(const float* dvel)
|
||||
{
|
||||
dtVcopy(m_dvel, dvel);
|
||||
}
|
||||
|
||||
void Mover::setNewVelocity(const float* nvel)
|
||||
{
|
||||
dtVcopy(m_nvel, nvel);
|
||||
}
|
||||
|
||||
void Mover::setCorridor(const float* target, const dtPolyRef* path, int npath)
|
||||
{
|
||||
dtAssert(npath > 0);
|
||||
dtAssert(npath < AGENT_MAX_PATH);
|
||||
dtVcopy(m_target, target);
|
||||
memcpy(m_path, path, sizeof(dtPolyRef)*npath);
|
||||
m_npath = npath;
|
||||
}
|
||||
|
||||
|
||||
CrowdManager::CrowdManager() :
|
||||
m_obstacleQuery(0),
|
||||
m_totalTime(0),
|
||||
m_rvoTime(0),
|
||||
m_sampleCount(0)
|
||||
m_sampleCount(0),
|
||||
m_moveRequestCount(0)
|
||||
{
|
||||
dtVset(m_ext, 2,4,2);
|
||||
|
||||
m_obstacleQuery = dtAllocObstacleAvoidanceQuery();
|
||||
m_obstacleQuery->init(6, 10);
|
||||
@@ -856,7 +757,7 @@ const Agent* CrowdManager::getAgent(const int idx)
|
||||
return &m_agents[idx];
|
||||
}
|
||||
|
||||
int CrowdManager::addAgent(const float* pos, const float radius, const float height)
|
||||
int CrowdManager::addAgent(const float* pos, const float radius, const float height, dtNavMeshQuery* navquery)
|
||||
{
|
||||
// Find empty slot.
|
||||
int idx = -1;
|
||||
@@ -872,12 +773,20 @@ int CrowdManager::addAgent(const float* pos, const float radius, const float hei
|
||||
return -1;
|
||||
|
||||
Agent* ag = &m_agents[idx];
|
||||
// memset(ag, 0, sizeof(Agent));
|
||||
|
||||
// Find nearest position on navmesh and place the agent there.
|
||||
float nearest[3];
|
||||
dtPolyRef ref = navquery->findNearestPoly(pos, m_ext, &m_filter, nearest);
|
||||
if (!ref)
|
||||
{
|
||||
// Could not find a location on navmesh.
|
||||
return -1;
|
||||
}
|
||||
|
||||
const float colRadius = radius * 7.5f;
|
||||
const float pathOptRange = colRadius * 4;
|
||||
|
||||
ag->mover.init(pos, radius, height, colRadius, pathOptRange);
|
||||
ag->mover.init(ref, nearest, radius, height, colRadius, pathOptRange);
|
||||
|
||||
ag->maxspeed = 0;
|
||||
ag->t = 0;
|
||||
@@ -888,7 +797,7 @@ int CrowdManager::addAgent(const float* pos, const float radius, const float hei
|
||||
|
||||
// Init trail
|
||||
for (int i = 0; i < AGENT_MAX_TRAIL; ++i)
|
||||
dtVcopy(&ag->trail[i*3], ag->mover.m_pos);
|
||||
dtVcopy(&ag->trail[i*3], ag->mover.getPos());
|
||||
ag->htrail = 0;
|
||||
|
||||
return idx;
|
||||
@@ -900,10 +809,38 @@ void CrowdManager::removeAgent(const int idx)
|
||||
memset(&m_agents[idx], 0, sizeof(Agent));
|
||||
}
|
||||
|
||||
void CrowdManager::requestMoveTarget(const int idx, const float* pos)
|
||||
bool CrowdManager::requestMoveTarget(const int idx, dtPolyRef ref, const float* pos)
|
||||
{
|
||||
Agent* ag = &m_agents[idx];
|
||||
ag->mover.requestMoveTarget(pos);
|
||||
if (idx < 0 || idx > MAX_AGENTS)
|
||||
return false;
|
||||
if (!ref)
|
||||
return false;
|
||||
|
||||
MoveRequest* req = 0;
|
||||
// Check if there is existing request and update that instead.
|
||||
for (int i = 0; i < m_moveRequestCount; ++i)
|
||||
{
|
||||
if (m_moveRequests[i].idx == idx)
|
||||
{
|
||||
req = &m_moveRequests[i];
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (!req)
|
||||
{
|
||||
if (m_moveRequestCount >= MAX_AGENTS)
|
||||
return false;
|
||||
req = &m_moveRequests[m_moveRequestCount++];
|
||||
}
|
||||
|
||||
// Initialize request.
|
||||
req->idx = idx;
|
||||
req->ref = ref;
|
||||
dtVcopy(req->pos, pos);
|
||||
req->pathqRef = PATHQ_INVALID;
|
||||
req->state = MR_TARGET_REQUESTING;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
int CrowdManager::getActiveAgents(Agent** agents, const int maxAgents)
|
||||
@@ -935,8 +872,8 @@ int CrowdManager::getNeighbours(const float* pos, const float height, const floa
|
||||
if (ag == skip) continue;
|
||||
|
||||
float diff[3];
|
||||
dtVsub(diff, pos, ag->mover.m_pos);
|
||||
if (fabsf(diff[1]) >= (height+ag->mover.m_height)/2.0f)
|
||||
dtVsub(diff, pos, ag->mover.getPos());
|
||||
if (fabsf(diff[1]) >= (height+ag->mover.getHeight())/2.0f)
|
||||
continue;
|
||||
diff[1] = 0;
|
||||
const float distSqr = dtVlenSqr(diff);
|
||||
@@ -960,9 +897,6 @@ void CrowdManager::update(const float dt, unsigned int flags, dtNavMeshQuery* na
|
||||
|
||||
TimeVal startTime = getPerfTime();
|
||||
|
||||
const float ext[3] = {2,4,2};
|
||||
dtQueryFilter filter;
|
||||
|
||||
Agent* agents[MAX_AGENTS];
|
||||
Agent* neis[MAX_AGENTS];
|
||||
int nagents = getActiveAgents(agents, MAX_AGENTS);
|
||||
@@ -970,15 +904,134 @@ void CrowdManager::update(const float dt, unsigned int flags, dtNavMeshQuery* na
|
||||
static const float MAX_ACC = 8.0f;
|
||||
static const float MAX_SPEED = 3.5f;
|
||||
|
||||
// Update move requests.
|
||||
for (int i = 0; i < m_moveRequestCount; ++i)
|
||||
{
|
||||
MoveRequest* req = &m_moveRequests[i];
|
||||
Agent* ag = &m_agents[req->idx];
|
||||
|
||||
if (!ag->active)
|
||||
req->state = MR_TARGET_FAILED;
|
||||
|
||||
if (req->state == MR_TARGET_REQUESTING)
|
||||
{
|
||||
// Calculate request position.
|
||||
// If there is a lot of latency between requests, it is possible to
|
||||
// project the current position ahead and use raycast to find the actual
|
||||
// location and path.
|
||||
const dtPolyRef* cor = ag->mover.getCorridor();
|
||||
const int ncor = ag->mover.getCorridorCount();
|
||||
dtAssert(ncor);
|
||||
|
||||
// Here we take the simple approach and set the path to be just the current location.
|
||||
float reqPos[3];
|
||||
dtVcopy(reqPos, ag->mover.getPos()); // The location of the request
|
||||
dtPolyRef reqPath[8]; // The path to the request location
|
||||
reqPath[0] = cor[0];
|
||||
int reqPathCount = 1;
|
||||
|
||||
req->pathqRef = m_pathq.request(reqPath[reqPathCount-1], req->ref, reqPos, req->pos, &m_filter);
|
||||
if (req->pathqRef != PATHQ_INVALID)
|
||||
{
|
||||
ag->mover.setCorridor(reqPos, reqPath, reqPathCount);
|
||||
req->state = MR_TARGET_WAITING_FOR_PATH;
|
||||
}
|
||||
}
|
||||
else if (req->state == MR_TARGET_WAITING_FOR_PATH)
|
||||
{
|
||||
// Poll path queue.
|
||||
int state = m_pathq.getRequestState(req->pathqRef);
|
||||
if (state == PATHQ_STATE_INVALID)
|
||||
{
|
||||
req->pathqRef = PATHQ_INVALID;
|
||||
req->state = MR_TARGET_FAILED;
|
||||
}
|
||||
else if (state == PATHQ_STATE_READY)
|
||||
{
|
||||
const dtPolyRef* cor = ag->mover.getCorridor();
|
||||
const int ncor = ag->mover.getCorridorCount();
|
||||
dtAssert(ncor);
|
||||
|
||||
// Apply results.
|
||||
float targetPos[3];
|
||||
dtVcopy(targetPos, req->pos);
|
||||
|
||||
bool valid = true;
|
||||
dtPolyRef res[AGENT_MAX_PATH];
|
||||
int nres = m_pathq.getPathResult(req->pathqRef, res, AGENT_MAX_PATH);
|
||||
if (!nres)
|
||||
valid = false;
|
||||
|
||||
// Merge result and existing path.
|
||||
// The agent might have moved whilst the request is
|
||||
// being processed, so the path may have changed.
|
||||
// We assume that the end of the path is at the same location
|
||||
// where the request was issued.
|
||||
|
||||
// The last ref in the old path should be the same as
|
||||
// the location where the request was issued..
|
||||
if (valid && cor[ncor-1] != res[0])
|
||||
valid = false;
|
||||
|
||||
if (valid)
|
||||
{
|
||||
// Put the old path infront of the old path.
|
||||
if (ncor > 1)
|
||||
{
|
||||
// Make space for the old path.
|
||||
if ((ncor-1)+nres > AGENT_MAX_PATH)
|
||||
nres = AGENT_MAX_PATH - (ncor-1);
|
||||
memmove(res+ncor-1, res, sizeof(dtPolyRef)*nres);
|
||||
// Copy old path in the beginning.
|
||||
memcpy(res, cor, sizeof(dtPolyRef)*(ncor-1));
|
||||
nres += ncor-1;
|
||||
}
|
||||
|
||||
// Check for partial path.
|
||||
if (res[nres-1] != req->ref)
|
||||
{
|
||||
// Partial path, constrain target position inside the last polygon.
|
||||
float nearest[3];
|
||||
if (navquery->closestPointOnPoly(res[nres-1], targetPos, nearest))
|
||||
dtVcopy(targetPos, nearest);
|
||||
else
|
||||
valid = false;
|
||||
}
|
||||
}
|
||||
|
||||
if (valid)
|
||||
{
|
||||
ag->mover.setCorridor(targetPos, res, nres);
|
||||
req->state = MR_TARGET_FAILED;
|
||||
}
|
||||
else
|
||||
{
|
||||
// Something went wrong.
|
||||
req->state = MR_TARGET_FAILED;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Remove request.
|
||||
if (req->state == MR_TARGET_VALID || req->state == MR_TARGET_FAILED)
|
||||
{
|
||||
m_moveRequestCount--;
|
||||
if (i != m_moveRequestCount)
|
||||
memcpy(&m_moveRequests[i], &m_moveRequests[m_moveRequestCount], sizeof(MoveRequest));
|
||||
--i;
|
||||
}
|
||||
}
|
||||
|
||||
m_pathq.update(navquery);
|
||||
|
||||
|
||||
// Register agents to proximity grid.
|
||||
m_grid.clear();
|
||||
for (int i = 0; i < nagents; ++i)
|
||||
{
|
||||
Agent* ag = agents[i];
|
||||
const float* p = ag->mover.m_pos;
|
||||
const float r = ag->mover.m_radius;
|
||||
const float* p = ag->mover.getPos();
|
||||
const float r = ag->mover.getRadius();
|
||||
const float minx = p[0] - r;
|
||||
const float miny = p[2] - r;
|
||||
const float maxx = p[0] + r;
|
||||
@@ -987,26 +1040,18 @@ void CrowdManager::update(const float dt, unsigned int flags, dtNavMeshQuery* na
|
||||
}
|
||||
|
||||
|
||||
// Update target and agent navigation state.
|
||||
// TODO: add queue for path queries.
|
||||
for (int i = 0; i < nagents; ++i)
|
||||
{
|
||||
Agent* ag = agents[i];
|
||||
ag->mover.updatePathState(navquery, &filter, ext, &m_pathq);
|
||||
}
|
||||
|
||||
// Get nearby navmesh segments to collide with.
|
||||
for (int i = 0; i < nagents; ++i)
|
||||
{
|
||||
Agent* ag = agents[i];
|
||||
ag->mover.updateLocalNeighbourhood(navquery, &filter);
|
||||
ag->mover.updateLocalNeighbourhood(navquery, &m_filter);
|
||||
}
|
||||
|
||||
// Find next corner to steer to.
|
||||
for (int i = 0; i < nagents; ++i)
|
||||
{
|
||||
Agent* ag = agents[i];
|
||||
ag->mover.updateCorners(navquery, &filter, ag->opts, ag->opte);
|
||||
ag->mover.updateCorners(navquery, &m_filter, ag->opts, ag->opte);
|
||||
}
|
||||
|
||||
// Calculate steering.
|
||||
@@ -1025,7 +1070,7 @@ void CrowdManager::update(const float dt, unsigned int flags, dtNavMeshQuery* na
|
||||
// Calculate steering speed.
|
||||
|
||||
// Calculate speed scale, which tells the agent to slowdown at the end of the path.
|
||||
const float slowDownRadius = ag->mover.m_radius*2;
|
||||
const float slowDownRadius = ag->mover.getRadius()*2; // TODO: make less hacky.
|
||||
const float speedScale = ag->mover.getDistanceToGoal(slowDownRadius) / slowDownRadius;
|
||||
|
||||
// Apply style.
|
||||
@@ -1054,7 +1099,7 @@ void CrowdManager::update(const float dt, unsigned int flags, dtNavMeshQuery* na
|
||||
}
|
||||
|
||||
// Set the desired velocity.
|
||||
dtVcopy(ag->mover.m_dvel, dvel);
|
||||
ag->mover.setDesiredVelocity(dvel);
|
||||
}
|
||||
|
||||
// Velocity planning.
|
||||
@@ -1064,18 +1109,22 @@ void CrowdManager::update(const float dt, unsigned int flags, dtNavMeshQuery* na
|
||||
{
|
||||
Agent* ag = agents[i];
|
||||
|
||||
float nvel[3] = {0,0,0};
|
||||
|
||||
if (flags & CROWDMAN_USE_VO)
|
||||
{
|
||||
m_obstacleQuery->reset();
|
||||
|
||||
// Find neighbours and add them as obstacles.
|
||||
const int nneis = getNeighbours(ag->mover.m_pos, ag->mover.m_height, ag->mover.m_colradius,
|
||||
const int nneis = getNeighbours(ag->mover.getPos(), ag->mover.getHeight(),
|
||||
ag->mover.getCollisionQueryRange(),
|
||||
ag, neis, MAX_AGENTS);
|
||||
for (int j = 0; j < nneis; ++j)
|
||||
{
|
||||
const Agent* nei = neis[j];
|
||||
m_obstacleQuery->addCircle(nei->mover.m_pos, nei->mover.m_radius, nei->mover.m_vel, nei->mover.m_dvel,
|
||||
dtVdist2DSqr(ag->mover.m_pos, nei->mover.m_pos));
|
||||
m_obstacleQuery->addCircle(nei->mover.getPos(), nei->mover.getRadius(),
|
||||
nei->mover.getVelocity(), nei->mover.getDesiredVelocity(),
|
||||
dtVdist2DSqr(ag->mover.getPos(), nei->mover.getPos()));
|
||||
}
|
||||
|
||||
// Append neighbour segments as obstacles.
|
||||
@@ -1083,27 +1132,32 @@ void CrowdManager::update(const float dt, unsigned int flags, dtNavMeshQuery* na
|
||||
|
||||
|
||||
bool adaptive = true;
|
||||
|
||||
|
||||
if (adaptive)
|
||||
{
|
||||
m_obstacleQuery->setSamplingGridSize(VO_ADAPTIVE_GRID_SIZE);
|
||||
m_obstacleQuery->setSamplingGridDepth(VO_ADAPTIVE_GRID_DEPTH);
|
||||
m_obstacleQuery->sampleVelocityAdaptive(ag->mover.m_pos, ag->mover.m_radius, ag->maxspeed,
|
||||
ag->mover.m_vel, ag->mover.m_dvel, ag->mover.m_nvel,
|
||||
m_vodebug[i]);
|
||||
m_obstacleQuery->sampleVelocityAdaptive(ag->mover.getPos(), ag->mover.getRadius(), ag->maxspeed,
|
||||
ag->mover.getVelocity(), ag->mover.getDesiredVelocity(),
|
||||
nvel, m_vodebug[i]);
|
||||
}
|
||||
else
|
||||
{
|
||||
m_obstacleQuery->setSamplingGridSize(VO_GRID_SIZE);
|
||||
m_obstacleQuery->sampleVelocity(ag->mover.m_pos, ag->mover.m_radius, ag->maxspeed,
|
||||
ag->mover.m_vel, ag->mover.m_dvel, ag->mover.m_nvel,
|
||||
m_vodebug[i]);
|
||||
m_obstacleQuery->sampleVelocity(ag->mover.getPos(), ag->mover.getRadius(), ag->maxspeed,
|
||||
ag->mover.getVelocity(), ag->mover.getDesiredVelocity(),
|
||||
nvel, m_vodebug[i]);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
dtVcopy(ag->mover.m_nvel, ag->mover.m_dvel);
|
||||
// If not using velocity planning, new velocity is directly the desired velocity.
|
||||
dtVcopy(nvel, ag->mover.getDesiredVelocity());
|
||||
}
|
||||
|
||||
ag->mover.setNewVelocity(nvel);
|
||||
|
||||
|
||||
}
|
||||
TimeVal rvoEndTime = getPerfTime();
|
||||
|
||||
@@ -1121,7 +1175,7 @@ void CrowdManager::update(const float dt, unsigned int flags, dtNavMeshQuery* na
|
||||
{
|
||||
Agent* ag = agents[i];
|
||||
|
||||
dtVset(ag->mover.m_disp, 0,0,0);
|
||||
dtVset(ag->disp, 0,0,0);
|
||||
|
||||
float w = 0;
|
||||
|
||||
@@ -1131,22 +1185,22 @@ void CrowdManager::update(const float dt, unsigned int flags, dtNavMeshQuery* na
|
||||
Agent* nei = agents[j];
|
||||
|
||||
float diff[3];
|
||||
dtVsub(diff, ag->mover.m_npos, nei->mover.m_npos);
|
||||
dtVsub(diff, ag->mover.getNewPos(), nei->mover.getNewPos());
|
||||
|
||||
if (fabsf(diff[1]) >= (ag->mover.m_height+nei->mover.m_height)/2.0f)
|
||||
if (fabsf(diff[1]) >= (ag->mover.getHeight()+nei->mover.getHeight())/2.0f)
|
||||
continue;
|
||||
|
||||
diff[1] = 0;
|
||||
|
||||
float dist = dtVlenSqr(diff);
|
||||
if (dist > dtSqr(ag->mover.m_radius+nei->mover.m_radius))
|
||||
if (dist > dtSqr(ag->mover.getRadius()+nei->mover.getRadius()))
|
||||
continue;
|
||||
dist = sqrtf(dist);
|
||||
float pen = (ag->mover.m_radius+nei->mover.m_radius) - dist;
|
||||
float pen = (ag->mover.getRadius()+nei->mover.getRadius()) - dist;
|
||||
if (dist > 0.0001f)
|
||||
pen = (1.0f/dist) * (pen*0.5f) * 0.7f;
|
||||
|
||||
dtVmad(ag->mover.m_disp, ag->mover.m_disp, diff, pen);
|
||||
dtVmad(ag->disp, ag->disp, diff, pen);
|
||||
|
||||
w += 1.0f;
|
||||
}
|
||||
@@ -1154,14 +1208,16 @@ void CrowdManager::update(const float dt, unsigned int flags, dtNavMeshQuery* na
|
||||
if (w > 0.0001f)
|
||||
{
|
||||
const float iw = 1.0f / w;
|
||||
dtVscale(ag->mover.m_disp, ag->mover.m_disp, iw);
|
||||
dtVscale(ag->disp, ag->disp, iw);
|
||||
}
|
||||
}
|
||||
|
||||
for (int i = 0; i < nagents; ++i)
|
||||
{
|
||||
Agent* ag = agents[i];
|
||||
dtVadd(ag->mover.m_npos, ag->mover.m_npos, ag->mover.m_disp);
|
||||
float npos[3];
|
||||
dtVadd(npos, ag->mover.getNewPos(), ag->disp);
|
||||
ag->mover.setNewPos(npos);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1169,7 +1225,7 @@ void CrowdManager::update(const float dt, unsigned int flags, dtNavMeshQuery* na
|
||||
{
|
||||
Agent* ag = agents[i];
|
||||
// Move along navmesh.
|
||||
ag->mover.updateLocation(navquery, &filter);
|
||||
ag->mover.updatePosition(navquery, &m_filter);
|
||||
}
|
||||
|
||||
|
||||
@@ -1189,7 +1245,7 @@ void CrowdManager::update(const float dt, unsigned int flags, dtNavMeshQuery* na
|
||||
|
||||
// Update agent movement trail.
|
||||
ag->htrail = (ag->htrail + 1) % AGENT_MAX_TRAIL;
|
||||
dtVcopy(&ag->trail[ag->htrail*3], ag->mover.m_pos);
|
||||
dtVcopy(&ag->trail[ag->htrail*3], ag->mover.getPos());
|
||||
}
|
||||
|
||||
m_sampleCount = ns;
|
||||
|
||||
+64
-213
@@ -77,104 +77,23 @@ static bool isectSegAABB(const float* sp, const float* sq,
|
||||
return true;
|
||||
}
|
||||
|
||||
static int insertIsect(float u, int inside, Isect* ints, int nints)
|
||||
static void getMoverBounds(const Mover* m, float* bmin, float* bmax)
|
||||
{
|
||||
int i;
|
||||
if (nints >= FORM_MAX_ISECT) return nints;
|
||||
if (!nints || u >= ints[nints-1].u)
|
||||
{
|
||||
ints[nints].u = u;
|
||||
ints[nints].inside = inside;
|
||||
return nints+1;
|
||||
}
|
||||
for (i = 0; i < nints; ++i)
|
||||
if (u <= ints[i].u) break;
|
||||
if (nints-i > 0) memmove(ints+i+1,ints+i,sizeof(Isect)*(nints-i));
|
||||
ints[i].u = u;
|
||||
ints[i].inside = inside;
|
||||
return nints+1;
|
||||
}
|
||||
|
||||
static int removeAdjacent(Isect* ints, int nints)
|
||||
{
|
||||
const float eps = 0.0001f;
|
||||
if (nints < 2)
|
||||
return nints;
|
||||
for (int i = 0; i < nints-1; ++i)
|
||||
{
|
||||
if (fabsf(ints[i].u - ints[i+1].u) < eps) // && ints[i].inside != ints[i+1].inside)
|
||||
{
|
||||
nints -= 2;
|
||||
for (int j = i; j < nints; ++j)
|
||||
ints[j] = ints[j+2];
|
||||
// if (nints-i > 0) memmove(ints+i,ints+i+2,sizeof(Isect)*(nints-i));
|
||||
i--;
|
||||
}
|
||||
}
|
||||
return nints;
|
||||
}
|
||||
|
||||
static int getPolyVerts(const dtNavMesh* navMesh, dtPolyRef ref, float* verts)
|
||||
{
|
||||
const dtMeshTile* tile = 0;
|
||||
const dtPoly* poly = 0;
|
||||
if (!navMesh->getTileAndPolyByRef(ref, &tile, &poly))
|
||||
return 0;
|
||||
for (int i = 0; i < (int)poly->vertCount; ++i)
|
||||
dtVcopy(&verts[i*3], &tile->verts[poly->verts[i]*3]);
|
||||
return poly->vertCount;
|
||||
}
|
||||
|
||||
static void createFormation(Formation* form, const dtNavMesh* navmesh)
|
||||
{
|
||||
float verts[DT_VERTS_PER_POLYGON*3];
|
||||
for (int i = 0; i < form->nsegs; i++)
|
||||
{
|
||||
FormationSeg* seg = &form->segs[i];
|
||||
seg->nints = 0;
|
||||
|
||||
int startInside = 0;
|
||||
|
||||
for (int j = 0; j < form->npolys; ++j)
|
||||
{
|
||||
const int nverts = getPolyVerts(navmesh, form->polys[j], verts);
|
||||
if (!nverts) continue;
|
||||
|
||||
float tmin, tmax;
|
||||
int smin, smax;
|
||||
bool res = dtIntersectSegmentPoly2D(seg->p, seg->q, verts, nverts, tmin, tmax, smin, smax);
|
||||
|
||||
if (!res)
|
||||
continue;
|
||||
|
||||
if (tmin >= 0.0f && tmin <= 1.0f)
|
||||
seg->nints = insertIsect(tmin, 1, seg->ints, seg->nints);
|
||||
if (tmax >= 0.0f && tmax <= 1.0f)
|
||||
seg->nints = insertIsect(tmax, -1, seg->ints, seg->nints);
|
||||
if (tmin < 0.0f && tmax > 0.0f)
|
||||
startInside++;
|
||||
}
|
||||
|
||||
seg->nints = removeAdjacent(seg->ints, seg->nints);
|
||||
}
|
||||
|
||||
// Calc winding
|
||||
for (int i = 0; i < form->nsegs; ++i)
|
||||
{
|
||||
FormationSeg* seg = &form->segs[i];
|
||||
int inside = 0;
|
||||
for (int j = 0; j < seg->nints; ++j)
|
||||
{
|
||||
inside += seg->ints[j].inside;
|
||||
seg->ints[j].inside = inside;
|
||||
}
|
||||
}
|
||||
const float* p = m->getPos();
|
||||
const float r = m->getRadius();
|
||||
const float h = m->getHeight();
|
||||
bmin[0] = p[0] - r;
|
||||
bmin[1] = p[1];
|
||||
bmin[2] = p[2] - r;
|
||||
bmax[0] = p[0] + r;
|
||||
bmax[1] = p[1] + h;
|
||||
bmax[2] = p[2] + r;
|
||||
}
|
||||
|
||||
CrowdTool::CrowdTool() :
|
||||
m_sample(0),
|
||||
m_oldFlags(0),
|
||||
m_targetPosSet(0),
|
||||
m_targetRef(0),
|
||||
m_expandDebugDraw(false),
|
||||
m_showLabels(true),
|
||||
m_showCorners(false),
|
||||
@@ -191,7 +110,6 @@ CrowdTool::CrowdTool() :
|
||||
m_run(true),
|
||||
m_mode(TOOLMODE_CREATE)
|
||||
{
|
||||
memset(&m_form, 0, sizeof(Formation));
|
||||
}
|
||||
|
||||
CrowdTool::~CrowdTool()
|
||||
@@ -214,7 +132,7 @@ void CrowdTool::init(Sample* sample)
|
||||
|
||||
void CrowdTool::reset()
|
||||
{
|
||||
m_targetPosSet = false;
|
||||
m_targetRef = 0;
|
||||
}
|
||||
|
||||
void CrowdTool::handleMenu()
|
||||
@@ -300,7 +218,7 @@ void CrowdTool::handleClick(const float* s, const float* p, bool shift)
|
||||
const Agent* ag = m_crowd.getAgent(i);
|
||||
if (!ag->active) continue;
|
||||
float bmin[3], bmax[3];
|
||||
ag->mover.getBounds(bmin, bmax);
|
||||
getMoverBounds(&ag->mover, bmin, bmax);
|
||||
float tmin, tmax;
|
||||
if (isectSegAABB(s, p, bmin,bmax, tmin, tmax))
|
||||
{
|
||||
@@ -318,97 +236,28 @@ void CrowdTool::handleClick(const float* s, const float* p, bool shift)
|
||||
}
|
||||
else
|
||||
{
|
||||
bool single = true;
|
||||
|
||||
const dtNavMesh* navmesh = m_sample->getNavMesh();
|
||||
// Add
|
||||
dtNavMeshQuery* navquery = m_sample->getNavMeshQuery();
|
||||
|
||||
if (single)
|
||||
{
|
||||
// Add
|
||||
int idx = m_crowd.addAgent(p, m_sample->getAgentRadius(), m_sample->getAgentHeight());
|
||||
if (idx != -1 && m_targetPosSet)
|
||||
m_crowd.requestMoveTarget(idx, m_targetPos);
|
||||
}
|
||||
else
|
||||
{
|
||||
memset(&m_form, 0, sizeof(Formation));
|
||||
|
||||
const float ext[3] = {2,4,2};
|
||||
dtQueryFilter filter;
|
||||
|
||||
const float r = m_sample->getAgentRadius();
|
||||
|
||||
float nearest[3];
|
||||
dtPolyRef centerRef = navquery->findNearestPoly(p, ext, &filter, nearest);
|
||||
if (centerRef)
|
||||
{
|
||||
const int rows = 6;
|
||||
for (int i = 0; i < rows; ++i)
|
||||
{
|
||||
const float x0 = -r*2.5f*rows/2 + (i&1)*r;
|
||||
const float x1 = r*2.5f*rows/2 + (i&1)*r;
|
||||
const float z = (i-rows*0.5f)*r*2.5f;
|
||||
dtVset(m_form.segs[m_form.nsegs].p, p[0]+x0, p[1]+2.0f, p[2]+z);
|
||||
dtVset(m_form.segs[m_form.nsegs].q, p[0]+x1, p[1]+2.0f, p[2]+z);
|
||||
m_form.nsegs++;
|
||||
}
|
||||
|
||||
m_form.npolys = navquery->findLocalNeighbourhood(centerRef, p, r*rows*2.5f, &filter, m_form.polys, 0, FORM_MAX_POLYS);
|
||||
|
||||
createFormation(&m_form, navmesh);
|
||||
|
||||
const int createCount = 25;
|
||||
int num = 0;
|
||||
|
||||
const float r = m_sample->getAgentRadius();
|
||||
for (int i = 0; i < m_form.nsegs; ++i)
|
||||
{
|
||||
const FormationSeg* seg = &m_form.segs[i];
|
||||
for (int j = 0; j < seg->nints-1; ++j)
|
||||
{
|
||||
if (seg->ints[j].inside == 0) continue;
|
||||
const float u0 = seg->ints[j].u;
|
||||
const float u1 = seg->ints[j+1].u;
|
||||
float ia[3], ib[3];
|
||||
dtVlerp(ia, seg->p,seg->q, u0);
|
||||
dtVlerp(ib, seg->p,seg->q, u1);
|
||||
|
||||
const float spacing = r*2.5f;
|
||||
float delta[3];
|
||||
dtVsub(delta, ib,ia);
|
||||
float d = dtVlen(delta);
|
||||
int np = (int)floorf(d/spacing);
|
||||
for (int k = 0; k < np; ++k)
|
||||
{
|
||||
float pos[3];
|
||||
dtVmad(pos, ia, delta, (float)(k+0.5f)/(float)np);
|
||||
|
||||
if (num < createCount)
|
||||
{
|
||||
num++;
|
||||
int idx = m_crowd.addAgent(pos, m_sample->getAgentRadius(), m_sample->getAgentHeight());
|
||||
if (idx != -1 && m_targetPosSet)
|
||||
m_crowd.requestMoveTarget(idx, m_targetPos);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
int idx = m_crowd.addAgent(p, m_sample->getAgentRadius(), m_sample->getAgentHeight(), navquery);
|
||||
if (idx != -1 && m_targetRef)
|
||||
m_crowd.requestMoveTarget(idx, m_targetRef, m_targetPos);
|
||||
}
|
||||
}
|
||||
else if (m_mode == TOOLMODE_MOVE)
|
||||
{
|
||||
dtVcopy(m_targetPos, p);
|
||||
m_targetPosSet = true;
|
||||
|
||||
for (int i = 0; i < m_crowd.getAgentCount(); ++i)
|
||||
// Find nearest point on navmesh and set move request to that location.
|
||||
dtNavMeshQuery* navquery = m_sample->getNavMeshQuery();
|
||||
const dtQueryFilter* filter = m_crowd.getFilter();
|
||||
const float* ext = m_crowd.getQueryExtents();
|
||||
m_targetRef = navquery->findNearestPoly(p, ext, filter, m_targetPos);
|
||||
if (m_targetRef)
|
||||
{
|
||||
const Agent* ag = m_crowd.getAgent(i);
|
||||
if (!ag->active) continue;
|
||||
m_crowd.requestMoveTarget(i, m_targetPos);
|
||||
for (int i = 0; i < m_crowd.getAgentCount(); ++i)
|
||||
{
|
||||
const Agent* ag = m_crowd.getAgent(i);
|
||||
if (!ag->active) continue;
|
||||
m_crowd.requestMoveTarget(i, m_targetRef, m_targetPos);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -450,7 +299,7 @@ void CrowdTool::handleRender()
|
||||
if (!nmesh)
|
||||
return;
|
||||
|
||||
if (m_targetPosSet)
|
||||
if (m_targetRef)
|
||||
duDebugDrawCross(&dd, m_targetPos[0],m_targetPos[1]+0.1f,m_targetPos[2], s, duRGBA(0,0,0,128), 2.0f);
|
||||
|
||||
|
||||
@@ -459,17 +308,26 @@ void CrowdTool::handleRender()
|
||||
const Agent* ag = m_crowd.getAgent(i);
|
||||
if (!ag->active) continue;
|
||||
|
||||
const float height = ag->mover.getHeight();
|
||||
const float radius = ag->mover.getRadius();
|
||||
const float* pos = ag->mover.getPos();
|
||||
const float* target = ag->mover.getCorridorTarget();
|
||||
const float* vel = ag->mover.getVelocity();
|
||||
const float* dvel = ag->mover.getDesiredVelocity();
|
||||
|
||||
dd.depthMask(false);
|
||||
|
||||
if (m_showPath)
|
||||
{
|
||||
for (int i = 0; i < ag->mover.m_npath; ++i)
|
||||
duDebugDrawNavMeshPoly(&dd, *nmesh, ag->mover.m_path[i], duRGBA(0,0,0,64));
|
||||
const dtPolyRef* cor = ag->mover.getCorridor();
|
||||
const int ncor = ag->mover.getCorridorCount();
|
||||
for (int i = 0; i < ncor; ++i)
|
||||
duDebugDrawNavMeshPoly(&dd, *nmesh, cor[i], duRGBA(0,0,0,64));
|
||||
}
|
||||
|
||||
dd.begin(DU_DRAW_LINES,3.0f);
|
||||
float prev[3], preva = 1;
|
||||
dtVcopy(prev, ag->mover.m_pos);
|
||||
dtVcopy(prev, pos);
|
||||
for (int j = 0; j < AGENT_MAX_TRAIL-1; ++j)
|
||||
{
|
||||
const int idx = (ag->htrail + AGENT_MAX_TRAIL-j) % AGENT_MAX_TRAIL;
|
||||
@@ -484,22 +342,21 @@ void CrowdTool::handleRender()
|
||||
|
||||
if (m_showTargets)
|
||||
{
|
||||
duDebugDrawArc(&dd, ag->mover.m_pos[0], ag->mover.m_pos[1], ag->mover.m_pos[2],
|
||||
ag->mover.m_target[0], ag->mover.m_target[1], ag->mover.m_target[2], 0.25f,
|
||||
0, 0.4f, duRGBA(0,0,0,128), 1.0f);
|
||||
duDebugDrawArc(&dd, pos[0], pos[1], pos[2], target[0], target[1], target[2],
|
||||
0.25f, 0, 0.4f, duRGBA(0,0,0,128), 1.0f);
|
||||
}
|
||||
|
||||
if (m_showCorners)
|
||||
{
|
||||
if (ag->mover.m_ncorners)
|
||||
if (ag->mover.getCornerCount())
|
||||
{
|
||||
dd.begin(DU_DRAW_LINES, 2.0f);
|
||||
for (int j = 0; j < ag->mover.m_ncorners; ++j)
|
||||
for (int j = 0; j < ag->mover.getCornerCount(); ++j)
|
||||
{
|
||||
const float* va = j == 0 ? ag->mover.m_pos : &ag->mover.m_cornerVerts[(j-1)*3];
|
||||
const float* vb = &ag->mover.m_cornerVerts[j*3];
|
||||
dd.vertex(va[0],va[1]+ag->mover.m_radius,va[2], duRGBA(128,0,0,64));
|
||||
dd.vertex(vb[0],vb[1]+ag->mover.m_radius,vb[2], duRGBA(128,0,0,64));
|
||||
const float* va = j == 0 ? pos : ag->mover.getCornerPos(j-1);
|
||||
const float* vb = ag->mover.getCornerPos(j);
|
||||
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));
|
||||
}
|
||||
dd.end();
|
||||
|
||||
@@ -530,16 +387,17 @@ void CrowdTool::handleRender()
|
||||
|
||||
if (m_showCollisionSegments)
|
||||
{
|
||||
const float off = ag->mover.m_radius;
|
||||
duDebugDrawCross(&dd, ag->mover.m_localCenter[0],ag->mover.m_localCenter[1]+off,ag->mover.m_localCenter[2], s, duRGBA(192,0,128,255), 2.0f);
|
||||
duDebugDrawCircle(&dd, ag->mover.m_localCenter[0],ag->mover.m_localCenter[1]+off,ag->mover.m_localCenter[2], ag->mover.m_colradius, duRGBA(192,0,128,128), 2.0f);
|
||||
const float* center = ag->mover.getLocalCenter();
|
||||
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->mover.getCollisionQueryRange(),
|
||||
duRGBA(192,0,128,128), 2.0f);
|
||||
|
||||
dd.begin(DU_DRAW_LINES, 3.0f);
|
||||
for (int j = 0; j < ag->mover.m_localSegCount; ++j)
|
||||
for (int j = 0; j < ag->mover.getLocalSegmentCount(); ++j)
|
||||
{
|
||||
const float* s = &ag->mover.m_localSegs[j*6];
|
||||
const float* s = ag->mover.getLocalSegment(j);
|
||||
unsigned int col = duRGBA(192,0,128,192);
|
||||
if (dtTriArea2D(ag->mover.m_pos, s, s+3) < 0.0f)
|
||||
if (dtTriArea2D(pos, s, s+3) < 0.0f)
|
||||
col = duDarkenCol(col);
|
||||
|
||||
duAppendArrow(&dd, s[0],s[1]+0.2f,s[2], s[3],s[4]+0.2f,s[5], 0.0f, 0.3f, col);
|
||||
@@ -560,9 +418,9 @@ void CrowdTool::handleRender()
|
||||
// Draw detail about agent sela
|
||||
const dtObstacleAvoidanceDebugData* debug = m_crowd.getVODebugData(i);
|
||||
|
||||
const float dx = ag->mover.m_pos[0];
|
||||
const float dy = ag->mover.m_pos[1]+ag->mover.m_height;
|
||||
const float dz = ag->mover.m_pos[2];
|
||||
const float dx = pos[0];
|
||||
const float dy = pos[1]+height;
|
||||
const float dz = pos[2];
|
||||
|
||||
dd.begin(DU_DRAW_QUADS);
|
||||
for (int i = 0; i < debug->getSampleCount(); ++i)
|
||||
@@ -582,13 +440,6 @@ void CrowdTool::handleRender()
|
||||
|
||||
}
|
||||
|
||||
const float height = ag->mover.m_height;
|
||||
const float radius = ag->mover.m_radius;
|
||||
const float* pos = ag->mover.m_pos;
|
||||
const float* vel = ag->mover.m_vel;
|
||||
const float* dvel = ag->mover.m_dvel;
|
||||
|
||||
|
||||
duDebugDrawArrow(&dd, pos[0],pos[1]+height,pos[2],
|
||||
pos[0]+vel[0],pos[1]+height+vel[1],pos[2]+vel[2],
|
||||
0.0f, 0.4f, duRGBA(0,0,0,192), 2.0f);
|
||||
@@ -611,7 +462,7 @@ void CrowdTool::handleRender()
|
||||
{
|
||||
const Agent* ag = m_crowd.getAgent(i);
|
||||
if (!ag->active) continue;
|
||||
const float* pos = ag->mover.m_pos;
|
||||
const float* pos = ag->mover.getPos();
|
||||
gridy = dtMax(gridy, pos[1]);
|
||||
}
|
||||
gridy += 1.0f;
|
||||
@@ -729,7 +580,7 @@ void CrowdTool::handleRenderOverlay(double* proj, double* model, int* view)
|
||||
GLdouble x, y, z;
|
||||
|
||||
// Draw start and end point labels
|
||||
if (m_targetPosSet && gluProject((GLdouble)m_targetPos[0], (GLdouble)m_targetPos[1], (GLdouble)m_targetPos[2],
|
||||
if (m_targetRef && gluProject((GLdouble)m_targetPos[0], (GLdouble)m_targetPos[1], (GLdouble)m_targetPos[2],
|
||||
model, proj, view, &x, &y, &z))
|
||||
{
|
||||
imguiDrawText((int)x, (int)(y+25), IMGUI_ALIGN_CENTER, "TARGET", imguiRGBA(0,0,0,220));
|
||||
@@ -742,8 +593,8 @@ void CrowdTool::handleRenderOverlay(double* proj, double* model, int* view)
|
||||
{
|
||||
const Agent* ag = m_crowd.getAgent(i);
|
||||
if (!ag->active) continue;
|
||||
const float* pos = ag->mover.m_pos;
|
||||
const float h = ag->mover.m_height;
|
||||
const float* pos = ag->mover.getPos();
|
||||
const float h = ag->mover.getHeight();
|
||||
if (gluProject((GLdouble)pos[0], (GLdouble)pos[1]+h, (GLdouble)pos[2],
|
||||
model, proj, view, &x, &y, &z))
|
||||
{
|
||||
|
||||
Reference in New Issue
Block a user