reworked obstacle simulation in order to have two realizations: with "cell" and "ray" sampling

6 files changed, 325 insertions, 190 deletions

diff --git a/source/blender/editors/util/navmesh_conversion.cpp b/source/blender/editors/util/navmesh_conversion.cpp
index 62e26e2cd55..873660baa13 100644
--- a/source/blender/editors/util/navmesh_conversion.cpp
+++ b/source/blender/editors/util/navmesh_conversion.cpp
@@ -341,7 +341,7 @@ bool buildNavMeshData(const int nverts, const float* verts,
 	{
 		memcpy(dtris+3*2*i, tris+3*dtrisToTrisMap[i], sizeof(unsigned short)*3);
 	}
-	//create new recast data corresponded to dtris and renumber for continious indices
+	//create new recast data corresponded to dtris and renumber for continuous indices
 	int prevPolyIdx=-1, curPolyIdx, newPolyIdx=0;
 	dtrisToPolysMap = new int[ndtris];
 	for (int i=0; i<ndtris; i++)
diff --git a/source/blender/makesdna/DNA_scene_types.h b/source/blender/makesdna/DNA_scene_types.h
index 0bd530bbd43..8909d96b03a 100644
--- a/source/blender/makesdna/DNA_scene_types.h
+++ b/source/blender/makesdna/DNA_scene_types.h
@@ -500,7 +500,8 @@ typedef struct GameData {
 
 /* obstacleSimulation */
 #define OBSTSIMULATION_NONE		0
-#define OBSTSIMULATION_TOI		1
+#define OBSTSIMULATION_TOI_rays		1
+#define OBSTSIMULATION_TOI_cells	2
 
 /* GameData.flag */
 #define GAME_ENABLE_ALL_FRAMES				(1 << 1)
diff --git a/source/blender/makesrna/intern/rna_scene.c b/source/blender/makesrna/intern/rna_scene.c
index 607b6363cb6..28af92eb745 100644
--- a/source/blender/makesrna/intern/rna_scene.c
+++ b/source/blender/makesrna/intern/rna_scene.c
@@ -1637,7 +1637,8 @@ static void rna_def_scene_game_data(BlenderRNA *brna)
 
 	static EnumPropertyItem obstacle_simulation_items[] = {
 		{OBSTSIMULATION_NONE, "NONE", 0, "None", ""},
-		{OBSTSIMULATION_TOI, "RVO", 0, "RVO", ""},
+		{OBSTSIMULATION_TOI_rays, "RVO (rays)", 0, "RVO (rays)", ""},
+		{OBSTSIMULATION_TOI_cells, "RVO (cells)", 0, "RVO (cells)", ""},
 		{0, NULL, 0, NULL, NULL}};
 
 	srna= RNA_def_struct(brna, "SceneGameData", NULL);
diff --git a/source/gameengine/Ketsji/KX_ObstacleSimulation.cpp b/source/gameengine/Ketsji/KX_ObstacleSimulation.cpp
index 4a1ea59e12c..9bca98d7fde 100644
--- a/source/gameengine/Ketsji/KX_ObstacleSimulation.cpp
+++ b/source/gameengine/Ketsji/KX_ObstacleSimulation.cpp
@@ -209,6 +209,20 @@ static bool inBetweenAngle(float a, float amin, float amax, float& t)
 	return false;
 }
 
+static float interpolateToi(float a, const float* dir, const float* toi, const int ntoi)
+{
+	for (int i = 0; i < ntoi; ++i)
+	{
+		int next = (i+1) % ntoi;
+		float t;
+		if (inBetweenAngle(a, dir[i], dir[next], t))
+		{
+			return lerp(toi[i], toi[next], t);
+		}
+	}
+	return 0;
+}
+
 KX_ObstacleSimulation::KX_ObstacleSimulation(MT_Scalar levelHeight, bool enableVisualization)
 :	m_levelHeight(levelHeight)
 ,	m_enableVisualization(enableVisualization)
@@ -404,52 +418,221 @@ static bool filterObstacle(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavM
 	return true;
 }
 
-KX_ObstacleSimulationTOI::KX_ObstacleSimulationTOI(MT_Scalar levelHeight, bool enableVisualization):
-	KX_ObstacleSimulation(levelHeight, enableVisualization),
-	m_avoidSteps(32),
-	m_minToi(0.5f),
-	m_maxToi(1.2f),
-	m_angleWeight(4.0f),
+///////////*********TOI_rays**********/////////////////
+KX_ObstacleSimulationTOI::KX_ObstacleSimulationTOI(MT_Scalar levelHeight, bool enableVisualization)
+:	KX_ObstacleSimulation(levelHeight, enableVisualization),
+	m_maxSamples(32),
+	m_minToi(0.0f),
+	m_maxToi(0.0f),
+	m_velWeight(1.0f),
+	m_curVelWeight(1.0f),
 	m_toiWeight(1.0f),
-	m_collisionWeight(100.0f)
+	m_collisionWeight(1.0f)
 {
-	
 }
 
-KX_ObstacleSimulationTOI::~KX_ObstacleSimulationTOI()
+
+void KX_ObstacleSimulationTOI::AdjustObstacleVelocity(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavMeshObj, 
+														   MT_Vector3& velocity, MT_Scalar maxDeltaSpeed, MT_Scalar maxDeltaAngle)
 {
-	for (size_t i=0; i<m_toiCircles.size(); i++)
-	{
-		TOICircle* toi = m_toiCircles[i];
-		delete toi;
-	}
-	m_toiCircles.clear();
+	int nobs = m_obstacles.size();
+	int obstidx = std::find(m_obstacles.begin(), m_obstacles.end(), activeObst) - m_obstacles.begin();
+	if (obstidx == nobs)
+		return;
+
+	vset(activeObst->dvel, velocity.x(), velocity.y());
+
+	//apply RVO
+	sampleRVO(activeObst, activeNavMeshObj, maxDeltaAngle);
+
+	// Fake dynamic constraint.
+	float dv[2];
+	float vel[2];
+	vsub(dv, activeObst->nvel, activeObst->vel);
+	float ds = vlen(dv);
+	if (ds > maxDeltaSpeed || ds<-maxDeltaSpeed)
+		vscale(dv, dv, fabs(maxDeltaSpeed/ds));
+	vadd(vel, activeObst->vel, dv);
+
+	velocity.x() = vel[0];
+	velocity.y() = vel[1];	
 }
 
-KX_Obstacle* KX_ObstacleSimulationTOI::CreateObstacle(KX_GameObject* gameobj)
+///////////*********TOI_rays**********/////////////////
+static const int AVOID_MAX_STEPS = 128;
+struct TOICircle
 {
-	KX_Obstacle* obstacle = KX_ObstacleSimulation::CreateObstacle(gameobj);
-	m_toiCircles.push_back(new TOICircle());
-	return obstacle;
+	TOICircle() : n(0), minToi(0), maxToi(1) {}
+	float	toi[AVOID_MAX_STEPS];	// Time of impact (seconds)
+	float	toie[AVOID_MAX_STEPS];	// Time of exit (seconds)
+	float	dir[AVOID_MAX_STEPS];	// Direction (radians)
+	int		n;						// Number of samples
+	float	minToi, maxToi;			// Min/max TOI (seconds)
+};
+
+KX_ObstacleSimulationTOI_rays::KX_ObstacleSimulationTOI_rays(MT_Scalar levelHeight, bool enableVisualization):
+	KX_ObstacleSimulationTOI(levelHeight, enableVisualization)
+{
+	m_maxSamples = 32;
+	m_minToi = 0.5f;
+	m_maxToi = 1.2f;
+	m_velWeight = 4.0f;
+	m_toiWeight = 1.0f;
+	m_collisionWeight = 100.0f;
 }
 
-static const float VEL_WEIGHT = 2.0f;
-static const float CUR_VEL_WEIGHT = 0.75f;
-static const float SIDE_WEIGHT = 0.75f;
-static const float TOI_WEIGHT = 2.5f;
+
+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;
+}
+
+///////////********* TOI_cells**********/////////////////
 
 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)
