/*
 * ***** 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.
 *
 * 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.
 *
 * Contributor(s): none yet.
 *
 * ***** END GPL LICENSE BLOCK *****
 */

/** \file gameengine/Ketsji/KX_ObstacleSimulation.cpp
 *  \ingroup ketsji
 *
 * Simulation for obstacle avoidance behavior
 */

#include "KX_ObstacleSimulation.h"
#include "KX_NavMeshObject.h"
#include "KX_PythonInit.h"
#include "DNA_object_types.h"
#include "BLI_math.h"

namespace
{
	inline float perp(const MT_Vector2& a, const MT_Vector2& b) { return a.x()*b.y() - a.y()*b.x(); }

	inline float sqr(float x) { return x * x; }
	inline float lerp(float a, float b, float t) { return a + (b - a) * t; }
	inline float clamp(float a, float mn, float mx) { return a < mn ? mn : (a > mx ? mx : a); }
	inline void vset(float v[2], float x, float y) { v[0] = x; v[1] = y; }
}

/* grr, seems moto provides no nice way to do this */
#define MT_3D_AS_2D(v) MT_Vector2((v)[0], (v)[1])

static int sweepCircleCircle(
        const MT_Vector2 &pos0, const MT_Scalar r0, const MT_Vector2 &v,
        const MT_Vector2 &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;
}

static int sweepCircleSegment(
        const MT_Vector2 &pos0, const MT_Scalar r0, const MT_Vector2 &v,
        const MT_Vector2& pa, const MT_Vector2 &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;
}

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(MT_Scalar levelHeight, bool enableVisualization)
:	m_levelHeight(levelHeight)
,	m_enableVisualization(enableVisualization)
{

}

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();
}
KX_Obstacle* KX_ObstacleSimulation::CreateObstacle(KX_GameObject* gameobj)
{
	KX_Obstacle* obstacle = new KX_Obstacle();
	obstacle->m_gameObj = gameobj;

	vset(obstacle->vel, 0,0);
	vset(obstacle->pvel, 0,0);
	vset(obstacle->dvel, 0,0);
	vset(obstacle->nvel, 0,0);
	for (int i = 0; i < VEL_HIST_SIZE; ++i)
		vset(&obstacle->hvel[i*2], 0,0);
	obstacle->hhead = 0;

	gameobj->RegisterObstacle(this);
	m_obstacles.push_back(obstacle);
	return obstacle;
}

void KX_ObstacleSimulation::AddObstacleForObj(KX_GameObject* gameobj)
{
	KX_Obstacle* obstacle = CreateObstacle(gameobj);
	struct Object* blenderobject = gameobj->GetBlenderObject();
	obstacle->m_type = KX_OBSTACLE_OBJ;
	obstacle->m_shape = KX_OBSTACLE_CIRCLE;
	obstacle->m_rad = blenderobject->obstacleRad;
}

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(navmeshobj);
				obstacle->m_type = KX_OBSTACLE_NAV_MESH;
				obstacle->m_shape = KX_OBSTACLE_SEGMENT;
				obstacle->m_pos = MT_Point3(vj[0], vj[2], vj[1]);
				obstacle->m_pos2 = MT_Point3(vi[0], vi[2], vi[1]);
				obstacle->m_rad = 0;
			}
		}
	}
}

void KX_ObstacleSimulation::DestroyObstacleForObj(KX_GameObject* gameobj)
{
	for (size_t i=0; i<m_obstacles.size(); )
	{
		if (m_obstacles[i]->m_gameObj == gameobj)
		{
			KX_Obstacle* obstacle = m_obstacles[i];
			obstacle->m_gameObj->UnregisterObstacle();
			m_obstacles[i] = m_obstacles.back();
			m_obstacles.pop_back();
			delete obstacle;
		}
		else
			i++;
	}
}

void KX_ObstacleSimulation::UpdateObstacles()
{
	for (size_t i=0; i<m_obstacles.size(); i++)
	{
		if (m_obstacles[i]->m_type==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->vel[0] = obs->m_gameObj->GetLinearVelocity().x();
		obs->vel[1] = obs->m_gameObj->GetLinearVelocity().y();

		// Update velocity history and calculate perceived (average) velocity.
		copy_v2_v2(&obs->hvel[obs->hhead * 2], obs->vel);
		obs->hhead = (obs->hhead+1) % VEL_HIST_SIZE;
		vset(obs->pvel,0,0);
		for (int j = 0; j < VEL_HIST_SIZE; ++j)
			add_v2_v2v2(obs->pvel, obs->pvel, &obs->hvel[j * 2]);
		mul_v2_fl(obs->pvel, 1.0f / VEL_HIST_SIZE);
	}
}

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, KX_NavMeshObject* activeNavMeshObj, 
										MT_Vector3& velocity, MT_Scalar maxDeltaSpeed,MT_Scalar maxDeltaAngle)
{
}

