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

Fix DetourCrowd

Browse Source 
XZY -> XYZ.
Crowd agents now traverse over the poly surfaces properly.
This commit is contained in:
Kawe Mazidjatari 2022-07-27 17:48:51 +02:00
parent 6d4b2ea7ef
commit 03698a4a5f
4 changed files with 69 additions and 74 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

67
r5dev/naveditor/CrowdTool.cpp
View File

@ -72,11 +72,11 @@ static void getAgentBounds(const dtCrowdAgent* ag, float* bmin, float* bmax)
const float r = ag->params.radius;
const float h = ag->params.height;
bmin[0] = p[0] - r;
bmin[1] = p[1];
bmin[2] = p[2] - r;
bmin[1] = p[1] - r;
bmin[2] = p[2];
bmax[0] = p[0] + r;
bmax[1] = p[1] + h;
bmax[2] = p[2] + r;
bmax[1] = p[1] + r;
bmax[2] = p[2] + h;
}
CrowdToolState::CrowdToolState() :
@ -220,20 +220,20 @@ void CrowdToolState::handleRender()
}
if (m_targetRef)
duDebugDrawCross(&dd, m_targetPos[0],m_targetPos[1]+0.1f,m_targetPos[2], rad, duRGBA(255,255,255,192), 2.0f);
duDebugDrawCross(&dd, m_targetPos[0],m_targetPos[1],m_targetPos[2]+0.1f, rad, duRGBA(255,255,255,192), 2.0f);
// Occupancy grid.
if (m_toolParams.m_showGrid)
{
float gridy = -FLT_MAX;
float gridz = -FLT_MAX;
for (int i = 0; i < crowd->getAgentCount(); ++i)
{
const dtCrowdAgent* ag = crowd->getAgent(i);
if (!ag->active) continue;
const float* pos = ag->corridor.getPos();
gridy = dtMax(gridy, pos[1]);
gridz = dtMax(gridz, pos[2]);
}
gridy += 1.0f;
gridz += 1.0f;
dd.begin(DU_DRAW_QUADS);
const dtProximityGrid* grid = crowd->getGrid();
@ -243,13 +243,14 @@ void CrowdToolState::handleRender()
{
for (int x = bounds[0]; x <= bounds[2]; ++x)
{
const int count = grid->getItemCountAt(x,y);
const int count = grid->getItemCountAt(x,y);
if (!count) continue;
unsigned int col = duRGBA(128,0,0,dtMin(count*40,255));
dd.vertex(x*cs, gridy, y*cs, col);
dd.vertex(x*cs, gridy, y*cs+cs, col);
dd.vertex(x*cs+cs, gridy, y*cs+cs, col);
dd.vertex(x*cs+cs, gridy, y*cs, col);
dd.vertex(x*cs+cs,y*cs,gridz, col);
dd.vertex(x*cs+cs,y*cs+cs,gridz,col);
dd.vertex(x*cs,y*cs+cs,gridz,col);
dd.vertex(x*cs,y*cs,gridz, col);
}
}
dd.end();
@ -272,8 +273,8 @@ void CrowdToolState::handleRender()
const int idx = (trail->htrail + AGENT_MAX_TRAIL-j) % AGENT_MAX_TRAIL;
const float* v = &trail->trail[idx*3];
float a = 1 - j/(float)AGENT_MAX_TRAIL;
dd.vertex(prev[0],prev[1]+0.1f,prev[2], duRGBA(0,0,0,(int)(128*preva)));
dd.vertex(v[0],v[1]+0.1f,v[2], duRGBA(0,0,0,(int)(128*a)));
dd.vertex(prev[0],prev[1],prev[2]+0.1f, duRGBA(0,0,0,(int)(128*preva)));
dd.vertex(v[0],v[1],v[2]+0.1f, duRGBA(0,0,0,(int)(128*a)));
preva = a;
dtVcopy(prev, v);
}
@ -302,14 +303,14 @@ void CrowdToolState::handleRender()
{
const float* va = j == 0 ? pos : &ag->cornerVerts[(j-1)*3];
const float* vb = &ag->cornerVerts[j*3];
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));
dd.vertex(va[0],va[1],va[2]+radius, duRGBA(128,0,0,192));
dd.vertex(vb[0],vb[1],vb[2]+radius, 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.vertex(v[0],v[1],v[2]+radius*2, duRGBA(192,0,0,192));
}
dd.end();
@ -343,8 +344,8 @@ void CrowdToolState::handleRender()
if (m_toolParams.m_showCollisionSegments)
{
const float* center = ag->boundary.getCenter();
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->params.collisionQueryRange,
duDebugDrawCross(&dd, center[0],center[1],center[2]+radius, 0.2f, duRGBA(192,0,128,255), 2.0f);
duDebugDrawCircle(&dd, center[0],center[1],center[2]+radius, ag->params.collisionQueryRange,
duRGBA(192,0,128,128), 2.0f);
dd.begin(DU_DRAW_LINES, 3.0f);
@ -362,7 +363,7 @@ void CrowdToolState::handleRender()
if (m_toolParams.m_showNeis)
{
duDebugDrawCircle(&dd, pos[0],pos[1]+radius,pos[2], ag->params.collisionQueryRange,
duDebugDrawCircle(&dd, pos[0],pos[1],pos[2]+radius, ag->params.collisionQueryRange,
duRGBA(0,192,128,128), 2.0f);
dd.begin(DU_DRAW_LINES, 2.0f);
@ -373,8 +374,8 @@ void CrowdToolState::handleRender()
const dtCrowdAgent* nei = crowd->getAgent(ag->neis[j].idx);
if (nei)
{
dd.vertex(pos[0],pos[1]+radius,pos[2], duRGBA(0,192,128,128));
dd.vertex(nei->npos[0],nei->npos[1]+radius,nei->npos[2], duRGBA(0,192,128,128));
dd.vertex(pos[0],pos[1],pos[2]+radius, duRGBA(0,192,128,128));
dd.vertex(nei->npos[0],nei->npos[1],nei->npos[2]+radius, duRGBA(0,192,128,128));
}
}
dd.end();
@ -383,8 +384,8 @@ void CrowdToolState::handleRender()
if (m_toolParams.m_showOpt)
{
dd.begin(DU_DRAW_LINES, 2.0f);
dd.vertex(m_agentDebug.optStart[0],m_agentDebug.optStart[1]+0.3f,m_agentDebug.optStart[2], duRGBA(0,128,0,192));
dd.vertex(m_agentDebug.optEnd[0],m_agentDebug.optEnd[1]+0.3f,m_agentDebug.optEnd[2], duRGBA(0,128,0,192));
dd.vertex(m_agentDebug.optStart[0],m_agentDebug.optStart[1],m_agentDebug.optStart[2]+0.3f, duRGBA(0,128,0,192));
dd.vertex(m_agentDebug.optEnd[0],m_agentDebug.optEnd[1],m_agentDebug.optEnd[2]+0.3f, duRGBA(0,128,0,192));
dd.end();
}
}
@ -428,7 +429,6 @@ void CrowdToolState::handleRender()
pos[0]+radius, pos[1]+radius, pos[2]+height, col);
}
if (m_toolParams.m_showVO)
{
for (int i = 0; i < crowd->getAgentCount(); i++)
@ -457,10 +457,10 @@ void CrowdToolState::handleRender()
const float pen2 = vod->getSamplePreferredSidePenalty(j);
unsigned int col = duLerpCol(duRGBA(255,255,255,220), duRGBA(128,96,0,220), (int)(pen*255));
col = duLerpCol(col, duRGBA(128,0,0,220), (int)(pen2*128));
dd.vertex(dx+p[0]-sr, dy, dz+p[2]-sr, col);
dd.vertex(dx+p[0]-sr, dy, dz+p[2]+sr, col);
dd.vertex(dx+p[0]+sr, dy, dz+p[2]+sr, col);
dd.vertex(dx+p[0]+sr, dy, dz+p[2]-sr, col);
dd.vertex(dx+p[0]-sr, dy+p[1]-sr, dz, col);
dd.vertex(dx+p[0]-sr, dy+p[1]+sr, dz, col);
dd.vertex(dx+p[0]+sr, dy+p[1]+sr, dz, col);
dd.vertex(dx+p[0]+sr, dy+p[1]-sr, dz, col);
}
dd.end();
}
@ -491,11 +491,11 @@ void CrowdToolState::handleRender()
duDebugDrawCircle(&dd, pos[0], pos[1], pos[2]+height, radius, col, 2.0f);
duDebugDrawArrow(&dd, pos[0],pos[1],pos[2]+height,
pos[0]+dvel[0],pos[1]+dvel[2],pos[2]+height+dvel[1],
pos[0]+dvel[0],pos[1]+dvel[1],pos[2]+height+dvel[2],
0.0f, 0.4f, duRGBA(0,192,255,192), (m_agentDebug.idx == i) ? 2.0f : 1.0f);
duDebugDrawArrow(&dd, pos[0],pos[1],pos[2]+height,
pos[0]+vel[0],pos[1]+vel[2],pos[2]+height+vel[1],
pos[0]+vel[0],pos[1]+vel[1],pos[2]+height+vel[2],
0.0f, 0.4f, duRGBA(0,0,0,160), 2.0f);
}
@ -681,7 +681,7 @@ void CrowdToolState::hilightAgent(const int idx)
static void calcVel(float* vel, const float* pos, const float* tgt, const float speed)
{
dtVsub(vel, tgt, pos);
vel[1] = 0.0;
vel[2] = 0.0;
dtVnormalize(vel);
dtVscale(vel, vel, speed);
}
@ -1029,7 +1029,6 @@ void CrowdTool::handleClick(const float* s, const float* p, bool shift)
}
}
}
}
void CrowdTool::handleStep()

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