+						   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;
 
-	// Max time of collision to be considered.
-	const float maxToi = 1.5f;
-
 	float adir[2], adist;
 	vcpy(adir, activeObst->pvel);
 	if (vlen(adir) > 0.01f)
@@ -583,10 +766,10 @@ static void processSamples(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavM
 		if (nside)
 			side /= nside;
 
-		const float vpen = VEL_WEIGHT * (vdist(vcand, activeObst->dvel) * ivmax);
-		const float vcpen = CUR_VEL_WEIGHT * (vdist(vcand, activeObst->vel) * ivmax);
-		const float spen = SIDE_WEIGHT * side;
-		const float tpen = TOI_WEIGHT * (1.0f/(0.1f+tmin/maxToi));
+		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;
 
@@ -598,134 +781,89 @@ static void processSamples(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavM
 	}
 }
 
-static const int RVO_SAMPLE_RAD = 15;
-static const int MAX_RVO_SAMPLES = (RVO_SAMPLE_RAD*2+1)*(RVO_SAMPLE_RAD*2+1) + 100;
-
-static void sampleRVO(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavMeshObj, 
-					  KX_Obstacles& obstacles, const float levelHeight,const float bias)
+void KX_ObstacleSimulationTOI_cells::sampleRVO(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavMeshObj, 
+					   const float maxDeltaAngle)
 {
-	float spos[2*MAX_RVO_SAMPLES];
-	int nspos = 0;
-
-	const float cvx = activeObst->dvel[0]*bias;
-	const float cvy = activeObst->dvel[1]*bias;
+	vset(activeObst->nvel, 0.f, 0.f);
 	float vmax = vlen(activeObst->dvel);
-	const float vrange = vmax*(1-bias);
-	const float cs = 1.0f / (float)RVO_SAMPLE_RAD*vrange;
 
-	for (int y = -RVO_SAMPLE_RAD; y <= RVO_SAMPLE_RAD; ++y)
+	float* spos = new float[2*m_maxSamples];
+	int nspos = 0;
+
+	if (!m_adaptive)
 	{
-		for (int x = -RVO_SAMPLE_RAD; x <= RVO_SAMPLE_RAD; ++x)
+		const float cvx = activeObst->dvel[0]*m_bias;
+		const float cvy = activeObst->dvel[1]*m_bias;
+		float vmax = vlen(activeObst->dvel);
+		const float vrange = vmax*(1-m_bias);
+		const float cs = 1.0f / (float)m_sampleRadius*vrange;
+
+		for (int y = -m_sampleRadius; y <= m_sampleRadius; ++y)
 		{
-			if (nspos < MAX_RVO_SAMPLES)
+			for (int x = -m_sampleRadius; x <= m_sampleRadius; ++x)
 			{
-				const float vx = cvx + (float)(x+0.5f)*cs;
-				const float vy = cvy + (float)(y+0.5f)*cs;
-				if (vx*vx+vy*vy > sqr(vmax+cs/2)) continue;
-				spos[nspos*2+0] = vx;
-				spos[nspos*2+1] = vy;
-				nspos++;
+				if (nspos < m_maxSamples)
+				{
+					const float vx = cvx + (float)(x+0.5f)*cs;
+					const float vy = cvy + (float)(y+0.5f)*cs;
+					if (vx*vx+vy*vy > sqr(vmax+cs/2)) continue;
+					spos[nspos*2+0] = vx;
+					spos[nspos*2+1] = vy;
+					nspos++;
+				}
 			}
 		}
+		processSamples(activeObst, activeNavMeshObj, m_obstacles, m_levelHeight, vmax, spos, cs/2, 
+			nspos,  activeObst->nvel, m_maxToi, m_velWeight, m_curVelWeight, m_collisionWeight, m_toiWeight);
 	}
-	processSamples(activeObst, activeNavMeshObj, obstacles, levelHeight, vmax, spos, cs/2, 
-					nspos,  activeObst->nvel);
-}
-
-static void sampleRVOAdaptive(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavMeshObj, 
-					   KX_Obstacles& obstacles, const float levelHeight,const float bias)
-{
-	vset(activeObst->nvel, 0.f, 0.f);
-	float vmax = vlen(activeObst->dvel);
-
-	float spos[2*MAX_RVO_SAMPLES];
-	int nspos = 0;
-
-	int rad;
-	float res[2];
-	float cs;
-
-	// First sample location.
-	rad = 4;
-	res[0] = activeObst->dvel[0]*bias;
-	res[1] = activeObst->dvel[1]*bias;
-	cs = vmax*(2-bias*2) / (float)(rad-1);
-
-	for (int k = 0; k < 5; ++k)
+	else
 	{
-		const float half = (rad-1)*cs*0.5f;
-
-		nspos = 0;
-		for (int y = 0; y < rad; ++y)
+		int rad;
+		float res[2];
+		float cs;
+		// First sample location.
+		rad = 4;
+		res[0] = activeObst->dvel[0]*m_bias;
+		res[1] = activeObst->dvel[1]*m_bias;
+		cs = vmax*(2-m_bias*2) / (float)(rad-1);
+
+		for (int k = 0; k < 5; ++k)
 		{
-			for (int x = 0; x < rad; ++x)
+			const float half = (rad-1)*cs*0.5f;
+
+			nspos = 0;
+			for (int y = 0; y < rad; ++y)
 			{
-				const float vx = res[0] + x*cs - half;
-				const float vy = res[1] + y*cs - half;
-				if (vx*vx+vy*vy > sqr(vmax+cs/2)) continue;
-				spos[nspos*2+0] = vx;
-				spos[nspos*2+1] = vy;
-				nspos++;
+				for (int x = 0; x < rad; ++x)
+				{
+					const float vx = res[0] + x*cs - half;
+					const float vy = res[1] + y*cs - half;
+					if (vx*vx+vy*vy > sqr(vmax+cs/2)) continue;
+					spos[nspos*2+0] = vx;
+					spos[nspos*2+1] = vy;
+					nspos++;
+				}
 			}
-		}
 
-		processSamples(activeObst, activeNavMeshObj, obstacles, levelHeight, vmax, spos, cs/2, 
-			nspos,  res);
+			processSamples(activeObst, activeNavMeshObj, m_obstacles, m_levelHeight, vmax, spos, cs/2, 
+				nspos,  res, m_maxToi, m_velWeight, m_curVelWeight, m_collisionWeight, m_toiWeight);
 
-		cs *= 0.5f;
+			cs *= 0.5f;
+		}
+		vcpy(activeObst->nvel, res);
 	}
