added obstacle avoidance code; object movement is implemented via setting velocity

8 files changed, 478 insertions, 63 deletions

diff --git a/source/gameengine/Converter/BL_BlenderDataConversion.cpp b/source/gameengine/Converter/BL_BlenderDataConversion.cpp
index 3fb8c2bd67e..1b15982339b 100644
--- a/source/gameengine/Converter/BL_BlenderDataConversion.cpp
+++ b/source/gameengine/Converter/BL_BlenderDataConversion.cpp
@@ -173,6 +173,7 @@ extern "C" {
 #include "BL_DeformableGameObject.h"
 
 #include "KX_NavMeshObject.h"
+#include "KX_ObstacleSimulation.h"
 
 #ifdef __cplusplus
 extern "C" {
@@ -2638,6 +2639,20 @@ void BL_ConvertBlenderObjects(struct Main* maggie,
 	converter->RegisterWorldInfo(worldinfo);
 	kxscene->SetWorldInfo(worldinfo);
 
+	//create object representations for obstacle simulation
+	KX_ObstacleSimulation* obssimulation = kxscene->GetObstacleSimulation();
+	if (obssimulation)
+	{
+		for ( i=0;i<objectlist->GetCount();i++)
+		{
+			KX_GameObject* gameobj = static_cast<KX_GameObject*>(objectlist->GetValue(i));
+			if (gameobj->IsDynamic())
+			{
+				obssimulation->AddObstacleForObj(gameobj);
+			}
+		}
+	}
+
 #define CONVERT_LOGIC
 #ifdef CONVERT_LOGIC
 	// convert logic bricks, sensors, controllers and actuators
@@ -2689,9 +2704,7 @@ void BL_ConvertBlenderObjects(struct Main* maggie,
 			pathfinder->BuildNavMesh();
 			pathfinder->SetVisible(0, true);
 		}
-	}		
-	
-
+	}
 	
 	// Calculate the scene btree -
 	// too slow - commented out.
diff --git a/source/gameengine/Converter/KX_ConvertActuators.cpp b/source/gameengine/Converter/KX_ConvertActuators.cpp
index f7151dfd8c3..a58b60844fc 100644
--- a/source/gameengine/Converter/KX_ConvertActuators.cpp
+++ b/source/gameengine/Converter/KX_ConvertActuators.cpp
@@ -1059,7 +1059,7 @@ void BL_ConvertActuators(char* maggiename,
 
 				KX_SteeringActuator *tmpstact
 					= new KX_SteeringActuator(gameobj, mode, targetob, navmeshob,
-												stAct->movement, stAct->dist);
+												stAct->velocity, stAct->dist, scene->GetObstacleSimulation());
 				baseact = tmpstact;
 				break;
 			}
