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); } } }