-
-	vcpy(activeObst->nvel, res);
 }
 
-
-void KX_ObstacleSimulationTOI::AdjustObstacleVelocity(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavMeshObj, 
-													MT_Vector3& velocity, MT_Scalar maxDeltaSpeed, MT_Scalar maxDeltaAngle)
-{
-	int nobs = m_obstacles.size();
-	int obstidx = std::find(m_obstacles.begin(), m_obstacles.end(), activeObst) - m_obstacles.begin();
-	if (obstidx == nobs)
-		return;
-	
-	vset(activeObst->dvel, velocity.x(), velocity.y());
-
-	//apply RVO
-	const float bias = 0.4f;
-	//sampleRVO(activeObst, activeNavMeshObj, m_obstacles, m_levelHeight, bias);
-	sampleRVOAdaptive(activeObst, activeNavMeshObj, m_obstacles, m_levelHeight, bias);
-
-	// Fake dynamic constraint.
-	float dv[2];
-	float vel[2];
-	vsub(dv, activeObst->nvel, activeObst->vel);
-	float ds = vlen(dv);
-	if (ds > maxDeltaSpeed || ds<-maxDeltaSpeed)
-		vscale(dv, dv, fabs(maxDeltaSpeed/ds));
-	vadd(vel, activeObst->vel, dv);
-	
-	velocity.x() = vel[0];
-	velocity.y() = vel[1];
-/*	printf("dvel: %f, nvel: %f, vel: %f\n", vlen(activeObst->dvel), vlen(activeObst->nvel),
-											vlen(vel));*/
-
-}
-/*
-#include "GL/glew.h"
-void KX_ObstacleSimulation::DebugDraw()
+KX_ObstacleSimulationTOI_cells::KX_ObstacleSimulationTOI_cells(MT_Scalar levelHeight, bool enableVisualization)
+:	KX_ObstacleSimulationTOI(levelHeight, enableVisualization)
+,	m_bias(0.4f)
+,	m_adaptive(true)
+,	m_sampleRadius(15)
 {
-	glDisable(GL_LIGHTING);
-	glDisable(GL_TEXTURE_2D);
-	glMatrixMode(GL_MODELVIEW);
-	glLoadIdentity();
-	glMatrixMode(GL_PROJECTION);
-	glLoadIdentity();
-	glOrtho(0.0, 100.0, 0.0, 100.0, -1.0, 1.0);
-	glBegin(GL_QUADS);
-	glColor4ub(255,0,0,255);
-	glVertex2f(0.f, 0.f);
-	glVertex2f(100.f, 25.f);
-	glVertex2f(100.f, 75.f);
-	glVertex2f(25.f, 75.f);
-	glEnd(); 
-
-}*/
\ No newline at end of file
+	m_maxSamples = (m_sampleRadius*2+1)*(m_sampleRadius*2+1) + 100;
+	m_maxToi = 1.5f;
+	m_velWeight = 2.0f;
+	m_curVelWeight = 0.75f;
+	m_toiWeight = 2.5f;
+	m_collisionWeight = 0.75f; //side_weight
+}
\ No newline at end of file
diff --git a/source/gameengine/Ketsji/KX_ObstacleSimulation.h b/source/gameengine/Ketsji/KX_ObstacleSimulation.h
index 559c4f8caf8..d926e8deb71 100644
--- a/source/gameengine/Ketsji/KX_ObstacleSimulation.h
+++ b/source/gameengine/Ketsji/KX_ObstacleSimulation.h
@@ -76,22 +76,6 @@ struct KX_Obstacle
 	KX_GameObject* m_gameObj;
 };
 typedef std::vector<KX_Obstacle*> KX_Obstacles;