diff --git a/source/gameengine/Ketsji/KX_ObstacleSimulation.cpp b/source/gameengine/Ketsji/KX_ObstacleSimulation.cpp
new file mode 100644
index 00000000000..93dc25ec108
--- /dev/null
+++ b/source/gameengine/Ketsji/KX_ObstacleSimulation.cpp
@@ -0,0 +1,245 @@
+/**
+* Simulation for obstacle avoidance behavior
+*
+* $Id$
+*
+* ***** BEGIN GPL LICENSE BLOCK *****
+*
+* This program is free software; you can redistribute it and/or
+* modify it under the terms of the GNU General Public License
+* as published by the Free Software Foundation; either version 2
+* of the License, or (at your option) any later version. The Blender
+* Foundation also sells licenses for use in proprietary software under
+* the Blender License.  See http://www.blender.org/BL/ for information
+* about this.
+*
+* This program is distributed in the hope that it will be useful,
+* but WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program; if not, write to the Free Software Foundation,
+* Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+*
+* The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
+* All rights reserved.
+*
+* The Original Code is: all of this file.
+*
+* Contributor(s): none yet.
+*
+* ***** END GPL LICENSE BLOCK *****
+*/
+
+#include "KX_ObstacleSimulation.h"
+#include "KX_GameObject.h"
+#include "DNA_object_types.h"
+#include "math.h"
+#define M_PI       3.14159265358979323846
+
+int sweepCircleCircle(const MT_Vector3& pos0, const MT_Scalar r0, const MT_Vector2& v,
+					  const MT_Vector3& pos1, const MT_Scalar r1,
+					  float& tmin, float& tmax)
+{
+	static const float EPS = 0.0001f;
+	MT_Vector2 c0(pos0.x(), pos0.y());
+	MT_Vector2 c1(pos1.x(), pos1.y());
+	MT_Vector2 s = c1 - c0;
+	MT_Scalar  r = r0+r1;
+	float c = s.length2() - r*r;
+	float a = v.length2();
+	if (a < EPS) return 0;	// not moving
+
+	// Overlap, calc time to exit.
+	float b = MT_dot(v,s);
+	float d = b*b - a*c;
+	if (d < 0.0f) return 0; // no intersection.
+	tmin = (b - sqrtf(d)) / a;
+	tmax = (b + sqrtf(d)) / a;
+	return 1;
+}
+
+
+KX_ObstacleSimulation::KX_ObstacleSimulation()
+{
+
+}
+
+KX_ObstacleSimulation::~KX_ObstacleSimulation()
+{
+	for (size_t i=0; i<m_obstacles.size(); i++)
+	{
+		KX_Obstacle* obs = m_obstacles[i];
+		delete obs;
+	}
+	m_obstacles.clear();
+}
+void KX_ObstacleSimulation::AddObstacleForObj(KX_GameObject* gameobj)
+{
+	KX_Obstacle* obstacle = new KX_Obstacle();
+	struct Object* blenderobject = gameobj->GetBlenderObject();
+	obstacle->m_rad = blenderobject->inertia; //.todo use radius of collision shape bound sphere 
+	obstacle->m_gameObj = gameobj;
+	m_obstacles.push_back(obstacle);
+}
+
+void KX_ObstacleSimulation::UpdateObstacles()
+{
+	for (size_t i=0; i<m_obstacles.size(); i++)
+	{
+		KX_Obstacle* obs = m_obstacles[i];
+		obs->m_pos = obs->m_gameObj->NodeGetWorldPosition();
+		obs->m_vel.x() = obs->m_gameObj->GetLinearVelocity().x();
+		obs->m_vel.y() = obs->m_gameObj->GetLinearVelocity().y();
+	}
+}
+
+KX_Obstacle* KX_ObstacleSimulation::GetObstacle(KX_GameObject* gameobj)
+{
+	for (size_t i=0; i<m_obstacles.size(); i++)
+	{
+		if (m_obstacles[i]->m_gameObj == gameobj)
+			return m_obstacles[i];
+	}
+
+	return NULL;
+}
+
+void KX_ObstacleSimulation::AdjustObstacleVelocity(KX_Obstacle* activeObst, MT_Vector3& velocity)
+{
+}
+
+KX_ObstacleSimulationTOI::KX_ObstacleSimulationTOI():
+	m_avoidSteps(32),
+	m_minToi(0.5f),
+	m_maxToi(1.2f),
+	m_angleWeight(4.0f),
+	m_toiWeight(1.0f),
+	m_collisionWeight(100.0f)
+{
+	
+}
+
+KX_ObstacleSimulationTOI::~KX_ObstacleSimulationTOI()
+{
+	for (size_t i=0; i<m_toiCircles.size(); i++)
+	{
+		TOICircle* toi = m_toiCircles[i];
+		delete toi;
+	}
+	m_toiCircles.clear();
+}
+
+void KX_ObstacleSimulationTOI::AddObstacleForObj(KX_GameObject* gameobj)
+{
+	KX_ObstacleSimulation::AddObstacleForObj(gameobj);
+	m_toiCircles.push_back(new TOICircle());
+}
+
+void KX_ObstacleSimulationTOI::AdjustObstacleVelocity(KX_Obstacle* activeObst, MT_Vector3& velocity)
+{
+	int nobs = m_obstacles.size();
+	int obstidx = std::find(m_obstacles.begin(), m_obstacles.end(), activeObst) - m_obstacles.begin();
+	if (obstidx == nobs)
+		return; 
+	TOICircle* tc = m_toiCircles[obstidx];
+
+	MT_Vector2 vel(velocity.x(), velocity.y());
+	float vmax = (float) velocity.length();
+	float odir = (float) atan2(velocity.y(), velocity.x());
+
+	MT_Vector2 ddir = vel;
+	ddir.normalize();
+
+	float bestScore = FLT_MAX;
+	float bestDir = odir;
+	float bestToi = 0;
+
+	tc->n = m_avoidSteps;
+	tc->minToi = m_minToi;
+	tc->maxToi = m_maxToi;
+
+	const int iforw = m_avoidSteps/2;
+	const float aoff = (float)iforw / (float)m_avoidSteps;
+
+	for (int iter = 0; iter < m_avoidSteps; ++iter)
+	{
+		// Calculate sample velocity
+		const float ndir = ((float)iter/(float)m_avoidSteps) - 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)
+		{
+			if (i==obstidx)
+				continue;
+			KX_Obstacle* ob = m_obstacles[i];
+
+			float htmin,htmax;
+
+			MT_Vector2 vab;
+			if (ob->m_vel.length2() < 0.01f*0.01f)
+			{
+				// Stationary, use VO
+				vab = svel;
+			}
+			else
+			{
+				// Moving, use RVO
+				vab = 2*svel - vel - ob->m_vel;
+			}
+
+			if (!sweepCircleCircle(activeObst->m_pos, activeObst->m_rad, 
+									vab, ob->m_pos, 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_angleWeight * 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;
+	}
+
+	// Adjust speed when time of impact is less than min TOI.
+	if (bestToi < m_minToi)
+		vmax *= bestToi/m_minToi;
+
+	// New steering velocity.
+	vel.x() = cosf(bestDir) * vmax;
+	vel.y() = sinf(bestDir) * vmax;
+
+	velocity.x() = vel.x();
+	velocity.y() = vel.y();
+}
\ No newline at end of file
diff --git a/source/gameengine/Ketsji/KX_ObstacleSimulation.h b/source/gameengine/Ketsji/KX_ObstacleSimulation.h
new file mode 100644
index 00000000000..d1920e5e6f2
--- /dev/null
+++ b/source/gameengine/Ketsji/KX_ObstacleSimulation.h
@@ -0,0 +1,98 @@
+/**
+* Simulation for obstacle avoidance behavior 
+* (based on Cane Project - http://code.google.com/p/cane  by Mikko Mononen (c) 2009)
+*
+*
+* $Id$
+*
+* ***** BEGIN GPL LICENSE BLOCK *****
+*
+* This program is free software; you can redistribute it and/or
+* modify it under the terms of the GNU General Public License
+* as published by the Free Software Foundation; either version 2
+* of the License, or (at your option) any later version. The Blender
+* Foundation also sells licenses for use in proprietary software under
+* the Blender License.  See http://www.blender.org/BL/ for information
+* about this.
+*
+* This program is distributed in the hope that it will be useful,
+* but WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program; if not, write to the Free Software Foundation,
+* Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+*
+* The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
+* All rights reserved.
+*
+* The Original Code is: all of this file.
+*
+* Contributor(s): none yet.
+*
+* ***** END GPL LICENSE BLOCK *****
+*/
+
+#ifndef __KX_OBSTACLESIMULATION
+#define __KX_OBSTACLESIMULATION
+
+#include <vector>
+#include "MT_Point2.h"
+#include "MT_Point3.h"
+
+class KX_GameObject;
+
+struct KX_Obstacle
+{
+	MT_Point3 m_pos;
+	MT_Scalar m_rad;
+	MT_Vector2 m_vel;
+	KX_GameObject* m_gameObj;
+};
+
+class KX_ObstacleSimulation
+{
+protected:
+	std::vector<KX_Obstacle*>	m_obstacles;
+public:
+	KX_ObstacleSimulation();
+	virtual ~KX_ObstacleSimulation();
+
+	virtual void AddObstacleForObj(KX_GameObject* gameobj);
+	KX_Obstacle* GetObstacle(KX_GameObject* gameobj);
+	void UpdateObstacles();	
+	virtual void AdjustObstacleVelocity(KX_Obstacle* activeObst, MT_Vector3& velocity);
+
+}; /* 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
+	float m_minToi;					// Min TOI
+	float m_maxToi;					// Max TOI
+	float m_angleWeight;			// Sample selection angle 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)
+public:
+	KX_ObstacleSimulationTOI();
+	~KX_ObstacleSimulationTOI();
+	virtual void AddObstacleForObj(KX_GameObject* gameobj);
+	virtual void AdjustObstacleVelocity(KX_Obstacle* activeObst, MT_Vector3& velocity);
+};
+
+#endif
diff --git a/source/gameengine/Ketsji/KX_Scene.cpp b/source/gameengine/Ketsji/KX_Scene.cpp
index 3af3c8a3096..63a7dc1ba2c 100644
--- a/source/gameengine/Ketsji/KX_Scene.cpp
+++ b/source/gameengine/Ketsji/KX_Scene.cpp
@@ -85,6 +85,7 @@
 #include "BL_DeformableGameObject.h"
 #include "KX_SoftBodyDeformer.h"
 
+#include "KX_ObstacleSimulation.h"
 // to get USE_BULLET!
 #include "KX_ConvertPhysicsObject.h"
 
@@ -210,6 +211,8 @@ KX_Scene::KX_Scene(class SCA_IInputDevice* keyboarddevice,
 
 	m_bucketmanager=new RAS_BucketManager();
 
+	m_obstacleSimulation = new KX_ObstacleSimulationTOI;//KX_ObstacleSimulation;
+
 #ifndef DISABLE_PYTHON
 	m_attr_dict = PyDict_New(); /* new ref */
 	m_draw_call_pre = NULL;
@@ -221,6 +224,9 @@ KX_Scene::KX_Scene(class SCA_IInputDevice* keyboarddevice,
 
 KX_Scene::~KX_Scene()
 {
+	if (m_obstacleSimulation)
+		delete m_obstacleSimulation;
+
 	// The release of debug properties used to be in SCA_IScene::~SCA_IScene
 	// It's still there but we remove all properties here otherwise some
 	// reference might be hanging and causing late release of objects
@@ -1460,6 +1466,10 @@ void KX_Scene::LogicBeginFrame(double curtime)
 			// all object is the tempObjectList should have a clock
 		}
 	}
+
+	//prepare obstacle simulation for new frame
+	m_obstacleSimulation->UpdateObstacles();
+
 	m_logicmgr->BeginFrame(curtime, 1.0/KX_KetsjiEngine::GetTicRate());
 }
 
diff --git a/source/gameengine/Ketsji/KX_Scene.h b/source/gameengine/Ketsji/KX_Scene.h
index bc608d9eb2a..6e75347fa30 100644
--- a/source/gameengine/Ketsji/KX_Scene.h
+++ b/source/gameengine/Ketsji/KX_Scene.h
@@ -83,6 +83,7 @@ class SCA_JoystickManager;
 class btCollisionShape;
 class KX_BlenderSceneConverter;
 struct KX_ClientObjectInfo;
+class KX_ObstacleSimulation;
 
 /* for ID freeing */
 #define IS_TAGGED(_id) ((_id) && (((ID *)_id)->flag & LIB_DOIT))
@@ -277,6 +278,8 @@ protected:
 
 	RAS_2DFilterManager m_filtermanager;
 
+	KX_ObstacleSimulation* m_obstacleSimulation;
+
 public:	
 	KX_Scene(class SCA_IInputDevice* keyboarddevice,
 		class SCA_IInputDevice* mousedevice,
@@ -541,6 +544,8 @@ public:
 	void Update2DFilter(vector<STR_String>& propNames, void* gameObj, RAS_2DFilterManager::RAS_2DFILTER_MODE filtermode, int pass, STR_String& text);
 	void Render2DFilters(RAS_ICanvas* canvas);
 
+	KX_ObstacleSimulation* GetObstacleSimulation() {return m_obstacleSimulation;};
+
 #ifndef DISABLE_PYTHON
 	/* --------------------------------------------------------------------- */
 	/* Python interface ---------------------------------------------------- */
diff --git a/source/gameengine/Ketsji/KX_SteeringActuator.cpp b/source/gameengine/Ketsji/KX_SteeringActuator.cpp
index 803ed510f45..45f3482ed5b 100644
--- a/source/gameengine/Ketsji/KX_SteeringActuator.cpp
+++ b/source/gameengine/Ketsji/KX_SteeringActuator.cpp
@@ -35,6 +35,7 @@
 #include "KX_SteeringActuator.h"
 #include "KX_GameObject.h"
 #include "KX_NavMeshObject.h"
+#include "KX_ObstacleSimulation.h"
 
 /* ------------------------------------------------------------------------- */
 /* Native functions                                                          */
@@ -44,19 +45,27 @@ KX_SteeringActuator::KX_SteeringActuator(SCA_IObject *gameobj,
 									int mode,
 									KX_GameObject *target,
 									KX_GameObject *navmesh,
-									MT_Scalar movement, 
-									MT_Scalar distance)	 : 
+									MT_Scalar velocity, 
+									MT_Scalar distance,
+									KX_ObstacleSimulation* simulation)	 : 
 	SCA_IActuator(gameobj, KX_ACT_STEERING),
 	m_mode(mode),
 	m_target(target),
-	m_movement(movement),
-	m_distance(distance)
+	m_velocity(velocity),
+	m_distance(distance),
+	m_updateTime(0),
+	m_isActive(false),
+	m_simulation(simulation),
+	m_obstacle(NULL)
 {
 	m_navmesh = static_cast<KX_NavMeshObject*>(navmesh);
 	if (m_navmesh)
 		m_navmesh->RegisterActuator(this);
 	if (m_target)
 		m_target->RegisterActuator(this);
+	
+	if (m_simulation)
+		m_obstacle = m_simulation->GetObstacle((KX_GameObject*)gameobj);
 } 
 
 KX_SteeringActuator::~KX_SteeringActuator()
@@ -121,64 +130,92 @@ void KX_SteeringActuator::Relink(GEN_Map<GEN_HashedPtr, void*> *obj_map)
 	}
 }
 
