diff --git a/r5dev/naveditor/CrowdTool.cpp b/r5dev/naveditor/CrowdTool.cpp index d0ed6f2f..d219fb34 100644 --- a/r5dev/naveditor/CrowdTool.cpp +++ b/r5dev/naveditor/CrowdTool.cpp @@ -26,6 +26,7 @@ #include "NavEditor/Include/InputGeom.h" #include "NavEditor/Include/Sample.h" #include "NavEditor/Include/SampleInterfaces.h" +#include "thirdparty/recast/DetourCrowd/Include/DetourCrowdInternal.h" static bool isectSegAABB(const float* sp, const float* sq, const float* amin, const float* amax, @@ -320,25 +321,24 @@ void CrowdToolState::handleRender() if (m_toolParams.m_anticipateTurns) { - /* float dvel[3], pos[3]; - calcSmoothSteerDirection(ag->pos, ag->cornerVerts, ag->ncorners, dvel); - pos[0] = ag->pos[0] + dvel[0]; - pos[1] = ag->pos[1] + dvel[1]; - pos[2] = ag->pos[2] + dvel[2]; - - const float off = ag->radius+0.1f; - const float* tgt = &ag->cornerVerts[0]; - const float y = ag->pos[1]+off; - - dd.begin(DU_DRAW_LINES, 2.0f); - - dd.vertex(ag->pos[0],y,ag->pos[2], duRGBA(255,0,0,192)); - dd.vertex(pos[0],y,pos[2], duRGBA(255,0,0,192)); - - dd.vertex(pos[0],y,pos[2], duRGBA(255,0,0,192)); - dd.vertex(tgt[0],y,tgt[2], duRGBA(255,0,0,192)); - - dd.end();*/ + float dvel[3], pos[3]; + calcSmoothSteerDirection(ag, dvel); + pos[0] = ag->npos[0] + dvel[0]; + pos[1] = ag->npos[1] + dvel[1]; + pos[2] = ag->npos[2] + dvel[2]; + + const float off = ag->params.radius+0.1f; + const float* tgt = &ag->cornerVerts[0]; + const float z = ag->npos[2]+off; + + dd.begin(DU_DRAW_LINES, 2.0f); + + dd.vertex(ag->npos[0],ag->npos[1],z, duRGBA(255,0,0,192)); + dd.vertex(pos[0],pos[1],z, duRGBA(255,0,0,192)); + dd.vertex(pos[0],pos[1],z, duRGBA(255,0,0,192)); + dd.vertex(tgt[0],tgt[1],z, duRGBA(255,0,0,192)); + + dd.end(); } } } diff --git a/r5dev/thirdparty/recast/DetourCrowd/Include/DetourCrowdInternal.h b/r5dev/thirdparty/recast/DetourCrowd/Include/DetourCrowdInternal.h new file mode 100644 index 00000000..cdf44baa --- /dev/null +++ b/r5dev/thirdparty/recast/DetourCrowd/Include/DetourCrowdInternal.h @@ -0,0 +1,35 @@ +// +// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgment in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +// + +#ifndef DETOURCROWDINTERNAL_H +#define DETOURCROWDINTERNAL_H + +void integrate(dtCrowdAgent* ag, const float dt); +bool overOffmeshConnection(const dtCrowdAgent* ag, const float radius); +float getDistanceToGoal(const dtCrowdAgent* ag, const float range); +void calcSmoothSteerDirection(const dtCrowdAgent* ag, float* dir); +void calcStraightSteerDirection(const dtCrowdAgent* ag, float* dir); +int addNeighbour(const int idx, const float dist, + dtCrowdNeighbour* neis, const int nneis, const int maxNeis); +int getNeighbours(const float* pos, const float height, const float range, + const dtCrowdAgent* skip, dtCrowdNeighbour* result, const int maxResult, + dtCrowdAgent** agents, const int /*nagents*/, dtProximityGrid* grid); +int addToOptQueue(dtCrowdAgent* newag, dtCrowdAgent** agents, const int nagents, const int maxAgents); +int addToPathQueue(dtCrowdAgent* newag, dtCrowdAgent** agents, const int nagents, const int maxAgents); + +#endif // DETOURCROWDINTERNAL_H diff --git a/r5dev/thirdparty/recast/DetourCrowd/Source/DetourCrowd.cpp b/r5dev/thirdparty/recast/DetourCrowd/Source/DetourCrowd.cpp index d487259a..e33b3eb7 100644 --- a/r5dev/thirdparty/recast/DetourCrowd/Source/DetourCrowd.cpp +++ b/r5dev/thirdparty/recast/DetourCrowd/Source/DetourCrowd.cpp @@ -22,6 +22,7 @@ #include #include #include "DetourCrowd\Include\DetourCrowd.h" +#include "DetourCrowd\Include\DetourCrowdInternal.h" #include "DetourCrowd\Include\DetourObstacleAvoidance.h" #include "Detour\Include\DetourNavMesh.h" #include "Detour\Include\DetourNavMeshQuery.h" @@ -56,240 +57,6 @@ inline float tween(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) -{ - // Fake dynamic constraint. - const float maxDelta = ag->params.maxAcceleration * dt; - float dv[3]; - dtVsub(dv, ag->nvel, ag->vel); - float ds = dtVlen(dv); - if (ds > maxDelta) - dtVscale(dv, dv, maxDelta/ds); - dtVadd(ag->vel, ag->vel, dv); - - // Integrate - if (dtVlen(ag->vel) > 0.0001f) - dtVmad(ag->npos, ag->npos, ag->vel, dt); - else - 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; - if (endOfPath) - return dtMin(dtVdist2D(ag->npos, &ag->cornerVerts[(ag->ncorners-1)*3]), range); - - return range; -} - -static void calcSmoothSteerDirection(const dtCrowdAgent* ag, float* dir) -{ - if (!ag->ncorners) - { - dtVset(dir, 0,0,0); - return; - } - - const int ip0 = 0; - const int ip1 = dtMin(1, ag->ncorners-1); - const float* p0 = &ag->cornerVerts[ip0*3]; - const float* p1 = &ag->cornerVerts[ip1*3]; - - float dir0[3], dir1[3]; - dtVsub(dir0, p0, ag->npos); - dtVsub(dir1, p1, ag->npos); - dir0[2] = 0; - dir1[2] = 0; - - float len0 = dtVlen(dir0); - float len1 = dtVlen(dir1); - if (len1 > 0.001f) - dtVscale(dir1,dir1,1.0f/len1); - - dir[0] = dir0[0] - dir1[0]*len0*0.5f; - dir[1] = dir0[1] - dir1[1]*len0*0.5f; - dir[2] = 0; - - dtVnormalize(dir); -} - -static void calcStraightSteerDirection(const dtCrowdAgent* ag, float* dir) -{ - if (!ag->ncorners) - { - dtVset(dir, 0,0,0); - return; - } - dtVsub(dir, &ag->cornerVerts[0], ag->npos); - dir[2] = 0; - dtVnormalize(dir); -} - -static int addNeighbour(const int idx, const float dist, - dtCrowdNeighbour* neis, const int nneis, const int maxNeis) -{ - // Insert neighbour based on the distance. - dtCrowdNeighbour* nei = 0; - if (!nneis) - { - nei = &neis[nneis]; - } - else if (dist >= neis[nneis-1].dist) - { - if (nneis >= maxNeis) - return nneis; - nei = &neis[nneis]; - } - else - { - int i; - for (i = 0; i < nneis; ++i) - if (dist <= neis[i].dist) - break; - - const int tgt = i+1; - const int n = dtMin(nneis-i, maxNeis-tgt); - - dtAssert(tgt+n <= maxNeis); - - if (n > 0) - memmove(&neis[tgt], &neis[i], sizeof(dtCrowdNeighbour)*n); - nei = &neis[i]; - } - - memset(nei, 0, sizeof(dtCrowdNeighbour)); - - nei->idx = idx; - nei->dist = dist; - - return dtMin(nneis+1, maxNeis); -} - -static int getNeighbours(const float* pos, const float height, const float range, - const dtCrowdAgent* skip, dtCrowdNeighbour* result, const int maxResult, - dtCrowdAgent** agents, const int /*nagents*/, dtProximityGrid* grid) -{ - int n = 0; - - static const int MAX_NEIS = 32; - unsigned short ids[MAX_NEIS]; - int nids = grid->queryItems(pos[0]-range, pos[1]-range, - pos[0]+range, pos[1]+range, - ids, MAX_NEIS); - - for (int i = 0; i < nids; ++i) - { - const dtCrowdAgent* ag = agents[ids[i]]; - - if (ag == skip) continue; - - // Check for overlap. - float diff[3]; - dtVsub(diff, pos, ag->npos); - if (dtMathFabsf(diff[2]) >= (height+ag->params.height)/2.0f) - continue; - diff[1] = 0; - const float distSqr = dtVlenSqr(diff); - if (distSqr > dtSqr(range)) - continue; - - n = addNeighbour(ids[i], distSqr, result, n, maxResult); - } - return n; -} - -static int addToOptQueue(dtCrowdAgent* newag, dtCrowdAgent** agents, const int nagents, const int maxAgents) -{ - // Insert neighbour based on greatest time. - int slot = 0; - if (!nagents) - { - slot = nagents; - } - else if (newag->topologyOptTime <= agents[nagents-1]->topologyOptTime) - { - if (nagents >= maxAgents) - return nagents; - slot = nagents; - } - else - { - int i; - for (i = 0; i < nagents; ++i) - if (newag->topologyOptTime >= agents[i]->topologyOptTime) - break; - - const int tgt = i+1; - const int n = dtMin(nagents-i, maxAgents-tgt); - - dtAssert(tgt+n <= maxAgents); - - if (n > 0) - memmove(&agents[tgt], &agents[i], sizeof(dtCrowdAgent*)*n); - slot = i; - } - - agents[slot] = newag; - - return dtMin(nagents+1, maxAgents); -} - -static int addToPathQueue(dtCrowdAgent* newag, dtCrowdAgent** agents, const int nagents, const int maxAgents) -{ - // Insert neighbour based on greatest time. - int slot = 0; - if (!nagents) - { - slot = nagents; - } - else if (newag->targetReplanTime <= agents[nagents-1]->targetReplanTime) - { - if (nagents >= maxAgents) - return nagents; - slot = nagents; - } - else - { - int i; - for (i = 0; i < nagents; ++i) - if (newag->targetReplanTime >= agents[i]->targetReplanTime) - break; - - const int tgt = i+1; - const int n = dtMin(nagents-i, maxAgents-tgt); - - dtAssert(tgt+n <= maxAgents); - - if (n > 0) - memmove(&agents[tgt], &agents[i], sizeof(dtCrowdAgent*)*n); - slot = i; - } - - agents[slot] = newag; - - return dtMin(nagents+1, maxAgents); -} - /** @class dtCrowd diff --git a/r5dev/thirdparty/recast/DetourCrowd/Source/DetourCrowdInternal.cpp b/r5dev/thirdparty/recast/DetourCrowd/Source/DetourCrowdInternal.cpp new file mode 100644 index 00000000..aa4182e4 --- /dev/null +++ b/r5dev/thirdparty/recast/DetourCrowd/Source/DetourCrowdInternal.cpp @@ -0,0 +1,267 @@ +// +// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgment in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +// + +#define _USE_MATH_DEFINES +#include +#include +#include +#include +#include "DetourCrowd\Include\DetourCrowd.h" +#include "DetourCrowd\Include\DetourCrowdInternal.h" +#include "DetourCrowd\Include\DetourObstacleAvoidance.h" +#include "Detour\Include\DetourNavMesh.h" +#include "Detour\Include\DetourNavMeshQuery.h" +#include "Detour\Include\DetourCommon.h" +#include "Detour\Include\DetourMath.h" +#include "Detour\Include\DetourAssert.h" +#include "Detour\Include\DetourAlloc.h" + + +void integrate(dtCrowdAgent* ag, const float dt) +{ + // Fake dynamic constraint. + const float maxDelta = ag->params.maxAcceleration * dt; + float dv[3]; + dtVsub(dv, ag->nvel, ag->vel); + float ds = dtVlen(dv); + if (ds > maxDelta) + dtVscale(dv, dv, maxDelta/ds); + dtVadd(ag->vel, ag->vel, dv); + + // Integrate + if (dtVlen(ag->vel) > 0.0001f) + dtVmad(ag->npos, ag->npos, ag->vel, dt); + else + dtVset(ag->vel,0,0,0); +} + +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; +} + +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; + if (endOfPath) + return dtMin(dtVdist2D(ag->npos, &ag->cornerVerts[(ag->ncorners-1)*3]), range); + + return range; +} + +void calcSmoothSteerDirection(const dtCrowdAgent* ag, float* dir) +{ + if (!ag->ncorners) + { + dtVset(dir, 0,0,0); + return; + } + + const int ip0 = 0; + const int ip1 = dtMin(1, ag->ncorners-1); + const float* p0 = &ag->cornerVerts[ip0*3]; + const float* p1 = &ag->cornerVerts[ip1*3]; + + float dir0[3], dir1[3]; + dtVsub(dir0, p0, ag->npos); + dtVsub(dir1, p1, ag->npos); + dir0[2] = 0; + dir1[2] = 0; + + float len0 = dtVlen(dir0); + float len1 = dtVlen(dir1); + if (len1 > 0.001f) + dtVscale(dir1,dir1,1.0f/len1); + + dir[0] = dir0[0] - dir1[0]*len0*0.5f; + dir[1] = dir0[1] - dir1[1]*len0*0.5f; + dir[2] = 0; + + dtVnormalize(dir); +} + +void calcStraightSteerDirection(const dtCrowdAgent* ag, float* dir) +{ + if (!ag->ncorners) + { + dtVset(dir, 0,0,0); + return; + } + dtVsub(dir, &ag->cornerVerts[0], ag->npos); + dir[2] = 0; + dtVnormalize(dir); +} + +int addNeighbour(const int idx, const float dist, + dtCrowdNeighbour* neis, const int nneis, const int maxNeis) +{ + // Insert neighbour based on the distance. + dtCrowdNeighbour* nei = 0; + if (!nneis) + { + nei = &neis[nneis]; + } + else if (dist >= neis[nneis-1].dist) + { + if (nneis >= maxNeis) + return nneis; + nei = &neis[nneis]; + } + else + { + int i; + for (i = 0; i < nneis; ++i) + if (dist <= neis[i].dist) + break; + + const int tgt = i+1; + const int n = dtMin(nneis-i, maxNeis-tgt); + + dtAssert(tgt+n <= maxNeis); + + if (n > 0) + memmove(&neis[tgt], &neis[i], sizeof(dtCrowdNeighbour)*n); + nei = &neis[i]; + } + + memset(nei, 0, sizeof(dtCrowdNeighbour)); + + nei->idx = idx; + nei->dist = dist; + + return dtMin(nneis+1, maxNeis); +} + +int getNeighbours(const float* pos, const float height, const float range, + const dtCrowdAgent* skip, dtCrowdNeighbour* result, const int maxResult, + dtCrowdAgent** agents, const int /*nagents*/, dtProximityGrid* grid) +{ + int n = 0; + + static const int MAX_NEIS = 32; + unsigned short ids[MAX_NEIS]; + int nids = grid->queryItems(pos[0]-range, pos[1]-range, + pos[0]+range, pos[1]+range, + ids, MAX_NEIS); + + for (int i = 0; i < nids; ++i) + { + const dtCrowdAgent* ag = agents[ids[i]]; + + if (ag == skip) continue; + + // Check for overlap. + float diff[3]; + dtVsub(diff, pos, ag->npos); + if (dtMathFabsf(diff[2]) >= (height+ag->params.height)/2.0f) + continue; + diff[1] = 0; + const float distSqr = dtVlenSqr(diff); + if (distSqr > dtSqr(range)) + continue; + + n = addNeighbour(ids[i], distSqr, result, n, maxResult); + } + return n; +} + +int addToOptQueue(dtCrowdAgent* newag, dtCrowdAgent** agents, const int nagents, const int maxAgents) +{ + // Insert neighbour based on greatest time. + int slot = 0; + if (!nagents) + { + slot = nagents; + } + else if (newag->topologyOptTime <= agents[nagents-1]->topologyOptTime) + { + if (nagents >= maxAgents) + return nagents; + slot = nagents; + } + else + { + int i; + for (i = 0; i < nagents; ++i) + if (newag->topologyOptTime >= agents[i]->topologyOptTime) + break; + + const int tgt = i+1; + const int n = dtMin(nagents-i, maxAgents-tgt); + + dtAssert(tgt+n <= maxAgents); + + if (n > 0) + memmove(&agents[tgt], &agents[i], sizeof(dtCrowdAgent*)*n); + slot = i; + } + + agents[slot] = newag; + + return dtMin(nagents+1, maxAgents); +} + +int addToPathQueue(dtCrowdAgent* newag, dtCrowdAgent** agents, const int nagents, const int maxAgents) +{ + // Insert neighbour based on greatest time. + int slot = 0; + if (!nagents) + { + slot = nagents; + } + else if (newag->targetReplanTime <= agents[nagents-1]->targetReplanTime) + { + if (nagents >= maxAgents) + return nagents; + slot = nagents; + } + else + { + int i; + for (i = 0; i < nagents; ++i) + if (newag->targetReplanTime >= agents[i]->targetReplanTime) + break; + + const int tgt = i+1; + const int n = dtMin(nagents-i, maxAgents-tgt); + + dtAssert(tgt+n <= maxAgents); + + if (n > 0) + memmove(&agents[tgt], &agents[i], sizeof(dtCrowdAgent*)*n); + slot = i; + } + + agents[slot] = newag; + + return dtMin(nagents+1, maxAgents); +} \ No newline at end of file diff --git a/r5dev/vproj/libdetourcrowd.vcxproj b/r5dev/vproj/libdetourcrowd.vcxproj index 50aba802..3a96b890 100644 --- a/r5dev/vproj/libdetourcrowd.vcxproj +++ b/r5dev/vproj/libdetourcrowd.vcxproj @@ -12,6 +12,7 @@ + @@ -20,6 +21,7 @@ + diff --git a/r5dev/vproj/libdetourcrowd.vcxproj.filters b/r5dev/vproj/libdetourcrowd.vcxproj.filters index c421464c..9f72cf14 100644 --- a/r5dev/vproj/libdetourcrowd.vcxproj.filters +++ b/r5dev/vproj/libdetourcrowd.vcxproj.filters @@ -27,6 +27,9 @@ Include + + Include + @@ -47,5 +50,8 @@ Source + + Source + \ No newline at end of file