- added acceleration and turn speed parameters for obstacle simulation

- added debug visualization for object velocities 

9 files changed, 105 insertions, 21 deletions

diff --git a/source/blender/blenkernel/intern/sca.c b/source/blender/blenkernel/intern/sca.c
index 80904c1b1e7..877669014bb 100644
--- a/source/blender/blenkernel/intern/sca.c
+++ b/source/blender/blenkernel/intern/sca.c
@@ -416,6 +416,7 @@ void init_actuator(bActuator *act)
 	bObjectActuator *oa;
 	bRandomActuator *ra;
 	bSoundActuator *sa;
+	bSteeringActuator *sta;
 	
 	if(act->data) MEM_freeN(act->data);
 	act->data= 0;
@@ -491,6 +492,9 @@ void init_actuator(bActuator *act)
 		break;
 	case ACT_STEERING:
 		act->data = MEM_callocN(sizeof( bSteeringActuator), "steering act");
+		sta = act->data;
+		sta->acceleration = 3;
+		sta->turnspeed = 120;
 	default:
 		; /* this is very severe... I cannot make any memory for this        */
 		/* logic brick...                                                    */
diff --git a/source/blender/editors/space_logic/logic_window.c b/source/blender/editors/space_logic/logic_window.c
index 0c06ed74080..208b9aeabc0 100644
--- a/source/blender/editors/space_logic/logic_window.c
+++ b/source/blender/editors/space_logic/logic_window.c
@@ -4273,6 +4273,9 @@ static void draw_actuator_steering(uiLayout *layout, PointerRNA *ptr)
 	row = uiLayoutRow(layout, 0);
 	uiItemR(row, ptr, "distance", 0, NULL, 0);
 	uiItemR(row, ptr, "velocity", 0, NULL, 0);
+	row = uiLayoutRow(layout, 0);
+	uiItemR(row, ptr, "acceleration", 0, NULL, 0);
+	uiItemR(row, ptr, "turnspeed", 0, NULL, 0);
 }
 
 
diff --git a/source/blender/makesdna/DNA_actuator_types.h b/source/blender/makesdna/DNA_actuator_types.h
index d3c37f7c6e3..89709948f50 100644
--- a/source/blender/makesdna/DNA_actuator_types.h
+++ b/source/blender/makesdna/DNA_actuator_types.h
@@ -219,6 +219,8 @@ typedef struct bSteeringActuator {
 	int type;		/* 0=seek, 1=flee, 2=path following */
 	float dist;
 	float velocity;
+	float acceleration;
+	float turnspeed;
 	struct Object *target;
 	struct Object *navmesh;
 } bSteeringActuator;
diff --git a/source/blender/makesrna/intern/rna_actuator.c b/source/blender/makesrna/intern/rna_actuator.c
index 5f4b9e4c472..df3b59b54bc 100644
--- a/source/blender/makesrna/intern/rna_actuator.c
+++ b/source/blender/makesrna/intern/rna_actuator.c
@@ -1872,6 +1872,18 @@ static void rna_def_steering_actuator(BlenderRNA *brna)
 	RNA_def_property_ui_text(prop, "Velocity", "Velocity magnitude");
 	RNA_def_property_update(prop, NC_LOGIC, NULL);
 
+	prop= RNA_def_property(srna, "acceleration", PROP_FLOAT, PROP_NONE);
+	RNA_def_property_float_sdna(prop, NULL, "acceleration");
+	RNA_def_property_range(prop, 0.0, 1000.0);
+	RNA_def_property_ui_text(prop, "Acceleration", "Max acceleration");
+	RNA_def_property_update(prop, NC_LOGIC, NULL);
+
+	prop= RNA_def_property(srna, "turnspeed", PROP_FLOAT, PROP_NONE);
+	RNA_def_property_float_sdna(prop, NULL, "turnspeed");
+	RNA_def_property_range(prop, 0.0, 720.0);
+	RNA_def_property_ui_text(prop, "Turn speed", "Max turn speed");
+	RNA_def_property_update(prop, NC_LOGIC, NULL);
+
 	prop= RNA_def_property(srna, "distance", PROP_FLOAT, PROP_NONE);
 	RNA_def_property_float_sdna(prop, NULL, "dist");
 	RNA_def_property_range(prop, 0.0, 1000.0);
diff --git a/source/gameengine/Converter/KX_ConvertActuators.cpp b/source/gameengine/Converter/KX_ConvertActuators.cpp
index a58b60844fc..0bcd74c9e8b 100644
--- a/source/gameengine/Converter/KX_ConvertActuators.cpp
+++ b/source/gameengine/Converter/KX_ConvertActuators.cpp
@@ -1058,8 +1058,9 @@ void BL_ConvertActuators(char* maggiename,
 				}
 
 				KX_SteeringActuator *tmpstact
