C++ dtClamp函数代码示例

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)
			{
//.........这里部分代码省略.........

/* 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;
		}
	}
//.........这里部分代码省略.........

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