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