void KX_ObstacleSimulation::DrawObstacles()
{
	if (!m_enableVisualization)
		return;
	static const MT_Vector3 bluecolor(0,0,1);
	static const MT_Vector3 normal(0.0, 0.0, 1.0);
	static const int SECTORS_NUM = 32;
	for (size_t i=0; i<m_obstacles.size(); i++)
	{
		if (m_obstacles[i]->m_shape==KX_OBSTACLE_SEGMENT)
		{
			MT_Point3 p1 = m_obstacles[i]->m_pos;
			MT_Point3 p2 = m_obstacles[i]->m_pos2;
			//apply world transform
			if (m_obstacles[i]->m_type == KX_OBSTACLE_NAV_MESH)
			{
				KX_NavMeshObject* navmeshobj = static_cast<KX_NavMeshObject*>(m_obstacles[i]->m_gameObj);
				p1 = navmeshobj->TransformToWorldCoords(p1);
				p2 = navmeshobj->TransformToWorldCoords(p2);
			}

			KX_RasterizerDrawDebugLine(p1, p2, bluecolor);
		}
		else if (m_obstacles[i]->m_shape==KX_OBSTACLE_CIRCLE)
		{
			KX_RasterizerDrawDebugCircle(m_obstacles[i]->m_pos, m_obstacles[i]->m_rad, bluecolor,
			                             normal, SECTORS_NUM);
		}
	}
}

static MT_Point3 nearestPointToObstacle(MT_Point3& pos ,KX_Obstacle* obstacle)
{
	switch (obstacle->m_shape)
	{
	case KX_OBSTACLE_SEGMENT :
	{
		MT_Vector3 ab = obstacle->m_pos2 - obstacle->m_pos;
		if (!ab.fuzzyZero())
		{
			const MT_Scalar dist = ab.length();
			MT_Vector3 abdir = ab.normalized();
			MT_Vector3  v = pos - obstacle->m_pos;
			MT_Scalar proj = abdir.dot(v);
			CLAMP(proj, 0, dist);
			MT_Point3 res = obstacle->m_pos + abdir*proj;
			return res;
		}
	}
	case KX_OBSTACLE_CIRCLE :
	default:
		return obstacle->m_pos;
	}
}

static bool filterObstacle(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavMeshObj, KX_Obstacle* otherObst,
							float levelHeight)
{
	//filter obstacles by type
	if ( (otherObst == activeObst) ||
		(otherObst->m_type==KX_OBSTACLE_NAV_MESH && otherObst->m_gameObj!=activeNavMeshObj)	)
		return false;

	//filter obstacles by position
	MT_Point3 p = nearestPointToObstacle(activeObst->m_pos, otherObst);
	if ( fabs(activeObst->m_pos.z() - p.z()) > levelHeight)
		return false;

	return true;
}

///////////*********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(1.0f)
{
}


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
	sampleRVO(activeObst, activeNavMeshObj, maxDeltaAngle);

	// Fake dynamic constraint.
	float dv[2];
	float vel[2];
	sub_v2_v2v2(dv, activeObst->nvel, activeObst->vel);
	float ds = len_v2(dv);
	if (ds > maxDeltaSpeed || ds<-maxDeltaSpeed)
		mul_v2_fl(dv, fabs(maxDeltaSpeed / ds));
	add_v2_v2v2(vel, activeObst->vel, dv);

	velocity.x() = vel[0];
	velocity.y() = vel[1];
}

