diff --git a/Detour/Include/DetourNavMeshQuery.h b/Detour/Include/DetourNavMeshQuery.h index 7a91888..b6101b6 100644 --- a/Detour/Include/DetourNavMeshQuery.h +++ b/Detour/Include/DetourNavMeshQuery.h @@ -332,15 +332,6 @@ public: // Returns: true if closest point found. dtStatus closestPointOnPolyBoundary(dtPolyRef ref, const float* pos, float* closest) const; - // Returns start and end location of an off-mesh link polygon. - // Params: - // prevRef - (in) ref to the polygon before the link (used to select direction). - // polyRef - (in) ref to the off-mesh link polygon. - // startPos[3] - (out) start point of the link. - // endPos[3] - (out) end point of the link. - // Returns: true if link is found. - dtStatus getOffMeshConnectionPolyEndPoints(dtPolyRef prevRef, dtPolyRef polyRef, float* startPos, float* endPos) const; - // Returns height of the polygon at specified location. // Params: // ref - (in) ref to the polygon. @@ -354,6 +345,8 @@ public: class dtNodePool* getNodePool() const { return m_nodePool; } + const dtNavMesh* getAttachedNavMesh() const { return m_nav; } + private: // Returns neighbour tile based on side. diff --git a/DetourCrowd/Include/DetourCrowd.h b/DetourCrowd/Include/DetourCrowd.h index b17957d..1f37397 100644 --- a/DetourCrowd/Include/DetourCrowd.h +++ b/DetourCrowd/Include/DetourCrowd.h @@ -36,10 +36,18 @@ struct dtCrowdNeighbour float dist; }; +enum CrowdAgentState +{ + DT_CROWDAGENT_STATE_WALKING, + DT_CROWDAGENT_STATE_OFFMESH, +}; + struct dtCrowdAgent { unsigned char active; - + + unsigned char state; + dtPathCorridor corridor; dtLocalBoundary boundary; @@ -71,6 +79,13 @@ struct dtCrowdAgent int ncorners; }; +struct dtCrowdAgentAnimation +{ + unsigned char active; + float initPos[3], startPos[3], endPos[3]; + dtPolyRef polyRef; + float t, tmax; +}; enum UpdateFlags { @@ -103,6 +118,7 @@ class dtCrowd int m_maxAgents; dtCrowdAgent* m_agents; dtCrowdAgent** m_activeAgents; + dtCrowdAgentAnimation* m_agentAnims; dtPathQueue m_pathq; diff --git a/DetourCrowd/Include/DetourPathCorridor.h b/DetourCrowd/Include/DetourPathCorridor.h index bbf1d5e..3a1f904 100644 --- a/DetourCrowd/Include/DetourPathCorridor.h +++ b/DetourCrowd/Include/DetourPathCorridor.h @@ -48,6 +48,10 @@ public: bool optimizePathTopology(dtNavMeshQuery* navquery, const dtQueryFilter* filter); + bool moveOverOffmeshConnection(dtPolyRef offMeshConRef, dtPolyRef* refs, + float* startPos, float* endPos, + dtNavMeshQuery* navquery); + void movePosition(const float* npos, dtNavMeshQuery* navquery, const dtQueryFilter* filter); void moveTargetPosition(const float* npos, dtNavMeshQuery* navquery, const dtQueryFilter* filter); diff --git a/DetourCrowd/Source/DetourCrowd.cpp b/DetourCrowd/Source/DetourCrowd.cpp index f90e8e6..b46d7c7 100644 --- a/DetourCrowd/Source/DetourCrowd.cpp +++ b/DetourCrowd/Source/DetourCrowd.cpp @@ -40,6 +40,10 @@ static const int MAX_ITERS_PER_UPDATE = 10; static const int MAX_PATHQUEUE_NODES = 4096; static const int MAX_COMMON_NODES = 512; +inline float between(const float t, const float t0, const float t1) +{ + return dtClamp((t-t0) / (t1-t0), 0.0f, 1.0f); +} static void integrate(dtCrowdAgent* ag, const float dt) { @@ -59,14 +63,29 @@ static void integrate(dtCrowdAgent* ag, const float dt) dtVset(ag->vel,0,0,0); } +static bool overOffmeshConnection(const dtCrowdAgent* ag, const float radius) +{ + if (!ag->ncorners) + return false; + + const bool offMeshConnection = (ag->cornerFlags[ag->ncorners-1] & DT_STRAIGHTPATH_OFFMESH_CONNECTION) ? true : false; + if (offMeshConnection) + { + const float distSq = dtVdist2DSqr(ag->npos, &ag->cornerVerts[(ag->ncorners-1)*3]); + if (distSq < radius*radius) + return true; + } + + return false; +} + static float getDistanceToGoal(const dtCrowdAgent* ag, const float range) { if (!ag->ncorners) return range; const bool endOfPath = (ag->cornerFlags[ag->ncorners-1] & DT_STRAIGHTPATH_END) ? true : false; - const bool offMeshConnection = (ag->cornerFlags[ag->ncorners-1] & DT_STRAIGHTPATH_OFFMESH_CONNECTION) ? true : false; - if (endOfPath || offMeshConnection) + if (endOfPath) return dtMin(dtVdist2D(ag->npos, &ag->cornerVerts[(ag->ncorners-1)*3]), range); return range; @@ -161,6 +180,7 @@ dtCrowd::dtCrowd() : m_maxAgents(0), m_agents(0), m_activeAgents(0), + m_agentAnims(0), m_obstacleQuery(0), m_grid(0), m_pathResult(0), @@ -188,6 +208,9 @@ void dtCrowd::purge() dtFree(m_activeAgents); m_activeAgents = 0; + + dtFree(m_agentAnims); + m_agentAnims = 0; dtFree(m_pathResult); m_pathResult = 0; @@ -207,6 +230,8 @@ void dtCrowd::purge() bool dtCrowd::init(const int maxAgents, const float maxAgentRadius, dtNavMesh* nav) { + purge(); + m_maxAgents = maxAgents; m_maxAgentRadius = maxAgentRadius; @@ -251,6 +276,10 @@ bool dtCrowd::init(const int maxAgents, const float maxAgentRadius, dtNavMesh* n m_activeAgents = (dtCrowdAgent**)dtAlloc(sizeof(dtCrowdAgent*)*m_maxAgents, DT_ALLOC_PERM); if (!m_activeAgents) return false; + + m_agentAnims = (dtCrowdAgentAnimation*)dtAlloc(sizeof(dtCrowdAgentAnimation)*m_maxAgents, DT_ALLOC_PERM); + if (!m_agentAnims) + return false; for (int i = 0; i < m_maxAgents; ++i) { @@ -260,6 +289,11 @@ bool dtCrowd::init(const int maxAgents, const float maxAgentRadius, dtNavMesh* n return false; } + for (int i = 0; i < m_maxAgents; ++i) + { + m_agentAnims[i].active = 0; + } + // The navquery is mostly used for local searches, no need for large node pool. m_navquery = dtAllocNavMeshQuery(); if (!m_navquery) @@ -329,6 +363,7 @@ int dtCrowd::addAgent(const float* pos, const dtCrowdAgentParams* params) ag->t = 0; ag->var = (rand() % 10) / 9.0f; + ag->state = DT_CROWDAGENT_STATE_WALKING; ag->active = 1; return idx; @@ -683,6 +718,8 @@ void dtCrowd::updateTopologyOptimization(dtCrowdAgent** agents, const int nagent for (int i = 0; i < nagents; ++i) { dtCrowdAgent* ag = agents[i]; + if (ag->state == DT_CROWDAGENT_STATE_OFFMESH) + continue; ag->topologyOptTime += dt; if (ag->topologyOptTime >= OPT_TIME_THR) nqueue = addToOptQueue(ag, queue, nqueue, OPT_MAX_AGENTS); @@ -727,6 +764,9 @@ void dtCrowd::update(const float dt, unsigned int flags, dtCrowdAgentDebugInfo* for (int i = 0; i < nagents; ++i) { dtCrowdAgent* ag = agents[i]; + if (ag->state == DT_CROWDAGENT_STATE_OFFMESH) + continue; + // 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, m_navquery, &m_filter); @@ -739,6 +779,9 @@ void dtCrowd::update(const float dt, unsigned int flags, dtCrowdAgentDebugInfo* { dtCrowdAgent* ag = agents[i]; + if (ag->state == DT_CROWDAGENT_STATE_OFFMESH) + continue; + // Find corners for steering ag->ncorners = ag->corridor.findCorners(ag->cornerVerts, ag->cornerFlags, ag->cornerPolys, DT_CROWDAGENT_MAX_CORNERS, m_navquery, &m_filter); @@ -769,10 +812,53 @@ void dtCrowd::update(const float dt, unsigned int flags, dtCrowdAgentDebugInfo* } } + // Trigger off-mesh connections (depends on corners). + for (int i = 0; i < nagents; ++i) + { + dtCrowdAgent* ag = agents[i]; + + if (ag->state != DT_CROWDAGENT_STATE_WALKING) + continue; + + // Check + const float triggerRadius = ag->radius*2.25f; + if (overOffmeshConnection(ag, triggerRadius)) + { + // Prepare to off-mesh connection. + const int idx = ag - m_agents; + dtCrowdAgentAnimation* anim = &m_agentAnims[idx]; + + // Adjust the path over the off-mesh connection. + dtPolyRef refs[2]; + if (ag->corridor.moveOverOffmeshConnection(ag->cornerPolys[ag->ncorners-1], refs, + anim->startPos, anim->endPos, m_navquery)) + { + dtVcopy(anim->initPos, ag->npos); + anim->polyRef = refs[1]; + anim->active = 1; + anim->t = 0.0f; + anim->tmax = (dtVdist2D(anim->startPos, anim->endPos) / ag->maxSpeed) * 0.5f; + + ag->state = DT_CROWDAGENT_STATE_OFFMESH; + ag->ncorners = 0; + ag->nneis = 0; + continue; + } + else + { + // TODO: Off-mesh connection failed, replan. + } + + } + } + // Calculate steering. for (int i = 0; i < nagents; ++i) { dtCrowdAgent* ag = agents[i]; + + if (ag->state != DT_CROWDAGENT_STATE_WALKING) + continue; float dvel[3] = {0,0,0}; @@ -785,7 +871,7 @@ void dtCrowd::update(const float dt, unsigned int flags, dtCrowdAgentDebugInfo* // 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 = getDistanceToGoal(ag, slowDownRadius) / slowDownRadius; - + // Apply style. // TODO: find way to express custom movement styles. /* if (flags & DT_CROWD_DRUNK) @@ -821,6 +907,9 @@ void dtCrowd::update(const float dt, unsigned int flags, dtCrowdAgentDebugInfo* { dtCrowdAgent* ag = agents[i]; + if (ag->state != DT_CROWDAGENT_STATE_WALKING) + continue; + if (flags & DT_CROWD_USE_VO) { m_obstacleQuery->reset(); @@ -870,11 +959,13 @@ void dtCrowd::update(const float dt, unsigned int flags, dtCrowdAgentDebugInfo* dtVcopy(ag->nvel, ag->dvel); } } - + // Integrate. for (int i = 0; i < nagents; ++i) { dtCrowdAgent* ag = agents[i]; + if (ag->state != DT_CROWDAGENT_STATE_WALKING) + continue; integrate(ag, dt); } @@ -886,7 +977,11 @@ void dtCrowd::update(const float dt, unsigned int flags, dtCrowdAgentDebugInfo* for (int i = 0; i < nagents; ++i) { dtCrowdAgent* ag = agents[i]; + const int idx0 = ag - m_agents; + if (ag->state != DT_CROWDAGENT_STATE_WALKING) + continue; + dtVset(ag->disp, 0,0,0); float w = 0; @@ -894,7 +989,8 @@ void dtCrowd::update(const float dt, unsigned int flags, dtCrowdAgentDebugInfo* for (int j = 0; j < ag->nneis; ++j) { const dtCrowdAgent* nei = &m_agents[ag->neis[j].idx]; - + const int idx1 = nei - m_agents; + float diff[3]; dtVsub(diff, ag->npos, nei->npos); @@ -908,8 +1004,19 @@ void dtCrowd::update(const float dt, unsigned int flags, dtCrowdAgentDebugInfo* continue; dist = sqrtf(dist); float pen = (ag->radius + nei->radius) - dist; - if (dist > 0.0001f) + if (dist < 0.0001f) + { + // Agents on top of each other, try to choose diverging separation directions. + if (idx0 > idx1) + dtVset(diff, -ag->dvel[2],0,ag->dvel[0]); + else + dtVset(diff, ag->dvel[2],0,-ag->dvel[0]); + pen = 0.01f; + } + else + { pen = (1.0f/dist) * (pen*0.5f) * COLLISION_RESOLVE_FACTOR; + } dtVmad(ag->disp, ag->disp, diff, pen); @@ -926,6 +1033,9 @@ void dtCrowd::update(const float dt, unsigned int flags, dtCrowdAgentDebugInfo* for (int i = 0; i < nagents; ++i) { dtCrowdAgent* ag = agents[i]; + if (ag->state != DT_CROWDAGENT_STATE_WALKING) + continue; + dtVadd(ag->npos, ag->npos, ag->disp); } } @@ -933,12 +1043,52 @@ void dtCrowd::update(const float dt, unsigned int flags, dtCrowdAgentDebugInfo* for (int i = 0; i < nagents; ++i) { dtCrowdAgent* ag = agents[i]; + if (ag->state != DT_CROWDAGENT_STATE_WALKING) + continue; + // Move along navmesh. ag->corridor.movePosition(ag->npos, m_navquery, &m_filter); // Get valid constrained position back. dtVcopy(ag->npos, ag->corridor.getPos()); } + // Update agents using off-mesh connection. + for (int i = 0; i < m_maxAgents; ++i) + { + dtCrowdAgentAnimation* anim = &m_agentAnims[i]; + if (!anim->active) + continue; + dtCrowdAgent* ag = agents[i]; + + anim->t += dt; + if (anim->t > anim->tmax) + { + // Reset animation + anim->active = 0; + // Prepare agent for walking. + ag->state = DT_CROWDAGENT_STATE_WALKING; + continue; + } + + // Update position + const float ta = anim->tmax*0.15f; + const float tb = anim->tmax; + if (anim->t < ta) + { + const float u = between(anim->t, 0.0, ta); + dtVlerp(ag->npos, anim->initPos, anim->startPos, u); + } + else + { + const float u = between(anim->t, ta, tb); + dtVlerp(ag->npos, anim->startPos, anim->endPos, u); + } + + // Update velocity. + dtVset(ag->vel, 0,0,0); + dtVset(ag->dvel, 0,0,0); + } + } diff --git a/DetourCrowd/Source/DetourPathCorridor.cpp b/DetourCrowd/Source/DetourPathCorridor.cpp index 28641fb..457f163 100644 --- a/DetourCrowd/Source/DetourPathCorridor.cpp +++ b/DetourCrowd/Source/DetourPathCorridor.cpp @@ -286,6 +286,50 @@ bool dtPathCorridor::optimizePathTopology(dtNavMeshQuery* navquery, const dtQuer return false; } +bool dtPathCorridor::moveOverOffmeshConnection(dtPolyRef offMeshConRef, dtPolyRef* refs, + float* startPos, float* endPos, + dtNavMeshQuery* navquery) +{ + dtAssert(navquery); + dtAssert(m_path); + dtAssert(m_npath); + + // Advance the path up to and over the off-mesh connection. + dtPolyRef prevRef = 0, polyRef = m_path[0]; + int npos = 0; + while (npos < m_npath && polyRef != offMeshConRef) + { + prevRef = polyRef; + polyRef = m_path[npos]; + npos++; + } + if (npos == m_npath) + { + // Could not find offMeshConRef + return false; + } + + // Prune path + for (int i = npos; i < m_npath; ++i) + m_path[i-npos] = m_path[i]; + m_npath -= npos; + + refs[0] = prevRef; + refs[1] = polyRef; + + const dtNavMesh* nav = navquery->getAttachedNavMesh(); + dtAssert(nav); + + dtStatus status = nav->getOffMeshConnectionPolyEndPoints(refs[0], refs[1], startPos, endPos); + if (dtStatusSucceed(status)) + { + dtVcopy(m_pos, endPos); + return true; + } + + return false; +} + void dtPathCorridor::movePosition(const float* npos, dtNavMeshQuery* navquery, const dtQueryFilter* filter) { dtAssert(m_path); diff --git a/RecastDemo/Bin/Recast.app/Contents/MacOS/Recast b/RecastDemo/Bin/Recast.app/Contents/MacOS/Recast index 42cf575..9698938 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/project.pbxproj b/RecastDemo/Build/Xcode/Recast.xcodeproj/project.pbxproj index f40603c..9538e37 100644 --- a/RecastDemo/Build/Xcode/Recast.xcodeproj/project.pbxproj +++ b/RecastDemo/Build/Xcode/Recast.xcodeproj/project.pbxproj @@ -351,12 +351,12 @@ 6BB5012B12F458AE001B1957 /* DetourCrowd */ = { isa = PBXGroup; children = ( - 6BB501E112F46B6A001B1957 /* DetourObstacleAvoidance.h */, - 6BB501E212F46B6A001B1957 /* DetourObstacleAvoidance.cpp */, 6BB5012C12F458CB001B1957 /* DetourCrowd.h */, 6BB5013112F458CB001B1957 /* DetourCrowd.cpp */, 6BB5012D12F458CB001B1957 /* DetourLocalBoundary.h */, 6BB5013212F458CB001B1957 /* DetourLocalBoundary.cpp */, + 6BB501E112F46B6A001B1957 /* DetourObstacleAvoidance.h */, + 6BB501E212F46B6A001B1957 /* DetourObstacleAvoidance.cpp */, 6BB5012E12F458CB001B1957 /* DetourPathCorridor.h */, 6BB5013312F458CB001B1957 /* DetourPathCorridor.cpp */, 6BB5012F12F458CB001B1957 /* DetourPathQueue.h */, diff --git a/RecastDemo/Source/CrowdTool.cpp b/RecastDemo/Source/CrowdTool.cpp index 63b1bd5..09bb8c7 100644 --- a/RecastDemo/Source/CrowdTool.cpp +++ b/RecastDemo/Source/CrowdTool.cpp @@ -514,8 +514,16 @@ void CrowdTool::handleRender() dd.vertex(va[0],va[1]+radius,va[2], duRGBA(128,0,0,192)); dd.vertex(vb[0],vb[1]+radius,vb[2], duRGBA(128,0,0,192)); } + if (ag->ncorners && ag->cornerFlags[ag->ncorners-1] & DT_STRAIGHTPATH_OFFMESH_CONNECTION) + { + const float* v = &ag->cornerVerts[(ag->ncorners-1)*3]; + dd.vertex(v[0],v[1],v[2], duRGBA(192,0,0,192)); + dd.vertex(v[0],v[1]+radius*2,v[2], duRGBA(192,0,0,192)); + } + dd.end(); + if (m_anticipateTurns) { /* float dvel[3], pos[3];