void dtCrowd::updateStepOffMeshAnim(const float dt, dtCrowdAgentDebugInfo*)
{
for (int i = 0; i < m_numActiveAgents; ++i)
{
dtCrowdAgent* ag = m_activeAgents[i];
const int agentIndex = getAgentIndex(ag);
dtCrowdAgentAnimation* anim = &m_agentAnims[agentIndex];
if (!anim->active)
continue;
anim->t += dt;
if (anim->t > anim->tmax)
{
// Reset animation
anim->active = 0;
// Prepare agent for walking.
ag->state = DT_CROWDAGENT_STATE_WALKING;
// UE4: m_keepOffmeshConnections support
if (m_keepOffmeshConnections)
{
ag->corridor.pruneOffmeshConenction(anim->polyRef);
}
continue;
}
// Update position
const float ta = anim->tmax*0.15f;
const float tb = anim->tmax;
if (anim->t < ta)
{
const float u = tween(anim->t, 0.0, ta);
dtVlerp(ag->npos, anim->initPos, anim->startPos, u);
}
else
{
const float u = tween(anim->t, ta, tb);
dtVlerp(ag->npos, anim->startPos, anim->endPos, u);
}
// Update velocity.
dtVset(ag->vel, 0, 0, 0);
dtVset(ag->dvel, 0, 0, 0);
}
}
bool nav_mesh::los(const float* start, const float* end, float* hitPos)
{
float t = 0;
float m_hitNormal[3] = { 0, };
dtPolyRef startRef = 0;
float polyPickExt[3] = { 2, 4, 2 };
dtStatus status = query_->findNearestPoly(start, polyPickExt, &filter_, &startRef, nullptr);
if (dtStatusFailed(status))
{
return false;
}
dtPolyRef polys[MAX_POLYS] = { 0, };
int npolys = 0;
query_->raycast(startRef, start, end, &filter_, &t, m_hitNormal, polys, &npolys, sizeof(polys) / sizeof(polys[0]));
if (t > 1)
{
dtVcopy(hitPos, end);
return true;
}
else
{
if (npolys > 0)
{
dtVlerp(hitPos, start, end, t);
float temp[3] = { 0, };
dtStatus statu = query_->closestPointOnPoly(polys[npolys - 1], hitPos, temp, nullptr);
if (dtStatusSucceed(statu))
{
dtVcopy(hitPos, temp);
return false;
}
}
}
dtVcopy(hitPos, start);
return false;
}
void dtNavMesh::closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest, bool* posOverPoly) const
{
const dtMeshTile* tile = 0;
const dtPoly* poly = 0;
getTileAndPolyByRefUnsafe(ref, &tile, &poly);
// Off-mesh connections don't have detail polygons.
if (poly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
{
const float* v0 = &tile->verts[poly->verts[0]*3];
const float* v1 = &tile->verts[poly->verts[1]*3];
const float d0 = dtVdist(pos, v0);
const float d1 = dtVdist(pos, v1);
const float u = d0 / (d0+d1);
dtVlerp(closest, v0, v1, u);
if (posOverPoly)
*posOverPoly = false;
return;
}
const unsigned int ip = (unsigned int)(poly - tile->polys);
const dtPolyDetail* pd = &tile->detailMeshes[ip];
// Clamp point to be inside the polygon.
float verts[DT_VERTS_PER_POLYGON*3];
float edged[DT_VERTS_PER_POLYGON];
float edget[DT_VERTS_PER_POLYGON];
const int nv = poly->vertCount;
for (int i = 0; i < nv; ++i)
dtVcopy(&verts[i*3], &tile->verts[poly->verts[i]*3]);
dtVcopy(closest, pos);
if (!dtDistancePtPolyEdgesSqr(pos, verts, nv, edged, edget))
{
// Point is outside the polygon, dtClamp to nearest edge.
float dmin = FLT_MAX;
int imin = -1;
for (int i = 0; i < nv; ++i)
{
if (edged[i] < dmin)
{
dmin = edged[i];
imin = i;
}
}
const float* va = &verts[imin*3];
const float* vb = &verts[((imin+1)%nv)*3];
dtVlerp(closest, va, vb, edget[imin]);
if (posOverPoly)
*posOverPoly = false;
}
else
{
if (posOverPoly)
*posOverPoly = true;
}
// Find height at the location.
for (int j = 0; j < pd->triCount; ++j)
{
const unsigned char* t = &tile->detailTris[(pd->triBase+j)*4];
const float* v[3];
for (int k = 0; k < 3; ++k)
{
if (t[k] < poly->vertCount)
v[k] = &tile->verts[poly->verts[t[k]]*3];
else
v[k] = &tile->detailVerts[(pd->vertBase+(t[k]-poly->vertCount))*3];
}
float h;
if (dtClosestHeightPointTriangle(pos, v[0], v[1], v[2], h))
{
closest[1] = h;
break;
}
}
}
void TestCase::doTests(dtNavMesh* navmesh, dtNavMeshQuery* navquery)
{
if (!navmesh || !navquery)
return;
resetTimes();
static const int MAX_POLYS = 256;
dtPolyRef polys[MAX_POLYS];
float straight[MAX_POLYS*3];
const float polyPickExt[3] = {2,4,2};
for (Test* iter = m_tests; iter; iter = iter->next)
{
delete [] iter->polys;
iter->polys = 0;
iter->npolys = 0;
delete [] iter->straight;
iter->straight = 0;
iter->nstraight = 0;
dtQueryFilter filter;
filter.setIncludeFlags(iter->includeFlags);
filter.setExcludeFlags(iter->excludeFlags);
// Find start points
TimeVal findNearestPolyStart = getPerfTime();
dtPolyRef startRef, endRef;
navquery->findNearestPoly(iter->spos, polyPickExt, &filter, &startRef, iter->nspos);
navquery->findNearestPoly(iter->epos, polyPickExt, &filter, &endRef, iter->nepos);
TimeVal findNearestPolyEnd = getPerfTime();
iter->findNearestPolyTime += getPerfTimeUsec(findNearestPolyEnd - findNearestPolyStart);
if (!startRef || ! endRef)
continue;
if (iter->type == TEST_PATHFIND)
{
// Find path
TimeVal findPathStart = getPerfTime();
navquery->findPath(startRef, endRef, iter->spos, iter->epos, &filter, polys, &iter->npolys, MAX_POLYS);
TimeVal findPathEnd = getPerfTime();
iter->findPathTime += getPerfTimeUsec(findPathEnd - findPathStart);
// Find straight path
if (iter->npolys)
{
TimeVal findStraightPathStart = getPerfTime();
navquery->findStraightPath(iter->spos, iter->epos, polys, iter->npolys,
straight, 0, 0, &iter->nstraight, MAX_POLYS);
TimeVal findStraightPathEnd = getPerfTime();
iter->findStraightPathTime += getPerfTimeUsec(findStraightPathEnd - findStraightPathStart);
}
// Copy results
if (iter->npolys)
{
iter->polys = new dtPolyRef[iter->npolys];
memcpy(iter->polys, polys, sizeof(dtPolyRef)*iter->npolys);
}
if (iter->nstraight)
{
iter->straight = new float[iter->nstraight*3];
memcpy(iter->straight, straight, sizeof(float)*3*iter->nstraight);
}
}
else if (iter->type == TEST_RAYCAST)
{
float t = 0;
float hitNormal[3], hitPos[3];
iter->straight = new float[2*3];
iter->nstraight = 2;
iter->straight[0] = iter->spos[0];
iter->straight[1] = iter->spos[1];
iter->straight[2] = iter->spos[2];
TimeVal findPathStart = getPerfTime();
navquery->raycast(startRef, iter->spos, iter->epos, &filter, &t, hitNormal, polys, &iter->npolys, MAX_POLYS);
TimeVal findPathEnd = getPerfTime();
iter->findPathTime += getPerfTimeUsec(findPathEnd - findPathStart);
if (t > 1)
{
// No hit
dtVcopy(hitPos, iter->epos);
}
else
{
// Hit
dtVlerp(hitPos, iter->spos, iter->epos, t);
}
// Adjust height.
if (iter->npolys > 0)
{
float h = 0;
navquery->getPolyHeight(polys[iter->npolys-1], hitPos, &h);
hitPos[1] = h;
}
dtVcopy(&iter->straight[3], hitPos);
if (iter->npolys)
{
iter->polys = new dtPolyRef[iter->npolys];
memcpy(iter->polys, polys, sizeof(dtPolyRef)*iter->npolys);
}
}
}
printf("Test Results:\n");
int n = 0;
for (Test* iter = m_tests; iter; iter = iter->next)
{
const int total = iter->findNearestPolyTime + iter->findPathTime + iter->findStraightPathTime;
printf(" - Path %02d: %.4f ms\n", n, (float)total/1000.0f);
printf(" - poly: %.4f ms\n", (float)iter->findNearestPolyTime/1000.0f);
printf(" - path: %.4f ms\n", (float)iter->findPathTime/1000.0f);
printf(" - straight: %.4f ms\n", (float)iter->findStraightPathTime/1000.0f);
n++;
}
}
void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
{
m_velocitySampleCount = 0;
const int debugIdx = debug ? debug->idx : -1;
dtCrowdAgent** agents = m_activeAgents;
int nagents = getActiveAgents(agents, m_maxAgents);
// Check that all agents still have valid paths.
checkPathValidity(agents, nagents, dt);
// Update async move request and path finder.
updateMoveRequest(dt);
// Optimize path topology.
updateTopologyOptimization(agents, nagents, dt);
// Register agents to proximity grid.
m_grid->clear();
for (int i = 0; i < nagents; ++i)
{
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);
}
// Get nearby navmesh segments and agents to collide with.
for (int i = 0; i < nagents; ++i)
{
dtCrowdAgent* ag = agents[i];
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
// Update the collision boundary after certain distance has been passed or
// if it has become invalid.
const float updateThr = ag->params.collisionQueryRange*0.25f;
if (dtVdist2DSqr(ag->npos, ag->boundary.getCenter()) > dtSqr(updateThr) ||
!ag->boundary.isValid(m_navquery, &m_filter))
{
ag->boundary.update(ag->corridor.getFirstPoly(), ag->npos, ag->params.collisionQueryRange,
m_navquery, &m_filter);
}
// Query neighbour agents
ag->nneis = getNeighbours(ag->npos, ag->params.height, ag->params.collisionQueryRange,
ag, ag->neis, DT_CROWDAGENT_MAX_NEIGHBOURS,
agents, nagents, m_grid);
for (int j = 0; j < ag->nneis; j++)
ag->neis[j].idx = getAgentIndex(agents[ag->neis[j].idx]);
}
// Find next corner to steer to.
for (int i = 0; i < nagents; ++i)
{
dtCrowdAgent* ag = agents[i];
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
if (ag->targetState == DT_CROWDAGENT_TARGET_NONE || ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
continue;
// Find corners for steering
ag->ncorners = ag->corridor.findCorners(ag->cornerVerts, ag->cornerFlags, ag->cornerPolys,
DT_CROWDAGENT_MAX_CORNERS, m_navquery, &m_filter);
// Check to see if the corner after the next corner is directly visible,
// and short cut to there.
if ((ag->params.updateFlags & DT_CROWD_OPTIMIZE_VIS) && ag->ncorners > 0)
{
const float* target = &ag->cornerVerts[dtMin(1,ag->ncorners-1)*3];
ag->corridor.optimizePathVisibility(target, ag->params.pathOptimizationRange, m_navquery, &m_filter);
// Copy data for debug purposes.
if (debugIdx == i)
{
dtVcopy(debug->optStart, ag->corridor.getPos());
dtVcopy(debug->optEnd, target);
}
}
else
{
// Copy data for debug purposes.
if (debugIdx == i)
{
dtVset(debug->optStart, 0,0,0);
dtVset(debug->optEnd, 0,0,0);
}
}
}
// Trigger off-mesh connections (depends on corners).
for (int i = 0; i < nagents; ++i)
{
dtCrowdAgent* ag = agents[i];
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
if (ag->targetState == DT_CROWDAGENT_TARGET_NONE || ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
continue;
// Check
const float triggerRadius = ag->params.radius*2.25f;
if (overOffmeshConnection(ag, triggerRadius))
{
// Prepare to off-mesh connection.
const int idx = (int)(ag - m_agents);
dtCrowdAgentAnimation* anim = &m_agentAnims[idx];
// Adjust the path over the off-mesh connection.
dtPolyRef refs[2];
if (ag->corridor.moveOverOffmeshConnection(ag->cornerPolys[ag->ncorners-1], refs,
anim->startPos, anim->endPos, m_navquery))
{
dtVcopy(anim->initPos, ag->npos);
anim->polyRef = refs[1];
anim->active = 1;
anim->t = 0.0f;
anim->tmax = (dtVdist2D(anim->startPos, anim->endPos) / ag->params.maxSpeed) * 0.5f;
ag->state = DT_CROWDAGENT_STATE_OFFMESH;
ag->ncorners = 0;
ag->nneis = 0;
continue;
}
else
{
// Path validity check will ensure that bad/blocked connections will be replanned.
}
}
}
// Calculate steering.
for (int i = 0; i < nagents; ++i)
{
dtCrowdAgent* ag = agents[i];
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
if (ag->targetState == DT_CROWDAGENT_TARGET_NONE)
continue;
float dvel[3] = {0,0,0};
if (ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
{
dtVcopy(dvel, ag->targetPos);
ag->desiredSpeed = dtVlen(ag->targetPos);
}
else
{
// Calculate steering direction.
if (ag->params.updateFlags & DT_CROWD_ANTICIPATE_TURNS)
calcSmoothSteerDirection(ag, dvel);
else
calcStraightSteerDirection(ag, dvel);
// Calculate speed scale, which tells the agent to slowdown at the end of the path.
const float slowDownRadius = ag->params.radius*2; // TODO: make less hacky.
const float speedScale = getDistanceToGoal(ag, slowDownRadius) / slowDownRadius;
ag->desiredSpeed = ag->params.maxSpeed;
dtVscale(dvel, dvel, ag->desiredSpeed * speedScale);
}
// Separation
if (ag->params.updateFlags & DT_CROWD_SEPARATION)
{
const float separationDist = ag->params.collisionQueryRange;
const float invSeparationDist = 1.0f / separationDist;
const float separationWeight = ag->params.separationWeight;
float w = 0;
float disp[3] = {0,0,0};
for (int j = 0; j < ag->nneis; ++j)
{
const dtCrowdAgent* nei = &m_agents[ag->neis[j].idx];
float diff[3];
dtVsub(diff, ag->npos, nei->npos);
diff[1] = 0;
const float distSqr = dtVlenSqr(diff);
if (distSqr < 0.00001f)
continue;
if (distSqr > dtSqr(separationDist))
continue;
const float dist = dtMathSqrtf(distSqr);
const float weight = separationWeight * (1.0f - dtSqr(dist*invSeparationDist));
dtVmad(disp, disp, diff, weight/dist);
w += 1.0f;
}
if (w > 0.0001f)
{
// Adjust desired velocity.
dtVmad(dvel, dvel, disp, 1.0f/w);
// Clamp desired velocity to desired speed.
const float speedSqr = dtVlenSqr(dvel);
const float desiredSqr = dtSqr(ag->desiredSpeed);
if (speedSqr > desiredSqr)
dtVscale(dvel, dvel, desiredSqr/speedSqr);
}
}
// Set the desired velocity.
dtVcopy(ag->dvel, dvel);
}
// Velocity planning.
for (int i = 0; i < nagents; ++i)
{
dtCrowdAgent* ag = agents[i];
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
if (ag->params.updateFlags & DT_CROWD_OBSTACLE_AVOIDANCE)
{
m_obstacleQuery->reset();
// Add neighbours as obstacles.
for (int j = 0; j < ag->nneis; ++j)
{
const dtCrowdAgent* nei = &m_agents[ag->neis[j].idx];
m_obstacleQuery->addCircle(nei->npos, nei->params.radius, nei->vel, nei->dvel);
}
// Append neighbour segments as obstacles.
for (int j = 0; j < ag->boundary.getSegmentCount(); ++j)
{
const float* s = ag->boundary.getSegment(j);
if (dtTriArea2D(ag->npos, s, s+3) < 0.0f)
continue;
m_obstacleQuery->addSegment(s, s+3);
}
dtObstacleAvoidanceDebugData* vod = 0;
if (debugIdx == i)
vod = debug->vod;
// Sample new safe velocity.
bool adaptive = true;
int ns = 0;
const dtObstacleAvoidanceParams* params = &m_obstacleQueryParams[ag->params.obstacleAvoidanceType];
if (adaptive)
{
ns = m_obstacleQuery->sampleVelocityAdaptive(ag->npos, ag->params.radius, ag->desiredSpeed,
ag->vel, ag->dvel, ag->nvel, params, vod);
}
else
{
ns = m_obstacleQuery->sampleVelocityGrid(ag->npos, ag->params.radius, ag->desiredSpeed,
ag->vel, ag->dvel, ag->nvel, params, vod);
}
m_velocitySampleCount += ns;
}
else
{
// If not using velocity planning, new velocity is directly the desired velocity.
dtVcopy(ag->nvel, ag->dvel);
}
}
// Integrate.
for (int i = 0; i < nagents; ++i)
{
dtCrowdAgent* ag = agents[i];
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
integrate(ag, dt);
}
// Handle collisions.
static const float COLLISION_RESOLVE_FACTOR = 0.7f;
for (int iter = 0; iter < 4; ++iter)
{
for (int i = 0; i < nagents; ++i)
{
dtCrowdAgent* ag = agents[i];
const int idx0 = getAgentIndex(ag);
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
dtVset(ag->disp, 0,0,0);
float w = 0;
for (int j = 0; j < ag->nneis; ++j)
{
const dtCrowdAgent* nei = &m_agents[ag->neis[j].idx];
const int idx1 = getAgentIndex(nei);
float diff[3];
dtVsub(diff, ag->npos, nei->npos);
diff[1] = 0;
float dist = dtVlenSqr(diff);
if (dist > dtSqr(ag->params.radius + nei->params.radius))
continue;
dist = dtMathSqrtf(dist);
float pen = (ag->params.radius + nei->params.radius) - dist;
if (dist < 0.0001f)
{
// Agents on top of each other, try to choose diverging separation directions.
if (idx0 > idx1)
dtVset(diff, -ag->dvel[2],0,ag->dvel[0]);
else
dtVset(diff, ag->dvel[2],0,-ag->dvel[0]);
pen = 0.01f;
}
else
{
pen = (1.0f/dist) * (pen*0.5f) * COLLISION_RESOLVE_FACTOR;
}
dtVmad(ag->disp, ag->disp, diff, pen);
w += 1.0f;
}
if (w > 0.0001f)
{
const float iw = 1.0f / w;
dtVscale(ag->disp, ag->disp, iw);
}
}
for (int i = 0; i < nagents; ++i)
{
dtCrowdAgent* ag = agents[i];
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
dtVadd(ag->npos, ag->npos, ag->disp);
}
}
for (int i = 0; i < nagents; ++i)
{
dtCrowdAgent* ag = agents[i];
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
// Move along navmesh.
ag->corridor.movePosition(ag->npos, m_navquery, &m_filter);
// Get valid constrained position back.
dtVcopy(ag->npos, ag->corridor.getPos());
// If not using path, truncate the corridor to just one poly.
if (ag->targetState == DT_CROWDAGENT_TARGET_NONE || ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
{
ag->corridor.reset(ag->corridor.getFirstPoly(), ag->npos);
}
}
// Update agents using off-mesh connection.
for (int i = 0; i < m_maxAgents; ++i)
{
dtCrowdAgentAnimation* anim = &m_agentAnims[i];
if (!anim->active)
continue;
dtCrowdAgent* ag = agents[i];
anim->t += dt;
if (anim->t > anim->tmax)
{
// Reset animation
anim->active = 0;
// Prepare agent for walking.
ag->state = DT_CROWDAGENT_STATE_WALKING;
continue;
}
// Update position
const float ta = anim->tmax*0.15f;
const float tb = anim->tmax;
if (anim->t < ta)
{
const float u = tween(anim->t, 0.0, ta);
dtVlerp(ag->npos, anim->initPos, anim->startPos, u);
}
else
{
const float u = tween(anim->t, ta, tb);
dtVlerp(ag->npos, anim->startPos, anim->endPos, u);
}
// Update velocity.
dtVset(ag->vel, 0,0,0);
dtVset(ag->dvel, 0,0,0);
}
}
void dtCrowd::updatePosition(const float dt, unsigned* agentsIdx, unsigned nbIdx)
{
// If we want to update every agent
if (nbIdx == 0)
{
agentsIdx = m_agentsToUpdate;
nbIdx = m_maxAgents;
}
nbIdx = (nbIdx < m_maxAgents) ? nbIdx : m_maxAgents;
// The current start position of the agent (not yet modified)
dtPolyRef* currentPosPoly = (dtPolyRef*) dtAlloc(sizeof(dtPolyRef) * nbIdx, DT_ALLOC_TEMP);
float* currentPos = (float*) dtAlloc(sizeof(float) * 3 * nbIdx, DT_ALLOC_TEMP);
for (unsigned i = 0; i < nbIdx; ++i)
{
dtCrowdAgent* ag = 0;
if (!getActiveAgent(&ag, agentsIdx[i]))
continue;
m_crowdQuery->getNavMeshQuery()->findNearestPoly(ag->position, m_crowdQuery->getQueryExtents(), m_crowdQuery->getQueryFilter(), currentPosPoly + i, currentPos + (i * 3));
}
// Integrate.
for (unsigned i = 0; i < nbIdx; ++i)
{
dtCrowdAgent* ag = 0;
if (!getActiveAgent(&ag, agentsIdx[i]))
continue;
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
integrate(ag, dt);
}
// Handle collisions.
static const float COLLISION_RESOLVE_FACTOR = 0.7f;
for (unsigned iter = 0; iter < 4; ++iter)
{
for (unsigned i = 0; i < nbIdx; ++i)
{
dtCrowdAgent* ag = 0;
if (!getActiveAgent(&ag, agentsIdx[i]))
continue;
const int idx0 = getAgentIndex(ag);
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
float* disp = m_disp[agentsIdx[i]];
dtVset(disp, 0, 0, 0);
float w = 0;
for (unsigned j = 0; j < m_agentsEnv[ag->id].nbNeighbors; ++j)
{
const dtCrowdAgent* nei = &m_agents[m_agentsEnv[ag->id].neighbors[j].idx];
const int idx1 = getAgentIndex(nei);
float diff[3];
dtVsub(diff, ag->position, nei->position);
diff[1] = 0;
float dist = dtVlenSqr(diff);
if (dist > dtSqr(ag->radius + nei->radius) + EPSILON)
continue;
dist = sqrtf(dist);
float pen = (ag->radius + nei->radius) - dist;
if (dist < EPSILON)
{
// Agents on top of each other, try to choose diverging separation directions.
if (idx0 > idx1)
dtVset(diff, -ag->desiredVelocity[2], 0, ag->desiredVelocity[0]);
else
dtVset(diff, ag->desiredVelocity[2], 0, -ag->desiredVelocity[0]);
pen = 0.01f;
}
else
{
pen = (1.0f / dist) * (pen * 0.5f) * COLLISION_RESOLVE_FACTOR;
}
dtVmad(disp, disp, diff, pen);
w += 1.0f;
}
if (w > EPSILON)
{
const float iw = 1.0f / w;
dtVscale(disp, disp, iw);
}
}
for (unsigned i = 0; i < nbIdx; ++i)
{
dtCrowdAgent* ag = 0;
if (!getActiveAgent(&ag, agentsIdx[i]))
continue;
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
float* disp = m_disp[agentsIdx[i]];
dtVadd(ag->position, ag->position, disp);
}
}
for (unsigned i = 0; i < nbIdx; ++i)
{
dtCrowdAgent* ag = 0;
if (!getActiveAgent(&ag, agentsIdx[i]))
continue;
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
// Move along navmesh.
float newPos[3];
dtPolyRef visited[dtPathCorridor::MAX_VISITED];
int visitedCount;
m_crowdQuery->getNavMeshQuery()->moveAlongSurface(currentPosPoly[i], currentPos + (i * 3), ag->position, m_crowdQuery->getQueryFilter(), newPos,
visited, &visitedCount, dtPathCorridor::MAX_VISITED);
// Get valid constrained position back.
float newHeight = *(currentPos + (i * 3) + 1);
m_crowdQuery->getNavMeshQuery()->getPolyHeight(currentPosPoly[i], newPos, &newHeight);
newPos[1] = newHeight;
dtVcopy(ag->position, newPos);
}
// Update agents using off-mesh connection.
for (unsigned i = 0; i < nbIdx; ++i)
{
dtCrowdAgent* ag = 0;
if (!getActiveAgent(&ag, agentsIdx[i]))
continue;
float offmeshTotalTime = ag->offmeshInitToStartTime + ag->offmeshStartToEndTime;
if (ag->state == DT_CROWDAGENT_STATE_OFFMESH && offmeshTotalTime > EPSILON)
{
ag->offmeshElaspedTime += dt;
if (ag->offmeshElaspedTime > offmeshTotalTime)
{
// Prepare agent for walking.
ag->state = DT_CROWDAGENT_STATE_WALKING;
continue;
}
// Update position
if (ag->offmeshElaspedTime < ag->offmeshInitToStartTime)
{
const float u = tween(ag->offmeshElaspedTime, 0.0, ag->offmeshInitToStartTime);
dtVlerp(ag->position, ag->offmeshInitPos, ag->offmeshStartPos, u);
}
else
{
const float u = tween(ag->offmeshElaspedTime, ag->offmeshInitToStartTime, offmeshTotalTime);
dtVlerp(ag->position, ag->offmeshStartPos, ag->offmeshEndPos, u);
}
// Update velocity.
dtVset(ag->velocity, 0,0,0);
dtVset(ag->desiredVelocity, 0,0,0);
}
}
dtFree(currentPosPoly);
dtFree(currentPos);
}
void NavMeshTesterTool::recalc()
{
if (!m_navMesh)
return;
if (m_sposSet)
m_navQuery->findNearestPoly(m_spos, m_polyPickExt, &m_filter, &m_startRef, 0);
else
m_startRef = 0;
if (m_eposSet)
m_navQuery->findNearestPoly(m_epos, m_polyPickExt, &m_filter, &m_endRef, 0);
else
m_endRef = 0;
m_pathFindStatus = DT_FAILURE;
if (m_toolMode == TOOLMODE_PATHFIND_FOLLOW)
{
m_pathIterNum = 0;
if (m_sposSet && m_eposSet && m_startRef && m_endRef)
{
#ifdef DUMP_REQS
printf("pi %f %f %f %f %f %f 0x%x 0x%x\n",
m_spos[0],m_spos[1],m_spos[2], m_epos[0],m_epos[1],m_epos[2],
m_filter.getIncludeFlags(), m_filter.getExcludeFlags());
#endif
m_navQuery->findPath(m_startRef, m_endRef, m_spos, m_epos, &m_filter, m_polys, &m_npolys, MAX_POLYS);
m_nsmoothPath = 0;
if (m_npolys)
{
// Iterate over the path to find smooth path on the detail mesh surface.
dtPolyRef polys[MAX_POLYS];
memcpy(polys, m_polys, sizeof(dtPolyRef)*m_npolys);
int npolys = m_npolys;
float iterPos[3], targetPos[3];
m_navQuery->closestPointOnPoly(m_startRef, m_spos, iterPos);
m_navQuery->closestPointOnPoly(polys[npolys-1], m_epos, targetPos);
static const float STEP_SIZE = 0.5f;
static const float SLOP = 0.01f;
m_nsmoothPath = 0;
dtVcopy(&m_smoothPath[m_nsmoothPath*3], iterPos);
m_nsmoothPath++;
// Move towards target a small advancement at a time until target reached or
// when ran out of memory to store the path.
while (npolys && m_nsmoothPath < MAX_SMOOTH)
{
// Find location to steer towards.
float steerPos[3];
unsigned char steerPosFlag;
dtPolyRef steerPosRef;
if (!getSteerTarget(m_navQuery, iterPos, targetPos, SLOP,
polys, npolys, steerPos, steerPosFlag, steerPosRef))
break;
bool endOfPath = (steerPosFlag & DT_STRAIGHTPATH_END) ? true : false;
bool offMeshConnection = (steerPosFlag & DT_STRAIGHTPATH_OFFMESH_CONNECTION) ? true : false;
// Find movement delta.
float delta[3], len;
dtVsub(delta, steerPos, iterPos);
len = dtSqrt(dtVdot(delta,delta));
// If the steer target is end of path or off-mesh link, do not move past the location.
if ((endOfPath || offMeshConnection) && len < STEP_SIZE)
len = 1;
else
len = STEP_SIZE / len;
float moveTgt[3];
dtVmad(moveTgt, iterPos, delta, len);
// Move
float result[3];
dtPolyRef visited[16];
int nvisited = 0;
m_navQuery->moveAlongSurface(polys[0], iterPos, moveTgt, &m_filter,
result, visited, &nvisited, 16);
npolys = fixupCorridor(polys, npolys, MAX_POLYS, visited, nvisited);
npolys = fixupShortcuts(polys, npolys, m_navQuery);
float h = 0;
m_navQuery->getPolyHeight(polys[0], result, &h);
result[1] = h;
dtVcopy(iterPos, result);
// Handle end of path and off-mesh links when close enough.
if (endOfPath && inRange(iterPos, steerPos, SLOP, 1.0f))
{
// Reached end of path.
dtVcopy(iterPos, targetPos);
if (m_nsmoothPath < MAX_SMOOTH)
{
dtVcopy(&m_smoothPath[m_nsmoothPath*3], iterPos);
m_nsmoothPath++;
}
break;
}
else if (offMeshConnection && inRange(iterPos, steerPos, SLOP, 1.0f))
{
// Reached off-mesh connection.
float startPos[3], endPos[3];
// Advance the path up to and over the off-mesh connection.
dtPolyRef prevRef = 0, polyRef = polys[0];
int npos = 0;
while (npos < npolys && polyRef != steerPosRef)
{
prevRef = polyRef;
polyRef = polys[npos];
npos++;
}
for (int i = npos; i < npolys; ++i)
polys[i-npos] = polys[i];
npolys -= npos;
// Handle the connection.
dtStatus status = m_navMesh->getOffMeshConnectionPolyEndPoints(prevRef, polyRef, startPos, endPos);
if (dtStatusSucceed(status))
{
if (m_nsmoothPath < MAX_SMOOTH)
{
dtVcopy(&m_smoothPath[m_nsmoothPath*3], startPos);
m_nsmoothPath++;
// Hack to make the dotted path not visible during off-mesh connection.
if (m_nsmoothPath & 1)
{
dtVcopy(&m_smoothPath[m_nsmoothPath*3], startPos);
m_nsmoothPath++;
}
}
// Move position at the other side of the off-mesh link.
dtVcopy(iterPos, endPos);
float eh = 0.0f;
m_navQuery->getPolyHeight(polys[0], iterPos, &eh);
iterPos[1] = eh;
}
}
// Store results.
if (m_nsmoothPath < MAX_SMOOTH)
{
dtVcopy(&m_smoothPath[m_nsmoothPath*3], iterPos);
m_nsmoothPath++;
}
}
}
}
else
{
m_npolys = 0;
m_nsmoothPath = 0;
}
}
else if (m_toolMode == TOOLMODE_PATHFIND_STRAIGHT)
{
if (m_sposSet && m_eposSet && m_startRef && m_endRef)
{
#ifdef DUMP_REQS
printf("ps %f %f %f %f %f %f 0x%x 0x%x\n",
m_spos[0],m_spos[1],m_spos[2], m_epos[0],m_epos[1],m_epos[2],
m_filter.getIncludeFlags(), m_filter.getExcludeFlags());
#endif
m_navQuery->findPath(m_startRef, m_endRef, m_spos, m_epos, &m_filter, m_polys, &m_npolys, MAX_POLYS);
m_nstraightPath = 0;
if (m_npolys)
{
// In case of partial path, make sure the end point is clamped to the last polygon.
float epos[3];
dtVcopy(epos, m_epos);
if (m_polys[m_npolys-1] != m_endRef)
m_navQuery->closestPointOnPoly(m_polys[m_npolys-1], m_epos, epos);
m_navQuery->findStraightPath(m_spos, epos, m_polys, m_npolys,
m_straightPath, m_straightPathFlags,
m_straightPathPolys, &m_nstraightPath, MAX_POLYS, m_straightPathOptions);
}
}
else
{
m_npolys = 0;
m_nstraightPath = 0;
}
}
else if (m_toolMode == TOOLMODE_PATHFIND_SLICED)
{
if (m_sposSet && m_eposSet && m_startRef && m_endRef)
{
#ifdef DUMP_REQS
printf("ps %f %f %f %f %f %f 0x%x 0x%x\n",
m_spos[0],m_spos[1],m_spos[2], m_epos[0],m_epos[1],m_epos[2],
m_filter.getIncludeFlags(), m_filter.getExcludeFlags());
#endif
m_npolys = 0;
m_nstraightPath = 0;
m_pathFindStatus = m_navQuery->initSlicedFindPath(m_startRef, m_endRef, m_spos, m_epos, &m_filter);
}
else
{
m_npolys = 0;
m_nstraightPath = 0;
}
}
else if (m_toolMode == TOOLMODE_RAYCAST)
{
m_nstraightPath = 0;
if (m_sposSet && m_eposSet && m_startRef)
{
#ifdef DUMP_REQS
printf("rc %f %f %f %f %f %f 0x%x 0x%x\n",
m_spos[0],m_spos[1],m_spos[2], m_epos[0],m_epos[1],m_epos[2],
m_filter.getIncludeFlags(), m_filter.getExcludeFlags());
#endif
float t = 0;
m_npolys = 0;
m_nstraightPath = 2;
m_straightPath[0] = m_spos[0];
m_straightPath[1] = m_spos[1];
m_straightPath[2] = m_spos[2];
m_navQuery->raycast(m_startRef, m_spos, m_epos, &m_filter, &t, m_hitNormal, m_polys, &m_npolys, MAX_POLYS);
if (t > 1)
{
// No hit
dtVcopy(m_hitPos, m_epos);
m_hitResult = false;
}
else
{
// Hit
dtVlerp(m_hitPos, m_spos, m_epos, t);
m_hitResult = true;
}
// Adjust height.
if (m_npolys > 0)
{
float h = 0;
m_navQuery->getPolyHeight(m_polys[m_npolys-1], m_hitPos, &h);
m_hitPos[1] = h;
}
dtVcopy(&m_straightPath[3], m_hitPos);
}
}
else if (m_toolMode == TOOLMODE_DISTANCE_TO_WALL)
{
m_distanceToWall = 0;
if (m_sposSet && m_startRef)
{
#ifdef DUMP_REQS
printf("dw %f %f %f %f 0x%x 0x%x\n",
m_spos[0],m_spos[1],m_spos[2], 100.0f,
m_filter.getIncludeFlags(), m_filter.getExcludeFlags());
#endif
m_distanceToWall = 0.0f;
m_navQuery->findDistanceToWall(m_startRef, m_spos, 100.0f, &m_filter, &m_distanceToWall, m_hitPos, m_hitNormal);
}
}
else if (m_toolMode == TOOLMODE_FIND_POLYS_IN_CIRCLE)
{
if (m_sposSet && m_startRef && m_eposSet)
{
const float dx = m_epos[0] - m_spos[0];
const float dz = m_epos[2] - m_spos[2];
float dist = sqrtf(dx*dx + dz*dz);
#ifdef DUMP_REQS
printf("fpc %f %f %f %f 0x%x 0x%x\n",
m_spos[0],m_spos[1],m_spos[2], dist,
m_filter.getIncludeFlags(), m_filter.getExcludeFlags());
#endif
m_navQuery->findPolysAroundCircle(m_startRef, m_spos, dist, &m_filter,
m_polys, m_parent, 0, &m_npolys, MAX_POLYS);
}
}
else if (m_toolMode == TOOLMODE_FIND_POLYS_IN_SHAPE)
{
if (m_sposSet && m_startRef && m_eposSet)
{
const float nx = (m_epos[2] - m_spos[2])*0.25f;
const float nz = -(m_epos[0] - m_spos[0])*0.25f;
const float agentHeight = m_sample ? m_sample->getAgentHeight() : 0;
m_queryPoly[0] = m_spos[0] + nx*1.2f;
m_queryPoly[1] = m_spos[1] + agentHeight/2;
m_queryPoly[2] = m_spos[2] + nz*1.2f;
m_queryPoly[3] = m_spos[0] - nx*1.3f;
m_queryPoly[4] = m_spos[1] + agentHeight/2;
m_queryPoly[5] = m_spos[2] - nz*1.3f;
m_queryPoly[6] = m_epos[0] - nx*0.8f;
m_queryPoly[7] = m_epos[1] + agentHeight/2;
m_queryPoly[8] = m_epos[2] - nz*0.8f;
m_queryPoly[9] = m_epos[0] + nx;
m_queryPoly[10] = m_epos[1] + agentHeight/2;
m_queryPoly[11] = m_epos[2] + nz;
#ifdef DUMP_REQS
printf("fpp %f %f %f %f %f %f %f %f %f %f %f %f 0x%x 0x%x\n",
m_queryPoly[0],m_queryPoly[1],m_queryPoly[2],
m_queryPoly[3],m_queryPoly[4],m_queryPoly[5],
m_queryPoly[6],m_queryPoly[7],m_queryPoly[8],
m_queryPoly[9],m_queryPoly[10],m_queryPoly[11],
m_filter.getIncludeFlags(), m_filter.getExcludeFlags());
#endif
m_navQuery->findPolysAroundShape(m_startRef, m_queryPoly, 4, &m_filter,
m_polys, m_parent, 0, &m_npolys, MAX_POLYS);
}
}
else if (m_toolMode == TOOLMODE_FIND_LOCAL_NEIGHBOURHOOD)
{
if (m_sposSet && m_startRef)
{
#ifdef DUMP_REQS
printf("fln %f %f %f %f 0x%x 0x%x\n",
m_spos[0],m_spos[1],m_spos[2], m_neighbourhoodRadius,
m_filter.getIncludeFlags(), m_filter.getExcludeFlags());
#endif
m_navQuery->findLocalNeighbourhood(m_startRef, m_spos, m_neighbourhoodRadius, &m_filter,
m_polys, m_parent, &m_npolys, MAX_POLYS);
}
}
}
