void dtNavMesh::connectExtLinks(dtMeshTile* tile, dtMeshTile* target, int side) { if (!tile) return; // Connect border links. for (int i = 0; i < tile->header->polyCount; ++i) { dtPoly* poly = &tile->polys[i]; // Create new links. unsigned short m = DT_EXT_LINK | (unsigned short)side; const int nv = poly->vertCount; for (int j = 0; j < nv; ++j) { // Skip edges which do not point to the right side. if (poly->neis[j] != m) continue; // Create new links const float* va = &tile->verts[poly->verts[j]*3]; const float* vb = &tile->verts[poly->verts[(j+1) % nv]*3]; dtPolyRef nei[4]; float neia[4*2]; int nnei = findConnectingPolys(va,vb, target, dtOppositeTile(side), nei,neia,4); for (int k = 0; k < nnei; ++k) { unsigned int idx = allocLink(tile); if (idx != DT_NULL_LINK) { dtLink* link = &tile->links[idx]; link->ref = nei[k]; link->edge = (unsigned char)j; link->side = (unsigned char)side; link->next = poly->firstLink; poly->firstLink = idx; // Compress portal limits to a byte value. if (side == 0 || side == 4) { float tmin = (neia[k*2+0]-va[2]) / (vb[2]-va[2]); float tmax = (neia[k*2+1]-va[2]) / (vb[2]-va[2]); if (tmin > tmax) dtSwap(tmin,tmax); link->bmin = (unsigned char)(dtClamp(tmin, 0.0f, 1.0f)*255.0f); link->bmax = (unsigned char)(dtClamp(tmax, 0.0f, 1.0f)*255.0f); } else if (side == 2 || side == 6) { float tmin = (neia[k*2+0]-va[0]) / (vb[0]-va[0]); float tmax = (neia[k*2+1]-va[0]) / (vb[0]-va[0]); if (tmin > tmax) dtSwap(tmin,tmax); link->bmin = (unsigned char)(dtClamp(tmin, 0.0f, 1.0f)*255.0f); link->bmax = (unsigned char)(dtClamp(tmax, 0.0f, 1.0f)*255.0f); } } } } } }
static void normalizeArray(float* arr, const int n) { // Normalize penaly range. float minPen = FLT_MAX; float maxPen = -FLT_MAX; for (int i = 0; i < n; ++i) { minPen = dtMin(minPen, arr[i]); maxPen = dtMax(maxPen, arr[i]); } const float penRange = maxPen-minPen; const float s = penRange > 0.001f ? (1.0f / penRange) : 1; for (int i = 0; i < n; ++i) arr[i] = dtClamp((arr[i]-minPen)*s, 0.0f, 1.0f); }
dtStatus dtNavMesh::addPolyTeamCost( dtPolyRef ref, dtTeam team, float cost ) { if ( !ref ) return DT_FAILURE; unsigned int salt, it, ip; decodePolyId( ref, salt, it, ip ); if ( it >= (unsigned int)m_maxTiles ) return DT_FAILURE | DT_INVALID_PARAM; if ( m_tiles[ it ].salt != salt || m_tiles[ it ].header == 0 ) return DT_FAILURE | DT_INVALID_PARAM; dtMeshTile* tile = &m_tiles[ it ]; if ( ip >= (unsigned int)tile->header->polyCount ) return DT_FAILURE | DT_INVALID_PARAM; dtPoly* poly = &tile->polys[ ip ]; // Change flags. poly->teamCost[ team ] = dtClamp( poly->teamCost[ team ] + cost, 0.0f, 3.0f ); return DT_SUCCESS; }
int dtObstacleAvoidanceQuery::sampleVelocityAdaptive(const float* pos, const float rad, const float vmax, const float* vel, const float* dvel, float* nvel, const dtObstacleAvoidanceParams* params, dtObstacleAvoidanceDebugData* debug) { prepare(pos, dvel); memcpy(&m_params, params, sizeof(dtObstacleAvoidanceParams)); m_invHorizTime = 1.0f / m_params.horizTime; m_vmax = vmax; m_invVmax = vmax > 0 ? 1.0f / vmax : FLT_MAX; dtVset(nvel, 0,0,0); if (debug) debug->reset(); // Build sampling pattern aligned to desired velocity. float pat[(DT_MAX_PATTERN_DIVS*DT_MAX_PATTERN_RINGS+1)*2]; int npat = 0; const int ndivs = (int)m_params.adaptiveDivs; const int nrings= (int)m_params.adaptiveRings; const int depth = (int)m_params.adaptiveDepth; const int nd = dtClamp(ndivs, 1, DT_MAX_PATTERN_DIVS); const int nr = dtClamp(nrings, 1, DT_MAX_PATTERN_RINGS); //const int nd2 = nd / 2; const float da = (1.0f/nd) * DT_PI*2; const float ca = cosf(da); const float sa = sinf(da); // desired direction float ddir[6]; dtVcopy(ddir, dvel); dtNormalize2D(ddir); dtRorate2D (ddir+3, ddir, da*0.5f); // rotated by da/2 // Always add sample at zero pat[npat*2+0] = 0; pat[npat*2+1] = 0; npat++; for (int j = 0; j < nr; ++j) { const float r = (float)(nr-j)/(float)nr; pat[npat*2+0] = ddir[(j%1)*3] * r; pat[npat*2+1] = ddir[(j%1)*3+2] * r; float* last1 = pat + npat*2; float* last2 = last1; npat++; for (int i = 1; i < nd-1; i+=2) { // get next point on the "right" (rotate CW) pat[npat*2+0] = last1[0]*ca + last1[1]*sa; pat[npat*2+1] = -last1[0]*sa + last1[1]*ca; // get next point on the "left" (rotate CCW) pat[npat*2+2] = last2[0]*ca - last2[1]*sa; pat[npat*2+3] = last2[0]*sa + last2[1]*ca; last1 = pat + npat*2; last2 = last1 + 2; npat += 2; } if ((nd&1) == 0) { pat[npat*2+2] = last2[0]*ca - last2[1]*sa; pat[npat*2+3] = last2[0]*sa + last2[1]*ca; npat++; } } // Start sampling. float cr = vmax * (1.0f - m_params.velBias); float res[3]; dtVset(res, dvel[0] * m_params.velBias, 0, dvel[2] * m_params.velBias); int ns = 0; for (int k = 0; k < depth; ++k) { float minPenalty = FLT_MAX; float bvel[3]; dtVset(bvel, 0,0,0); for (int i = 0; i < npat; ++i) { float vcand[3]; vcand[0] = res[0] + pat[i*2+0]*cr; vcand[1] = 0; vcand[2] = res[2] + pat[i*2+1]*cr; if (dtSqr(vcand[0])+dtSqr(vcand[2]) > dtSqr(vmax+0.001f)) continue; const float penalty = processSample(vcand,cr/10, pos,rad,vel,dvel, minPenalty, debug); ns++; if (penalty < minPenalty) { minPenalty = penalty; dtVcopy(bvel, vcand); } } dtVcopy(res, bvel); cr *= 0.5f; } dtVcopy(nvel, res); return ns; }
/* 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; }
int dtNavMesh::queryPolygonsInTile(const dtMeshTile* tile, const float* qmin, const float* qmax, dtPolyRef* polys, const int maxPolys) const { if (tile->bvTree) { const dtBVNode* node = &tile->bvTree[0]; const dtBVNode* end = &tile->bvTree[tile->header->bvNodeCount]; const float* tbmin = tile->header->bmin; const float* tbmax = tile->header->bmax; const float qfac = tile->header->bvQuantFactor; // Calculate quantized box unsigned short bmin[3], bmax[3]; // dtClamp query box to world box. float minx = dtClamp(qmin[0], tbmin[0], tbmax[0]) - tbmin[0]; float miny = dtClamp(qmin[1], tbmin[1], tbmax[1]) - tbmin[1]; float minz = dtClamp(qmin[2], tbmin[2], tbmax[2]) - tbmin[2]; float maxx = dtClamp(qmax[0], tbmin[0], tbmax[0]) - tbmin[0]; float maxy = dtClamp(qmax[1], tbmin[1], tbmax[1]) - tbmin[1]; float maxz = dtClamp(qmax[2], tbmin[2], tbmax[2]) - tbmin[2]; // Quantize bmin[0] = (unsigned short)(qfac * minx) & 0xfffe; bmin[1] = (unsigned short)(qfac * miny) & 0xfffe; bmin[2] = (unsigned short)(qfac * minz) & 0xfffe; bmax[0] = (unsigned short)(qfac * maxx + 1) | 1; bmax[1] = (unsigned short)(qfac * maxy + 1) | 1; bmax[2] = (unsigned short)(qfac * maxz + 1) | 1; // Traverse tree dtPolyRef base = getPolyRefBase(tile); int n = 0; while (node < end) { const bool overlap = dtOverlapQuantBounds(bmin, bmax, node->bmin, node->bmax); const bool isLeafNode = node->i >= 0; if (isLeafNode && overlap) { if (n < maxPolys) polys[n++] = base | (dtPolyRef)node->i; } if (overlap || isLeafNode) node++; else { const int escapeIndex = -node->i; node += escapeIndex; } } return n; } else { float bmin[3], bmax[3]; int n = 0; dtPolyRef base = getPolyRefBase(tile); for (int i = 0; i < tile->header->polyCount; ++i) { dtPoly* p = &tile->polys[i]; // Do not return off-mesh connection polygons. if (p->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) continue; // Calc polygon bounds. const float* v = &tile->verts[p->verts[0]*3]; dtVcopy(bmin, v); dtVcopy(bmax, v); for (int j = 1; j < p->vertCount; ++j) { v = &tile->verts[p->verts[j]*3]; dtVmin(bmin, v); dtVmax(bmax, v); } if (dtOverlapBounds(qmin,qmax, bmin,bmax)) { if (n < maxPolys) polys[n++] = base | (dtPolyRef)i; } } return n; } }
void dtObstacleAvoidanceQuery::sampleVelocityAdaptive(const float* pos, const float rad, const float vmax, const float* vel, const float* dvel, float* nvel, const int ndivs, const int nrings, const int depth, dtObstacleAvoidanceDebugData* debug) { prepare(pos, dvel); dtVset(nvel, 0,0,0); if (debug) debug->reset(); // Build sampling pattern aligned to desired velocity. static const int MAX_PATTERN_DIVS = 32; static const int MAX_PATTERN_RINGS = 4; float pat[(MAX_PATTERN_DIVS*MAX_PATTERN_RINGS+1)*2]; int npat = 0; const int nd = dtClamp(ndivs, 1, MAX_PATTERN_DIVS); const int nr = dtClamp(nrings, 1, MAX_PATTERN_RINGS); const float da = (1.0f/nd) * DT_PI*2; const float dang = atan2f(dvel[2], dvel[0]); // Always add sample at zero pat[npat*2+0] = 0; pat[npat*2+1] = 0; npat++; for (int j = 0; j < nr; ++j) { const float rad = (float)(nr-j)/(float)nr; float a = dang + (j&1)*0.5f*da; for (int i = 0; i < nd; ++i) { pat[npat*2+0] = cosf(a)*rad; pat[npat*2+1] = sinf(a)*rad; npat++; a += da; } } // Start sampling. float cr = vmax * (1.0f-m_velBias); float res[3]; dtVset(res, dvel[0] * m_velBias, 0, dvel[2] * m_velBias); for (int k = 0; k < depth; ++k) { float minPenalty = FLT_MAX; float bvel[3]; dtVset(bvel, 0,0,0); for (int i = 0; i < npat; ++i) { float vcand[3]; vcand[0] = res[0] + pat[i*2+0]*cr; vcand[1] = 0; vcand[2] = res[2] + pat[i*2+1]*cr; if (dtSqr(vcand[0])+dtSqr(vcand[2]) > dtSqr(vmax+0.001f)) continue; const float penalty = processSample(vcand,cr/10, pos,rad,vmax,vel,dvel, debug); if (penalty < minPenalty) { minPenalty = penalty; dtVcopy(bvel, vcand); } } dtVcopy(res, bvel); cr *= 0.5f; } dtVcopy(nvel, res); }
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; }
inline float tween(const float t, const float t0, const float t1) { return dtClamp((t-t0) / (t1-t0), 0.0f, 1.0f); }
int dtObstacleAvoidanceQuery::sampleVelocityAdaptive(const float* pos, const float rad, const float vmax, const float* vel, const float* dvel, float* nvel, const dtObstacleAvoidanceParams* params, dtObstacleAvoidanceDebugData* debug) { prepare(pos, dvel); memcpy(&m_params, params, sizeof(dtObstacleAvoidanceParams)); m_invHorizTime = 1.0f / m_params.horizTime; m_vmax = vmax; m_invVmax = 1.0f / vmax; dtVset(nvel, 0,0,0); if (debug) debug->reset(); // Build sampling pattern aligned to desired velocity. float pat[(DT_MAX_PATTERN_DIVS*DT_MAX_PATTERN_RINGS+1)*2]; int npat = 0; const int ndivs = (int)m_params.adaptiveDivs; const int nrings= (int)m_params.adaptiveRings; const int depth = (int)m_params.adaptiveDepth; const int nd = dtClamp(ndivs, 1, DT_MAX_PATTERN_DIVS); const int nr = dtClamp(nrings, 1, DT_MAX_PATTERN_RINGS); const float da = (1.0f/nd) * DT_PI*2; const float dang = atan2f(dvel[2], dvel[0]); // Always add sample at zero pat[npat*2+0] = 0; pat[npat*2+1] = 0; npat++; for (int j = 0; j < nr; ++j) { const float r = (float)(nr-j)/(float)nr; float a = dang + (j&1)*0.5f*da; for (int i = 0; i < nd; ++i) { pat[npat*2+0] = cosf(a)*r; pat[npat*2+1] = sinf(a)*r; npat++; a += da; } } // Start sampling. float cr = vmax * (1.0f - m_params.velBias); float res[3]; dtVset(res, dvel[0] * m_params.velBias, 0, dvel[2] * m_params.velBias); int ns = 0; for (int k = 0; k < depth; ++k) { float minPenalty = FLT_MAX; float bvel[3]; dtVset(bvel, 0,0,0); for (int i = 0; i < npat; ++i) { float vcand[3]; vcand[0] = res[0] + pat[i*2+0]*cr; vcand[1] = 0; vcand[2] = res[2] + pat[i*2+1]*cr; if (dtSqr(vcand[0])+dtSqr(vcand[2]) > dtSqr(vmax+0.001f)) continue; const float penalty = processSample(vcand,cr/10, pos,rad,vel,dvel, debug); ns++; if (penalty < minPenalty) { minPenalty = penalty; dtVcopy(bvel, vcand); } } dtVcopy(res, bvel); cr *= 0.5f; } dtVcopy(nvel, res); return ns; }