R5Reloaded/r5sdk
1
0
Fork 0
mirror of https://github.com/Mauler125/r5sdk.git synced 2025-02-09 19:15:03 +01:00
Code Issues Packages Projects Releases Wiki Activity

Uncomment and fix crowd anticipate turn debug draw

Browse Source 
Moved internal functions to their own translation unit.
This commit is contained in:
Kawe Mazidjatari 2022-07-27 21:05:27 +02:00
parent 88a29f2770
commit 87a42efd72
6 changed files with 330 additions and 253 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
r5dev/naveditor/CrowdTool.cpp
View File

@ -26,6 +26,7 @@
#include "NavEditor/Include/InputGeom.h" #include "NavEditor/Include/InputGeom.h"
#include "NavEditor/Include/Sample.h" #include "NavEditor/Include/Sample.h"
#include "NavEditor/Include/SampleInterfaces.h" #include "NavEditor/Include/SampleInterfaces.h"
#include "thirdparty/recast/DetourCrowd/Include/DetourCrowdInternal.h"
static bool isectSegAABB(const float* sp, const float* sq, static bool isectSegAABB(const float* sp, const float* sq,
const float* amin, const float* amax, const float* amin, const float* amax,
@ -320,25 +321,24 @@ void CrowdToolState::handleRender()
if (m_toolParams.m_anticipateTurns) if (m_toolParams.m_anticipateTurns)
{ {
/* float dvel[3], pos[3]; float dvel[3], pos[3];
calcSmoothSteerDirection(ag->pos, ag->cornerVerts, ag->ncorners, dvel); calcSmoothSteerDirection(ag, dvel);
pos[0] = ag->pos[0] + dvel[0]; pos[0] = ag->npos[0] + dvel[0];
pos[1] = ag->pos[1] + dvel[1]; pos[1] = ag->npos[1] + dvel[1];
pos[2] = ag->pos[2] + dvel[2]; pos[2] = ag->npos[2] + dvel[2];
const float off = ag->radius+0.1f; const float off = ag->params.radius+0.1f;
const float* tgt = &ag->cornerVerts[0]; const float* tgt = &ag->cornerVerts[0];
const float y = ag->pos[1]+off; const float z = ag->npos[2]+off;
dd.begin(DU_DRAW_LINES, 2.0f); dd.begin(DU_DRAW_LINES, 2.0f);
dd.vertex(ag->pos[0],y,ag->pos[2], duRGBA(255,0,0,192)); dd.vertex(ag->npos[0],ag->npos[1],z, duRGBA(255,0,0,192));
dd.vertex(pos[0],y,pos[2], 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(pos[0],y,pos[2], duRGBA(255,0,0,192)); dd.vertex(tgt[0],tgt[1],z, duRGBA(255,0,0,192));
dd.vertex(tgt[0],y,tgt[2], duRGBA(255,0,0,192));
dd.end();
dd.end();*/
} }
} }
} }

35
r5dev/thirdparty/recast/DetourCrowd/Include/DetourCrowdInternal.h vendored Normal file
View File

@ -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

235
r5dev/thirdparty/recast/DetourCrowd/Source/DetourCrowd.cpp vendored
View File

@ -22,6 +22,7 @@
#include <stdlib.h> #include <stdlib.h>
#include <new> #include <new>
#include "DetourCrowd\Include\DetourCrowd.h" #include "DetourCrowd\Include\DetourCrowd.h"
#include "DetourCrowd\Include\DetourCrowdInternal.h"
#include "DetourCrowd\Include\DetourObstacleAvoidance.h" #include "DetourCrowd\Include\DetourObstacleAvoidance.h"
#include "Detour\Include\DetourNavMesh.h" #include "Detour\Include\DetourNavMesh.h"
#include "Detour\Include\DetourNavMeshQuery.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); 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 @class dtCrowd

267
r5dev/thirdparty/recast/DetourCrowd/Source/DetourCrowdInternal.cpp vendored Normal file
View File

