Merge pull request #42 from axelrodR/master

Detour crowd optmizations

DetourCrowd/Include/DetourObstacleAvoidance.h

@@ -128,6 +128,7 @@ private:
 	float processSample(const float* vcand, const float cs,
 						const float* pos, const float rad,
 						const float* vel, const float* dvel,
+						const float minPenalty,
 						dtObstacleAvoidanceDebugData* debug);
 
 	dtObstacleCircle* insertCircle(const float dist);

DetourCrowd/Source/DetourObstacleAvoidance.cpp

@@ -311,11 +311,30 @@ void dtObstacleAvoidanceQuery::prepare(const float* pos, const float* dvel)
 	}	
 }
 
+
+/* 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;
@@ -350,7 +369,11 @@ float dtObstacleAvoidanceQuery::processSample(const float* vcand, const float cs
 		{
 			// The closest obstacle is somewhere ahead of us, keep track of nearest obstacle.
 			if (htmin < tmin)
+			{
 				tmin = htmin;
+				if (tmin < tThresold)
+					return minPenalty;
+			}
 		}
 	}
 
@@ -383,15 +406,17 @@ float dtObstacleAvoidanceQuery::processSample(const float* vcand, const float cs
 		
 		// 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 vpen = m_params.weightDesVel * (dtVdist2D(vcand, dvel) * m_invVmax);
-	const float vcpen = m_params.weightCurVel * (dtVdist2D(vcand, vel) * m_invVmax);
 	const float spen = m_params.weightSide * side;
 	const float tpen = m_params.weightToi * (1.0f/(0.1f+tmin*m_invHorizTime));
 	
@@ -414,7 +439,7 @@ int dtObstacleAvoidanceQuery::sampleVelocityGrid(const float* pos, const float r
 	memcpy(&m_params, params, sizeof(dtObstacleAvoidanceParams));
 	m_invHorizTime = 1.0f / m_params.horizTime;
 	m_vmax = vmax;
-	m_invVmax = 1.0f / vmax;
+	m_invVmax = vmax > 0 ? 1.0f / vmax : FLT_MAX;
 	
 	dtVset(nvel, 0,0,0);
 	
@@ -440,7 +465,7 @@ int dtObstacleAvoidanceQuery::sampleVelocityGrid(const float* pos, const float r
 			
 			if (dtSqr(vcand[0])+dtSqr(vcand[2]) > dtSqr(vmax+cs/2)) continue;
 			
-			const float penalty = processSample(vcand, cs, pos,rad,vel,dvel, debug);
+			const float penalty = processSample(vcand, cs, pos,rad,vel,dvel, minPenalty, debug);
 			ns++;
 			if (penalty < minPenalty)
 			{
@@ -454,6 +479,28 @@ int dtObstacleAvoidanceQuery::sampleVelocityGrid(const float* pos, const float r
 }
 
 
+// vector normalization that ignores the y-component.
+inline void dtNormalize2D(float* v)
+{
+	float d = dtSqrt(v[0]*v[0]+v[2]*v[2]);
+	if (d==0)
+		return;
+	d = 1.0f / d;
+	v[0] *= d;
+	v[2] *= d;
+}
+
+// vector normalization that ignores the y-component.
+inline void dtRorate2D(float* dest, const float* v, float ang)
+{
+	float c = cosf(ang);
+	float s = sinf(ang);
+	dest[0] = v[0]*c - v[2]*s;
+	dest[2] = v[0]*s + v[2]*c;
+	dest[1] = v[1];
+}
+
+
 int dtObstacleAvoidanceQuery::sampleVelocityAdaptive(const float* pos, const float rad, const float vmax,
 													 const float* vel, const float* dvel, float* nvel,
 													 const dtObstacleAvoidanceParams* params,
@@ -464,7 +511,7 @@ int dtObstacleAvoidanceQuery::sampleVelocityAdaptive(const float* pos, const flo
 	memcpy(&m_params, params, sizeof(dtObstacleAvoidanceParams));
 	m_invHorizTime = 1.0f / m_params.horizTime;
 	m_vmax = vmax;
-	m_invVmax = 1.0f / vmax;
+	m_invVmax = vmax > 0 ? 1.0f / vmax : FLT_MAX;
 	
 	dtVset(nvel, 0,0,0);
 	
@@ -481,9 +528,17 @@ int dtObstacleAvoidanceQuery::sampleVelocityAdaptive(const float* pos, const flo
 	
 	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 dang = dtMathAtan2f(dvel[2], dvel[0]);
+	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;
@@ -492,16 +547,35 @@ int dtObstacleAvoidanceQuery::sampleVelocityAdaptive(const float* pos, const flo
 	for (int j = 0; j < nr; ++j)
 	{
 		const float r = (float)(nr-j)/(float)nr;
-		float a = dang + (j&1)*0.5f*da;
+		pat[npat*2+0] = ddir[(j%1)*3] * r;
-		for (int i = 0; i < nd; ++i)
+		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)
 		{
-			pat[npat*2+0] = cosf(a)*r;
+			// get next point on the "right" (rotate CW)
-			pat[npat*2+1] = sinf(a)*r;
+			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++;
-			a += da;
 		}
 	}
 
+
 	// Start sampling.
 	float cr = vmax * (1.0f - m_params.velBias);
 	float res[3];
@@ -523,7 +597,7 @@ int dtObstacleAvoidanceQuery::sampleVelocityAdaptive(const float* pos, const flo
 			
 			if (dtSqr(vcand[0])+dtSqr(vcand[2]) > dtSqr(vmax+0.001f)) continue;
 			
-			const float penalty = processSample(vcand,cr/10, pos,rad,vel,dvel, debug);
+			const float penalty = processSample(vcand,cr/10, pos,rad,vel,dvel, minPenalty, debug);
 			ns++;
 			if (penalty < minPenalty)
 			{
@@ -540,5 +614,4 @@ int dtObstacleAvoidanceQuery::sampleVelocityAdaptive(const float* pos, const flo
 	dtVcopy(nvel, res);
 	
 	return ns;
-}
+}
-