Ejemplo n.º 1
0
/* 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;
}
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;
}
Ejemplo n.º 3
0
void KX_ObstacleSimulationTOI_rays::sampleRVO(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavMeshObj, 
										const float maxDeltaAngle)
{
	MT_Vector2 vel(activeObst->dvel[0], activeObst->dvel[1]);
	float vmax = (float) vel.length();
	float odir = (float) atan2(vel.y(), vel.x());

	MT_Vector2 ddir = vel;
	ddir.normalize();

	float bestScore = FLT_MAX;
	float bestDir = odir;
	float bestToi = 0;

	TOICircle tc;
	tc.n = m_maxSamples;
	tc.minToi = m_minToi;
	tc.maxToi = m_maxToi;

	const int iforw = m_maxSamples/2;
	const float aoff = (float)iforw / (float)m_maxSamples;

	size_t nobs = m_obstacles.size();
	for (int iter = 0; iter < m_maxSamples; ++iter)
	{
		// Calculate sample velocity
		const float ndir = ((float)iter/(float)m_maxSamples) - aoff;
		const float dir = odir+ndir*M_PI*2;
		MT_Vector2 svel;
		svel.x() = cosf(dir) * vmax;
		svel.y() = sinf(dir) * vmax;

		// Find min time of impact and exit amongst all obstacles.
		float tmin = m_maxToi;
		float tmine = 0;
		for (int i = 0; i < nobs; ++i)
		{
			KX_Obstacle* ob = m_obstacles[i];
			bool res = filterObstacle(activeObst, activeNavMeshObj, ob, m_levelHeight);
			if (!res)
				continue;

			float htmin,htmax;

			if (ob->m_shape == KX_OBSTACLE_CIRCLE)
			{
				MT_Vector2 vab;
				if (vlen(ob->vel) < 0.01f*0.01f)
				{
					// Stationary, use VO
					vab = svel;
				}
				else
				{
					// Moving, use RVO
					vab = 2*svel - vel - ob->vel;
				}

				if (!sweepCircleCircle(activeObst->m_pos, activeObst->m_rad, 
					vab, ob->m_pos, ob->m_rad, htmin, htmax))
					continue;
			}
			else if (ob->m_shape == KX_OBSTACLE_SEGMENT)
			{
				MT_Point3 p1 = ob->m_pos;
				MT_Point3 p2 = ob->m_pos2;
				//apply world transform
				if (ob->m_type == KX_OBSTACLE_NAV_MESH)
				{
					KX_NavMeshObject* navmeshobj = static_cast<KX_NavMeshObject*>(ob->m_gameObj);
					p1 = navmeshobj->TransformToWorldCoords(p1);
					p2 = navmeshobj->TransformToWorldCoords(p2);
				}
				if (!sweepCircleSegment(activeObst->m_pos, activeObst->m_rad, svel, 
					p1, p2, ob->m_rad, htmin, htmax))
					continue;
			}

			if (htmin > 0.0f)
			{
				// The closest obstacle is somewhere ahead of us, keep track of nearest obstacle.
				if (htmin < tmin)
					tmin = htmin;
			}
			else if	(htmax > 0.0f)
			{
				// The agent overlaps the obstacle, keep track of first safe exit.
				if (htmax > tmine)
					tmine = htmax;
			}
		}

		// Calculate sample penalties and final score.
		const float apen = m_velWeight * fabsf(ndir);
		const float tpen = m_toiWeight * (1.0f/(0.0001f+tmin/m_maxToi));
		const float cpen = m_collisionWeight * (tmine/m_minToi)*(tmine/m_minToi);
		const float score = apen + tpen + cpen;

		// Update best score.
		if (score < bestScore)
		{
			bestDir = dir;
			bestToi = tmin;
			bestScore = score;
		}

		tc.dir[iter] = dir;
		tc.toi[iter] = tmin;
		tc.toie[iter] = tmine;
	}

	if (vlen(activeObst->vel) > 0.1)
	{
		// Constrain max turn rate.
		float cura = atan2(activeObst->vel[1],activeObst->vel[0]);
		float da = bestDir - cura;
		if (da < -M_PI) da += (float)M_PI*2;
		if (da > M_PI) da -= (float)M_PI*2;
		if (da < -maxDeltaAngle)
		{
			bestDir = cura - maxDeltaAngle;
			bestToi = min(bestToi, interpolateToi(bestDir, tc.dir, tc.toi, tc.n));
		}
		else if (da > maxDeltaAngle)
		{
			bestDir = cura + maxDeltaAngle;
			bestToi = min(bestToi, interpolateToi(bestDir, tc.dir, tc.toi, tc.n));
		}
	}

	// Adjust speed when time of impact is less than min TOI.
	if (bestToi < m_minToi)
		vmax *= bestToi/m_minToi;

	// New steering velocity.
	activeObst->nvel[0] = cosf(bestDir) * vmax;
	activeObst->nvel[1] = sinf(bestDir) * vmax;
}
Ejemplo n.º 4
0
static void processSamples(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavMeshObj, 
						   KX_Obstacles& obstacles,  float levelHeight, const float vmax,
						   const float* spos, const float cs, const int nspos, float* res, 						   
						   float maxToi, float velWeight, float curVelWeight, float sideWeight,
						   float toiWeight)
{
	vset(res, 0,0);

	const float ivmax = 1.0f / vmax;

	float adir[2], adist;
	vcpy(adir, activeObst->pvel);
	if (vlen(adir) > 0.01f)
		vnorm(adir);
	else
		vset(adir,0,0);
	float activeObstPos[2];
	vset(activeObstPos, activeObst->m_pos.x(), activeObst->m_pos.y()); 
	adist = vdot(adir, activeObstPos);

	float minPenalty = FLT_MAX;

	for (int n = 0; n < nspos; ++n)
	{
		float vcand[2];
		vcpy(vcand, &spos[n*2]);		

		// Find min time of impact and exit amongst all obstacles.
		float tmin = maxToi;
		float side = 0;
		int nside = 0;

		for (int i = 0; i < obstacles.size(); ++i)
		{
			KX_Obstacle* ob = obstacles[i];
			bool res = filterObstacle(activeObst, activeNavMeshObj, ob, levelHeight);
			if (!res)
				continue;
			float htmin, htmax;

			if (ob->m_shape==KX_OBSTACLE_CIRCLE)
			{
				float vab[2];

				// Moving, use RVO
				vscale(vab, vcand, 2);
				vsub(vab, vab, activeObst->vel);
				vsub(vab, vab, ob->vel);

				// Side
				// NOTE: dp, and dv are constant over the whole calculation,
				// they can be precomputed per object. 
				const float* pa = activeObstPos;
				float pb[2];
				vset(pb, ob->m_pos.x(), ob->m_pos.y());

				const float orig[2] = {0,0};
				float dp[2],dv[2],np[2];
				vsub(dp,pb,pa);
				vnorm(dp);
				vsub(dv,ob->dvel, activeObst->dvel);

				const float a = triarea(orig, dp,dv);
				if (a < 0.01f)
				{
					np[0] = -dp[1];
					np[1] = dp[0];
				}
				else
				{
					np[0] = dp[1];
					np[1] = -dp[0];
				}

				side += clamp(min(vdot(dp,vab)*2,vdot(np,vab)*2), 0.0f, 1.0f);
				nside++;

				if (!sweepCircleCircle(activeObst->m_pos, activeObst->m_rad, vab, ob->m_pos, ob->m_rad, 
					htmin, htmax))
					continue;

				// Handle overlapping obstacles.
				if (htmin < 0.0f && htmax > 0.0f)
				{
					// Avoid more when overlapped.
					htmin = -htmin * 0.5f;
				}
			}
			else if (ob->m_shape == KX_OBSTACLE_SEGMENT)
			{
				MT_Point3 p1 = ob->m_pos;
				MT_Point3 p2 = ob->m_pos2;
				//apply world transform
				if (ob->m_type == KX_OBSTACLE_NAV_MESH)
				{
					KX_NavMeshObject* navmeshobj = static_cast<KX_NavMeshObject*>(ob->m_gameObj);
					p1 = navmeshobj->TransformToWorldCoords(p1);
					p2 = navmeshobj->TransformToWorldCoords(p2);
				}
				float p[2], q[2];
				vset(p, p1.x(), p1.y());
				vset(q, p2.x(), p2.y());

				// NOTE: the segments are assumed to come from a navmesh which is shrunken by
				// the agent radius, hence the use of really small radius.
				// This can be handle more efficiently by using seg-seg test instead.
				// If the whole segment is to be treated as obstacle, use agent->rad instead of 0.01f!
				const float r = 0.01f; // agent->rad
				if (distPtSegSqr(activeObstPos, p, q) < sqr(r+ob->m_rad))
				{
					float sdir[2], snorm[2];
					vsub(sdir, q, p);
					snorm[0] = sdir[1];
					snorm[1] = -sdir[0];
					// If the velocity is pointing towards the segment, no collision.
					if (vdot(snorm, vcand) < 0.0f)
						continue;
					// Else immediate collision.
					htmin = 0.0f;
					htmax = 10.0f;
				}
				else
				{
					if (!sweepCircleSegment(activeObstPos, r, vcand, p, q, ob->m_rad, htmin, htmax))
						continue;
				}

				// Avoid less when facing walls.
				htmin *= 2.0f;
			}

			if (htmin >= 0.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 vpen = velWeight * (vdist(vcand, activeObst->dvel) * ivmax);
		const float vcpen = curVelWeight * (vdist(vcand, activeObst->vel) * ivmax);
		const float spen = sideWeight * side;
		const float tpen = toiWeight * (1.0f/(0.1f+tmin/maxToi));

		const float penalty = vpen + vcpen + spen + tpen;

		if (penalty < minPenalty)
		{
			minPenalty = penalty;
			vcpy(res, vcand);
		}
	}
}
Ejemplo n.º 5
0
static int sweepCircleSegment(const MT_Vector3& pos0, const MT_Scalar r0, const MT_Vector2& v,
					   const MT_Vector3& pa, const MT_Vector3& pb, const MT_Scalar sr,
					   float& tmin, float &tmax)
{
	// equation parameters
	MT_Vector2 c0(pos0.x(), pos0.y());
	MT_Vector2 sa(pa.x(), pa.y());
	MT_Vector2 sb(pb.x(), pb.y());
	MT_Vector2 L = sb-sa;
	MT_Vector2 H = c0-sa;
	MT_Scalar radius = r0+sr;
	float l2 = L.length2();
	float r2 = radius * radius;
	float dl = perp(v, L);
	float hl = perp(H, L);
	float a = dl * dl;
	float b = 2.0f * hl * dl;
	float c = hl * hl - (r2 * l2);
	float d = (b*b) - (4.0f * a * c);

	// infinite line missed by infinite ray.
	if (d < 0.0f)
		return 0;

	d = sqrtf(d);
	tmin = (-b - d) / (2.0f * a);
	tmax = (-b + d) / (2.0f * a);

	// line missed by ray range.
	/*	if (tmax < 0.0f || tmin > 1.0f)
	return 0;*/

	// find what part of the ray was collided.
	MT_Vector2 Pedge;
	Pedge = c0+v*tmin;
	H = Pedge - sa;
	float e0 = MT_dot(H, L) / l2;
	Pedge = c0 + v*tmax;
	H = Pedge - sa;
	float e1 = MT_dot(H, L) / l2;

	if (e0 < 0.0f || e1 < 0.0f)
	{
		float ctmin, ctmax;
		if (sweepCircleCircle(pos0, r0, v, pa, sr, ctmin, ctmax))
		{
			if (e0 < 0.0f && ctmin > tmin)
				tmin = ctmin;
			if (e1 < 0.0f && ctmax < tmax)
				tmax = ctmax;
		}
		else
		{
			return 0;
		}
	}

	if (e0 > 1.0f || e1 > 1.0f)
	{
		float ctmin, ctmax;
		if (sweepCircleCircle(pos0, r0, v, pb, sr, ctmin, ctmax))
		{
			if (e0 > 1.0f && ctmin > tmin)
				tmin = ctmin;
			if (e1 > 1.0f && ctmax < tmax)
				tmax = ctmax;
		}
		else
		{
			return 0;
		}
	}

	return 1;
}