@ -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 <string.h>
#include <float.h>
#include <stdlib.h>
#include <new>
#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);
}

2
r5dev/vproj/libdetourcrowd.vcxproj
View File

@ -12,6 +12,7 @@
</ItemGroup> </ItemGroup>
<ItemGroup> <ItemGroup>
<ClInclude Include="..\thirdparty\recast\detourcrowd\Include\DetourCrowd.h" /> <ClInclude Include="..\thirdparty\recast\detourcrowd\Include\DetourCrowd.h" />
<ClInclude Include="..\thirdparty\recast\DetourCrowd\Include\DetourCrowdInternal.h" />
<ClInclude Include="..\thirdparty\recast\detourcrowd\Include\DetourLocalBoundary.h" /> <ClInclude Include="..\thirdparty\recast\detourcrowd\Include\DetourLocalBoundary.h" />
<ClInclude Include="..\thirdparty\recast\detourcrowd\Include\DetourObstacleAvoidance.h" /> <ClInclude Include="..\thirdparty\recast\detourcrowd\Include\DetourObstacleAvoidance.h" />
<ClInclude Include="..\thirdparty\recast\detourcrowd\Include\DetourPathCorridor.h" /> <ClInclude Include="..\thirdparty\recast\detourcrowd\Include\DetourPathCorridor.h" />
@ -20,6 +21,7 @@
</ItemGroup> </ItemGroup>
<ItemGroup> <ItemGroup>
<ClCompile Include="..\thirdparty\recast\detourcrowd\Source\DetourCrowd.cpp" /> <ClCompile Include="..\thirdparty\recast\detourcrowd\Source\DetourCrowd.cpp" />
<ClCompile Include="..\thirdparty\recast\DetourCrowd\Source\DetourCrowdInternal.cpp" />
<ClCompile Include="..\thirdparty\recast\detourcrowd\Source\DetourLocalBoundary.cpp" /> <ClCompile Include="..\thirdparty\recast\detourcrowd\Source\DetourLocalBoundary.cpp" />
<ClCompile Include="..\thirdparty\recast\detourcrowd\Source\DetourObstacleAvoidance.cpp" /> <ClCompile Include="..\thirdparty\recast\detourcrowd\Source\DetourObstacleAvoidance.cpp" />
<ClCompile Include="..\thirdparty\recast\detourcrowd\Source\DetourPathCorridor.cpp" /> <ClCompile Include="..\thirdparty\recast\detourcrowd\Source\DetourPathCorridor.cpp" />

6
r5dev/vproj/libdetourcrowd.vcxproj.filters
View File

@ -27,6 +27,9 @@
<ClInclude Include="..\thirdparty\recast\detourcrowd\Include\DetourCrowd.h"> <ClInclude Include="..\thirdparty\recast\detourcrowd\Include\DetourCrowd.h">
<Filter>Include</Filter> <Filter>Include</Filter>
</ClInclude> </ClInclude>
<ClInclude Include="..\thirdparty\recast\DetourCrowd\Include\DetourCrowdInternal.h">
<Filter>Include</Filter>
</ClInclude>
</ItemGroup> </ItemGroup>
<ItemGroup> <ItemGroup>
<ClCompile Include="..\thirdparty\recast\detourcrowd\Source\DetourLocalBoundary.cpp"> <ClCompile Include="..\thirdparty\recast\detourcrowd\Source\DetourLocalBoundary.cpp">
@ -47,5 +50,8 @@
<ClCompile Include="..\thirdparty\recast\detourcrowd\Source\DetourCrowd.cpp"> <ClCompile Include="..\thirdparty\recast\detourcrowd\Source\DetourCrowd.cpp">
<Filter>Source</Filter> <Filter>Source</Filter>
</ClCompile> </ClCompile>
<ClCompile Include="..\thirdparty\recast\DetourCrowd\Source\DetourCrowdInternal.cpp">
<Filter>Source</Filter>
</ClCompile>
</ItemGroup> </ItemGroup>
</Project> </Project>