void dtObstacleAvoidanceQuery::prepare(const float* pos, const float* dvel) { // Prepare obstacles for (int i = 0; i < m_ncircles; ++i) { dtObstacleCircle* cir = &m_circles[i]; // Side const float* pa = pos; const float* pb = cir->p; const float orig[3] = {0,0}; float dv[3]; dtVsub(cir->dp,pb,pa); dtVnormalize(cir->dp); dtVsub(dv, cir->dvel, dvel); const float a = dtTriArea2D(orig, cir->dp,dv); if (a < 0.01f) { cir->np[0] = -cir->dp[2]; cir->np[2] = cir->dp[0]; } else { cir->np[0] = cir->dp[2]; cir->np[2] = -cir->dp[0]; } } for (int i = 0; i < m_nsegments; ++i) { dtObstacleSegment* seg = &m_segments[i]; // Precalc if the agent is really close to the segment. const float r = 0.01f; float t; seg->touch = dtDistancePtSegSqr2D(pos, seg->p, seg->q, t) < dtSqr(r); } }
void Agent::Force(const float* p) { float vel[3]; if (m_CrowdAgent && m_CrowdAgent->active) { // calculate velocity dtVsub(vel, p, m_CrowdAgent->npos); vel[1] = 0.0; dtVnormalize(vel); dtVscale(vel, vel, m_CrowdAgent->params.maxSpeed); m_NavMesh->Crowd()->requestMoveVelocity(m_AgentID, vel); } }
float dtDistancePtSegSqr(const float* pt, const float* p, const float* q) { float seg[3], toPt[3], closest[3]; dtVsub(seg, q, p); dtVsub(toPt, pt, p); const float d1 = dtVdot(toPt, seg); const float d2 = dtVdot(seg, seg); if (d1 <= 0) { dtVcopy(closest, p); } else if (d2 <= d1) { dtVcopy(closest, q); } else { dtVmad(closest, p, seg, d1 / d2); } dtVsub(toPt, closest, pt); return dtVlenSqr(toPt); }
static void integrate(dtCrowdAgent* ag, const float dt) { // Fake dynamic constraint. const float maxDelta = ag->params.maxAcceleration * dt; float dv[3]; dtVsub(dv, ag->nvel, ag->vel); float ds = dtVlen(dv); if (ds > maxDelta) dtVscale(dv, dv, maxDelta/ds); dtVadd(ag->vel, ag->vel, dv); // Integrate if (dtVlen(ag->vel) > 0.0001f) dtVmad(ag->npos, ag->npos, ag->vel, dt); else dtVset(ag->vel,0,0,0); }
void dtCrowd::updateVelocity(const float dt, unsigned* agentsIdx, unsigned nbIdx) { nbIdx = (nbIdx < m_maxAgents) ? nbIdx : m_maxAgents; // If we want to update every agent if (agentsIdx == 0) { agentsIdx = m_agentsToUpdate; nbIdx = m_maxAgents; } for (unsigned i = 0; i < nbIdx; ++i) { dtCrowdAgent* ag = 0; if (!getActiveAgent(&ag, agentsIdx[i])) continue; if (ag->behavior) ag->behavior->update(*m_crowdQuery, *ag, *ag, dt); } // Fake dynamic constraint for (unsigned i = 0; i < nbIdx; ++i) { dtCrowdAgent* ag = 0; if (!getActiveAgent(&ag, agentsIdx[i])) continue; if (ag->state != DT_CROWDAGENT_STATE_WALKING) continue; const float maxDelta = ag->maxAcceleration * dt; float dv[3]; dtVsub(dv, ag->desiredVelocity, ag->velocity); float ds = dtVlen(dv); if (ds > maxDelta) dtVscale(dv, dv, maxDelta/ds); dtVadd(ag->velocity, ag->velocity, dv); } }
void dtSeekBehavior::computeForce(const dtCrowdAgent* ag, float* force) { const dtCrowdAgent* target = m_agentParams[ag->id]->targetID; const float predictionFactor = m_agentParams[ag->id]->seekPredictionFactor; // Required force in order to reach the target dtVsub(force, target->npos, ag->npos); // We take into account the prediction factor float scaledVelocity[3] = {0, 0, 0}; dtVscale(scaledVelocity, target->vel, predictionFactor); dtVadd(force, force, scaledVelocity); // Set the force according to the maximum acceleration dtVclamp(force, dtVlen(force), ag->params.maxAcceleration); force[1] = 0; }
void dtCrowd::updateStepOffMeshVelocity(const float dt, dtCrowdAgentDebugInfo*) { // UE4 version of offmesh anims, updates velocity and checks distance instead of fixed time 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; const float dist = dtVdistSqr(ag->npos, anim->endPos); const float distThres = dtSqr(5.0f); if (dist < distThres) { // 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); } } if (ag->state == DT_CROWDAGENT_STATE_OFFMESH) { float dir[3] = { 0 }; dtVsub(dir, anim->endPos, anim->initPos); dir[1] = 0.0f; dtVnormalize(dir); dtVscale(ag->nvel, dir, ag->params.maxSpeed); dtVcopy(ag->vel, ag->nvel); dtVset(ag->dvel, 0, 0, 0); } } }
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 = dtMathSqrtf(d); tmin = (b - rd) * a; tmax = (b + rd) * a; return 1; }
static bool isectSegAABB(const float* sp, const float* sq, const float* amin, const float* amax, float& tmin, float& tmax) { static const float EPS = 1e-6f; float d[3]; dtVsub(d, sq, sp); tmin = 0; // set to -FLT_MAX to get first hit on line tmax = FLT_MAX; // set to max distance ray can travel (for segment) // For all three slabs for (int i = 0; i < 3; i++) { if (fabsf(d[i]) < EPS) { // Ray is parallel to slab. No hit if origin not within slab if (sp[i] < amin[i] || sp[i] > amax[i]) return false; } else { // Compute intersection t value of ray with near and far plane of slab const float ood = 1.0f / d[i]; float t1 = (amin[i] - sp[i]) * ood; float t2 = (amax[i] - sp[i]) * ood; // Make t1 be intersection with near plane, t2 with far plane if (t1 > t2) dtSwap(t1, t2); // Compute the intersection of slab intersections intervals if (t1 > tmin) tmin = t1; if (t2 < tmax) tmax = t2; // Exit with no collision as soon as slab intersection becomes empty if (tmin > tmax) return false; } } return true; }
static int getNeighbours(const float* pos, const float height, const float range, const dtCrowdAgent* skip, dtCrowdNeighbour* result, const int maxResult, dtCrowdAgent** agents, const int /*nagents*/, dtProximityGrid* grid) { int n = 0; static const int MAX_NEIS = 32; unsigned short ids[MAX_NEIS]; int nids = grid->queryItems(pos[0]-range, pos[2]-range, pos[0]+range, pos[2]+range, ids, MAX_NEIS); for (int i = 0; i < nids; ++i) { const dtCrowdAgent* ag = agents[ids[i]]; if (ag == skip) continue; // Check for overlap. float diff[3]; dtVsub(diff, pos, ag->npos); if (fabsf(diff[1]) >= (height+ag->params.height)/2.0f) continue; diff[1] = 0; const float distSqr = dtVlenSqr(diff); if (distSqr > dtSqr(range)) continue; // [UE4] add only when avoidance group allows it const bool bDontAvoid = (skip->params.groupsToIgnore & ag->params.avoidanceGroup) || !(skip->params.groupsToAvoid & ag->params.avoidanceGroup); if (bDontAvoid) continue; n = addNeighbour(ids[i], distSqr, result, n, maxResult); } return n; }
/** @par Inaccurate locomotion or dynamic obstacle avoidance can force the argent position significantly outside the original corridor. Over time this can result in the formation of a non-optimal corridor. Non-optimal paths can also form near the corners of tiles. This function uses an efficient local visibility search to try to optimize the corridor between the current position and @p next. The corridor will change only if @p next is visible from the current position and moving directly toward the point is better than following the existing path. The more inaccurate the agent movement, the more beneficial this function becomes. Simply adjust the frequency of the call to match the needs to the agent. This function is not suitable for long distance searches. */ bool dtPathCorridor::optimizePathVisibility(const float* next, const float pathOptimizationRange, dtNavMeshQuery* navquery, const dtQueryFilter* filter) { dtAssert(m_path); // Clamp the ray to max distance. float goal[3]; dtVcopy(goal, next); float dist = dtVdist2D(m_pos, goal); // If too close to the goal, do not try to optimize. if (dist < 0.01f) return true; // Overshoot a little. This helps to optimize open fields in tiled meshes. // UE4: changes to ray adjustment - make sure it's not going further than newDist float newDist = dtMin(dist+0.01f, pathOptimizationRange); // Adjust ray length. float delta[3]; dtVsub(delta, goal, m_pos); dtVmad(goal, m_pos, delta, newDist / dist); static const int MAX_RES = 32; dtPolyRef res[MAX_RES]; float t, norm[3]; int nres = 0; navquery->raycast(m_path[0], m_pos, goal, filter, &t, norm, res, &nres, MAX_RES); if (nres > 1 && t > 0.99f) { m_npath = dtMergeCorridorStartShortcut(m_path, m_npath, m_maxPath, res, nres); return true; } return false; }
bool TestCase::handleRenderOverlay(double* proj, double* model, int* view) { GLdouble x, y, z; char text[64], subtext[64]; int n = 0; static const float LABEL_DIST = 1.0f; for (Test* iter = m_tests; iter; iter = iter->next) { float pt[3], dir[3]; if (iter->nstraight) { dtVcopy(pt, &iter->straight[3]); if (dtVdist(pt, iter->spos) > LABEL_DIST) { dtVsub(dir, pt, iter->spos); dtVnormalize(dir); dtVmad(pt, iter->spos, dir, LABEL_DIST); } pt[1]+=0.5f; } else { dtVsub(dir, iter->epos, iter->spos); dtVnormalize(dir); dtVmad(pt, iter->spos, dir, LABEL_DIST); pt[1]+=0.5f; } if (gluProject((GLdouble)pt[0], (GLdouble)pt[1], (GLdouble)pt[2], model, proj, view, &x, &y, &z)) { snprintf(text, 64, "Path %d\n", n); unsigned int col = imguiRGBA(0,0,0,128); if (iter->expand) col = imguiRGBA(255,192,0,220); imguiDrawText((int)x, (int)(y-25), IMGUI_ALIGN_CENTER, text, col); } n++; } static int resScroll = 0; bool mouseOverMenu = imguiBeginScrollArea("Test Results", 10, view[3] - 10 - 350, 200, 350, &resScroll); // mouseOverMenu = true; n = 0; for (Test* iter = m_tests; iter; iter = iter->next) { const int total = iter->findNearestPolyTime + iter->findPathTime + iter->findStraightPathTime; snprintf(subtext, 64, "%.4f ms", (float)total/1000.0f); snprintf(text, 64, "Path %d", n); if (imguiCollapse(text, subtext, iter->expand)) iter->expand = !iter->expand; if (iter->expand) { snprintf(text, 64, "Poly: %.4f ms", (float)iter->findNearestPolyTime/1000.0f); imguiValue(text); snprintf(text, 64, "Path: %.4f ms", (float)iter->findPathTime/1000.0f); imguiValue(text); snprintf(text, 64, "Straight: %.4f ms", (float)iter->findStraightPathTime/1000.0f); imguiValue(text); imguiSeparator(); } n++; } imguiEndScrollArea(); return mouseOverMenu; }
float dtObstacleAvoidanceQuery::processSample(const float* vcand, const float cs, const float* pos, const float rad, const float vmax, const float* vel, const float* dvel, dtObstacleAvoidanceDebugData* debug) { // Find min time of impact and exit amongst all obstacles. float tmin = m_horizTime; float side = 0; int nside = 0; for (int i = 0; i < m_ncircles; ++i) { const dtObstacleCircle* cir = &m_circles[i]; // RVO float vab[3]; dtVscale(vab, vcand, 2); dtVsub(vab, vab, vel); dtVsub(vab, vab, cir->vel); // Side side += dtClamp(dtMin(dtVdot2D(cir->dp,vab)*0.5f+0.5f, dtVdot2D(cir->np,vab)*2), 0.0f, 1.0f); nside++; float htmin = 0, htmax = 0; if (!sweepCircleCircle(pos,rad, vab, cir->p,cir->rad, htmin, htmax)) continue; // Handle overlapping obstacles. if (htmin < 0.0f && htmax > 0.0f) { // Avoid more when overlapped. htmin = -htmin * 0.5f; } if (htmin >= 0.0f) { // The closest obstacle is somewhere ahead of us, keep track of nearest obstacle. if (htmin < tmin) tmin = htmin; } } for (int i = 0; i < m_nsegments; ++i) { const dtObstacleSegment* seg = &m_segments[i]; float htmin = 0; if (seg->touch) { // Special case when the agent is very close to the segment. float sdir[3], snorm[3]; dtVsub(sdir, seg->q, seg->p); snorm[0] = -sdir[2]; snorm[2] = sdir[0]; // If the velocity is pointing towards the segment, no collision. if (dtVdot2D(snorm, vcand) < 0.0f) continue; // Else immediate collision. htmin = 0.0f; } else { if (!isectRaySeg(pos, vcand, seg->p, seg->q, htmin)) continue; } // Avoid less when facing walls. htmin *= 2.0f; // The closest obstacle is somewhere ahead of us, keep track of nearest obstacle. if (htmin < tmin) tmin = htmin; } // Normalize side bias, to prevent it dominating too much. if (nside) side /= nside; const float ivmax = 1.0f / vmax; const float vpen = m_weightDesVel * (dtVdist2D(vcand, dvel) * ivmax); const float vcpen = m_weightCurVel * (dtVdist2D(vcand, vel) * ivmax); const float spen = m_weightSide * side; const float tpen = m_weightToi * (1.0f/(0.1f+tmin / m_horizTime)); const float penalty = vpen + vcpen + spen + tpen; // Store different penalties for debug viewing if (debug) debug->addSample(vcand, cs, penalty, vpen, vcpen, spen, tpen); return penalty; }
void cocos2d::NavMesh::findPath(const Vec3 &start, const Vec3 &end, std::vector<Vec3> &pathPoints) { static const int MAX_POLYS = 256; static const int MAX_SMOOTH = 2048; float ext[3]; ext[0] = 2; ext[1] = 4; ext[2] = 2; dtQueryFilter filter; dtPolyRef startRef, endRef; dtPolyRef polys[MAX_POLYS]; int npolys = 0; _navMeshQuery->findNearestPoly(&start.x, ext, &filter, &startRef, 0); _navMeshQuery->findNearestPoly(&end.x, ext, &filter, &endRef, 0); _navMeshQuery->findPath(startRef, endRef, &start.x, &end.x, &filter, polys, &npolys, MAX_POLYS); if (npolys) { //// Iterate over the path to find smooth path on the detail mesh surface. //dtPolyRef polys[MAX_POLYS]; //memcpy(polys, polys, sizeof(dtPolyRef)*npolys); //int npolys = npolys; float iterPos[3], targetPos[3]; _navMeshQuery->closestPointOnPoly(startRef, &start.x, iterPos, 0); _navMeshQuery->closestPointOnPoly(polys[npolys - 1], &end.x, targetPos, 0); static const float STEP_SIZE = 0.5f; static const float SLOP = 0.01f; int nsmoothPath = 0; //dtVcopy(&m_smoothPath[m_nsmoothPath * 3], iterPos); //m_nsmoothPath++; pathPoints.push_back(Vec3(iterPos[0], iterPos[1], iterPos[2])); 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 < MAX_SMOOTH) { // Find location to steer towards. float steerPos[3]; unsigned char steerPosFlag; dtPolyRef steerPosRef; if (!getSteerTarget(_navMeshQuery, 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 = dtMathSqrtf(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; _navMeshQuery->moveAlongSurface(polys[0], iterPos, moveTgt, &filter, result, visited, &nvisited, 16); npolys = fixupCorridor(polys, npolys, MAX_POLYS, visited, nvisited); npolys = fixupShortcuts(polys, npolys, _navMeshQuery); float h = 0; _navMeshQuery->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 (nsmoothPath < MAX_SMOOTH) { //dtVcopy(&m_smoothPath[m_nsmoothPath * 3], iterPos); //m_nsmoothPath++; pathPoints.push_back(Vec3(iterPos[0], iterPos[1], iterPos[2])); 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 = _navMesh->getOffMeshConnectionPolyEndPoints(prevRef, polyRef, startPos, endPos); if (dtStatusSucceed(status)) { if (nsmoothPath < MAX_SMOOTH) { //dtVcopy(&m_smoothPath[m_nsmoothPath * 3], startPos); //m_nsmoothPath++; pathPoints.push_back(Vec3(startPos[0], startPos[1], startPos[2])); nsmoothPath++; // Hack to make the dotted path not visible during off-mesh connection. if (nsmoothPath & 1) { //dtVcopy(&m_smoothPath[m_nsmoothPath * 3], startPos); //m_nsmoothPath++; pathPoints.push_back(Vec3(startPos[0], startPos[1], startPos[2])); nsmoothPath++; } } // Move position at the other side of the off-mesh link. dtVcopy(iterPos, endPos); float eh = 0.0f; _navMeshQuery->getPolyHeight(polys[0], iterPos, &eh); iterPos[1] = eh; } } // Store results. if (nsmoothPath < MAX_SMOOTH) { //dtVcopy(&m_smoothPath[m_nsmoothPath * 3], iterPos); //m_nsmoothPath++; pathPoints.push_back(Vec3(iterPos[0], iterPos[1], iterPos[2])); nsmoothPath++; } } } }
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 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 dtCrowd::updateStepSteering(const float dt, dtCrowdAgentDebugInfo*) { // Calculate steering. for (int i = 0; i < m_numActiveAgents; ++i) { dtCrowdAgent* ag = m_activeAgents[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); float speedScale = 1.0f; if (ag->params.updateFlags & DT_CROWD_SLOWDOWN_AT_GOAL) { // 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. 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 = sqrtf(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); } }
void dtClosestPtPointTriangle(float* closest, const float* p, const float* a, const float* b, const float* c) { // Check if P in vertex region outside A float ab[3], ac[3], ap[3]; dtVsub(ab, b, a); dtVsub(ac, c, a); dtVsub(ap, p, a); float d1 = dtVdot(ab, ap); float d2 = dtVdot(ac, ap); if (d1 <= 0.0f && d2 <= 0.0f) { // barycentric coordinates (1,0,0) dtVcopy(closest, a); return; } // Check if P in vertex region outside B float bp[3]; dtVsub(bp, p, b); float d3 = dtVdot(ab, bp); float d4 = dtVdot(ac, bp); if (d3 >= 0.0f && d4 <= d3) { // barycentric coordinates (0,1,0) dtVcopy(closest, b); return; } // Check if P in edge region of AB, if so return projection of P onto AB float vc = d1*d4 - d3*d2; if (vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f) { // barycentric coordinates (1-v,v,0) float v = d1 / (d1 - d3); closest[0] = a[0] + v * ab[0]; closest[1] = a[1] + v * ab[1]; closest[2] = a[2] + v * ab[2]; return; } // Check if P in vertex region outside C float cp[3]; dtVsub(cp, p, c); float d5 = dtVdot(ab, cp); float d6 = dtVdot(ac, cp); if (d6 >= 0.0f && d5 <= d6) { // barycentric coordinates (0,0,1) dtVcopy(closest, c); return; } // Check if P in edge region of AC, if so return projection of P onto AC float vb = d5*d2 - d1*d6; if (vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f) { // barycentric coordinates (1-w,0,w) float w = d2 / (d2 - d6); closest[0] = a[0] + w * ac[0]; closest[1] = a[1] + w * ac[1]; closest[2] = a[2] + w * ac[2]; return; } // Check if P in edge region of BC, if so return projection of P onto BC float va = d3*d6 - d5*d4; if (va <= 0.0f && (d4 - d3) >= 0.0f && (d5 - d6) >= 0.0f) { // barycentric coordinates (0,1-w,w) float w = (d4 - d3) / ((d4 - d3) + (d5 - d6)); closest[0] = b[0] + w * (c[0] - b[0]); closest[1] = b[1] + w * (c[1] - b[1]); closest[2] = b[2] + w * (c[2] - b[2]); return; } // P inside face region. Compute Q through its barycentric coordinates (u,v,w) float denom = 1.0f / (va + vb + vc); float v = vb * denom; float w = vc * denom; closest[0] = a[0] + ab[0] * v + ac[0] * w; closest[1] = a[1] + ab[1] * v + ac[1] * w; closest[2] = a[2] + ab[2] * v + ac[2] * w; }
/** @par This is the function used to plan local movement within the corridor. One or more corners can be detected in order to plan movement. It performs essentially the same function as #dtNavMeshQuery::findStraightPath. Due to internal optimizations, the maximum number of corners returned will be (@p maxCorners - 1) For example: If the buffers are sized to hold 10 corners, the function will never return more than 9 corners. So if 10 corners are needed, the buffers should be sized for 11 corners. If the target is within range, it will be the last corner and have a polygon reference id of zero. */ int dtPathCorridor::findCorners(float* cornerVerts, unsigned char* cornerFlags, dtPolyRef* cornerPolys, const int maxCorners, dtNavMeshQuery* navquery, const dtQueryFilter* filter, float radius) { dtAssert(m_path); dtAssert(m_npath); static const float MIN_TARGET_DIST = 0.01f; dtQueryResult result; navquery->findStraightPath(m_pos, m_target, m_path, m_npath, result); int ncorners = dtMin(result.size(), maxCorners); result.copyRefs(cornerPolys, maxCorners); result.copyFlags(cornerFlags, maxCorners); result.copyPos(cornerVerts, maxCorners); // Prune points in the beginning of the path which are too close. while (ncorners) { if ((cornerFlags[0] & DT_STRAIGHTPATH_OFFMESH_CONNECTION) || dtVdist2DSqr(&cornerVerts[0], m_pos) > dtSqr(MIN_TARGET_DIST)) break; ncorners--; if (ncorners) { memmove(cornerFlags, cornerFlags+1, sizeof(unsigned char)*ncorners); memmove(cornerPolys, cornerPolys+1, sizeof(dtPolyRef)*ncorners); memmove(cornerVerts, cornerVerts+3, sizeof(float)*3*ncorners); } } // Prune points after an off-mesh connection. for (int i = 0; i < ncorners; ++i) { if (cornerFlags[i] & DT_STRAIGHTPATH_OFFMESH_CONNECTION) { ncorners = i+1; break; } } // [UE4] Offset path points from corners const dtNavMesh* nav = navquery->getAttachedNavMesh(); float v1[3], v2[3], dir[3]; for (int i = 0; i < ncorners - 1; i++) { int fromIdx = 0; while (m_path[fromIdx] != cornerPolys[i] && fromIdx < m_npath) { fromIdx++; } if (m_path[fromIdx] != cornerPolys[i] || fromIdx == 0) continue; const dtMeshTile* tile0 = 0; const dtMeshTile* tile1 = 0; const dtPoly* poly0 = 0; const dtPoly* poly1 = 0; nav->getTileAndPolyByRefUnsafe(m_path[fromIdx - 1], &tile0, &poly0); nav->getTileAndPolyByRefUnsafe(m_path[fromIdx], &tile1, &poly1); if (tile0 != tile1) continue; float* corner = &cornerVerts[i * 3]; unsigned char dummyT1, dummyT2; navquery->getPortalPoints(m_path[fromIdx - 1], m_path[fromIdx], v1, v2, dummyT1, dummyT2); const float edgeLen = dtVdist(v1, v2); if (edgeLen > 0.001f) { const float edgeOffset = dtMin(radius, edgeLen * 0.75f) / edgeLen; if (dtVequal(corner, v1)) { dtVsub(dir, v2, v1); dtVmad(corner, corner, dir, edgeOffset); } else { dtVsub(dir, v1, v2); dtVmad(corner, corner, dir, edgeOffset); } } } return ncorners; }
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 dtCrowd::updateStepMove(const float dt, dtCrowdAgentDebugInfo*) { // Integrate. for (int i = 0; i < m_numActiveAgents; ++i) { dtCrowdAgent* ag = m_activeAgents[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 < m_numActiveAgents; ++i) { dtCrowdAgent* ag = m_activeAgents[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 = sqrtf(dist); float pen = (ag->params.radius + nei->params.radius) - dist; if (dist < 0.0001f) { // m_activeAgents 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 < m_numActiveAgents; ++i) { dtCrowdAgent* ag = m_activeAgents[i]; if (ag->state != DT_CROWDAGENT_STATE_WALKING) continue; dtVadd(ag->npos, ag->npos, ag->disp); } } }
void NavMeshTesterTool::handleToggle() { // TODO: merge separate to a path iterator. Use same code in recalc() too. if (m_toolMode != TOOLMODE_PATHFIND_FOLLOW) return; if (!m_sposSet || !m_eposSet || !m_startRef || !m_endRef) return; static const float STEP_SIZE = 0.5f; static const float SLOP = 0.01f; if (m_pathIterNum == 0) { m_navQuery->findPath(m_startRef, m_endRef, m_spos, m_epos, &m_filter, m_polys, &m_npolys, MAX_POLYS); m_nsmoothPath = 0; m_pathIterPolyCount = m_npolys; if (m_pathIterPolyCount) memcpy(m_pathIterPolys, m_polys, sizeof(dtPolyRef)*m_pathIterPolyCount); if (m_pathIterPolyCount) { // Iterate over the path to find smooth path on the detail mesh surface. m_navQuery->closestPointOnPolyBoundary(m_startRef, m_spos, m_iterPos); m_navQuery->closestPointOnPolyBoundary(m_pathIterPolys[m_pathIterPolyCount-1], m_epos, m_targetPos); m_nsmoothPath = 0; dtVcopy(&m_smoothPath[m_nsmoothPath*3], m_iterPos); m_nsmoothPath++; } } dtVcopy(m_prevIterPos, m_iterPos); m_pathIterNum++; if (!m_pathIterPolyCount) return; if (m_nsmoothPath >= MAX_SMOOTH) return; // Move towards target a small advancement at a time until target reached or // when ran out of memory to store the path. // Find location to steer towards. float steerPos[3]; unsigned char steerPosFlag; dtPolyRef steerPosRef; if (!getSteerTarget(m_navQuery, m_iterPos, m_targetPos, SLOP, m_pathIterPolys, m_pathIterPolyCount, steerPos, steerPosFlag, steerPosRef, m_steerPoints, &m_steerPointCount)) return; dtVcopy(m_steerPos, steerPos); 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, m_iterPos); len = sqrtf(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, m_iterPos, delta, len); // Move float result[3]; dtPolyRef visited[16]; int nvisited = 0; m_navQuery->moveAlongSurface(m_pathIterPolys[0], m_iterPos, moveTgt, &m_filter, result, visited, &nvisited, 16); m_pathIterPolyCount = fixupCorridor(m_pathIterPolys, m_pathIterPolyCount, MAX_POLYS, visited, nvisited); float h = 0; m_navQuery->getPolyHeight(m_pathIterPolys[0], result, &h); result[1] = h; dtVcopy(m_iterPos, result); // Handle end of path and off-mesh links when close enough. if (endOfPath && inRange(m_iterPos, steerPos, SLOP, 1.0f)) { // Reached end of path. dtVcopy(m_iterPos, m_targetPos); if (m_nsmoothPath < MAX_SMOOTH) { dtVcopy(&m_smoothPath[m_nsmoothPath*3], m_iterPos); m_nsmoothPath++; } return; } else if (offMeshConnection && inRange(m_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 = m_pathIterPolys[0]; int npos = 0; while (npos < m_pathIterPolyCount && polyRef != steerPosRef) { prevRef = polyRef; polyRef = m_pathIterPolys[npos]; npos++; } for (int i = npos; i < m_pathIterPolyCount; ++i) m_pathIterPolys[i-npos] = m_pathIterPolys[i]; m_pathIterPolyCount -= 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(m_iterPos, endPos); float h; m_navQuery->getPolyHeight(m_pathIterPolys[0], m_iterPos, &h); m_iterPos[1] = h; } } // Store results. if (m_nsmoothPath < MAX_SMOOTH) { dtVcopy(&m_smoothPath[m_nsmoothPath*3], m_iterPos); m_nsmoothPath++; } }
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); } } }
/* Calculate the collision penalty for a given velocity vector * * @param vcand sampled velocity * @param dvel desired velocity * @param minPenalty threshold penalty for early out */ float dtObstacleAvoidanceQuery::processSample(const float* vcand, const float cs, const float* pos, const float rad, const float* vel, const float* dvel, const float minPenalty, dtObstacleAvoidanceDebugData* debug) { // penalty for straying away from the desired and current velocities const float vpen = m_params.weightDesVel * (dtVdist2D(vcand, dvel) * m_invVmax); const float vcpen = m_params.weightCurVel * (dtVdist2D(vcand, vel) * m_invVmax); // find the threshold hit time to bail out based on the early out penalty // (see how the penalty is calculated below to understnad) float minPen = minPenalty - vpen - vcpen; float tThresold = ((double)m_params.weightToi/(double)minPen - 0.1) * (double)m_params.horizTime; if (tThresold - m_params.horizTime > -FLT_EPSILON) return minPenalty; // already too much // Find min time of impact and exit amongst all obstacles. float tmin = m_params.horizTime; float side = 0; int nside = 0; for (int i = 0; i < m_ncircles; ++i) { const dtObstacleCircle* cir = &m_circles[i]; // RVO float vab[3]; dtVscale(vab, vcand, 2); dtVsub(vab, vab, vel); dtVsub(vab, vab, cir->vel); // Side side += dtClamp(dtMin(dtVdot2D(cir->dp,vab)*0.5f+0.5f, dtVdot2D(cir->np,vab)*2), 0.0f, 1.0f); nside++; float htmin = 0, htmax = 0; if (!sweepCircleCircle(pos,rad, vab, cir->p,cir->rad, htmin, htmax)) continue; // Handle overlapping obstacles. if (htmin < 0.0f && htmax > 0.0f) { // Avoid more when overlapped. htmin = -htmin * 0.5f; } if (htmin >= 0.0f) { // The closest obstacle is somewhere ahead of us, keep track of nearest obstacle. if (htmin < tmin) { tmin = htmin; if (tmin < tThresold) return minPenalty; } } } for (int i = 0; i < m_nsegments; ++i) { const dtObstacleSegment* seg = &m_segments[i]; float htmin = 0; if (seg->touch) { // Special case when the agent is very close to the segment. float sdir[3], snorm[3]; dtVsub(sdir, seg->q, seg->p); snorm[0] = -sdir[2]; snorm[2] = sdir[0]; // If the velocity is pointing towards the segment, no collision. if (dtVdot2D(snorm, vcand) < 0.0f) continue; // Else immediate collision. htmin = 0.0f; } else { if (!isectRaySeg(pos, vcand, seg->p, seg->q, htmin)) continue; } // Avoid less when facing walls. htmin *= 2.0f; // The closest obstacle is somewhere ahead of us, keep track of nearest obstacle. if (htmin < tmin) { tmin = htmin; if (tmin < tThresold) return minPenalty; } } // Normalize side bias, to prevent it dominating too much. if (nside) side /= nside; const float spen = m_params.weightSide * side; const float tpen = m_params.weightToi * (1.0f/(0.1f+tmin*m_invHorizTime)); const float penalty = vpen + vcpen + spen + tpen; // Store different penalties for debug viewing if (debug) debug->addSample(vcand, cs, penalty, vpen, vcpen, spen, tpen); return penalty; }
void dtCrowd::updateStepProximityData(const float dt, dtCrowdAgentDebugInfo*) { // Register agents to proximity grid. m_grid->clear(); for (int i = 0; i < m_numActiveAgents; ++i) { dtCrowdAgent* ag = m_activeAgents[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 < m_numActiveAgents; ++i) { dtCrowdAgent* ag = m_activeAgents[i]; if (ag->state != DT_CROWDAGENT_STATE_WALKING) continue; m_navquery->updateLinkFilter(ag->params.linkFilter); // 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_filters[ag->params.filter])) { // UE4: force removing segments too close to offmesh links const bool useForcedRemove = m_linkRemovalRadius > 0.0f && ag->ncorners && (ag->cornerFlags[ag->ncorners - 1] & DT_STRAIGHTPATH_OFFMESH_CONNECTION); const float* removePos = useForcedRemove ? &ag->cornerVerts[(ag->ncorners - 1) * 3] : 0; const float removeRadius = m_linkRemovalRadius; // UE4: prepare filter containing only current area // boundary update will take all corner polys and include them in local neighborhood unsigned char allowedArea = DT_WALKABLE_AREA; if (m_raycastSingleArea) { m_navquery->getAttachedNavMesh()->getPolyArea(ag->corridor.getFirstPoly(), &allowedArea); m_raycastFilter.setAreaCost(allowedArea, 1.0f); } // UE4: move dir for segment scoring float moveDir[3] = { 0.0f }; if (ag->ncorners) { dtVsub(moveDir, &ag->cornerVerts[2], &ag->cornerVerts[0]); } else { dtVcopy(moveDir, ag->vel); } dtVnormalize(moveDir); ag->boundary.update(ag->corridor.getFirstPoly(), ag->npos, ag->params.collisionQueryRange, removePos, removeRadius, useForcedRemove, ag->corridor.getPath(), ag->corridor.getPathCount(), moveDir, m_navquery, m_raycastSingleArea ? &m_raycastFilter : &m_filters[ag->params.filter]); m_raycastFilter.setAreaCost(allowedArea, DT_UNWALKABLE_POLY_COST); } // Query neighbour agents ag->nneis = getNeighbours(ag->npos, ag->params.height, ag->params.collisionQueryRange, ag, ag->neis, DT_CROWDAGENT_MAX_NEIGHBOURS, m_activeAgents, m_numActiveAgents, m_grid); for (int j = 0; j < ag->nneis; j++) ag->neis[j].idx = getAgentIndex(m_activeAgents[ag->neis[j].idx]); } }