C++ dtVsub函数代码示例

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

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