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 r = ag->params.radius;
const float h = ag->params.height; const float h = ag->params.height;
bmin[0] = p[0] - r; bmin[0] = p[0] - r;
bmin[1] = p[1]; bmin[1] = p[1] - r;
bmin[2] = p[2] - r; bmin[2] = p[2];
bmax[0] = p[0] + r; bmax[0] = p[0] + r;
bmax[1] = p[1] + h; bmax[1] = p[1] + r;
bmax[2] = p[2] + r; bmax[2] = p[2] + h;
} }
CrowdToolState::CrowdToolState() : CrowdToolState::CrowdToolState() :
@ -220,20 +220,20 @@ void CrowdToolState::handleRender()
} }
if (m_targetRef) 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. // Occupancy grid.
if (m_toolParams.m_showGrid) if (m_toolParams.m_showGrid)
{ {
float gridy = -FLT_MAX; float gridz = -FLT_MAX;
for (int i = 0; i < crowd->getAgentCount(); ++i) for (int i = 0; i < crowd->getAgentCount(); ++i)
{ {
const dtCrowdAgent* ag = crowd->getAgent(i); const dtCrowdAgent* ag = crowd->getAgent(i);
if (!ag->active) continue; if (!ag->active) continue;
const float* pos = ag->corridor.getPos(); 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); dd.begin(DU_DRAW_QUADS);
const dtProximityGrid* grid = crowd->getGrid(); const dtProximityGrid* grid = crowd->getGrid();
@ -243,13 +243,14 @@ void CrowdToolState::handleRender()
{ {
for (int x = bounds[0]; x <= bounds[2]; ++x) 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; if (!count) continue;
unsigned int col = duRGBA(128,0,0,dtMin(count*40,255)); 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,y*cs,gridz, col);
dd.vertex(x*cs+cs, gridy, y*cs+cs, col); dd.vertex(x*cs+cs,y*cs+cs,gridz,col);
dd.vertex(x*cs+cs, gridy, y*cs, col); dd.vertex(x*cs,y*cs+cs,gridz,col);
dd.vertex(x*cs,y*cs,gridz, col);
} }
} }
dd.end(); dd.end();
@ -272,8 +273,8 @@ void CrowdToolState::handleRender()
const int idx = (trail->htrail + AGENT_MAX_TRAIL-j) % AGENT_MAX_TRAIL; const int idx = (trail->htrail + AGENT_MAX_TRAIL-j) % AGENT_MAX_TRAIL;
const float* v = &trail->trail[idx*3]; const float* v = &trail->trail[idx*3];
float a = 1 - j/(float)AGENT_MAX_TRAIL; 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(prev[0],prev[1],prev[2]+0.1f, 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(v[0],v[1],v[2]+0.1f, duRGBA(0,0,0,(int)(128*a)));
preva = a; preva = a;
dtVcopy(prev, v); dtVcopy(prev, v);
} }
@ -302,14 +303,14 @@ void CrowdToolState::handleRender()
{ {
const float* va = j == 0 ? pos : &ag->cornerVerts[(j-1)*3]; const float* va = j == 0 ? pos : &ag->cornerVerts[(j-1)*3];
const float* vb = &ag->cornerVerts[j*3]; const float* vb = &ag->cornerVerts[j*3];
dd.vertex(va[0],va[1]+radius,va[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]+radius,vb[2], 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) if (ag->ncorners && ag->cornerFlags[ag->ncorners-1] & DT_STRAIGHTPATH_OFFMESH_CONNECTION)
{ {
const float* v = &ag->cornerVerts[(ag->ncorners-1)*3]; 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],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(); dd.end();
@ -343,8 +344,8 @@ void CrowdToolState::handleRender()
if (m_toolParams.m_showCollisionSegments) if (m_toolParams.m_showCollisionSegments)
{ {
const float* center = ag->boundary.getCenter(); const float* center = ag->boundary.getCenter();
duDebugDrawCross(&dd, center[0],center[1]+radius,center[2], 0.2f, duRGBA(192,0,128,255), 2.0f); duDebugDrawCross(&dd, center[0],center[1],center[2]+radius, 0.2f, duRGBA(192,0,128,255), 2.0f);
duDebugDrawCircle(&dd, center[0],center[1]+radius,center[2], ag->params.collisionQueryRange, duDebugDrawCircle(&dd, center[0],center[1],center[2]+radius, ag->params.collisionQueryRange,
duRGBA(192,0,128,128), 2.0f); duRGBA(192,0,128,128), 2.0f);
dd.begin(DU_DRAW_LINES, 3.0f); dd.begin(DU_DRAW_LINES, 3.0f);
@ -362,7 +363,7 @@ void CrowdToolState::handleRender()
if (m_toolParams.m_showNeis) 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); duRGBA(0,192,128,128), 2.0f);
dd.begin(DU_DRAW_LINES, 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); const dtCrowdAgent* nei = crowd->getAgent(ag->neis[j].idx);
if (nei) if (nei)
{ {
dd.vertex(pos[0],pos[1]+radius,pos[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]+radius,nei->npos[2], duRGBA(0,192,128,128)); dd.vertex(nei->npos[0],nei->npos[1],nei->npos[2]+radius, duRGBA(0,192,128,128));
} }
} }
dd.end(); dd.end();
@ -383,8 +384,8 @@ void CrowdToolState::handleRender()
if (m_toolParams.m_showOpt) if (m_toolParams.m_showOpt)
{ {
dd.begin(DU_DRAW_LINES, 2.0f); 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.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]+0.3f,m_agentDebug.optEnd[2], 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(); dd.end();
} }
} }
@ -428,7 +429,6 @@ void CrowdToolState::handleRender()
pos[0]+radius, pos[1]+radius, pos[2]+height, col); pos[0]+radius, pos[1]+radius, pos[2]+height, col);
} }
if (m_toolParams.m_showVO) if (m_toolParams.m_showVO)
{ {
for (int i = 0; i < crowd->getAgentCount(); i++) for (int i = 0; i < crowd->getAgentCount(); i++)
@ -457,10 +457,10 @@ void CrowdToolState::handleRender()
const float pen2 = vod->getSamplePreferredSidePenalty(j); const float pen2 = vod->getSamplePreferredSidePenalty(j);
unsigned int col = duLerpCol(duRGBA(255,255,255,220), duRGBA(128,96,0,220), (int)(pen*255)); 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)); 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+p[1]-sr, dz, 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, dz+p[2]+sr, col); dd.vertex(dx+p[0]+sr, dy+p[1]+sr, dz, 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.end(); dd.end();
} }
@ -491,11 +491,11 @@ void CrowdToolState::handleRender()
duDebugDrawCircle(&dd, pos[0], pos[1], pos[2]+height, radius, col, 2.0f); duDebugDrawCircle(&dd, pos[0], pos[1], pos[2]+height, radius, col, 2.0f);
duDebugDrawArrow(&dd, pos[0],pos[1],pos[2]+height, 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); 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, 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); 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) static void calcVel(float* vel, const float* pos, const float* tgt, const float speed)
{ {
dtVsub(vel, tgt, pos); dtVsub(vel, tgt, pos);
vel[1] = 0.0; vel[2] = 0.0;
dtVnormalize(vel); dtVnormalize(vel);
dtVscale(vel, vel, speed); dtVscale(vel, vel, speed);
} }
@ -1029,7 +1029,6 @@ void CrowdTool::handleClick(const float* s, const float* p, bool shift)
} }
} }
} }
} }
void CrowdTool::handleStep() 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]; float dir0[3], dir1[3];
dtVsub(dir0, p0, ag->npos); dtVsub(dir0, p0, ag->npos);
dtVsub(dir1, p1, ag->npos); dtVsub(dir1, p1, ag->npos);
dir0[1] = 0; dir0[2] = 0;
dir1[1] = 0; dir1[2] = 0;
float len0 = dtVlen(dir0); float len0 = dtVlen(dir0);
float len1 = dtVlen(dir1); float len1 = dtVlen(dir1);
@ -127,8 +127,8 @@ static void calcSmoothSteerDirection(const dtCrowdAgent* ag, float* dir)
dtVscale(dir1,dir1,1.0f/len1); dtVscale(dir1,dir1,1.0f/len1);
dir[0] = dir0[0] - dir1[0]*len0*0.5f; dir[0] = dir0[0] - dir1[0]*len0*0.5f;
dir[1] = 0; dir[1] = dir0[1] - dir1[1]*len0*0.5f;
dir[2] = dir0[2] - dir1[2]*len0*0.5f; dir[2] = 0;
dtVnormalize(dir); dtVnormalize(dir);
} }
@ -141,7 +141,7 @@ static void calcStraightSteerDirection(const dtCrowdAgent* ag, float* dir)
return; return;
} }
dtVsub(dir, &ag->cornerVerts[0], ag->npos); dtVsub(dir, &ag->cornerVerts[0], ag->npos);
dir[1] = 0; dir[2] = 0;
dtVnormalize(dir); dtVnormalize(dir);
} }
@ -193,8 +193,8 @@ static int getNeighbours(const float* pos, const float height, const float range
static const int MAX_NEIS = 32; static const int MAX_NEIS = 32;
unsigned short ids[MAX_NEIS]; unsigned short ids[MAX_NEIS];
int nids = grid->queryItems(pos[0]-range, pos[2]-range, int nids = grid->queryItems(pos[0]-range, pos[1]-range,
pos[0]+range, pos[2]+range, pos[0]+range, pos[1]+range,
ids, MAX_NEIS); ids, MAX_NEIS);
for (int i = 0; i < nids; ++i) 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. // Check for overlap.
float diff[3]; float diff[3];
dtVsub(diff, pos, ag->npos); 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; continue;
diff[1] = 0; diff[1] = 0;
const float distSqr = dtVlenSqr(diff); const float distSqr = dtVlenSqr(diff);
@ -524,7 +524,7 @@ int dtCrowd::addAgent(const float* pos, const dtCrowdAgentParams* params)
if (idx == -1) if (idx == -1)
return -1; return -1;
dtCrowdAgent* ag = &m_agents[idx]; dtCrowdAgent* ag = &m_agents[idx];
updateAgentParameters(idx, params); 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->corridor.optimizePathTopology(m_navquery, &m_filters[ag->params.queryFilterType]);
ag->topologyOptTime = 0; ag->topologyOptTime = 0;
} }
} }
void dtCrowd::checkPathValidity(dtCrowdAgent** agents, const int nagents, const float dt) 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) void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
{ {
m_velocitySampleCount = 0; m_velocitySampleCount = 0;
@ -1069,7 +1067,7 @@ void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
dtCrowdAgent* ag = agents[i]; dtCrowdAgent* ag = agents[i];
const float* p = ag->npos; const float* p = ag->npos;
const float r = ag->params.radius; 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. // Get nearby navmesh segments and agents to collide with.
@ -1225,7 +1223,7 @@ void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
float diff[3]; float diff[3];
dtVsub(diff, ag->npos, nei->npos); dtVsub(diff, ag->npos, nei->npos);
diff[1] = 0; diff[2] = 0;
const float distSqr = dtVlenSqr(diff); const float distSqr = dtVlenSqr(diff);
if (distSqr < 0.00001f) if (distSqr < 0.00001f)
@ -1345,7 +1343,7 @@ void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
float diff[3]; float diff[3];
dtVsub(diff, ag->npos, nei->npos); dtVsub(diff, ag->npos, nei->npos);
diff[1] = 0; diff[2] = 0;
float dist = dtVlenSqr(diff); float dist = dtVlenSqr(diff);
if (dist > dtSqr(ag->params.radius + nei->params.radius)) 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. // Agents on top of each other, try to choose diverging separation directions.
if (idx0 > idx1) if (idx0 > idx1)
dtVset(diff, -ag->dvel[2],0,ag->dvel[0]); dtVset(diff, -ag->dvel[1],0,ag->dvel[0]);
else else
dtVset(diff, ag->dvel[2],0,-ag->dvel[0]); dtVset(diff, ag->dvel[1],0,-ag->dvel[0]);
pen = 0.01f; pen = 0.01f;
} }
else else
@ -1366,7 +1364,7 @@ void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
pen = (1.0f/dist) * (pen*0.5f) * COLLISION_RESOLVE_FACTOR; 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; w += 1.0f;
} }
@ -1405,7 +1403,6 @@ void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
ag->corridor.reset(ag->corridor.getFirstPoly(), ag->npos); ag->corridor.reset(ag->corridor.getFirstPoly(), ag->npos);
ag->partial = false; ag->partial = false;
} }
} }
// Update agents using off-mesh connection. // 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->vel, 0,0,0);
dtVset(ag->dvel, 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. // Special case when the agent is very close to the segment.
float sdir[3], snorm[3]; float sdir[3], snorm[3];
dtVsub(sdir, seg->q, seg->p); dtVsub(sdir, seg->q, seg->p);
snorm[0] = -sdir[2]; snorm[0] = -sdir[1];
snorm[2] = sdir[0]; snorm[1] = sdir[0];
// If the velocity is pointing towards the segment, no collision. // If the velocity is pointing towards the segment, no collision.
if (dtVdot2D(snorm, vcand) < 0.0f) if (dtVdot2D(snorm, vcand) < 0.0f)
continue; continue;
@ -450,7 +450,7 @@ int dtObstacleAvoidanceQuery::sampleVelocityGrid(const float* pos, const float r
debug->reset(); debug->reset();
const float cvx = dvel[0] * m_params.velBias; 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 cs = vmax * 2 * (1 - m_params.velBias) / (float)(m_params.gridSize-1);
const float half = (m_params.gridSize-1)*cs*0.5f; 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]; float vcand[3];
vcand[0] = cvx + x*cs - half; vcand[0] = cvx + x*cs - half;
vcand[1] = 0; vcand[1] = cvy + y*cs - half;
vcand[2] = cvz + 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); const float penalty = processSample(vcand, cs, pos,rad,vel,dvel, minPenalty, debug);
ns++; 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) 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) if (d==0)
return; return;
d = 1.0f / d; d = 1.0f / d;
v[0] *= 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) inline void dtRorate2D(float* dest, const float* v, float ang)
{ {
float c = cosf(ang); float c = cosf(ang);
float s = sinf(ang); float s = sinf(ang);
dest[0] = v[0]*c - v[2]*s; dest[0] = v[0]*c - v[1]*s;
dest[2] = v[0]*s + v[2]*c; dest[1] = v[0]*s + v[1]*c;
dest[1] = v[1]; dest[2] = v[2];
} }
@ -550,7 +550,7 @@ int dtObstacleAvoidanceQuery::sampleVelocityAdaptive(const float* pos, const flo
{ {
const float r = (float)(nr-j)/(float)nr; const float r = (float)(nr-j)/(float)nr;
pat[npat*2+0] = ddir[(j%2)*3] * r; 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* last1 = pat + npat*2;
float* last2 = last1; float* last2 = last1;
npat++; npat++;
@ -594,10 +594,10 @@ int dtObstacleAvoidanceQuery::sampleVelocityAdaptive(const float* pos, const flo
{ {
float vcand[3]; float vcand[3];
vcand[0] = res[0] + pat[i*2+0]*cr; vcand[0] = res[0] + pat[i*2+0]*cr;
vcand[1] = 0; vcand[1] = res[1] + pat[i*2+1]*cr;
vcand[2] = res[2] + 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); const float penalty = processSample(vcand,cr/10, pos,rad,vel,dvel, minPenalty, debug);
ns++; 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) if (furthestPath == -1 || furthestVisited == -1)
return npath; return npath;
// Concatenate paths. // Concatenate paths.
// Adjust beginning of the buffer to include the visited. // Adjust beginning of the buffer to include the visited.
const int req = furthestVisited; 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); m_npath = dtMergeCorridorStartMoved(m_path, m_npath, m_maxPath, visited, nvisited);
// Adjust the position to stay on top of the navmesh. // 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); navquery->getPolyHeight(m_path[0], result, &h);
result[1] = h; result[2] = h;
dtVcopy(m_pos, result); dtVcopy(m_pos, result);
return true; return true;
} }