dtPolyRef dtNavMesh::findNearestPolyInTile(const dtMeshTile* tile, const float* center, const float* extents, float* nearestPt) const { float bmin[3], bmax[3]; dtVsub(bmin, center, extents); dtVadd(bmax, center, extents); // Get nearby polygons from proximity grid. dtPolyRef polys[128]; int polyCount = queryPolygonsInTile(tile, bmin, bmax, polys, 128); // Find nearest polygon amongst the nearby polygons. dtPolyRef nearest = 0; float nearestDistanceSqr = FLT_MAX; for (int i = 0; i < polyCount; ++i) { dtPolyRef ref = polys[i]; float closestPtPoly[3]; closestPointOnPolyInTile(tile, decodePolyIdPoly(ref), center, closestPtPoly); float d = dtVdistSqr(center, closestPtPoly); if (d < nearestDistanceSqr) { if (nearestPt) dtVcopy(nearestPt, closestPtPoly); nearestDistanceSqr = d; nearest = ref; } } return nearest; }
dtPolyRef dtNavMesh::findNearestPolyInTile(const dtMeshTile* tile, const float* center, const float* extents, float* nearestPt) const { float bmin[3], bmax[3]; dtVsub(bmin, center, extents); dtVadd(bmax, center, extents); // Get nearby polygons from proximity grid. dtPolyRef polys[128]; int polyCount = queryPolygonsInTile(tile, bmin, bmax, polys, 128); // Find nearest polygon amongst the nearby polygons. dtPolyRef nearest = 0; float nearestDistanceSqr = FLT_MAX; for (int i = 0; i < polyCount; ++i) { dtPolyRef ref = polys[i]; float closestPtPoly[3]; float diff[3]; bool posOverPoly = false; float d = 0; closestPointOnPoly(ref, center, closestPtPoly, &posOverPoly); // If a point is directly over a polygon and closer than // climb height, favor that instead of straight line nearest point. dtVsub(diff, center, closestPtPoly); if (posOverPoly) { d = dtAbs(diff[1]) - tile->header->walkableClimb; d = d > 0 ? d*d : 0; } else { d = dtVlenSqr(diff); } if (d < nearestDistanceSqr) { dtVcopy(nearestPt, closestPtPoly); nearestDistanceSqr = d; nearest = ref; } } return nearest; }
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 dtSeekBehavior::applyForce(const dtCrowdAgent* oldAgent, dtCrowdAgent* newAgent, float* force, float dt) { float tmpForce[] = {0, 0, 0}; float newVelocity[] = {0, 0, 0}; const dtCrowdAgent* target = m_agentParams[oldAgent->id]->targetID; const float distance = m_agentParams[oldAgent->id]->seekDistance; // Adapting the force to the dt and the previous velocity dtVscale(tmpForce, force, dt); dtVadd(newVelocity, oldAgent->vel, tmpForce); float currentSpeed = dtVlen(oldAgent->vel); // Required distance to reach nil speed according to the acceleration and current speed float slowDist = currentSpeed * (currentSpeed - 0) / oldAgent->params.maxAcceleration; float distToObj = dtVdist(oldAgent->npos, target->npos) - oldAgent->params.radius - target->params.radius - distance; // If we have reached the target, we stop if (distToObj <= EPSILON) { dtVset(newVelocity, 0, 0, 0); newAgent->desiredSpeed = 0.f; } // If the have to slow down else if (distToObj < slowDist) { float slowDownRadius = distToObj / slowDist; dtVscale(newVelocity, newVelocity, slowDownRadius); newAgent->desiredSpeed = dtVlen(newVelocity); } // Else, we move as fast as possible else newAgent->desiredSpeed = oldAgent->params.maxSpeed; // Check for maximal speed dtVclamp(newVelocity, dtVlen(newVelocity), oldAgent->params.maxSpeed); dtVcopy(newAgent->dvel, newVelocity); }
/// @par /// /// The output data array is allocated using the detour allocator (dtAlloc()). The method /// used to free the memory will be determined by how the tile is added to the navigation /// mesh. /// /// @see dtNavMesh, dtNavMesh::addTile() bool dtCreateNavMeshData(dtNavMeshCreateParams* params, unsigned char** outData, int* outDataSize) { if (params->nvp > DT_VERTS_PER_POLYGON) return false; if (params->vertCount >= 0xffff) return false; if (!params->vertCount || !params->verts) return false; if (!params->polyCount || !params->polys) return false; const int nvp = params->nvp; // Classify off-mesh connection points. We store only the connections // whose start point is inside the tile. unsigned char* offMeshConClass = 0; int storedOffMeshConCount = 0; int offMeshConLinkCount = 0; int storedOffMeshSegCount = 0; if (params->offMeshConCount > 0) { offMeshConClass = (unsigned char*)dtAlloc(sizeof(unsigned char)*params->offMeshConCount*2, DT_ALLOC_TEMP); if (!offMeshConClass) return false; memset(offMeshConClass, 0, sizeof(unsigned char)*params->offMeshConCount*2); // Find tight heigh bounds, used for culling out off-mesh start locations. float hmin = FLT_MAX; float hmax = -FLT_MAX; for (int i = 0; i < params->vertCount; ++i) { const unsigned short* iv = ¶ms->verts[i*3]; const float h = params->bmin[1] + iv[1] * params->ch; hmin = dtMin(hmin,h); hmax = dtMax(hmax,h); } if (params->detailVerts && params->detailVertsCount) { for (int i = 0; i < params->detailVertsCount; ++i) { const float h = params->detailVerts[i*3+1]; hmin = dtMin(hmin,h); hmax = dtMax(hmax,h); } } hmin -= params->walkableClimb; hmax += params->walkableClimb; float bmin[3], bmax[3]; dtVcopy(bmin, params->bmin); dtVcopy(bmax, params->bmax); bmin[1] = hmin; bmax[1] = hmax; float bverts[3*4]; bverts[ 0] = bmin[0]; bverts[ 2] = bmin[2]; bverts[ 3] = bmax[0]; bverts[ 5] = bmin[2]; bverts[ 6] = bmax[0]; bverts[ 8] = bmax[2]; bverts[ 9] = bmin[0]; bverts[11] = bmax[2]; for (int i = 0; i < params->offMeshConCount; ++i) { const dtOffMeshLinkCreateParams& offMeshCon = params->offMeshCons[i]; if (offMeshCon.type & DT_OFFMESH_CON_POINT) { offMeshConClass[i*2+0] = classifyOffMeshPoint(offMeshCon.vertsA0, bmin, bmax); offMeshConClass[i*2+1] = classifyOffMeshPoint(offMeshCon.vertsB0, bmin, bmax); // Zero out off-mesh start positions which are not even potentially touching the mesh. if (offMeshConClass[i*2+0] == 0xff) { if ((offMeshCon.vertsA0[1] - offMeshCon.snapHeight) < bmin[1] && (offMeshCon.vertsA0[1] + offMeshCon.snapHeight) > bmax[1]) { offMeshConClass[i * 2 + 0] = 0; } } // Count how many links should be allocated for off-mesh connections. if (offMeshConClass[i*2+0] == 0xff) offMeshConLinkCount++; if (offMeshConClass[i*2+1] == 0xff) offMeshConLinkCount++; if (offMeshConClass[i*2+0] == 0xff) storedOffMeshConCount++; } else if (offMeshCon.type & DT_OFFMESH_CON_SEGMENT) { int smin, smax; float tmin, tmax; if ((offMeshCon.vertsA0[1] >= bmin[1] && offMeshCon.vertsA0[1] <= bmax[1] && classifyOffMeshPoint(offMeshCon.vertsA0, bmin, bmax) == 0xff) || (offMeshCon.vertsA1[1] >= bmin[1] && offMeshCon.vertsA1[1] <= bmax[1] && classifyOffMeshPoint(offMeshCon.vertsA1, bmin, bmax) == 0xff) || (offMeshCon.vertsB0[1] >= bmin[1] && offMeshCon.vertsB0[1] <= bmax[1] && classifyOffMeshPoint(offMeshCon.vertsB0, bmin, bmax) == 0xff) || (offMeshCon.vertsB1[1] >= bmin[1] && offMeshCon.vertsB1[1] <= bmax[1] && classifyOffMeshPoint(offMeshCon.vertsB1, bmin, bmax) == 0xff) || dtIntersectSegmentPoly2D(offMeshCon.vertsA0, offMeshCon.vertsA1, bverts, 4, tmin, tmax, smin, smax) || dtIntersectSegmentPoly2D(offMeshCon.vertsB0, offMeshCon.vertsB1, bverts, 4, tmin, tmax, smin, smax)) { offMeshConClass[i*2] = 0xff; storedOffMeshSegCount++; } } } } // Off-mesh connections are stored as polygons, adjust values. const int firstSegVert = params->vertCount + storedOffMeshConCount*2; const int firstSegPoly = params->polyCount + storedOffMeshConCount; const int totPolyCount = firstSegPoly + storedOffMeshSegCount*DT_MAX_OFFMESH_SEGMENT_PARTS; const int totVertCount = firstSegVert + storedOffMeshSegCount*DT_MAX_OFFMESH_SEGMENT_PARTS*4; // Find portal edges which are at tile borders. int edgeCount = 0; int portalCount = 0; for (int i = 0; i < params->polyCount; ++i) { const unsigned short* p = ¶ms->polys[i*2*nvp]; for (int j = 0; j < nvp; ++j) { if (p[j] == MESH_NULL_IDX) break; edgeCount++; if (p[nvp+j] & 0x8000) { unsigned short dir = p[nvp+j] & 0xf; if (dir != 0xf) portalCount++; } } } // offmesh links will be added in dynamic array const int maxLinkCount = edgeCount + portalCount*2; // Find unique detail vertices. int uniqueDetailVertCount = 0; int detailTriCount = 0; if (params->detailMeshes) { // Has detail mesh, count unique detail vertex count and use input detail tri count. detailTriCount = params->detailTriCount; for (int i = 0; i < params->polyCount; ++i) { const unsigned short* p = ¶ms->polys[i*nvp*2]; int ndv = params->detailMeshes[i*4+1]; int nv = 0; for (int j = 0; j < nvp; ++j) { if (p[j] == MESH_NULL_IDX) break; nv++; } ndv -= nv; uniqueDetailVertCount += ndv; } } else { // No input detail mesh, build detail mesh from nav polys. uniqueDetailVertCount = 0; // No extra detail verts. detailTriCount = 0; for (int i = 0; i < params->polyCount; ++i) { const unsigned short* p = ¶ms->polys[i*nvp*2]; int nv = 0; for (int j = 0; j < nvp; ++j) { if (p[j] == MESH_NULL_IDX) break; nv++; } detailTriCount += nv-2; } } // Calculate data size const int headerSize = dtAlign4(sizeof(dtMeshHeader)); const int vertsSize = dtAlign4(sizeof(float)*3*totVertCount); const int polysSize = dtAlign4(sizeof(dtPoly)*totPolyCount); const int linksSize = dtAlign4(sizeof(dtLink)*maxLinkCount); const int detailMeshesSize = dtAlign4(sizeof(dtPolyDetail)*params->polyCount); const int detailVertsSize = dtAlign4(sizeof(float)*3*uniqueDetailVertCount); const int detailTrisSize = dtAlign4(sizeof(unsigned char)*4*detailTriCount); const int bvTreeSize = params->buildBvTree ? dtAlign4(sizeof(dtBVNode)*params->polyCount*2) : 0; const int offMeshConsSize = dtAlign4(sizeof(dtOffMeshConnection)*storedOffMeshConCount); const int offMeshSegSize = dtAlign4(sizeof(dtOffMeshSegmentConnection)*storedOffMeshSegCount); const int clustersSize = dtAlign4(sizeof(dtCluster)*params->clusterCount); const int polyClustersSize = dtAlign4(sizeof(unsigned short)*params->polyCount); const int dataSize = headerSize + vertsSize + polysSize + linksSize + detailMeshesSize + detailVertsSize + detailTrisSize + bvTreeSize + offMeshConsSize + offMeshSegSize + clustersSize + polyClustersSize; unsigned char* data = (unsigned char*)dtAlloc(sizeof(unsigned char)*dataSize, DT_ALLOC_PERM); if (!data) { dtFree(offMeshConClass); return false; } memset(data, 0, dataSize); unsigned char* d = data; dtMeshHeader* header = (dtMeshHeader*)d; d += headerSize; float* navVerts = (float*)d; d += vertsSize; dtPoly* navPolys = (dtPoly*)d; d += polysSize; d += linksSize; dtPolyDetail* navDMeshes = (dtPolyDetail*)d; d += detailMeshesSize; float* navDVerts = (float*)d; d += detailVertsSize; unsigned char* navDTris = (unsigned char*)d; d += detailTrisSize; dtBVNode* navBvtree = (dtBVNode*)d; d += bvTreeSize; dtOffMeshConnection* offMeshCons = (dtOffMeshConnection*)d; d += offMeshConsSize; dtOffMeshSegmentConnection* offMeshSegs = (dtOffMeshSegmentConnection*)d; d += offMeshSegSize; dtCluster* clusters = (dtCluster*)d; d += clustersSize; unsigned short* polyClusters = (unsigned short*)d; d += polyClustersSize; // Store header header->magic = DT_NAVMESH_MAGIC; header->version = DT_NAVMESH_VERSION; header->x = params->tileX; header->y = params->tileY; header->layer = params->tileLayer; header->userId = params->userId; header->polyCount = totPolyCount; header->vertCount = totVertCount; header->maxLinkCount = maxLinkCount; dtVcopy(header->bmin, params->bmin); dtVcopy(header->bmax, params->bmax); header->detailMeshCount = params->polyCount; header->detailVertCount = uniqueDetailVertCount; header->detailTriCount = detailTriCount; header->bvQuantFactor = 1.0f / params->cs; header->offMeshBase = params->polyCount; header->offMeshSegPolyBase = firstSegPoly; header->offMeshSegVertBase = firstSegVert; header->walkableHeight = params->walkableHeight; header->walkableRadius = params->walkableRadius; header->walkableClimb = params->walkableClimb; header->offMeshConCount = storedOffMeshConCount; header->offMeshSegConCount = storedOffMeshSegCount; header->bvNodeCount = params->buildBvTree ? params->polyCount*2 : 0; header->clusterCount = params->clusterCount; const int offMeshVertsBase = params->vertCount; const int offMeshPolyBase = params->polyCount; // Store vertices // Mesh vertices for (int i = 0; i < params->vertCount; ++i) { const unsigned short* iv = ¶ms->verts[i*3]; float* v = &navVerts[i*3]; v[0] = params->bmin[0] + iv[0] * params->cs; v[1] = params->bmin[1] + iv[1] * params->ch; v[2] = params->bmin[2] + iv[2] * params->cs; } // Off-mesh point link vertices. int n = 0; for (int i = 0; i < params->offMeshConCount; ++i) { const dtOffMeshLinkCreateParams& offMeshCon = params->offMeshCons[i]; // Only store connections which start from this tile. if ((offMeshConClass[i*2+0] == 0xff) && (offMeshCon.type & DT_OFFMESH_CON_POINT)) { float* v = &navVerts[(offMeshVertsBase + n*2)*3]; dtVcopy(&v[0], &offMeshCon.vertsA0[0]); dtVcopy(&v[3], &offMeshCon.vertsB0[0]); n++; } } // Store polygons // Mesh polys const unsigned short* src = params->polys; for (int i = 0; i < params->polyCount; ++i) { dtPoly* p = &navPolys[i]; p->vertCount = 0; p->flags = params->polyFlags[i]; p->setArea(params->polyAreas[i]); p->setType(DT_POLYTYPE_GROUND); for (int j = 0; j < nvp; ++j) { if (src[j] == MESH_NULL_IDX) break; p->verts[j] = src[j]; if (src[nvp+j] & 0x8000) { // Border or portal edge. unsigned short dir = src[nvp+j] & 0xf; if (dir == 0xf) // Border p->neis[j] = 0; else if (dir == 0) // Portal x- p->neis[j] = DT_EXT_LINK | 4; else if (dir == 1) // Portal z+ p->neis[j] = DT_EXT_LINK | 2; else if (dir == 2) // Portal x+ p->neis[j] = DT_EXT_LINK | 0; else if (dir == 3) // Portal z- p->neis[j] = DT_EXT_LINK | 6; } else { // Normal connection p->neis[j] = src[nvp+j]+1; } p->vertCount++; } src += nvp*2; } // Off-mesh point connection polygons. n = 0; int nseg = 0; for (int i = 0; i < params->offMeshConCount; ++i) { const dtOffMeshLinkCreateParams& offMeshCon = params->offMeshCons[i]; // Only store connections which start from this tile. if (offMeshConClass[i*2+0] == 0xff) { if (offMeshCon.type & DT_OFFMESH_CON_POINT) { dtPoly* p = &navPolys[offMeshPolyBase+n]; p->vertCount = 2; p->verts[0] = (unsigned short)(offMeshVertsBase + n*2+0); p->verts[1] = (unsigned short)(offMeshVertsBase + n*2+1); p->flags = offMeshCon.polyFlag; p->setArea(offMeshCon.area); p->setType(DT_POLYTYPE_OFFMESH_POINT); n++; } else { for (int j = 0; j < DT_MAX_OFFMESH_SEGMENT_PARTS; j++) { dtPoly* p = &navPolys[firstSegPoly+nseg]; p->vertCount = 0; p->flags = offMeshCon.polyFlag; p->setArea(offMeshCon.area); p->setType(DT_POLYTYPE_OFFMESH_SEGMENT); nseg++; } } } } // Store detail meshes and vertices. // The nav polygon vertices are stored as the first vertices on each mesh. // We compress the mesh data by skipping them and using the navmesh coordinates. if (params->detailMeshes) { unsigned short vbase = 0; for (int i = 0; i < params->polyCount; ++i) { dtPolyDetail& dtl = navDMeshes[i]; const int vb = (int)params->detailMeshes[i*4+0]; const int ndv = (int)params->detailMeshes[i*4+1]; const int nv = navPolys[i].vertCount; dtl.vertBase = (unsigned int)vbase; dtl.vertCount = (unsigned char)(ndv-nv); dtl.triBase = (unsigned int)params->detailMeshes[i*4+2]; dtl.triCount = (unsigned char)params->detailMeshes[i*4+3]; // Copy vertices except the first 'nv' verts which are equal to nav poly verts. if (ndv-nv) { memcpy(&navDVerts[vbase*3], ¶ms->detailVerts[(vb+nv)*3], sizeof(float)*3*(ndv-nv)); vbase += (unsigned short)(ndv-nv); } } // Store triangles. memcpy(navDTris, params->detailTris, sizeof(unsigned char)*4*params->detailTriCount); } else { // Create dummy detail mesh by triangulating polys. int tbase = 0; for (int i = 0; i < params->polyCount; ++i) { dtPolyDetail& dtl = navDMeshes[i]; const int nv = navPolys[i].vertCount; dtl.vertBase = 0; dtl.vertCount = 0; dtl.triBase = (unsigned int)tbase; dtl.triCount = (unsigned char)(nv-2); // Triangulate polygon (local indices). for (int j = 2; j < nv; ++j) { unsigned char* t = &navDTris[tbase*4]; t[0] = 0; t[1] = (unsigned char)(j-1); t[2] = (unsigned char)j; // Bit for each edge that belongs to poly boundary. t[3] = (1<<2); if (j == 2) t[3] |= (1<<0); if (j == nv-1) t[3] |= (1<<4); tbase++; } } } // Store and create BVtree. if (params->buildBvTree) { createBVTree(params->verts, params->vertCount, params->polys, params->polyCount, nvp, navDMeshes, navDVerts, navDTris, params->bmin, params->cs, params->ch, params->polyCount*2, navBvtree); } // Store Off-Mesh connections. n = 0; nseg = 0; for (int i = 0; i < params->offMeshConCount; ++i) { const dtOffMeshLinkCreateParams& offMeshCon = params->offMeshCons[i]; // Only store connections which start from this tile. if (offMeshConClass[i*2+0] == 0xff) { if (offMeshCon.type & DT_OFFMESH_CON_POINT) { dtOffMeshConnection* con = &offMeshCons[n]; con->poly = (unsigned short)(offMeshPolyBase + n); // Copy connection end-points. dtVcopy(&con->pos[0], &offMeshCon.vertsA0[0]); dtVcopy(&con->pos[3], &offMeshCon.vertsB0[0]); con->rad = offMeshCon.snapRadius; con->height = offMeshCon.snapHeight; con->setFlags(offMeshCon.type); con->side = offMeshConClass[i*2+1] == 0xff ? DT_CONNECTION_INTERNAL : offMeshConClass[i*2+1]; if (offMeshCon.userID) con->userId = offMeshCon.userID; n++; } else { dtOffMeshSegmentConnection* con = &offMeshSegs[nseg]; dtVcopy(con->startA, &offMeshCon.vertsA0[0]); dtVcopy(con->endA, &offMeshCon.vertsA1[0]); dtVcopy(con->startB, &offMeshCon.vertsB0[0]); dtVcopy(con->endB, &offMeshCon.vertsB1[0]); con->rad = offMeshCon.snapRadius; con->height = offMeshCon.snapHeight; con->setFlags(offMeshCon.type); if (offMeshCon.userID) con->userId = offMeshCon.userID; nseg++; } } } dtFree(offMeshConClass); // Store clusters if (params->polyClusters) { memcpy(polyClusters, params->polyClusters, sizeof(unsigned short)*params->polyCount); } for (int i = 0; i < params->clusterCount; i++) { dtCluster& cluster = clusters[i]; cluster.firstLink = DT_NULL_LINK; cluster.numLinks = 0; dtVset(cluster.center, 0.f, 0.f, 0.f); // calculate center point: take from first poly for (int j = 0; j < params->polyCount; j++) { if (polyClusters[j] != i) { continue; } const dtPoly* poly = &navPolys[j]; float c[3] = { 0.0f, 0.0f, 0.0f }; for (int iv = 0; iv < poly->vertCount; iv++) { dtVadd(c, c, &navVerts[poly->verts[iv] * 3]); } dtVmad(cluster.center, cluster.center, c, 1.0f / poly->vertCount); break; } } *outData = data; *outDataSize = dataSize; return true; }
void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug) { m_velocitySampleCount = 0; const int debugIdx = debug ? debug->idx : -1; dtCrowdAgent** agents = m_activeAgents; int nagents = getActiveAgents(agents, m_maxAgents); // Check that all agents still have valid paths. checkPathValidity(agents, nagents, dt); // Update async move request and path finder. updateMoveRequest(dt); // Optimize path topology. updateTopologyOptimization(agents, nagents, dt); // Register agents to proximity grid. m_grid->clear(); for (int i = 0; i < nagents; ++i) { dtCrowdAgent* ag = agents[i]; const float* p = ag->npos; const float r = ag->params.radius; m_grid->addItem((unsigned short)i, p[0]-r, p[2]-r, p[0]+r, p[2]+r); } // Get nearby navmesh segments and agents to collide with. for (int i = 0; i < nagents; ++i) { dtCrowdAgent* ag = agents[i]; if (ag->state != DT_CROWDAGENT_STATE_WALKING) continue; // Update the collision boundary after certain distance has been passed or // if it has become invalid. const float updateThr = ag->params.collisionQueryRange*0.25f; if (dtVdist2DSqr(ag->npos, ag->boundary.getCenter()) > dtSqr(updateThr) || !ag->boundary.isValid(m_navquery, &m_filter)) { ag->boundary.update(ag->corridor.getFirstPoly(), ag->npos, ag->params.collisionQueryRange, m_navquery, &m_filter); } // Query neighbour agents ag->nneis = getNeighbours(ag->npos, ag->params.height, ag->params.collisionQueryRange, ag, ag->neis, DT_CROWDAGENT_MAX_NEIGHBOURS, agents, nagents, m_grid); for (int j = 0; j < ag->nneis; j++) ag->neis[j].idx = getAgentIndex(agents[ag->neis[j].idx]); } // Find next corner to steer to. for (int i = 0; i < nagents; ++i) { dtCrowdAgent* ag = agents[i]; if (ag->state != DT_CROWDAGENT_STATE_WALKING) continue; if (ag->targetState == DT_CROWDAGENT_TARGET_NONE || ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY) continue; // Find corners for steering ag->ncorners = ag->corridor.findCorners(ag->cornerVerts, ag->cornerFlags, ag->cornerPolys, DT_CROWDAGENT_MAX_CORNERS, m_navquery, &m_filter); // Check to see if the corner after the next corner is directly visible, // and short cut to there. if ((ag->params.updateFlags & DT_CROWD_OPTIMIZE_VIS) && ag->ncorners > 0) { const float* target = &ag->cornerVerts[dtMin(1,ag->ncorners-1)*3]; ag->corridor.optimizePathVisibility(target, ag->params.pathOptimizationRange, m_navquery, &m_filter); // Copy data for debug purposes. if (debugIdx == i) { dtVcopy(debug->optStart, ag->corridor.getPos()); dtVcopy(debug->optEnd, target); } } else { // Copy data for debug purposes. if (debugIdx == i) { dtVset(debug->optStart, 0,0,0); dtVset(debug->optEnd, 0,0,0); } } } // Trigger off-mesh connections (depends on corners). for (int i = 0; i < nagents; ++i) { dtCrowdAgent* ag = agents[i]; if (ag->state != DT_CROWDAGENT_STATE_WALKING) continue; if (ag->targetState == DT_CROWDAGENT_TARGET_NONE || ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY) continue; // Check const float triggerRadius = ag->params.radius*2.25f; if (overOffmeshConnection(ag, triggerRadius)) { // Prepare to off-mesh connection. const int idx = (int)(ag - m_agents); dtCrowdAgentAnimation* anim = &m_agentAnims[idx]; // Adjust the path over the off-mesh connection. dtPolyRef refs[2]; if (ag->corridor.moveOverOffmeshConnection(ag->cornerPolys[ag->ncorners-1], refs, anim->startPos, anim->endPos, m_navquery)) { dtVcopy(anim->initPos, ag->npos); anim->polyRef = refs[1]; anim->active = 1; anim->t = 0.0f; anim->tmax = (dtVdist2D(anim->startPos, anim->endPos) / ag->params.maxSpeed) * 0.5f; ag->state = DT_CROWDAGENT_STATE_OFFMESH; ag->ncorners = 0; ag->nneis = 0; continue; } else { // Path validity check will ensure that bad/blocked connections will be replanned. } } } // Calculate steering. for (int i = 0; i < nagents; ++i) { dtCrowdAgent* ag = agents[i]; if (ag->state != DT_CROWDAGENT_STATE_WALKING) continue; if (ag->targetState == DT_CROWDAGENT_TARGET_NONE) continue; float dvel[3] = {0,0,0}; if (ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY) { dtVcopy(dvel, ag->targetPos); ag->desiredSpeed = dtVlen(ag->targetPos); } else { // Calculate steering direction. if (ag->params.updateFlags & DT_CROWD_ANTICIPATE_TURNS) calcSmoothSteerDirection(ag, dvel); else calcStraightSteerDirection(ag, dvel); // Calculate speed scale, which tells the agent to slowdown at the end of the path. const float slowDownRadius = ag->params.radius*2; // TODO: make less hacky. const float speedScale = getDistanceToGoal(ag, slowDownRadius) / slowDownRadius; ag->desiredSpeed = ag->params.maxSpeed; dtVscale(dvel, dvel, ag->desiredSpeed * speedScale); } // Separation if (ag->params.updateFlags & DT_CROWD_SEPARATION) { const float separationDist = ag->params.collisionQueryRange; const float invSeparationDist = 1.0f / separationDist; const float separationWeight = ag->params.separationWeight; float w = 0; float disp[3] = {0,0,0}; for (int j = 0; j < ag->nneis; ++j) { const dtCrowdAgent* nei = &m_agents[ag->neis[j].idx]; float diff[3]; dtVsub(diff, ag->npos, nei->npos); diff[1] = 0; const float distSqr = dtVlenSqr(diff); if (distSqr < 0.00001f) continue; if (distSqr > dtSqr(separationDist)) continue; const float dist = dtMathSqrtf(distSqr); const float weight = separationWeight * (1.0f - dtSqr(dist*invSeparationDist)); dtVmad(disp, disp, diff, weight/dist); w += 1.0f; } if (w > 0.0001f) { // Adjust desired velocity. dtVmad(dvel, dvel, disp, 1.0f/w); // Clamp desired velocity to desired speed. const float speedSqr = dtVlenSqr(dvel); const float desiredSqr = dtSqr(ag->desiredSpeed); if (speedSqr > desiredSqr) dtVscale(dvel, dvel, desiredSqr/speedSqr); } } // Set the desired velocity. dtVcopy(ag->dvel, dvel); } // Velocity planning. for (int i = 0; i < nagents; ++i) { dtCrowdAgent* ag = agents[i]; if (ag->state != DT_CROWDAGENT_STATE_WALKING) continue; if (ag->params.updateFlags & DT_CROWD_OBSTACLE_AVOIDANCE) { m_obstacleQuery->reset(); // Add neighbours as obstacles. for (int j = 0; j < ag->nneis; ++j) { const dtCrowdAgent* nei = &m_agents[ag->neis[j].idx]; m_obstacleQuery->addCircle(nei->npos, nei->params.radius, nei->vel, nei->dvel); } // Append neighbour segments as obstacles. for (int j = 0; j < ag->boundary.getSegmentCount(); ++j) { const float* s = ag->boundary.getSegment(j); if (dtTriArea2D(ag->npos, s, s+3) < 0.0f) continue; m_obstacleQuery->addSegment(s, s+3); } dtObstacleAvoidanceDebugData* vod = 0; if (debugIdx == i) vod = debug->vod; // Sample new safe velocity. bool adaptive = true; int ns = 0; const dtObstacleAvoidanceParams* params = &m_obstacleQueryParams[ag->params.obstacleAvoidanceType]; if (adaptive) { ns = m_obstacleQuery->sampleVelocityAdaptive(ag->npos, ag->params.radius, ag->desiredSpeed, ag->vel, ag->dvel, ag->nvel, params, vod); } else { ns = m_obstacleQuery->sampleVelocityGrid(ag->npos, ag->params.radius, ag->desiredSpeed, ag->vel, ag->dvel, ag->nvel, params, vod); } m_velocitySampleCount += ns; } else { // If not using velocity planning, new velocity is directly the desired velocity. dtVcopy(ag->nvel, ag->dvel); } } // Integrate. for (int i = 0; i < nagents; ++i) { dtCrowdAgent* ag = agents[i]; if (ag->state != DT_CROWDAGENT_STATE_WALKING) continue; integrate(ag, dt); } // Handle collisions. static const float COLLISION_RESOLVE_FACTOR = 0.7f; for (int iter = 0; iter < 4; ++iter) { for (int i = 0; i < nagents; ++i) { dtCrowdAgent* ag = agents[i]; const int idx0 = getAgentIndex(ag); if (ag->state != DT_CROWDAGENT_STATE_WALKING) continue; dtVset(ag->disp, 0,0,0); float w = 0; for (int j = 0; j < ag->nneis; ++j) { const dtCrowdAgent* nei = &m_agents[ag->neis[j].idx]; const int idx1 = getAgentIndex(nei); float diff[3]; dtVsub(diff, ag->npos, nei->npos); diff[1] = 0; float dist = dtVlenSqr(diff); if (dist > dtSqr(ag->params.radius + nei->params.radius)) continue; dist = dtMathSqrtf(dist); float pen = (ag->params.radius + nei->params.radius) - dist; if (dist < 0.0001f) { // Agents on top of each other, try to choose diverging separation directions. if (idx0 > idx1) dtVset(diff, -ag->dvel[2],0,ag->dvel[0]); else dtVset(diff, ag->dvel[2],0,-ag->dvel[0]); pen = 0.01f; } else { pen = (1.0f/dist) * (pen*0.5f) * COLLISION_RESOLVE_FACTOR; } dtVmad(ag->disp, ag->disp, diff, pen); w += 1.0f; } if (w > 0.0001f) { const float iw = 1.0f / w; dtVscale(ag->disp, ag->disp, iw); } } for (int i = 0; i < nagents; ++i) { dtCrowdAgent* ag = agents[i]; if (ag->state != DT_CROWDAGENT_STATE_WALKING) continue; dtVadd(ag->npos, ag->npos, ag->disp); } } for (int i = 0; i < nagents; ++i) { dtCrowdAgent* ag = agents[i]; if (ag->state != DT_CROWDAGENT_STATE_WALKING) continue; // Move along navmesh. ag->corridor.movePosition(ag->npos, m_navquery, &m_filter); // Get valid constrained position back. dtVcopy(ag->npos, ag->corridor.getPos()); // If not using path, truncate the corridor to just one poly. if (ag->targetState == DT_CROWDAGENT_TARGET_NONE || ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY) { ag->corridor.reset(ag->corridor.getFirstPoly(), ag->npos); } } // Update agents using off-mesh connection. for (int i = 0; i < m_maxAgents; ++i) { dtCrowdAgentAnimation* anim = &m_agentAnims[i]; if (!anim->active) continue; dtCrowdAgent* ag = agents[i]; anim->t += dt; if (anim->t > anim->tmax) { // Reset animation anim->active = 0; // Prepare agent for walking. ag->state = DT_CROWDAGENT_STATE_WALKING; continue; } // Update position const float ta = anim->tmax*0.15f; const float tb = anim->tmax; if (anim->t < ta) { const float u = tween(anim->t, 0.0, ta); dtVlerp(ag->npos, anim->initPos, anim->startPos, u); } else { const float u = tween(anim->t, ta, tb); dtVlerp(ag->npos, anim->startPos, anim->endPos, u); } // Update velocity. dtVset(ag->vel, 0,0,0); dtVset(ag->dvel, 0,0,0); } }
void dtCrowd::updatePosition(const float dt, unsigned* agentsIdx, unsigned nbIdx) { // If we want to update every agent if (nbIdx == 0) { agentsIdx = m_agentsToUpdate; nbIdx = m_maxAgents; } nbIdx = (nbIdx < m_maxAgents) ? nbIdx : m_maxAgents; // The current start position of the agent (not yet modified) dtPolyRef* currentPosPoly = (dtPolyRef*) dtAlloc(sizeof(dtPolyRef) * nbIdx, DT_ALLOC_TEMP); float* currentPos = (float*) dtAlloc(sizeof(float) * 3 * nbIdx, DT_ALLOC_TEMP); for (unsigned i = 0; i < nbIdx; ++i) { dtCrowdAgent* ag = 0; if (!getActiveAgent(&ag, agentsIdx[i])) continue; m_crowdQuery->getNavMeshQuery()->findNearestPoly(ag->position, m_crowdQuery->getQueryExtents(), m_crowdQuery->getQueryFilter(), currentPosPoly + i, currentPos + (i * 3)); } // Integrate. for (unsigned i = 0; i < nbIdx; ++i) { dtCrowdAgent* ag = 0; if (!getActiveAgent(&ag, agentsIdx[i])) continue; if (ag->state != DT_CROWDAGENT_STATE_WALKING) continue; integrate(ag, dt); } // Handle collisions. static const float COLLISION_RESOLVE_FACTOR = 0.7f; for (unsigned iter = 0; iter < 4; ++iter) { for (unsigned i = 0; i < nbIdx; ++i) { dtCrowdAgent* ag = 0; if (!getActiveAgent(&ag, agentsIdx[i])) continue; const int idx0 = getAgentIndex(ag); if (ag->state != DT_CROWDAGENT_STATE_WALKING) continue; float* disp = m_disp[agentsIdx[i]]; dtVset(disp, 0, 0, 0); float w = 0; for (unsigned j = 0; j < m_agentsEnv[ag->id].nbNeighbors; ++j) { const dtCrowdAgent* nei = &m_agents[m_agentsEnv[ag->id].neighbors[j].idx]; const int idx1 = getAgentIndex(nei); float diff[3]; dtVsub(diff, ag->position, nei->position); diff[1] = 0; float dist = dtVlenSqr(diff); if (dist > dtSqr(ag->radius + nei->radius) + EPSILON) continue; dist = sqrtf(dist); float pen = (ag->radius + nei->radius) - dist; if (dist < EPSILON) { // Agents on top of each other, try to choose diverging separation directions. if (idx0 > idx1) dtVset(diff, -ag->desiredVelocity[2], 0, ag->desiredVelocity[0]); else dtVset(diff, ag->desiredVelocity[2], 0, -ag->desiredVelocity[0]); pen = 0.01f; } else { pen = (1.0f / dist) * (pen * 0.5f) * COLLISION_RESOLVE_FACTOR; } dtVmad(disp, disp, diff, pen); w += 1.0f; } if (w > EPSILON) { const float iw = 1.0f / w; dtVscale(disp, disp, iw); } } for (unsigned i = 0; i < nbIdx; ++i) { dtCrowdAgent* ag = 0; if (!getActiveAgent(&ag, agentsIdx[i])) continue; if (ag->state != DT_CROWDAGENT_STATE_WALKING) continue; float* disp = m_disp[agentsIdx[i]]; dtVadd(ag->position, ag->position, disp); } } for (unsigned i = 0; i < nbIdx; ++i) { dtCrowdAgent* ag = 0; if (!getActiveAgent(&ag, agentsIdx[i])) continue; if (ag->state != DT_CROWDAGENT_STATE_WALKING) continue; // Move along navmesh. float newPos[3]; dtPolyRef visited[dtPathCorridor::MAX_VISITED]; int visitedCount; m_crowdQuery->getNavMeshQuery()->moveAlongSurface(currentPosPoly[i], currentPos + (i * 3), ag->position, m_crowdQuery->getQueryFilter(), newPos, visited, &visitedCount, dtPathCorridor::MAX_VISITED); // Get valid constrained position back. float newHeight = *(currentPos + (i * 3) + 1); m_crowdQuery->getNavMeshQuery()->getPolyHeight(currentPosPoly[i], newPos, &newHeight); newPos[1] = newHeight; dtVcopy(ag->position, newPos); } // Update agents using off-mesh connection. for (unsigned i = 0; i < nbIdx; ++i) { dtCrowdAgent* ag = 0; if (!getActiveAgent(&ag, agentsIdx[i])) continue; float offmeshTotalTime = ag->offmeshInitToStartTime + ag->offmeshStartToEndTime; if (ag->state == DT_CROWDAGENT_STATE_OFFMESH && offmeshTotalTime > EPSILON) { ag->offmeshElaspedTime += dt; if (ag->offmeshElaspedTime > offmeshTotalTime) { // Prepare agent for walking. ag->state = DT_CROWDAGENT_STATE_WALKING; continue; } // Update position if (ag->offmeshElaspedTime < ag->offmeshInitToStartTime) { const float u = tween(ag->offmeshElaspedTime, 0.0, ag->offmeshInitToStartTime); dtVlerp(ag->position, ag->offmeshInitPos, ag->offmeshStartPos, u); } else { const float u = tween(ag->offmeshElaspedTime, ag->offmeshInitToStartTime, offmeshTotalTime); dtVlerp(ag->position, ag->offmeshStartPos, ag->offmeshEndPos, u); } // Update velocity. dtVset(ag->velocity, 0,0,0); dtVset(ag->desiredVelocity, 0,0,0); } } dtFree(currentPosPoly); dtFree(currentPos); }
void 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); } } }