@ -118,8 +118,8 @@ static void calcSmoothSteerDirection(const dtCrowdAgent* ag, float* dir)
float dir0[3], dir1[3];
dtVsub(dir0, p0, ag->npos);
dtVsub(dir1, p1, ag->npos);
dir0[1] = 0;
dir1[1] = 0;
dir0[2] = 0;
dir1[2] = 0;
float len0 = dtVlen(dir0);
float len1 = dtVlen(dir1);
@ -127,8 +127,8 @@ static void calcSmoothSteerDirection(const dtCrowdAgent* ag, float* dir)
dtVscale(dir1,dir1,1.0f/len1);
dir[0] = dir0[0] - dir1[0]*len0*0.5f;
dir[1] = 0;
dir[2] = dir0[2] - dir1[2]*len0*0.5f;
dir[1] = dir0[1] - dir1[1]*len0*0.5f;
dir[2] = 0;
dtVnormalize(dir);
}
@ -141,7 +141,7 @@ static void calcStraightSteerDirection(const dtCrowdAgent* ag, float* dir)
return;
}
dtVsub(dir, &ag->cornerVerts[0], ag->npos);
dir[1] = 0;
dir[2] = 0;
dtVnormalize(dir);
}
@ -193,8 +193,8 @@ static int getNeighbours(const float* pos, const float height, const float range
static const int MAX_NEIS = 32;
unsigned short ids[MAX_NEIS];
int nids = grid->queryItems(pos[0]-range, pos[2]-range,
pos[0]+range, pos[2]+range,
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)
@ -206,7 +206,7 @@ static int getNeighbours(const float* pos, const float height, const float range
// Check for overlap.
float diff[3];
dtVsub(diff, pos, ag->npos);
if (dtMathFabsf(diff[1]) >= (height+ag->params.height)/2.0f)
if (dtMathFabsf(diff[2]) >= (height+ag->params.height)/2.0f)
continue;
diff[1] = 0;
const float distSqr = dtVlenSqr(diff);
@ -524,7 +524,7 @@ int dtCrowd::addAgent(const float* pos, const dtCrowdAgentParams* params)
if (idx == -1)
return -1;
dtCrowdAgent* ag = &m_agents[idx];
dtCrowdAgent* ag = &m_agents[idx];
updateAgentParameters(idx, params);
@ -901,7 +901,6 @@ void dtCrowd::updateMoveRequest(const float /*dt*/)
}
}
}
}
@ -935,7 +934,6 @@ void dtCrowd::updateTopologyOptimization(dtCrowdAgent** agents, const int nagent
ag->corridor.optimizePathTopology(m_navquery, &m_filters[ag->params.queryFilterType]);
ag->topologyOptTime = 0;
}
}
void dtCrowd::checkPathValidity(dtCrowdAgent** agents, const int nagents, const float dt)
@ -1043,7 +1041,7 @@ void dtCrowd::checkPathValidity(dtCrowdAgent** agents, const int nagents, const
}
}
}
void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
{
m_velocitySampleCount = 0;
@ -1069,7 +1067,7 @@ void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
dtCrowdAgent* ag = agents[i];
const float* p = ag->npos;
const float r = ag->params.radius;
m_grid->addItem((unsigned short)i, p[0]-r, p[2]-r, p[0]+r, p[2]+r);
m_grid->addItem((unsigned short)i, p[0]-r, p[1]-r, p[0]+r, p[1]+r);
}
// Get nearby navmesh segments and agents to collide with.
@ -1225,7 +1223,7 @@ void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
float diff[3];
dtVsub(diff, ag->npos, nei->npos);
diff[1] = 0;
diff[2] = 0;
const float distSqr = dtVlenSqr(diff);
if (distSqr < 0.00001f)
@ -1345,7 +1343,7 @@ void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
float diff[3];
dtVsub(diff, ag->npos, nei->npos);
diff[1] = 0;
diff[2] = 0;
float dist = dtVlenSqr(diff);
if (dist > dtSqr(ag->params.radius + nei->params.radius))
@ -1356,9 +1354,9 @@ void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
{
// Agents on top of each other, try to choose diverging separation directions.
if (idx0 > idx1)
dtVset(diff, -ag->dvel[2],0,ag->dvel[0]);
dtVset(diff, -ag->dvel[1],0,ag->dvel[0]);
else
dtVset(diff, ag->dvel[2],0,-ag->dvel[0]);
dtVset(diff, ag->dvel[1],0,-ag->dvel[0]);
pen = 0.01f;
}
else
@ -1366,7 +1364,7 @@ void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
pen = (1.0f/dist) * (pen*0.5f) * COLLISION_RESOLVE_FACTOR;
}
dtVmad(ag->disp, ag->disp, diff, pen);
dtVmad(ag->disp, ag->disp, diff, pen);
w += 1.0f;
}
@ -1405,7 +1403,6 @@ void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
ag->corridor.reset(ag->corridor.getFirstPoly(), ag->npos);
ag->partial = false;
}
}
// Update agents using off-mesh connection.
@ -1446,5 +1443,4 @@ void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
dtVset(ag->vel, 0,0,0);
dtVset(ag->dvel, 0,0,0);
}
}

