void NavMeshTesterTool::drawAgent(const float* pos, float r, float h, float c, const unsigned int col)
{
DebugDrawGL dd;
dd.depthMask(false);
// Agent dimensions.
duDebugDrawCylinderWire(&dd, pos[0]-r, pos[1]+0.02f, pos[2]-r, pos[0]+r, pos[1]+h, pos[2]+r, col, 2.0f);
duDebugDrawCircle(&dd, pos[0],pos[1]+c,pos[2],r,duRGBA(0,0,0,64),1.0f);
unsigned int colb = duRGBA(0,0,0,196);
dd.begin(DU_DRAW_LINES);
dd.vertex(pos[0], pos[1]-c, pos[2], colb);
dd.vertex(pos[0], pos[1]+c, pos[2], colb);
dd.vertex(pos[0]-r/2, pos[1]+0.02f, pos[2], colb);
dd.vertex(pos[0]+r/2, pos[1]+0.02f, pos[2], colb);
dd.vertex(pos[0], pos[1]+0.02f, pos[2]-r/2, colb);
dd.vertex(pos[0], pos[1]+0.02f, pos[2]+r/2, colb);
dd.end();
dd.depthMask(true);
}
void CrowdTool::handleRender()
{
DebugDrawGL dd;
const float s = m_sample->getAgentRadius();
dtNavMesh* nmesh = m_sample->getNavMesh();
if (!nmesh)
return;
dtNavMeshQuery* navquery = m_sample->getNavMeshQuery();
if (m_showNodes)
{
if (navquery)
duDebugDrawNavMeshNodes(&dd, *navquery);
}
dd.depthMask(false);
// Draw paths
if (m_showPath)
{
for (int i = 0; i < m_crowd.getAgentCount(); ++i)
{
const Agent* ag = m_crowd.getAgent(i);
if (!ag->active) continue;
const dtPolyRef* path = ag->corridor.getPath();
const int npath = ag->corridor.getPathCount();
for (int i = 0; i < npath; ++i)
duDebugDrawNavMeshPoly(&dd, *nmesh, path[i], duRGBA(0,0,0,32));
}
}
if (m_targetRef)
duDebugDrawCross(&dd, m_targetPos[0],m_targetPos[1]+0.1f,m_targetPos[2], s, duRGBA(255,255,255,192), 2.0f);
// Occupancy grid.
if (m_showGrid)
{
float gridy = -FLT_MAX;
for (int i = 0; i < m_crowd.getAgentCount(); ++i)
{
const Agent* ag = m_crowd.getAgent(i);
if (!ag->active) continue;
const float* pos = ag->corridor.getPos();
gridy = dtMax(gridy, pos[1]);
}
gridy += 1.0f;
dd.begin(DU_DRAW_QUADS);
const ProximityGrid* grid = m_crowd.getGrid();
const int* bounds = grid->getBounds();
const float cs = grid->getCellSize();
for (int y = bounds[1]; y <= bounds[3]; ++y)
{
for (int x = bounds[0]; x <= bounds[2]; ++x)
{
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.end();
}
// Trail
for (int i = 0; i < m_crowd.getAgentCount(); ++i)
{
const Agent* ag = m_crowd.getAgent(i);
if (!ag->active) continue;
const float* pos = ag->npos;
dd.begin(DU_DRAW_LINES,3.0f);
float prev[3], preva = 1;
dtVcopy(prev, pos);
for (int j = 0; j < AGENT_MAX_TRAIL-1; ++j)
{
const int idx = (ag->htrail + AGENT_MAX_TRAIL-j) % AGENT_MAX_TRAIL;
const float* v = &ag->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)));
preva = a;
dtVcopy(prev, v);
}
dd.end();
}
// Corners & co
for (int i = 0; i < m_crowd.getAgentCount(); ++i)
{
const Agent* ag = m_crowd.getAgent(i);
if (!ag->active) continue;
const float radius = ag->radius;
const float* pos = ag->npos;
if (m_showCorners)
{
if (ag->ncorners)
{
dd.begin(DU_DRAW_LINES, 2.0f);
for (int j = 0; j < ag->ncorners; ++j)
{
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.end();
if (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();*/
}
}
}
if (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->collisionQueryRange,
duRGBA(192,0,128,128), 2.0f);
dd.begin(DU_DRAW_LINES, 3.0f);
for (int j = 0; j < ag->boundary.getSegmentCount(); ++j)
{
const float* s = ag->boundary.getSegment(j);
unsigned int col = duRGBA(192,0,128,192);
if (dtTriArea2D(pos, s, s+3) < 0.0f)
col = duDarkenCol(col);
duAppendArrow(&dd, s[0],s[1]+0.2f,s[2], s[3],s[4]+0.2f,s[5], 0.0f, 0.3f, col);
}
dd.end();
}
if (m_showOpt)
{
dd.begin(DU_DRAW_LINES, 2.0f);
dd.vertex(ag->opts[0],ag->opts[1]+0.3f,ag->opts[2], duRGBA(0,128,0,192));
dd.vertex(ag->opte[0],ag->opte[1]+0.3f,ag->opte[2], duRGBA(0,128,0,192));
dd.end();
}
}
// Agent cylinders.
for (int i = 0; i < m_crowd.getAgentCount(); ++i)
{
const Agent* ag = m_crowd.getAgent(i);
if (!ag->active) continue;
const float radius = ag->radius;
const float* pos = ag->npos;
duDebugDrawCircle(&dd, pos[0], pos[1], pos[2], radius, duRGBA(0,0,0,32), 2.0f);
}
for (int i = 0; i < m_crowd.getAgentCount(); ++i)
{
const Agent* ag = m_crowd.getAgent(i);
if (!ag->active) continue;
const float height = ag->height;
const float radius = ag->radius;
const float* pos = ag->npos;
duDebugDrawCylinder(&dd, pos[0]-radius, pos[1]+radius*0.1f, pos[2]-radius,
pos[0]+radius, pos[1]+height, pos[2]+radius,
duRGBA(220,220,220,128));
}
// Velocity stuff.
for (int i = 0; i < m_crowd.getAgentCount(); ++i)
{
const Agent* ag = m_crowd.getAgent(i);
if (!ag->active) continue;
const float radius = ag->radius;
const float height = ag->height;
const float* pos = ag->npos;
const float* vel = ag->vel;
const float* dvel = ag->dvel;
duDebugDrawCircle(&dd, pos[0], pos[1]+height, pos[2], radius, duRGBA(220,220,220,192), 2.0f);
if (m_showVO)
{
// Draw detail about agent sela
const dtObstacleAvoidanceDebugData* debug = m_crowd.getVODebugData(i);
const float dx = pos[0];
const float dy = pos[1]+height;
const float dz = pos[2];
dd.begin(DU_DRAW_QUADS);
for (int i = 0; i < debug->getSampleCount(); ++i)
{
const float* p = debug->getSampleVelocity(i);
const float sr = debug->getSampleSize(i);
const float pen = debug->getSamplePenalty(i);
const float pen2 = debug->getSamplePreferredSidePenalty(i);
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.end();
}
duDebugDrawArrow(&dd, pos[0],pos[1]+height,pos[2],
pos[0]+dvel[0],pos[1]+height+dvel[1],pos[2]+dvel[2],
0.0f, 0.4f, duRGBA(0,192,255,192), 1.0f);
duDebugDrawArrow(&dd, pos[0],pos[1]+height,pos[2],
pos[0]+vel[0],pos[1]+height+vel[1],pos[2]+vel[2],
0.0f, 0.4f, duRGBA(0,0,0,192), 2.0f);
}
// Targets
for (int i = 0; i < m_crowd.getAgentCount(); ++i)
{
const Agent* ag = m_crowd.getAgent(i);
if (!ag->active) continue;
const float* pos = ag->npos;
const float* target = ag->corridor.getTarget();
if (m_showTargets)
{
duDebugDrawArc(&dd, pos[0], pos[1], pos[2], target[0], target[1], target[2],
0.25f, 0, 0.4f, duRGBA(0,0,0,128), 1.0f);
}
}
dd.depthMask(true);
}
void NavMeshTesterTool::handleRender()
{
DebugDrawGL dd;
static const unsigned int startCol = duRGBA(128,25,0,192);
static const unsigned int endCol = duRGBA(51,102,0,129);
static const unsigned int pathCol = duRGBA(0,0,0,64);
const float agentRadius = m_sample->getAgentRadius();
const float agentHeight = m_sample->getAgentHeight();
const float agentClimb = m_sample->getAgentClimb();
dd.depthMask(false);
if (m_sposSet)
drawAgent(m_spos, agentRadius, agentHeight, agentClimb, startCol);
if (m_eposSet)
drawAgent(m_epos, agentRadius, agentHeight, agentClimb, endCol);
dd.depthMask(true);
if (!m_navMesh)
{
return;
}
if (m_toolMode == TOOLMODE_PATHFIND_FOLLOW)
{
duDebugDrawNavMeshPoly(&dd, *m_navMesh, m_startRef, startCol);
duDebugDrawNavMeshPoly(&dd, *m_navMesh, m_endRef, endCol);
if (m_npolys)
{
for (int i = 1; i < m_npolys-1; ++i)
duDebugDrawNavMeshPoly(&dd, *m_navMesh, m_polys[i], pathCol);
}
if (m_nsmoothPath)
{
dd.depthMask(false);
const unsigned int pathCol = duRGBA(0,0,0,220);
dd.begin(DU_DRAW_LINES, 3.0f);
for (int i = 0; i < m_nsmoothPath; ++i)
dd.vertex(m_smoothPath[i*3], m_smoothPath[i*3+1]+0.1f, m_smoothPath[i*3+2], pathCol);
dd.end();
dd.depthMask(true);
}
if (m_pathIterNum)
{
duDebugDrawNavMeshPoly(&dd, *m_navMesh, m_pathIterPolys[0], duRGBA(255,255,255,128));
dd.depthMask(false);
dd.begin(DU_DRAW_LINES, 1.0f);
const unsigned int prevCol = duRGBA(255,192,0,220);
const unsigned int curCol = duRGBA(255,255,255,220);
const unsigned int steerCol = duRGBA(0,192,255,220);
dd.vertex(m_prevIterPos[0],m_prevIterPos[1]-0.3f,m_prevIterPos[2], prevCol);
dd.vertex(m_prevIterPos[0],m_prevIterPos[1]+0.3f,m_prevIterPos[2], prevCol);
dd.vertex(m_iterPos[0],m_iterPos[1]-0.3f,m_iterPos[2], curCol);
dd.vertex(m_iterPos[0],m_iterPos[1]+0.3f,m_iterPos[2], curCol);
dd.vertex(m_prevIterPos[0],m_prevIterPos[1]+0.3f,m_prevIterPos[2], prevCol);
dd.vertex(m_iterPos[0],m_iterPos[1]+0.3f,m_iterPos[2], prevCol);
dd.vertex(m_prevIterPos[0],m_prevIterPos[1]+0.3f,m_prevIterPos[2], steerCol);
dd.vertex(m_steerPos[0],m_steerPos[1]+0.3f,m_steerPos[2], steerCol);
for (int i = 0; i < m_steerPointCount-1; ++i)
{
dd.vertex(m_steerPoints[i*3+0],m_steerPoints[i*3+1]+0.2f,m_steerPoints[i*3+2], duDarkenCol(steerCol));
dd.vertex(m_steerPoints[(i+1)*3+0],m_steerPoints[(i+1)*3+1]+0.2f,m_steerPoints[(i+1)*3+2], duDarkenCol(steerCol));
}
dd.end();
dd.depthMask(true);
}
}
else if (m_toolMode == TOOLMODE_PATHFIND_STRAIGHT || m_toolMode == TOOLMODE_PATHFIND_SLICED)
{
duDebugDrawNavMeshPoly(&dd, *m_navMesh, m_startRef, startCol);
duDebugDrawNavMeshPoly(&dd, *m_navMesh, m_endRef, endCol);
if (m_npolys)
{
for (int i = 1; i < m_npolys-1; ++i)
duDebugDrawNavMeshPoly(&dd, *m_navMesh, m_polys[i], pathCol);
}
if (m_nstraightPath)
{
dd.depthMask(false);
const unsigned int pathCol = duRGBA(64,16,0,220);
const unsigned int offMeshCol = duRGBA(128,96,0,220);
dd.begin(DU_DRAW_LINES, 2.0f);
for (int i = 0; i < m_nstraightPath-1; ++i)
{
unsigned int col = 0;
if (m_straightPathFlags[i] & DT_STRAIGHTPATH_OFFMESH_CONNECTION)
col = offMeshCol;
else
col = pathCol;
dd.vertex(m_straightPath[i*3], m_straightPath[i*3+1]+0.4f, m_straightPath[i*3+2], col);
dd.vertex(m_straightPath[(i+1)*3], m_straightPath[(i+1)*3+1]+0.4f, m_straightPath[(i+1)*3+2], col);
}
dd.end();
dd.begin(DU_DRAW_POINTS, 6.0f);
for (int i = 0; i < m_nstraightPath; ++i)
{
unsigned int col = 0;
if (m_straightPathFlags[i] & DT_STRAIGHTPATH_START)
col = startCol;
else if (m_straightPathFlags[i] & DT_STRAIGHTPATH_START)
col = endCol;
else if (m_straightPathFlags[i] & DT_STRAIGHTPATH_OFFMESH_CONNECTION)
col = offMeshCol;
else
col = pathCol;
dd.vertex(m_straightPath[i*3], m_straightPath[i*3+1]+0.4f, m_straightPath[i*3+2], pathCol);
}
dd.end();
dd.depthMask(true);
}
}
else if (m_toolMode == TOOLMODE_RAYCAST)
{
duDebugDrawNavMeshPoly(&dd, *m_navMesh, m_startRef, startCol);
if (m_nstraightPath)
{
for (int i = 1; i < m_npolys; ++i)
duDebugDrawNavMeshPoly(&dd, *m_navMesh, m_polys[i], pathCol);
dd.depthMask(false);
const unsigned int pathCol = m_hitResult ? duRGBA(64,16,0,220) : duRGBA(240,240,240,220);
dd.begin(DU_DRAW_LINES, 2.0f);
for (int i = 0; i < m_nstraightPath-1; ++i)
{
dd.vertex(m_straightPath[i*3], m_straightPath[i*3+1]+0.4f, m_straightPath[i*3+2], pathCol);
dd.vertex(m_straightPath[(i+1)*3], m_straightPath[(i+1)*3+1]+0.4f, m_straightPath[(i+1)*3+2], pathCol);
}
dd.end();
dd.begin(DU_DRAW_POINTS, 4.0f);
for (int i = 0; i < m_nstraightPath; ++i)
dd.vertex(m_straightPath[i*3], m_straightPath[i*3+1]+0.4f, m_straightPath[i*3+2], pathCol);
dd.end();
if (m_hitResult)
{
const unsigned int hitCol = duRGBA(0,0,0,128);
dd.begin(DU_DRAW_LINES, 2.0f);
dd.vertex(m_hitPos[0], m_hitPos[1] + 0.4f, m_hitPos[2], hitCol);
dd.vertex(m_hitPos[0] + m_hitNormal[0]*agentRadius,
m_hitPos[1] + 0.4f + m_hitNormal[1]*agentRadius,
m_hitPos[2] + m_hitNormal[2]*agentRadius, hitCol);
dd.end();
}
dd.depthMask(true);
}
}
else if (m_toolMode == TOOLMODE_DISTANCE_TO_WALL)
{
duDebugDrawNavMeshPoly(&dd, *m_navMesh, m_startRef, startCol);
dd.depthMask(false);
duDebugDrawCircle(&dd, m_spos[0], m_spos[1]+agentHeight/2, m_spos[2], m_distanceToWall, duRGBA(64,16,0,220), 2.0f);
dd.begin(DU_DRAW_LINES, 3.0f);
dd.vertex(m_hitPos[0], m_hitPos[1] + 0.02f, m_hitPos[2], duRGBA(0,0,0,192));
dd.vertex(m_hitPos[0], m_hitPos[1] + agentHeight, m_hitPos[2], duRGBA(0,0,0,192));
dd.end();
dd.depthMask(true);
}
else if (m_toolMode == TOOLMODE_FIND_POLYS_IN_CIRCLE)
{
for (int i = 0; i < m_npolys; ++i)
{
duDebugDrawNavMeshPoly(&dd, *m_navMesh, m_polys[i], pathCol);
dd.depthMask(false);
if (m_parent[i])
{
float p0[3], p1[3];
dd.depthMask(false);
getPolyCenter(m_navMesh, m_parent[i], p0);
getPolyCenter(m_navMesh, m_polys[i], p1);
duDebugDrawArc(&dd, p0[0],p0[1],p0[2], p1[0],p1[1],p1[2], 0.25f, 0.0f, 0.4f, duRGBA(0,0,0,128), 2.0f);
dd.depthMask(true);
}
dd.depthMask(true);
}
if (m_sposSet && m_eposSet)
{
dd.depthMask(false);
const float dx = m_epos[0] - m_spos[0];
const float dz = m_epos[2] - m_spos[2];
const float dist = sqrtf(dx*dx + dz*dz);
duDebugDrawCircle(&dd, m_spos[0], m_spos[1]+agentHeight/2, m_spos[2], dist, duRGBA(64,16,0,220), 2.0f);
dd.depthMask(true);
}
}
else if (m_toolMode == TOOLMODE_FIND_POLYS_IN_SHAPE)
{
for (int i = 0; i < m_npolys; ++i)
{
duDebugDrawNavMeshPoly(&dd, *m_navMesh, m_polys[i], pathCol);
dd.depthMask(false);
if (m_parent[i])
{
float p0[3], p1[3];
dd.depthMask(false);
getPolyCenter(m_navMesh, m_parent[i], p0);
getPolyCenter(m_navMesh, m_polys[i], p1);
duDebugDrawArc(&dd, p0[0],p0[1],p0[2], p1[0],p1[1],p1[2], 0.25f, 0.0f, 0.4f, duRGBA(0,0,0,128), 2.0f);
dd.depthMask(true);
}
dd.depthMask(true);
}
if (m_sposSet && m_eposSet)
{
dd.depthMask(false);
const unsigned int col = duRGBA(64,16,0,220);
dd.begin(DU_DRAW_LINES, 2.0f);
for (int i = 0, j = 3; i < 4; j=i++)
{
const float* p0 = &m_queryPoly[j*3];
const float* p1 = &m_queryPoly[i*3];
dd.vertex(p0, col);
dd.vertex(p1, col);
}
dd.end();
dd.depthMask(true);
}
}
else if (m_toolMode == TOOLMODE_FIND_LOCAL_NEIGHBOURHOOD)
{
for (int i = 0; i < m_npolys; ++i)
{
duDebugDrawNavMeshPoly(&dd, *m_navMesh, m_polys[i], pathCol);
dd.depthMask(false);
if (m_parent[i])
{
float p0[3], p1[3];
dd.depthMask(false);
getPolyCenter(m_navMesh, m_parent[i], p0);
getPolyCenter(m_navMesh, m_polys[i], p1);
duDebugDrawArc(&dd, p0[0],p0[1],p0[2], p1[0],p1[1],p1[2], 0.25f, 0.0f, 0.4f, duRGBA(0,0,0,128), 2.0f);
dd.depthMask(true);
}
static const int MAX_SEGS = DT_VERTS_PER_POLYGON*2;
float segs[MAX_SEGS*6];
int nsegs = 0;
m_navQuery->getPolyWallSegments(m_polys[i], &m_filter, segs, &nsegs, MAX_SEGS);
dd.begin(DU_DRAW_LINES, 2.0f);
for (int j = 0; j < nsegs; ++j)
{
const float* s = &segs[j*6];
// Skip too distant segments.
float tseg;
float distSqr = dtDistancePtSegSqr2D(m_spos, s, s+3, tseg);
if (distSqr > dtSqr(m_neighbourhoodRadius))
continue;
float delta[3], norm[3], p0[3], p1[3];
dtVsub(delta, s+3,s);
dtVmad(p0, s, delta, 0.5f);
norm[0] = delta[2];
norm[1] = 0;
norm[2] = -delta[0];
dtVnormalize(norm);
dtVmad(p1, p0, norm, agentRadius*0.5f);
// Skip backfacing segments.
unsigned int col = duRGBA(255,255,255,192);
if (dtTriArea2D(m_spos, s, s+3) < 0.0f)
col = duRGBA(255,255,255,64);
dd.vertex(p0[0],p0[1]+agentClimb,p0[2],duRGBA(0,0,0,128));
dd.vertex(p1[0],p1[1]+agentClimb,p1[2],duRGBA(0,0,0,128));
dd.vertex(s[0],s[1]+agentClimb,s[2],col);
dd.vertex(s[3],s[4]+agentClimb,s[5],col);
}
dd.end();
dd.depthMask(true);
}
if (m_sposSet)
{
dd.depthMask(false);
duDebugDrawCircle(&dd, m_spos[0], m_spos[1]+agentHeight/2, m_spos[2], m_neighbourhoodRadius, duRGBA(64,16,0,220), 2.0f);
dd.depthMask(true);
}
}
}
void cocos2d::NavMesh::drawAgents()
{
for (auto iter : _agentList)
{
if (iter){
auto agent = _crowed->getAgent(iter->_agentID);
float r = iter->getRadius();
float h = iter->getHeight();
unsigned int col = duRGBA(0, 0, 0, 32);
duDebugDrawCircle(&_debugDraw, agent->npos[0], agent->npos[1], agent->npos[2], r, col, 2.0f);
col = duRGBA(220, 220, 220, 128);
if (agent->targetState == DT_CROWDAGENT_TARGET_REQUESTING || agent->targetState == DT_CROWDAGENT_TARGET_WAITING_FOR_QUEUE)
col = duLerpCol(col, duRGBA(128, 0, 255, 128), 32);
else if (agent->targetState == DT_CROWDAGENT_TARGET_WAITING_FOR_PATH)
col = duLerpCol(col, duRGBA(128, 0, 255, 128), 128);
else if (agent->targetState == DT_CROWDAGENT_TARGET_FAILED)
col = duRGBA(255, 32, 16, 128);
else if (agent->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
col = duLerpCol(col, duRGBA(64, 255, 0, 128), 128);
duDebugDrawCylinder(&_debugDraw, agent->npos[0] - r, agent->npos[1] + r*0.1f, agent->npos[2] - r,
agent->npos[0] + r, agent->npos[1] + h, agent->npos[2] + r, col);
}
}
// Velocity stuff.
for (auto iter : _agentList)
{
if (iter){
auto agent = _crowed->getAgent(iter->_agentID);
const float radius = agent->params.radius;
const float height = agent->params.height;
const float* pos = agent->npos;
const float* vel = agent->vel;
// const float* dvel = agent->dvel;
unsigned int col = duRGBA(220, 220, 220, 192);
if (agent->targetState == DT_CROWDAGENT_TARGET_REQUESTING || agent->targetState == DT_CROWDAGENT_TARGET_WAITING_FOR_QUEUE)
col = duLerpCol(col, duRGBA(128, 0, 255, 192), 32);
else if (agent->targetState == DT_CROWDAGENT_TARGET_WAITING_FOR_PATH)
col = duLerpCol(col, duRGBA(128, 0, 255, 192), 128);
else if (agent->targetState == DT_CROWDAGENT_TARGET_FAILED)
col = duRGBA(255, 32, 16, 192);
else if (agent->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
col = duLerpCol(col, duRGBA(64, 255, 0, 192), 128);
duDebugDrawCircle(&_debugDraw, pos[0], pos[1] + height, pos[2], radius, col, 2.0f);
//duDebugDrawArrow(&_debugDraw, pos[0], pos[1] + height, pos[2],
// pos[0] + dvel[0], pos[1] + height + dvel[1], pos[2] + dvel[2],
// 0.0f, 0.4f, duRGBA(0, 192, 255, 192), 2.0f);
duDebugDrawArrow(&_debugDraw, pos[0], pos[1] + height, pos[2],
pos[0] + vel[0], pos[1] + height + vel[1], pos[2] + vel[2],
0.0f, 0.4f, duRGBA(0, 0, 0, 160), 2.0f);
}
}
}
void CrowdToolState::handleRender()
{
DebugDrawGL dd;
const float rad = m_sample->getAgentRadius();
dtNavMesh* nav = m_sample->getNavMesh();
dtCrowd* crowd = m_sample->getCrowd();
if (!nav || !crowd)
return;
if (m_toolParams.m_showNodes && crowd->getPathQueue())
{
const dtNavMeshQuery* navquery = crowd->getPathQueue()->getNavQuery();
if (navquery)
duDebugDrawNavMeshNodes(&dd, *navquery);
}
dd.depthMask(false);
// Draw paths
if (m_toolParams.m_showPath)
{
for (int i = 0; i < crowd->getAgentCount(); i++)
{
if (m_toolParams.m_showDetailAll == false && i != m_agentDebug.idx)
continue;
const dtCrowdAgent* ag =crowd->getAgent(i);
if (!ag->active)
continue;
const dtPolyRef* path = ag->corridor.getPath();
const int npath = ag->corridor.getPathCount();
for (int j = 0; j < npath; ++j)
duDebugDrawNavMeshPoly(&dd, *nav, path[j], duRGBA(255,255,255,24));
}
}
if (m_targetRef)
duDebugDrawCross(&dd, m_targetPos[0],m_targetPos[1]+0.1f,m_targetPos[2], rad, duRGBA(255,255,255,192), 2.0f);
// Occupancy grid.
if (m_toolParams.m_showGrid)
{
float gridy = -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]);
}
gridy += 1.0f;
dd.begin(DU_DRAW_QUADS);
const dtProximityGrid* grid = crowd->getGrid();
const int* bounds = grid->getBounds();
const float cs = grid->getCellSize();
for (int y = bounds[1]; y <= bounds[3]; ++y)
{
for (int x = bounds[0]; x <= bounds[2]; ++x)
{
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.end();
}
// Trail
for (int i = 0; i < crowd->getAgentCount(); ++i)
{
const dtCrowdAgent* ag = crowd->getAgent(i);
if (!ag->active) continue;
const AgentTrail* trail = &m_trails[i];
const float* pos = ag->npos;
dd.begin(DU_DRAW_LINES,3.0f);
float prev[3], preva = 1;
dtVcopy(prev, pos);
for (int j = 0; j < AGENT_MAX_TRAIL-1; ++j)
{
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)));
preva = a;
dtVcopy(prev, v);
}
dd.end();
}
// Corners & co
for (int i = 0; i < crowd->getAgentCount(); i++)
{
if (m_toolParams.m_showDetailAll == false && i != m_agentDebug.idx)
continue;
const dtCrowdAgent* ag =crowd->getAgent(i);
if (!ag->active)
continue;
const float radius = ag->params.radius;
const float* pos = ag->npos;
if (m_toolParams.m_showCorners)
{
if (ag->ncorners)
{
dd.begin(DU_DRAW_LINES, 2.0f);
for (int j = 0; j < ag->ncorners; ++j)
{
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));
}
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.end();
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();*/
}
}
}
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,
duRGBA(192,0,128,128), 2.0f);
dd.begin(DU_DRAW_LINES, 3.0f);
for (int j = 0; j < ag->boundary.getSegmentCount(); ++j)
{
const float* s = ag->boundary.getSegment(j);
unsigned int col = duRGBA(192,0,128,192);
if (dtTriArea2D(pos, s, s+3) < 0.0f)
col = duDarkenCol(col);
duAppendArrow(&dd, s[0],s[1]+0.2f,s[2], s[3],s[4]+0.2f,s[5], 0.0f, 0.3f, col);
}
dd.end();
}
if (m_toolParams.m_showNeis)
{
duDebugDrawCircle(&dd, pos[0],pos[1]+radius,pos[2], ag->params.collisionQueryRange,
duRGBA(0,192,128,128), 2.0f);
dd.begin(DU_DRAW_LINES, 2.0f);
for (int j = 0; j < ag->nneis; ++j)
{
// Get 'n'th active agent.
// TODO: fix this properly.
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.end();
}
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.end();
}
}
// Agent cylinders.
for (int i = 0; i < crowd->getAgentCount(); ++i)
{
const dtCrowdAgent* ag = crowd->getAgent(i);
if (!ag->active) continue;
const float radius = ag->params.radius;
const float* pos = ag->npos;
unsigned int col = duRGBA(0,0,0,32);
if (m_agentDebug.idx == i)
col = duRGBA(255,0,0,128);
duDebugDrawCircle(&dd, pos[0], pos[1], pos[2], radius, col, 2.0f);
}
for (int i = 0; i < crowd->getAgentCount(); ++i)
{
const dtCrowdAgent* ag = crowd->getAgent(i);
if (!ag->active) continue;
const float height = ag->params.height;
const float radius = ag->params.radius;
const float* pos = ag->npos;
unsigned int col = duRGBA(220,220,220,128);
if (ag->targetState == DT_CROWDAGENT_TARGET_REQUESTING || ag->targetState == DT_CROWDAGENT_TARGET_WAITING_FOR_QUEUE)
col = duLerpCol(col, duRGBA(128,0,255,128), 32);
else if (ag->targetState == DT_CROWDAGENT_TARGET_WAITING_FOR_PATH)
col = duLerpCol(col, duRGBA(128,0,255,128), 128);
else if (ag->targetState == DT_CROWDAGENT_TARGET_FAILED)
col = duRGBA(255,32,16,128);
else if (ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
col = duLerpCol(col, duRGBA(64,255,0,128), 128);
duDebugDrawCylinder(&dd, pos[0]-radius, pos[1]+radius*0.1f, pos[2]-radius,
pos[0]+radius, pos[1]+height, pos[2]+radius, col);
}
if (m_toolParams.m_showVO)
{
for (int i = 0; i < crowd->getAgentCount(); i++)
{
if (m_toolParams.m_showDetailAll == false && i != m_agentDebug.idx)
continue;
const dtCrowdAgent* ag =crowd->getAgent(i);
if (!ag->active)
continue;
// Draw detail about agent sela
const dtObstacleAvoidanceDebugData* vod = m_agentDebug.vod;
const float dx = ag->npos[0];
const float dy = ag->npos[1]+ag->params.height;
const float dz = ag->npos[2];
duDebugDrawCircle(&dd, dx,dy,dz, ag->params.maxSpeed, duRGBA(255,255,255,64), 2.0f);
dd.begin(DU_DRAW_QUADS);
for (int j = 0; j < vod->getSampleCount(); ++j)
{
const float* p = vod->getSampleVelocity(j);
const float sr = vod->getSampleSize(j);
const float pen = vod->getSamplePenalty(j);
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.end();
}
}
// Velocity stuff.
for (int i = 0; i < crowd->getAgentCount(); ++i)
{
const dtCrowdAgent* ag = crowd->getAgent(i);
if (!ag->active) continue;
const float radius = ag->params.radius;
const float height = ag->params.height;
const float* pos = ag->npos;
const float* vel = ag->vel;
const float* dvel = ag->dvel;
unsigned int col = duRGBA(220,220,220,192);
if (ag->targetState == DT_CROWDAGENT_TARGET_REQUESTING || ag->targetState == DT_CROWDAGENT_TARGET_WAITING_FOR_QUEUE)
col = duLerpCol(col, duRGBA(128,0,255,192), 32);
else if (ag->targetState == DT_CROWDAGENT_TARGET_WAITING_FOR_PATH)
col = duLerpCol(col, duRGBA(128,0,255,192), 128);
else if (ag->targetState == DT_CROWDAGENT_TARGET_FAILED)
col = duRGBA(255,32,16,192);
else if (ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
col = duLerpCol(col, duRGBA(64,255,0,192), 128);
duDebugDrawCircle(&dd, pos[0], pos[1]+height, pos[2], radius, col, 2.0f);
duDebugDrawArrow(&dd, pos[0],pos[1]+height,pos[2],
pos[0]+dvel[0],pos[1]+height+dvel[1],pos[2]+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]+height,pos[2],
pos[0]+vel[0],pos[1]+height+vel[1],pos[2]+vel[2],
0.0f, 0.4f, duRGBA(0,0,0,160), 2.0f);
}
dd.depthMask(true);
}
void Visualization::renderCrowd()
{
if (m_crowd)
{
DebugDrawGL dd;
//Agents cylinders
for (int i = 0, size(m_crowd->getAgentCount()); i < size; ++i)
{
const dtCrowdAgent* ag = m_crowd->getAgent(i);
if (ag->active)
{
const float height = ag->height;
const float radius = ag->radius;
const float* pos = ag->position;
unsigned int col = duRGBA(220,220,220,128);
if (i >= 0 && i < 8) // Red
col = duRGBA(220,0,0,128);
else if (i >= 8 && i < 74) // Green
col = duRGBA(0,220,0,128);
else // Blue
col = duRGBA(0,0,220,128);
duDebugDrawCircle(&dd, pos[0], pos[1], pos[2], radius, duRGBA(0,0,0,32), 2.0f);
duDebugDrawCircle(&dd, pos[0], pos[1]+height, pos[2], radius, duRGBA(0,0,0,32), 2.0f);
duDebugDrawCylinder(&dd, pos[0]-radius, pos[1]+radius*0.1f, pos[2]-radius,
pos[0]+radius, pos[1]+height, pos[2]+radius, col);
}
}
// Agents Trail
for (int i = 0, size(m_crowd->getAgentCount()); i < size; ++i)
{
const dtCrowdAgent* ag = m_crowd->getAgent(i);
if (ag->active)
{
const DebugInfo::AgentTrail* trail = &m_debugInfo->m_agentTrails[i];
const float* pos = ag->position;
dd.begin(DU_DRAW_LINES,3.0f);
float prev[3];
float preva = 1;
dtVcopy(prev, pos);
for (int j = 0; j < maxAgentTrailLen-1; ++j)
{
const int idx = (trail->htrail + maxAgentTrailLen-j) % maxAgentTrailLen;
const float* v = &trail->trail[idx*3];
float a = 1 - j/(float)maxAgentTrailLen;
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)));
preva = a;
dtVcopy(prev, v);
}
dd.end();
}
}
// Agents velocity
for (int i = 0, size(m_crowd->getAgentCount()); i < size; ++i)
{
const dtCrowdAgent* ag = m_crowd->getAgent(i);
if (ag->active)
{
const float radius = ag->radius;
const float height = ag->height;
const float* pos = ag->position;
const float* vel = ag->velocity;
const float* dvel = ag->desiredVelocity;
unsigned int col = duRGBA(220,220,220,192);
if (i >= 0 && i < 8) // Red
col = duRGBA(220,0,0,192);
else if (i >= 8 && i < 74) // Green
col = duRGBA(0,220,0,192);
else // Blue
col = duRGBA(0,0,220,192);
duDebugDrawCircle(&dd, pos[0], pos[1]+height, pos[2], radius, col, 2.0f);
if (dtVlen(ag->desiredVelocity) > 0.1f)
{
duDebugDrawArrow(&dd, pos[0],pos[1]+height + 0.01f,pos[2],
pos[0]+dvel[0],pos[1]+height+dvel[1] + 0.01f,pos[2]+dvel[2],
0.0f, 0.4f, duRGBA(0,192,255,192), 1.0f);
}
duDebugDrawArrow(&dd, pos[0],pos[1]+height + 0.01f,pos[2],
pos[0]+vel[0],pos[1]+height+vel[1] + 0.01f,pos[2]+vel[2],
0.0f, 0.4f, duRGBA(0,0,0,160), 2.0f);
}
}
dd.depthMask(true);
}
}