-/*
-struct RVO
-{
-	inline RVO() : ns(0) {}
-	float spos[MAX_RVO_SAMPLES*2];
-	float scs[MAX_RVO_SAMPLES];
-	float spen[MAX_RVO_SAMPLES];
-
-	float svpen[MAX_RVO_SAMPLES];
-	float svcpen[MAX_RVO_SAMPLES];
-	float sspen[MAX_RVO_SAMPLES];
-	float stpen[MAX_RVO_SAMPLES];
-
-	int ns;
-};
-*/
 
 class KX_ObstacleSimulation
 {
@@ -101,7 +85,7 @@ protected:
 	MT_Scalar m_levelHeight;
 	bool m_enableVisualization;
 
-	virtual KX_Obstacle* CreateObstacle(KX_GameObject* gameobj);
+	KX_Obstacle* CreateObstacle(KX_GameObject* gameobj);
 public:
 	KX_ObstacleSimulation(MT_Scalar levelHeight, bool enableVisualization);
 	virtual ~KX_ObstacleSimulation();
@@ -117,36 +101,45 @@ public:
 	virtual void AdjustObstacleVelocity(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavMeshObj, 
 								MT_Vector3& velocity, MT_Scalar maxDeltaSpeed,MT_Scalar maxDeltaAngle);
 
-}; /* end of class KX_ObstacleSimulation*/
-
-static const int AVOID_MAX_STEPS = 128;
-struct TOICircle
-{
-	TOICircle() : n(0), minToi(0), maxToi(1) {}
-	float	toi[AVOID_MAX_STEPS];	// Time of impact (seconds)
-	float	toie[AVOID_MAX_STEPS];	// Time of exit (seconds)
-	float	dir[AVOID_MAX_STEPS];	// Direction (radians)
-	int		n;						// Number of samples
-	float	minToi, maxToi;			// Min/max TOI (seconds)
-};
-
+}; 
 class KX_ObstacleSimulationTOI: public KX_ObstacleSimulation
 {
 protected:
-	int m_avoidSteps;				// Number of sample steps
+	int m_maxSamples;				// Number of sample steps
 	float m_minToi;					// Min TOI
 	float m_maxToi;					// Max TOI
-	float m_angleWeight;			// Sample selection angle weight
+	float m_velWeight;				// Sample selection angle weight
+	float m_curVelWeight;			// Sample selection current velocity weight
 	float m_toiWeight;				// Sample selection TOI weight
 	float m_collisionWeight;		// Sample selection collision weight
 
-	std::vector<TOICircle*>	m_toiCircles; // TOI circles (one per active agent)
-	virtual KX_Obstacle* CreateObstacle(KX_GameObject* gameobj);
+	virtual void sampleRVO(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavMeshObj, 
+							const float maxDeltaAngle) = 0;
 public:
 	KX_ObstacleSimulationTOI(MT_Scalar levelHeight, bool enableVisualization);
-	~KX_ObstacleSimulationTOI();
 	virtual void AdjustObstacleVelocity(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavMeshObj, 
-									MT_Vector3& velocity, MT_Scalar maxDeltaSpeed,MT_Scalar maxDeltaAngle);
+		MT_Vector3& velocity, MT_Scalar maxDeltaSpeed,MT_Scalar maxDeltaAngle);
+};
+
+class KX_ObstacleSimulationTOI_rays: public KX_ObstacleSimulationTOI
+{
+protected:
+	virtual void sampleRVO(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavMeshObj, 
+							const float maxDeltaAngle);
+public:
+	KX_ObstacleSimulationTOI_rays(MT_Scalar levelHeight, bool enableVisualization);
+};
+
+class KX_ObstacleSimulationTOI_cells: public KX_ObstacleSimulationTOI
+{
+protected:
+	float m_bias;
+	bool m_adaptive;
+	int m_sampleRadius;
+	virtual void sampleRVO(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavMeshObj, 
+							const float maxDeltaAngle);
+public:
+	KX_ObstacleSimulationTOI_cells(MT_Scalar levelHeight, bool enableVisualization);
 };
 
 #endif