-bool KX_SteeringActuator::Update()
+bool KX_SteeringActuator::Update(double curtime, bool frame)
 {
-	bool bNegativeEvent = IsNegativeEvent();
-	RemoveAllEvents();
-
-	if (bNegativeEvent)
-		return false; // do nothing on negative events
-
-	KX_GameObject *obj = (KX_GameObject*) GetParent();
-	const MT_Point3& mypos = obj->NodeGetWorldPosition();
-	const MT_Point3& targpos = m_target->NodeGetWorldPosition();
-	MT_Vector3 vectotarg = targpos - mypos;
-	MT_Vector3 steervec = MT_Vector3(0, 0, 0);
-	bool apply_steerforce = false;
-
-	switch (m_mode) {
-		case KX_STEERING_SEEK:
-			if (vectotarg.length2()>m_distance*m_distance)
-			{
-				apply_steerforce = true;
-				steervec = vectotarg;
-				steervec.normalize();
-			}
-			break;
-		case KX_STEERING_FLEE:
-			if (vectotarg.length2()<m_distance*m_distance)
-			{
-				apply_steerforce = true;
-				steervec = -vectotarg;
-				steervec.normalize();
-			}
-		case KX_STEERING_PATHFOLLOWING:
-			if (m_navmesh && vectotarg.length2()>m_distance*m_distance)
-			{
-				static const int MAX_PATH_LENGTH  = 128;
-				static const MT_Vector3 PATH_COLOR(1,0,0);
-
-				float path[MAX_PATH_LENGTH*3];
-				int pathlen = m_navmesh->FindPath(mypos, targpos, path, MAX_PATH_LENGTH);
-				if (pathlen > 1)
+	if (frame)
+	{
+		double delta =  curtime - m_updateTime;
+		m_updateTime = curtime;
+		
+		if (m_posevent && !m_isActive)
+		{
+			delta = 0;
+			m_updateTime = curtime;
+			m_isActive = true;
+		}
+		bool bNegativeEvent = IsNegativeEvent();
+		if (bNegativeEvent)
+			m_isActive = false;
+
+		RemoveAllEvents();
+
+		if (bNegativeEvent || !delta)
+			return false; // do nothing on negative events
+
+		KX_GameObject *obj = (KX_GameObject*) GetParent();
+		const MT_Point3& mypos = obj->NodeGetWorldPosition();
+		const MT_Point3& targpos = m_target->NodeGetWorldPosition();
+		MT_Vector3 vectotarg = targpos - mypos;
+		MT_Vector3 steervec = MT_Vector3(0, 0, 0);
+		bool apply_steerforce = false;
+
+		switch (m_mode) {
+			case KX_STEERING_SEEK:
+				if (vectotarg.length2()>m_distance*m_distance)
 				{
-					//debug draw
-					m_navmesh->DrawPath(path, pathlen, PATH_COLOR);
-
 					apply_steerforce = true;
-					MT_Vector3 waypoint(&path[3]);
-					steervec = waypoint - mypos;
-					steervec.z() = 0;
+					steervec = vectotarg;
 					steervec.normalize();
 				}
+				break;
+			case KX_STEERING_FLEE:
+				if (vectotarg.length2()<m_distance*m_distance)
+				{
+					apply_steerforce = true;
+					steervec = -vectotarg;
+					steervec.normalize();
+				}
+			case KX_STEERING_PATHFOLLOWING:
+				if (m_navmesh && vectotarg.length2()>m_distance*m_distance)
+				{
+					static const int MAX_PATH_LENGTH  = 128;
+					static const MT_Vector3 PATH_COLOR(1,0,0);
+
+					float path[MAX_PATH_LENGTH*3];
+					int pathlen = m_navmesh->FindPath(mypos, targpos, path, MAX_PATH_LENGTH);
+					if (pathlen > 1)
+					{
+						//debug draw
+						m_navmesh->DrawPath(path, pathlen, PATH_COLOR);
+
+						apply_steerforce = true;
+						MT_Vector3 waypoint(&path[3]);
+						steervec = waypoint - mypos;
+						steervec.z() = 0;
+						steervec.normalize();
+					}
+				}
+				break;
+		}
+
+		if (apply_steerforce)
+		{
+			MT_Vector3 newvel = m_velocity*steervec;
+
+			//adjust velocity to avoid obstacles
+			if (m_simulation && m_obstacle)
+			{
+				m_simulation->AdjustObstacleVelocity(m_obstacle, newvel);
 			}
-			break;
-	}
 
-	if (apply_steerforce)
-	{
-		MT_Vector3 vel = m_movement*steervec;
-		obj->ApplyMovement(vel, false);
+			//temporary solution: set 2D steering velocity directly to obj
+			//correct way is to apply physical force
+			//MT_Vector3 movement = delta*m_velocity*steervec;
+			//obj->ApplyMovement(movement, false);
+			MT_Vector3 curvel = obj->GetLinearVelocity();
+			newvel.z() = curvel.z();			
+			obj->setLinearVelocity(newvel, false);
+		}
 	}
 
 	return true;
diff --git a/source/gameengine/Ketsji/KX_SteeringActuator.h b/source/gameengine/Ketsji/KX_SteeringActuator.h
index fce2205ca6e..d1ba24279e1 100644
--- a/source/gameengine/Ketsji/KX_SteeringActuator.h
+++ b/source/gameengine/Ketsji/KX_SteeringActuator.h
@@ -41,6 +41,8 @@
 
 class KX_GameObject;
 class KX_NavMeshObject;
+struct KX_Obstacle;
+class KX_ObstacleSimulation;
 
 class KX_SteeringActuator : public SCA_IActuator
 {
@@ -49,10 +51,14 @@ class KX_SteeringActuator : public SCA_IActuator
 	/** Target object */
 	KX_GameObject *m_target;
 	KX_NavMeshObject *m_navmesh;
-
 	int	m_mode;
 	MT_Scalar m_distance;
-	MT_Scalar m_movement;
+	MT_Scalar m_velocity;
+	KX_ObstacleSimulation* m_simulation;
+	
+	KX_Obstacle* m_obstacle;
+	double m_updateTime;
+	bool m_isActive;
 public:
 	enum KX_STEERINGACT_MODE
 	{
@@ -68,9 +74,10 @@ public:
 						KX_GameObject *target, 
 						KX_GameObject *navmesh,
 						MT_Scalar movement, 
-						MT_Scalar distance);
+						MT_Scalar distance,
+						KX_ObstacleSimulation* simulation);
 	virtual ~KX_SteeringActuator();
-	virtual bool Update();
+	virtual bool Update(double curtime, bool frame);
 
 	virtual CValue* GetReplica();
 	virtual void ProcessReplica();