-					= new KX_SteeringActuator(gameobj, mode, targetob, navmeshob,
-												stAct->velocity, stAct->dist, scene->GetObstacleSimulation());
+					= new KX_SteeringActuator(gameobj, mode, targetob, navmeshob,stAct->dist, 
+											stAct->velocity, stAct->acceleration, stAct->turnspeed,
+											scene->GetObstacleSimulation());
 				baseact = tmpstact;
 				break;
 			}
diff --git a/source/gameengine/Ketsji/KX_ObstacleSimulation.cpp b/source/gameengine/Ketsji/KX_ObstacleSimulation.cpp
index 3a2d27de911..038a1e07b1b 100644
--- a/source/gameengine/Ketsji/KX_ObstacleSimulation.cpp
+++ b/source/gameengine/Ketsji/KX_ObstacleSimulation.cpp
@@ -36,13 +36,12 @@
 #include "KX_NavMeshObject.h"
 #include "KX_PythonInit.h"
 #include "DNA_object_types.h"
-#include <math.h>
+#include "BLI_math.h"
 
-#ifndef M_PI
-#define M_PI		3.14159265358979323846
-#endif
+inline float perp(const MT_Vector2& a, const MT_Vector2& b) { return a.x()*b.y() - a.y()*b.x(); }
+inline float lerp(float a, float b, float t) { return a + (b-a)*t; }
 
-int sweepCircleCircle(const MT_Vector3& pos0, const MT_Scalar r0, const MT_Vector2& v,
+static 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)
 {
@@ -64,10 +63,7 @@ int sweepCircleCircle(const MT_Vector3& pos0, const MT_Scalar r0, const MT_Vecto
 	return 1;
 }
 
-inline float perp(const MT_Vector2& a, const MT_Vector2& b) { return a.x()*b.y() - a.y()*b.x(); }
-
-
-int sweepCircleSegment(const MT_Vector3& pos0, const MT_Scalar r0, const MT_Vector2& v,
+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)
 {
@@ -143,6 +139,32 @@ int sweepCircleSegment(const MT_Vector3& pos0, const MT_Scalar r0, const MT_Vect
 	return 1;
 }
 
+static bool inBetweenAngle(float a, float amin, float amax, float& t)
+{
+	if (amax < amin) amax += (float)M_PI*2;
+	if (a < amin-(float)M_PI) a += (float)M_PI*2;
+	if (a > amin+(float)M_PI) a -= (float)M_PI*2;
+	if (a >= amin && a < amax)
+	{
+		t = (a-amin) / (amax-amin);
+		return true;
+	}
+	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()
 {
@@ -231,7 +253,7 @@ KX_Obstacle* KX_ObstacleSimulation::GetObstacle(KX_GameObject* gameobj)
 }
 
 void KX_ObstacleSimulation::AdjustObstacleVelocity(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavMeshObj, 
-													MT_Vector3& velocity)
+										MT_Vector3& velocity, MT_Scalar maxDeltaSpeed,MT_Scalar maxDeltaAngle)
 {
 }
 
@@ -282,7 +304,8 @@ KX_Obstacle* KX_ObstacleSimulationTOI::CreateObstacle()
 	return obstacle;
 }
 
-void KX_ObstacleSimulationTOI::AdjustObstacleVelocity(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavMeshObj, MT_Vector3& velocity)
+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();
@@ -387,10 +410,35 @@ void KX_ObstacleSimulationTOI::AdjustObstacleVelocity(KX_Obstacle* activeObst, K
 		tc->toie[iter] = tmine;
 	}
 
+	if (activeObst->m_vel.length() > 0.1)
+	{
+		// Constrain max turn rate.
+		float cura = atan2(activeObst->m_vel.y(),activeObst->m_vel.x());
+		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;
 
+	// Constrain velocity change.
+	const float curSpeed = (float) activeObst->m_vel.length();
+	float deltaSpeed =  vmax - curSpeed; 
+	CLAMP(deltaSpeed, -maxDeltaSpeed, maxDeltaSpeed);
+	vmax = curSpeed + deltaSpeed;
+
 	// New steering velocity.
 	vel.x() = cosf(bestDir) * vmax;
 	vel.y() = sinf(bestDir) * vmax;
diff --git a/source/gameengine/Ketsji/KX_ObstacleSimulation.h b/source/gameengine/Ketsji/KX_ObstacleSimulation.h
index 9e5895a466c..52da1e938ac 100644
--- a/source/gameengine/Ketsji/KX_ObstacleSimulation.h
+++ b/source/gameengine/Ketsji/KX_ObstacleSimulation.h
@@ -85,7 +85,7 @@ public:
 	KX_Obstacle* GetObstacle(KX_GameObject* gameobj);
 	void UpdateObstacles();	
 	virtual void AdjustObstacleVelocity(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavMeshObj, 
-										MT_Vector3& velocity);
+								MT_Vector3& velocity, MT_Scalar maxDeltaSpeed,MT_Scalar maxDeltaAngle);
 
 }; /* end of class KX_ObstacleSimulation*/
 
