dtPolyRef PathFinder::getPathPolyByPosition(const dtPolyRef* polyPath, uint32 polyPathSize, const float* point, float* distance) const { if (!polyPath || !polyPathSize) return INVALID_POLYREF; dtPolyRef nearestPoly = INVALID_POLYREF; float minDist2d = FLT_MAX; float minDist3d = 0.0f; for (uint32 i = 0; i < polyPathSize; ++i) { float closestPoint[VERTEX_SIZE]; dtStatus dtResult = m_navMeshQuery->closestPointOnPoly(polyPath[i], point, closestPoint); if (dtStatusFailed(dtResult)) continue; float d = dtVdist2DSqr(point, closestPoint); if (d < minDist2d) { minDist2d = d; nearestPoly = polyPath[i]; minDist3d = dtVdistSqr(point, closestPoint); } if (minDist2d < 1.0f) // shortcut out - close enough for us break; } if (distance) *distance = dtSqrt(minDist3d); return (minDist2d < 3.0f) ? nearestPoly : INVALID_POLYREF; }
dtPolyRef PathFinder::getPathPolyByPosition(const dtPolyRef *polyPath, uint32 polyPathSize, const float* point, float *distance) const { if (!polyPath || !polyPathSize) return INVALID_POLYREF; dtPolyRef nearestPoly = INVALID_POLYREF; float minDist2d = FLT_MAX; float minDist3d = 0.0f; for (uint32 i = 0; i < polyPathSize; ++i) { // skip DT_POLYTYPE_OFFMESH_CONNECTION they aren't handled in closestPointOnPoly const dtMeshTile* tile = 0; const dtPoly* poly = 0; if (m_navMesh->getTileAndPolyByRef(polyPath[i], &tile, &poly) != DT_SUCCESS) continue; if (poly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) continue; float closestPoint[VERTEX_SIZE]; if (DT_SUCCESS != m_navMeshQuery->closestPointOnPoly(polyPath[i], point, closestPoint)) continue; float d = dtVdist2DSqr(point, closestPoint); if (d < minDist2d) { minDist2d = d; nearestPoly = m_pathPolyRefs[i]; minDist3d = dtVdistSqr(point, closestPoint); } if(minDist2d < 1.0f) // shortcut out - close enough for us break; } if (distance) *distance = dtSqrt(minDist3d); return (minDist2d < 3.0f) ? nearestPoly : INVALID_POLYREF; }
static int sweepCircleCircle(const float* c0, const float r0, const float* v, const float* c1, const float r1, float& tmin, float& tmax) { static const float EPS = 0.0001f; float s[3]; dtVsub(s,c1,c0); float r = r0+r1; float c = dtVdot2D(s,s) - r*r; float a = dtVdot2D(v,v); if (a < EPS) return 0; // not moving // Overlap, calc time to exit. float b = dtVdot2D(v,s); float d = b*b - a*c; if (d < 0.0f) return 0; // no intersection. a = 1.0f / a; const float rd = dtSqrt(d); tmin = (b - rd) * a; tmax = (b + rd) * a; return 1; }
// Returns a random point in a convex polygon. // Adapted from Graphics Gems article. void dtRandomPointInConvexPoly(const float* pts, const int npts, float* areas, const float s, const float t, float* out) { // Calc triangle araes float areasum = 0.0f; for (int i = 2; i < npts; i++) { areas[i] = dtTriArea2D(&pts[0], &pts[(i-1)*3], &pts[i*3]); areasum += dtMax(0.001f, areas[i]); } // Find sub triangle weighted by area. const float thr = s*areasum; float acc = 0.0f; float u = 0.0f; int tri = 0; for (int i = 2; i < npts; i++) { const float dacc = areas[i]; if (thr >= acc && thr < (acc+dacc)) { u = (thr - acc) / dacc; tri = i; break; } acc += dacc; } float v = dtSqrt(t); const float a = 1 - v; const float b = (1 - u) * v; const float c = u * v; const float* pa = &pts[0]; const float* pb = &pts[(tri-1)*3]; const float* pc = &pts[tri*3]; out[0] = a*pa[0] + b*pb[0] + c*pc[0]; out[1] = a*pa[1] + b*pb[1] + c*pc[1]; out[2] = a*pa[2] + b*pb[2] + c*pc[2]; }
dtStatus PathInfo::findSmoothPath(const float* startPos, const float* endPos, const dtPolyRef* polyPath, const uint32 polyPathSize, float* smoothPath, int* smoothPathSize, bool &usedOffmesh, const uint32 maxSmoothPathSize) { MANGOS_ASSERT(polyPathSize <= MAX_PATH_LENGTH); *smoothPathSize = 0; uint32 nsmoothPath = 0; usedOffmesh = false; dtPolyRef polys[MAX_PATH_LENGTH]; memcpy(polys, polyPath, sizeof(dtPolyRef)*polyPathSize); uint32 npolys = polyPathSize; float iterPos[VERTEX_SIZE], targetPos[VERTEX_SIZE]; if(DT_SUCCESS != m_navMeshQuery->closestPointOnPolyBoundary(polys[0], startPos, iterPos)) return DT_FAILURE; if(DT_SUCCESS != m_navMeshQuery->closestPointOnPolyBoundary(polys[npolys-1], endPos, targetPos)) return DT_FAILURE; dtVcopy(&smoothPath[nsmoothPath*VERTEX_SIZE], iterPos); 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 && nsmoothPath < maxSmoothPathSize) { // Find location to steer towards. float steerPos[VERTEX_SIZE]; unsigned char steerPosFlag; dtPolyRef steerPosRef = INVALID_POLYREF; if (!getSteerTarget(iterPos, targetPos, SMOOTH_PATH_SLOP, polys, npolys, steerPos, steerPosFlag, steerPosRef)) break; bool endOfPath = (steerPosFlag & DT_STRAIGHTPATH_END); bool offMeshConnection = (steerPosFlag & DT_STRAIGHTPATH_OFFMESH_CONNECTION); // Find movement delta. float delta[VERTEX_SIZE]; dtVsub(delta, steerPos, iterPos); float 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 < SMOOTH_PATH_STEP_SIZE) len = 1.0f; else len = SMOOTH_PATH_STEP_SIZE / len; float moveTgt[VERTEX_SIZE]; dtVmad(moveTgt, iterPos, delta, len); // Move float result[VERTEX_SIZE]; const static uint32 MAX_VISIT_POLY = 16; dtPolyRef visited[MAX_VISIT_POLY]; uint32 nvisited = 0; m_navMeshQuery->moveAlongSurface(polys[0], iterPos, moveTgt, &m_filter, result, visited, (int*)&nvisited, MAX_VISIT_POLY); npolys = fixupCorridor(polys, npolys, MAX_PATH_LENGTH, visited, nvisited); m_navMeshQuery->getPolyHeight(polys[0], result, &result[1]); dtVcopy(iterPos, result); // Handle end of path and off-mesh links when close enough. if (endOfPath && inRangeYZX(iterPos, steerPos, SMOOTH_PATH_SLOP, 2.0f)) { // Reached end of path. dtVcopy(iterPos, targetPos); if (nsmoothPath < maxSmoothPathSize) { dtVcopy(&smoothPath[nsmoothPath*VERTEX_SIZE], iterPos); nsmoothPath++; } break; } else if (offMeshConnection && inRangeYZX(iterPos, steerPos, SMOOTH_PATH_SLOP, 2.0f)) { // Reached off-mesh connection. usedOffmesh = true; // Advance the path up to and over the off-mesh connection. dtPolyRef prevRef = INVALID_POLYREF; dtPolyRef polyRef = polys[0]; uint32 npos = 0; while (npos < npolys && polyRef != steerPosRef) { prevRef = polyRef; polyRef = polys[npos]; npos++; } for (uint32 i = npos; i < npolys; ++i) polys[i-npos] = polys[i]; npolys -= npos; // Handle the connection. float startPos[VERTEX_SIZE], endPos[VERTEX_SIZE]; if (DT_SUCCESS == m_navMesh->getOffMeshConnectionPolyEndPoints(prevRef, polyRef, startPos, endPos)) { if (nsmoothPath < maxSmoothPathSize) { dtVcopy(&smoothPath[nsmoothPath*VERTEX_SIZE], startPos); nsmoothPath++; } // Move position at the other side of the off-mesh link. dtVcopy(iterPos, endPos); m_navMeshQuery->getPolyHeight(polys[0], iterPos, &iterPos[1]); } } // Store results. if (nsmoothPath < maxSmoothPathSize) { dtVcopy(&smoothPath[nsmoothPath*VERTEX_SIZE], iterPos); nsmoothPath++; } } *smoothPathSize = nsmoothPath; // this is most likely loop return nsmoothPath < maxSmoothPathSize ? DT_SUCCESS : DT_FAILURE; }
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->closestPointOnPolyBoundary(m_startRef, m_spos, iterPos); m_navQuery->closestPointOnPolyBoundary(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); 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. if (m_navMesh->getOffMeshConnectionPolyEndPoints(prevRef, polyRef, startPos, endPos) == DT_SUCCESS) { 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 h; m_navQuery->getPolyHeight(polys[0], iterPos, &h); iterPos[1] = h; } } // 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); } } 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 m_hitPos[0] = m_spos[0] + (m_epos[0] - m_spos[0]) * t; m_hitPos[1] = m_spos[1] + (m_epos[1] - m_spos[1]) * t; m_hitPos[2] = m_spos[2] + (m_epos[2] - m_spos[2]) * t; if (m_npolys) { float h = 0; m_navQuery->getPolyHeight(m_polys[m_npolys-1], m_hitPos, &h); m_hitPos[1] = h; } m_hitResult = true; } 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); } } }
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); // heat-modif: mendsley - Fixing 64bit compile issues 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 = dtSqrt(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 = dtSqrt(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); } }