Added segment obstacles, creation obstacles for walls of navigation mesh

1 files changed, 165 insertions, 20 deletions

diff --git a/source/gameengine/Ketsji/KX_ObstacleSimulation.cpp b/source/gameengine/Ketsji/KX_ObstacleSimulation.cpp
index 93dc25ec108..1d0b0b40750 100644
--- a/source/gameengine/Ketsji/KX_ObstacleSimulation.cpp
+++ b/source/gameengine/Ketsji/KX_ObstacleSimulation.cpp
@@ -33,7 +33,8 @@
 */
 
 #include "KX_ObstacleSimulation.h"
-#include "KX_GameObject.h"
+#include "KX_NavMeshObject.h"
+#include "KX_PythonInit.h"
 #include "DNA_object_types.h"
 #include "math.h"
 #define M_PI       3.14159265358979323846
@@ -60,6 +61,85 @@ 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,
+					   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;
+}
+
 
 KX_ObstacleSimulation::KX_ObstacleSimulation()
 {
@@ -75,19 +155,60 @@ KX_ObstacleSimulation::~KX_ObstacleSimulation()
 	}
 	m_obstacles.clear();
 }
-void KX_ObstacleSimulation::AddObstacleForObj(KX_GameObject* gameobj)
+KX_Obstacle* KX_ObstacleSimulation::CreateObstacle()
 {
 	KX_Obstacle* obstacle = new KX_Obstacle();
+	m_obstacles.push_back(obstacle);
+	return obstacle;
+}
+
+void KX_ObstacleSimulation::AddObstacleForObj(KX_GameObject* gameobj)
+{
+	KX_Obstacle* obstacle = CreateObstacle();
 	struct Object* blenderobject = gameobj->GetBlenderObject();
+	obstacle->m_type = KX_OBSTACLE_OBJ;
+	obstacle->m_shape = KX_OBSTACLE_CIRCLE;
 	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::AddObstaclesForNavMesh(KX_NavMeshObject* navmeshobj)
+{	
+	dtStatNavMesh* navmesh = navmeshobj->GetNavMesh();
+	if (navmesh)
+	{
+		int npoly = navmesh->getPolyCount();
+		for (int pi=0; pi<npoly; pi++)
+		{
+			const dtStatPoly* poly = navmesh->getPoly(pi);
+
+			for (int i = 0, j = (int)poly->nv-1; i < (int)poly->nv; j = i++)
+			{	
+				if (poly->n[j]) continue;
+				const float* vj = navmesh->getVertex(poly->v[j]);
+				const float* vi = navmesh->getVertex(poly->v[i]);
+		
+				KX_Obstacle* obstacle = CreateObstacle();
+				obstacle->m_type = KX_OBSTACLE_NAV_MESH;
+				obstacle->m_shape = KX_OBSTACLE_SEGMENT;
+				obstacle->m_gameObj = navmeshobj;
+				obstacle->m_pos = MT_Vector3(vj[0], vj[2], vj[1]);
+				obstacle->m_pos2 = MT_Vector3(vi[0], vi[2], vi[1]);
+				obstacle->m_rad = 0;
+				obstacle->m_vel = MT_Vector2(0,0);
+			}
+		}
+	}
 }
 
 void KX_ObstacleSimulation::UpdateObstacles()
 {
 	for (size_t i=0; i<m_obstacles.size(); i++)
 	{
+		if (m_obstacles[i]->m_shape==KX_OBSTACLE_NAV_MESH || m_obstacles[i]->m_shape==KX_OBSTACLE_SEGMENT)
+			continue;
+
 		KX_Obstacle* obs = m_obstacles[i];
 		obs->m_pos = obs->m_gameObj->NodeGetWorldPosition();
 		obs->m_vel.x() = obs->m_gameObj->GetLinearVelocity().x();
@@ -106,8 +227,21 @@ KX_Obstacle* KX_ObstacleSimulation::GetObstacle(KX_GameObject* gameobj)
 	return NULL;
 }
 
-void KX_ObstacleSimulation::AdjustObstacleVelocity(KX_Obstacle* activeObst, MT_Vector3& velocity)
+void KX_ObstacleSimulation::AdjustObstacleVelocity(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavMeshObj, 
+													MT_Vector3& velocity)
+{
+}
+
+void KX_ObstacleSimulation::DrawObstacles()
 {
+	static const MT_Vector3 bluecolor(0,0,1);
+	for (size_t i=0; i<m_obstacles.size(); i++)
+	{
+		if (m_obstacles[i]->m_shape==KX_OBSTACLE_SEGMENT)
+		{
+			KX_RasterizerDrawDebugLine(m_obstacles[i]->m_pos, m_obstacles[i]->m_pos2, bluecolor);
+		}
+	}	
 }
 
 KX_ObstacleSimulationTOI::KX_ObstacleSimulationTOI():
@@ -131,13 +265,14 @@ KX_ObstacleSimulationTOI::~KX_ObstacleSimulationTOI()
 	m_toiCircles.clear();
 }
 
-void KX_ObstacleSimulationTOI::AddObstacleForObj(KX_GameObject* gameobj)
+KX_Obstacle* KX_ObstacleSimulationTOI::CreateObstacle()
 {
-	KX_ObstacleSimulation::AddObstacleForObj(gameobj);
+	KX_Obstacle* obstacle = KX_ObstacleSimulation::CreateObstacle();
 	m_toiCircles.push_back(new TOICircle());
+	return obstacle;
 }
 
-void KX_ObstacleSimulationTOI::AdjustObstacleVelocity(KX_Obstacle* activeObst, MT_Vector3& velocity)
+void KX_ObstacleSimulationTOI::AdjustObstacleVelocity(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavMeshObj, MT_Vector3& velocity)
 {
 	int nobs = m_obstacles.size();
 	int obstidx = std::find(m_obstacles.begin(), m_obstacles.end(), activeObst) - m_obstacles.begin();
@@ -177,28 +312,38 @@ void KX_ObstacleSimulationTOI::AdjustObstacleVelocity(KX_Obstacle* activeObst, M
 		float tmine = 0;
 		for (int i = 0; i < nobs; ++i)
 		{
-			if (i==obstidx)
-				continue;
 			KX_Obstacle* ob = m_obstacles[i];
+			if ( (ob==activeObst) ||
+				 (ob->m_type==KX_OBSTACLE_NAV_MESH && ob->m_gameObj!=activeNavMeshObj)	)
+				continue;
 
 			float htmin,htmax;
 
-			MT_Vector2 vab;
-			if (ob->m_vel.length2() < 0.01f*0.01f)
+			if (ob->m_type == KX_OBSTACLE_CIRCLE)
 			{
-				// Stationary, use VO
-				vab = svel;
+				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;
 			}
-			else
+			else if (ob->m_type == KX_OBSTACLE_SEGMENT)
 			{
-				// Moving, use RVO
-				vab = 2*svel - vel - ob->m_vel;
+				if (!sweepCircleSegment(activeObst->m_pos, activeObst->m_rad, svel, 
+										ob->m_pos, ob->m_pos2, ob->m_rad, htmin, htmax))
+					continue;
 			}
 
-			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.