///////////*********TOI_rays**********/////////////////
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)
};

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;
}


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 (len_v2(ob->vel) < 0.01f * 0.01f) {
					// Stationary, use VO
					vab = svel;
				}
				else
				{
					// Moving, use RVO
					vab = 2*svel - vel - ob->vel;
				}

				if (!sweepCircleCircle(MT_3D_AS_2D(activeObst->m_pos), activeObst->m_rad,
				                       vab, MT_3D_AS_2D(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(MT_3D_AS_2D(activeObst->m_pos), activeObst->m_rad, svel,
				                        MT_3D_AS_2D(p1), MT_3D_AS_2D(p2), ob->m_rad, htmin, htmax))
				{
					continue;
				}
			}
			else {
				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 (len_v2(activeObst->vel) > 0.1f) {
		// 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,
                           float maxToi, float velWeight, float curVelWeight, float sideWeight,
                           float toiWeight)
{
	vset(res, 0,0);

	const float ivmax = 1.0f / vmax;

	float adir[2] /*, adist */;
	if (normalize_v2_v2(adir, activeObst->pvel) <= 0.01f) {
		zero_v2(adir);
	}

	float activeObstPos[2];
	vset(activeObstPos, activeObst->m_pos.x(), activeObst->m_pos.y()); 
	/* adist = vdot(adir, activeObstPos); */

	float minPenalty = FLT_MAX;

	for (int n = 0; n < nspos; ++n)
	{
		float vcand[2];
		copy_v2_v2(vcand, &spos[n * 2]);

		// Find min time of impact and exit amongst all obstacles.
		float tmin = maxToi;
		float side = 0;
		int nside = 0;

		for (int i = 0; i < obstacles.size(); ++i)
		{
			KX_Obstacle* ob = obstacles[i];
			bool res = filterObstacle(activeObst, activeNavMeshObj, ob, levelHeight);
			if (!res)
				continue;
			float htmin, htmax;

			if (ob->m_shape==KX_OBSTACLE_CIRCLE)
			{
				float vab[2];

				// Moving, use RVO
				mul_v2_v2fl(vab, vcand, 2);
				sub_v2_v2v2(vab, vab, activeObst->vel);
				sub_v2_v2v2(vab, vab, ob->vel);

				// Side
				// NOTE: dp, and dv are constant over the whole calculation,
				// they can be precomputed per object. 
				const float* pa = activeObstPos;
				float pb[2];
				vset(pb, ob->m_pos.x(), ob->m_pos.y());

				const float orig[2] = {0, 0};
				float dp[2], dv[2], np[2];
				sub_v2_v2v2(dp, pb, pa);
				normalize_v2(dp);
				sub_v2_v2v2(dv, ob->dvel, activeObst->dvel);

				/* TODO: use line_point_side_v2 */
				if (area_tri_signed_v2(orig, dp, dv) < 0.01f) {
					np[0] = -dp[1];
					np[1] = dp[0];
				}
				else {
					np[0] = dp[1];
					np[1] = -dp[0];
				}

				side += clamp(min(dot_v2v2(dp, vab),
				                  dot_v2v2(np, vab)) * 2.0f, 0.0f, 1.0f);
				nside++;

				if (!sweepCircleCircle(MT_3D_AS_2D(activeObst->m_pos), activeObst->m_rad,
				                       vab, MT_3D_AS_2D(ob->m_pos), ob->m_rad, htmin, htmax))
				{
					continue;
				}

				// Handle overlapping obstacles.
				if (htmin < 0.0f && htmax > 0.0f)
				{
					// Avoid more when overlapped.
					htmin = -htmin * 0.5f;
				}
			}
			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);
				}
				float p[2], q[2];
				vset(p, p1.x(), p1.y());
				vset(q, p2.x(), p2.y());

				// NOTE: the segments are assumed to come from a navmesh which is shrunken by
				// the agent radius, hence the use of really small radius.
				// This can be handle more efficiently by using seg-seg test instead.
				// If the whole segment is to be treated as obstacle, use agent->rad instead of 0.01f!
				const float r = 0.01f; // agent->rad
				if (dist_squared_to_line_segment_v2(activeObstPos, p, q) < sqr(r + ob->m_rad)) {
					float sdir[2], snorm[2];
					sub_v2_v2v2(sdir, q, p);
					snorm[0] = sdir[1];
					snorm[1] = -sdir[0];
					// If the velocity is pointing towards the segment, no collision.
					if (dot_v2v2(snorm, vcand) < 0.0f)
						continue;
					// Else immediate collision.
					htmin = 0.0f;
					htmax = 10.0f;
				}
				else
				{
					if (!sweepCircleSegment(activeObstPos, r, vcand, p, q, ob->m_rad, htmin, htmax))
						continue;
				}

				// Avoid less when facing walls.
				htmin *= 2.0f;
			}
			else {
				continue;
			}

			if (htmin >= 0.0f)
			{
				// The closest obstacle is somewhere ahead of us, keep track of nearest obstacle.
				if (htmin < tmin)
					tmin = htmin;
			}
		}

		// Normalize side bias, to prevent it dominating too much.
		if (nside)
			side /= nside;

		const float vpen = velWeight * (len_v2v2(vcand, activeObst->dvel) * ivmax);
		const float vcpen = curVelWeight * (len_v2v2(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;

		if (penalty < minPenalty) {
			minPenalty = penalty;
			copy_v2_v2(res, vcand);
		}
	}
}

void KX_ObstacleSimulationTOI_cells::sampleRVO(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavMeshObj, 
					   const float maxDeltaAngle)
{
	vset(activeObst->nvel, 0.f, 0.f);
	float vmax = len_v2(activeObst->dvel);

	float* spos = new float[2*m_maxSamples];
	int nspos = 0;

	if (!m_adaptive)
	{
		const float cvx = activeObst->dvel[0]*m_bias;
		const float cvy = activeObst->dvel[1]*m_bias;
		float vmax = len_v2(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)
		{
			for (int x = -m_sampleRadius; x <= m_sampleRadius; ++x)
			{
				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);
	}
	else
	{
		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)
		{
			const float half = (rad-1)*cs*0.5f;

			nspos = 0;
			for (int y = 0; y < rad; ++y)
			{
				for (int x = 0; x < rad; ++x)
				{
					const float v_xy[2] = {
					    res[0] + x * cs - half,
					    res[1] + y * cs - half};

					if (len_squared_v2(v_xy) > sqr(vmax + cs / 2))
						continue;

					copy_v2_v2(&spos[nspos * 2 + 0], v_xy);
					nspos++;
				}
			}

			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;
		}
		copy_v2_v2(activeObst->nvel, res);
	}

	delete [] spos;
}

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)
{
	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
}