@@ -116,7 +116,7 @@ public:
 	KX_ObstacleSimulationTOI();
 	~KX_ObstacleSimulationTOI();
 	virtual void AdjustObstacleVelocity(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavMeshObj, 
-										MT_Vector3& velocity);
+									MT_Vector3& velocity, MT_Scalar maxDeltaSpeed,MT_Scalar maxDeltaAngle);
 };
 
 #endif
diff --git a/source/gameengine/Ketsji/KX_SteeringActuator.cpp b/source/gameengine/Ketsji/KX_SteeringActuator.cpp
index a8f8f35b1b8..04f01b01751 100644
--- a/source/gameengine/Ketsji/KX_SteeringActuator.cpp
+++ b/source/gameengine/Ketsji/KX_SteeringActuator.cpp
@@ -32,10 +32,13 @@
 * ***** END GPL LICENSE BLOCK *****
 */
 
+#include "BLI_math.h"
 #include "KX_SteeringActuator.h"
 #include "KX_GameObject.h"
 #include "KX_NavMeshObject.h"
 #include "KX_ObstacleSimulation.h"
+#include "KX_PythonInit.h"
+
 
 /* ------------------------------------------------------------------------- */
 /* Native functions                                                          */
@@ -45,14 +48,18 @@ KX_SteeringActuator::KX_SteeringActuator(SCA_IObject *gameobj,
 									int mode,
 									KX_GameObject *target,
 									KX_GameObject *navmesh,
-									MT_Scalar velocity, 
 									MT_Scalar distance,
+									MT_Scalar velocity, 
+									MT_Scalar acceleration,									
+									MT_Scalar turnspeed,
 									KX_ObstacleSimulation* simulation)	 : 
 	SCA_IActuator(gameobj, KX_ACT_STEERING),
 	m_mode(mode),
 	m_target(target),
-	m_velocity(velocity),
 	m_distance(distance),
+	m_velocity(velocity),
+	m_acceleration(acceleration),
+	m_turnspeed(turnspeed),
 	m_updateTime(0),
 	m_isActive(false),
 	m_simulation(simulation),
@@ -157,7 +164,7 @@ bool KX_SteeringActuator::Update(double curtime, bool frame)
 		const MT_Point3& targpos = m_target->NodeGetWorldPosition();
 		MT_Vector3 vectotarg = targpos - mypos;
 		MT_Vector3 steervec = MT_Vector3(0, 0, 0);
-		bool apply_steerforce = false;
+		bool apply_steerforce = true;
 
 		switch (m_mode) {
 			case KX_STEERING_SEEK:
@@ -204,9 +211,12 @@ bool KX_SteeringActuator::Update(double curtime, bool frame)
 			MT_Vector3 newvel = m_velocity*steervec;
 
 			//adjust velocity to avoid obstacles
-			if (m_simulation && m_obstacle)
+			if (m_simulation && m_obstacle && !newvel.fuzzyZero())
 			{
-				m_simulation->AdjustObstacleVelocity(m_obstacle, m_navmesh, newvel);
+				KX_RasterizerDrawDebugLine(mypos, mypos + newvel, MT_Vector3(1.,0.,0.));
+				m_simulation->AdjustObstacleVelocity(m_obstacle, m_navmesh, newvel, 
+								m_acceleration*delta, m_turnspeed/180.0f*M_PI*delta);
+				KX_RasterizerDrawDebugLine(mypos, mypos + newvel, MT_Vector3(0.,1.,0.));
 			}
 
 			//temporary solution: set 2D steering velocity directly to obj
diff --git a/source/gameengine/Ketsji/KX_SteeringActuator.h b/source/gameengine/Ketsji/KX_SteeringActuator.h
index d1ba24279e1..b303a2a8037 100644
--- a/source/gameengine/Ketsji/KX_SteeringActuator.h
+++ b/source/gameengine/Ketsji/KX_SteeringActuator.h
@@ -54,6 +54,8 @@ class KX_SteeringActuator : public SCA_IActuator
 	int	m_mode;
 	MT_Scalar m_distance;
 	MT_Scalar m_velocity;
+	MT_Scalar m_acceleration;									
+	MT_Scalar m_turnspeed;
 	KX_ObstacleSimulation* m_simulation;
 	
 	KX_Obstacle* m_obstacle;
@@ -73,8 +75,10 @@ public:
 						int mode,
 						KX_GameObject *target, 
 						KX_GameObject *navmesh,
-						MT_Scalar movement, 
 						MT_Scalar distance,
+						MT_Scalar velocity, 
+						MT_Scalar acceleration,									
+						MT_Scalar turnspeed,
 						KX_ObstacleSimulation* simulation);
 	virtual ~KX_SteeringActuator();
 	virtual bool Update(double curtime, bool frame);