34
r5dev/thirdparty/recast/DetourCrowd/Source/DetourObstacleAvoidance.cpp vendored
View File

@ -390,8 +390,8 @@ float dtObstacleAvoidanceQuery::processSample(const float* vcand, const float cs
// Special case when the agent is very close to the segment.
float sdir[3], snorm[3];
dtVsub(sdir, seg->q, seg->p);
snorm[0] = -sdir[2];
snorm[2] = sdir[0];
snorm[0] = -sdir[1];
snorm[1] = sdir[0];
// If the velocity is pointing towards the segment, no collision.
if (dtVdot2D(snorm, vcand) < 0.0f)
continue;
@ -450,7 +450,7 @@ int dtObstacleAvoidanceQuery::sampleVelocityGrid(const float* pos, const float r
debug->reset();
const float cvx = dvel[0] * m_params.velBias;
const float cvz = dvel[2] * m_params.velBias;
const float cvy = dvel[1] * m_params.velBias;
const float cs = vmax * 2 * (1 - m_params.velBias) / (float)(m_params.gridSize-1);
const float half = (m_params.gridSize-1)*cs*0.5f;
@ -463,10 +463,10 @@ int dtObstacleAvoidanceQuery::sampleVelocityGrid(const float* pos, const float r
{
float vcand[3];
vcand[0] = cvx + x*cs - half;
vcand[1] = 0;
vcand[2] = cvz + y*cs - half;
vcand[1] = cvy + y*cs - half;
vcand[2] = 0;
if (dtSqr(vcand[0])+dtSqr(vcand[2]) > dtSqr(vmax+cs/2)) continue;
if (dtSqr(vcand[0])+dtSqr(vcand[1]) > dtSqr(vmax+cs/2)) continue;
const float penalty = processSample(vcand, cs, pos,rad,vel,dvel, minPenalty, debug);
ns++;
@ -482,25 +482,25 @@ int dtObstacleAvoidanceQuery::sampleVelocityGrid(const float* pos, const float r
}
// vector normalization that ignores the y-component.
// vector normalization that ignores the z-component.
inline void dtNormalize2D(float* v)
{
float d = dtMathSqrtf(v[0] * v[0] + v[2] * v[2]);
float d = dtMathSqrtf(v[0] * v[0] + v[1] * v[1]);
if (d==0)
return;
d = 1.0f / d;
v[0] *= d;
v[2] *= d;
v[1] *= d;
}
// vector normalization that ignores the y-component.
// vector normalization that ignores the z-component.
inline void dtRorate2D(float* dest, const float* v, float ang)
{
float c = cosf(ang);
float s = sinf(ang);
dest[0] = v[0]*c - v[2]*s;
dest[2] = v[0]*s + v[2]*c;
dest[1] = v[1];
dest[0] = v[0]*c - v[1]*s;
dest[1] = v[0]*s + v[1]*c;
dest[2] = v[2];
}
@ -550,7 +550,7 @@ int dtObstacleAvoidanceQuery::sampleVelocityAdaptive(const float* pos, const flo
{
const float r = (float)(nr-j)/(float)nr;
pat[npat*2+0] = ddir[(j%2)*3] * r;
pat[npat*2+1] = ddir[(j%2)*3+2] * r;
pat[npat*2+1] = ddir[(j%2)*3+1] * r;
float* last1 = pat + npat*2;
float* last2 = last1;
npat++;
@ -594,10 +594,10 @@ int dtObstacleAvoidanceQuery::sampleVelocityAdaptive(const float* pos, const flo
{
float vcand[3];
vcand[0] = res[0] + pat[i*2+0]*cr;
vcand[1] = 0;
vcand[2] = res[2] + pat[i*2+1]*cr;
vcand[1] = res[1] + pat[i*2+1]*cr;
vcand[2] = 0;
if (dtSqr(vcand[0])+dtSqr(vcand[2]) > dtSqr(vmax+0.001f)) continue;
if (dtSqr(vcand[0])+dtSqr(vcand[1]) > dtSqr(vmax+0.001f)) continue;
const float penalty = processSample(vcand,cr/10, pos,rad,vel,dvel, minPenalty, debug);
ns++;

6
r5dev/thirdparty/recast/DetourCrowd/Source/DetourPathCorridor.cpp vendored
View File

@ -134,7 +134,7 @@ int dtMergeCorridorStartShortcut(dtPolyRef* path, const int npath, const int max
if (furthestPath == -1 || furthestVisited == -1)
return npath;
// Concatenate paths.
// Concatenate paths.
// Adjust beginning of the buffer to include the visited.
const int req = furthestVisited;
@ -452,9 +452,9 @@ bool dtPathCorridor::movePosition(const float* npos, dtNavMeshQuery* navquery, c
m_npath = dtMergeCorridorStartMoved(m_path, m_npath, m_maxPath, visited, nvisited);
// Adjust the position to stay on top of the navmesh.
float h = m_pos[1];
float h = m_pos[2];
navquery->getPolyHeight(m_path[0], result, &h);
result[1] = h;
result[2] = h;
dtVcopy(m_pos, result);
return true;
}