From 03698a4a5fbb6d62c8ad4be810b78751c7023633 Mon Sep 17 00:00:00 2001 From: Kawe Mazidjatari <48657826+Mauler125@users.noreply.github.com> Date: Wed, 27 Jul 2022 17:48:51 +0200 Subject: [PATCH] Fix DetourCrowd XZY -> XYZ. Crowd agents now traverse over the poly surfaces properly. --- r5dev/naveditor/CrowdTool.cpp | 67 +++++++++---------- .../recast/DetourCrowd/Source/DetourCrowd.cpp | 36 +++++----- .../Source/DetourObstacleAvoidance.cpp | 34 +++++----- .../DetourCrowd/Source/DetourPathCorridor.cpp | 6 +- 4 files changed, 69 insertions(+), 74 deletions(-) diff --git a/r5dev/naveditor/CrowdTool.cpp b/r5dev/naveditor/CrowdTool.cpp index c6f98c71..f6fba6fe 100644 --- a/r5dev/naveditor/CrowdTool.cpp +++ b/r5dev/naveditor/CrowdTool.cpp @@ -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() diff --git a/r5dev/thirdparty/recast/DetourCrowd/Source/DetourCrowd.cpp b/r5dev/thirdparty/recast/DetourCrowd/Source/DetourCrowd.cpp index 318f1f2d..329493c4 100644 --- a/r5dev/thirdparty/recast/DetourCrowd/Source/DetourCrowd.cpp +++ b/r5dev/thirdparty/recast/DetourCrowd/Source/DetourCrowd.cpp @@ -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); } - } diff --git a/r5dev/thirdparty/recast/DetourCrowd/Source/DetourObstacleAvoidance.cpp b/r5dev/thirdparty/recast/DetourCrowd/Source/DetourObstacleAvoidance.cpp index 4a6a513c..8d1d2fe2 100644 --- a/r5dev/thirdparty/recast/DetourCrowd/Source/DetourObstacleAvoidance.cpp +++ b/r5dev/thirdparty/recast/DetourCrowd/Source/DetourObstacleAvoidance.cpp @@ -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++; diff --git a/r5dev/thirdparty/recast/DetourCrowd/Source/DetourPathCorridor.cpp b/r5dev/thirdparty/recast/DetourCrowd/Source/DetourPathCorridor.cpp index 4dba8244..47f4e127 100644 --- a/r5dev/thirdparty/recast/DetourCrowd/Source/DetourPathCorridor.cpp +++ b/r5dev/thirdparty/recast/DetourCrowd/Source/DetourPathCorridor.cpp @@ -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; }