diff --git a/source/gameengine/Ketsji/KX_Scene.cpp b/source/gameengine/Ketsji/KX_Scene.cpp
index 6b4c8ae2f42..cbd4b96f566 100644
--- a/source/gameengine/Ketsji/KX_Scene.cpp
+++ b/source/gameengine/Ketsji/KX_Scene.cpp
@@ -212,9 +212,11 @@ KX_Scene::KX_Scene(class SCA_IInputDevice* keyboarddevice,
 	bool showObstacleSimulation = scene->gm.flag & GAME_SHOW_OBSTACLE_SIMULATION;
 	switch (scene->gm.obstacleSimulation)
 	{
-	case OBSTSIMULATION_TOI:
-
-		m_obstacleSimulation = new KX_ObstacleSimulationTOI((MT_Scalar)scene->gm.levelHeight, showObstacleSimulation);
+	case OBSTSIMULATION_TOI_rays:
+		m_obstacleSimulation = new KX_ObstacleSimulationTOI_rays((MT_Scalar)scene->gm.levelHeight, showObstacleSimulation);
+		break;
+	case OBSTSIMULATION_TOI_cells:
+		m_obstacleSimulation = new KX_ObstacleSimulationTOI_cells((MT_Scalar)scene->gm.levelHeight, showObstacleSimulation);
 		break;
 	default:
 		m_obstacleSimulation = NULL;
@@ -1475,10 +1477,6 @@ void KX_Scene::LogicBeginFrame(double curtime)
 		}
 	}
 
-	//prepare obstacle simulation for new frame
-	if (m_obstacleSimulation)
-		m_obstacleSimulation->UpdateObstacles();
-
 	m_logicmgr->BeginFrame(curtime, 1.0/KX_KetsjiEngine::GetTicRate());
 }
 
@@ -1506,6 +1504,10 @@ void KX_Scene::LogicEndFrame()
 		obj->Release();
 		RemoveObject(obj);
 	}
+
+	//prepare obstacle simulation for new frame
+	if (m_obstacleSimulation)
+		m_obstacleSimulation->UpdateObstacles();
 }