/*=====================================================================*\
  Friday December 7th 2012
  Arash HABIBI
  agent.cpp
  New behaviour algorithm based on a dynamic model
\*=====================================================================*/


#include "agent.h"
#include "simulator.h"

unsigned int Agent::maxNeighbors_ = 10 ;
unsigned int Agent::maxMovingObstacles_ = 10;
float Agent::averageMaxSpeed_ = 2.0f ;
// float Agent::averageMaxSpeed_ = 20.0f ;
float Agent::neighborDist_ = 10.0f ;
float Agent::neighborDistSq_ = neighborDist_ * neighborDist_ ;
float Agent::radius_ = 1.5f ;
//float Agent::timeHorizon_ = 10.0f ;
float Agent::timeHorizon_ = 100.0f ;
float Agent::timeHorizonObst_ = 10.0f ;
float Agent::range_ = 2*timeHorizonObst_ * averageMaxSpeed_ + radius_ ;
float Agent::rangeSq_ = range_ * range_ ;

unsigned int Agent::cptAgent = 0 ;

Agent::Agent(Simulator* sim, const VEC3& start, const VEC3& goal, Dart d) :
#ifdef SPATIAL_HASHING
	pos(position),
#else
	part_(sim->envMap_.map, d, start, sim->envMap_.position),
#endif
	curGoal_(0),
	velocity_(0),
	newVelocity_(0),
	prefVelocity_(0),
	meanDirection_(0),
	sim_(sim),
	alive(true)
{
	init(start,goal);
}

Agent::Agent(Simulator* sim, const VEC3& start, const VEC3& goal) :
#ifdef SPATIAL_HASHING
	pos(position),
#else
	part_(sim->envMap_.map, sim->envMap_.getBelongingCell(start), start, sim->envMap_.position),
#endif
	curGoal_(0),
	velocity_(0),
	newVelocity_(0),
	prefVelocity_(0),
	meanDirection_(0),
	sim_(sim),
	alive(true)
{
	init(start,goal);
}

void Agent::init(const VEC3& start, const VEC3& goal)
{
#ifdef SPATIAL_HASHING
	sim->envMap_.addAgentInGrid(this) ;
#endif
	goals_.clear() ;
	goals_.push_back(goal) ;
	goals_.push_back(start) ;
	float ratio = (80.0f + rand() % 20) / 100.0f ;
	maxSpeed_ = averageMaxSpeed_ * ratio ;	// from 80% to 100% of averageMaxSpeed_
	color1 = 1.0f * ratio ;					// red = high speed agents
	color2 = 1.0f * 5 * (1 - ratio) ;		// green = low speed agents
	color3 = 0 ;
	for (unsigned int i=0 ; i<4 ; ++i) meanVelocity_[i].set(0) ;
	agentNeighbors_.reserve(maxNeighbors_ * 2) ;
	obstacleNeighbors_.reserve(maxNeighbors_ * 2) ;
	movingObstacleNeighbors_.reserve(maxNeighbors_ * 2) ;
	movingObstacles_ = new MovingObstacle* [maxMovingObstacles_];
	nb_mos=0;
	agentNo = cptAgent++ ;
}


//-----------------------------------------------------------------

//-----------------------------------------------------------------

//-----------------------------------------------------------------

VEC3 Agent::getPosition()
{
#ifdef SPATIAL_HASHING
	return pos ;
#else
	return part_.getPosition() ;
#endif
}

//-----------------------------------------------------------------
//-----------------------------------------------------------------

bool agentSort(const std::pair<float, Agent*>& a1, const std::pair<float, Agent*>& a2)
{
	return a1.first < a2.first ;
}

//-----------------------------------------------------------------

bool obstacleSort(const std::pair<float, Obstacle*>& o1, const std::pair<float, Obstacle*>& o2)
{
	return o1.first < o2.first ;
}

//-----------------------------------------------------------------

void Agent::updateAgentNeighbors()
{
	agentNeighbors_.clear() ;

#ifdef SPATIAL_HASHING
	const std::vector<Agent*>& agents = sim_->envMap_.getNeighbors(this) ;
	const std::vector<Agent*>& neighborAgents =	sim_->envMap_.getNeighborAgents(this);
#else
	const std::vector<Agent*>& agents = sim_->envMap_.agentvect[part_.d] ;
	const std::vector<Agent*>& neighborAgents = sim_->envMap_.neighborAgentvect[part_.d] ;
#endif

	float maxDist = 0.0f ;

	for (std::vector<Agent*>::const_iterator it = agents.begin(); it != agents.end(); ++it)
	{
		if ((*it)->alive)
		{
			if (*it != this)
			{
				float distSq = (getPosition() - (*it)->getPosition()).norm2() ;
				if ((agentNeighbors_.size() < maxNeighbors_ || distSq < maxDist)
				    && distSq < neighborDistSq_)
				{
					if (distSq > maxDist) maxDist = distSq ;
					agentNeighbors_.push_back(std::make_pair(distSq, *it)) ;
				}
			}
		}
	}

	for (std::vector<Agent*>::const_iterator it = neighborAgents.begin(); it != neighborAgents.end(); ++it)
	{
		if ((*it)->alive)
		{
			float distSq = (getPosition() - (*it)->getPosition()).norm2() ;
			if ((agentNeighbors_.size() < maxNeighbors_ || distSq < maxDist)
			    && distSq < neighborDistSq_)
			{
				if (distSq > maxDist)
					maxDist = distSq ;
				agentNeighbors_.push_back(std::make_pair(distSq, *it)) ;
			}
		}

		sim_->nbUpdates++ ;
		sim_->nearNeighbors += agentNeighbors_.size() ;
		sim_->totalNeighbors += agents.size() + neighborAgents.size() ;


	}
	if (agentNeighbors_.size() > maxNeighbors_)
	{
		sim_->nbSorts++ ;
		std::sort(agentNeighbors_.begin(), agentNeighbors_.end(), agentSort) ;
	}
}

//-----------------------------------------------------------------
//-----------------------------------------------------------------

void Agent::updateObstacleNeighbors()
{
	obstacleNeighbors_.clear() ;
	movingObstacleNeighbors_.clear() ;

#ifdef SPATIAL_HASHING
	Geom::Vec2ui c = sim_->envMap_.obstaclePositionCell(pos) ;
	if(sim_->envMap_.ht_obstacles.hasData(c))
	{
		const std::vector<Obstacle*>& obst = sim_->envMap_.ht_obstacles[c] ;
		for(std::vector<Obstacle*>::const_iterator it = obst.begin() ; it != obst.end() ; ++it)
		{
			float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, pos) ;
			if(distSq < rangeSq_)
			{
				if(Geom::testOrientation2D(pos, (*it)->p1, (*it)->p2) == Geom::RIGHT)
					obstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
			}
		}
	}
#else
	std::vector<Obstacle*>& obst = sim_->envMap_.obstvect[part_.d] ;
	std::vector<Obstacle*>& neighborObst = sim_->envMap_.neighborObstvect[part_.d] ;
	float maxDistObst = 0.0f ;
	float maxDistMovingObst = 0.0f ;

	for(std::vector<Obstacle*>::const_iterator it = obst.begin() ; it != obst.end() ; ++it)
	{
		if ((*it)->mo==NULL)
		{
			float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, part_.getPosition()) ;
			if ((obstacleNeighbors_.size() < maxNeighbors_ || distSq < maxDistObst)
				    && distSq < rangeSq_)
			{
				if (Geom::testOrientation2D(part_.getPosition(), (*it)->p1, (*it)->p2) == Geom::RIGHT)
				{

					if (distSq > maxDistObst)
						maxDistObst = distSq ;
					obstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;

				}

			}
		}
		else
		{
			float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, part_.getPosition()) ;
			if ((movingObstacleNeighbors_.size() < maxNeighbors_ || distSq < maxDistMovingObst)
					&& distSq < rangeSq_)
			{
//				if (Geom::testOrientation2D(part_.getPosition(), (*it)->p1, (*it)->p2) == Geom::RIGHT)
				{

					if (distSq > maxDistMovingObst)
						maxDistMovingObst = distSq ;
					movingObstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;

				}

			}
		}
	}

	for(std::vector<Obstacle*>::const_iterator it = neighborObst.begin() ; it != neighborObst.end() ; ++it)
	{
		if ((*it)->mo==NULL)
				{
					float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, part_.getPosition()) ;
					if ((obstacleNeighbors_.size() < maxNeighbors_ || distSq < maxDistObst)
						    &&distSq < rangeSq_)
					{
						if (Geom::testOrientation2D(part_.getPosition(), (*it)->p1, (*it)->p2) == Geom::RIGHT)
						{

							if (distSq > maxDistObst) maxDistObst = distSq ;
								obstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;

						}

					}
				}
				else
				{
					float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, part_.getPosition()) ;
					if ((movingObstacleNeighbors_.size() < maxNeighbors_ || distSq < maxDistMovingObst)
							&&distSq < rangeSq_)
					{
//						if (Geom::testOrientation2D(part_.getPosition(), (*it)->p1, (*it)->p2) == Geom::RIGHT)
						{

							if (distSq > maxDistMovingObst) maxDistMovingObst = distSq ;
							movingObstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;

						}

					}
				}
	}

//	if (obstacleNeighbors_.size() > maxNeighbors_)
//	{
//		sim_->nbSorts++ ;
//		std::sort(obstacleNeighbors_.begin(), obstacleNeighbors_.end(), obstacleSort) ;
//	}
//
//	if (movingObstacleNeighbors_.size() > maxNeighbors_)
//	{
//		sim_->nbSorts++ ;
//		std::sort(movingObstacleNeighbors_.begin(), movingObstacleNeighbors_.end(), obstacleSort) ;
//	}



#endif

//	if (obstacleNeighbors_.size() > maxNeighbors_)
//		std::sort(obstacleNeighbors_.begin(), obstacleNeighbors_.end(), obstacleSort) ;
//	CGoGNout<<"obstacles perçus : "<<CGoGNendl;
//	for (std::vector<std::pair<float, Obstacle*> >::iterator it = movingObstacleNeighbors_.begin() ;
//			it != movingObstacleNeighbors_.end() ;
//			++it)
//	{
//		(*it).second->mo->color1=0.0f;
//		(*it).second->mo->color2=0.0f;
//		(*it).second->mo->seen=true;
//		CGoGNout<<"obstacle : "<< (*it).second->mo->index<<CGoGNendl;
//
//	}
//	for (int it= 0; it < nb_mos; it ++)
//		{
//			movingObstacles_[it]->color1=0.0f;
//			movingObstacles_[it]->color2=0.0f;
//			movingObstacles_[it]->seen =true;
//		}
}

//-----------------------------------------------------------------
//-----------------------------------------------------------------

void Agent::update()
{
	if (alive)
	{
		finalGoal = goals_[curGoal_] ;
		velocity_ = newVelocity_ ;
#ifdef SPATIAL_HASHING
		pos = pos + (velocity_ * sim_->timeStep_) ;
#else
		VEC3 target = part_.getPosition() + (velocity_ * sim_->timeStep_) ;
		part_.move(target) ;
#endif

		meanDirection_.set(0) ;
		for (unsigned int i=0 ; i<4 ; ++i) {
			meanDirection_ += meanVelocity_[i] ;
			meanVelocity_[i] = meanVelocity_[(i+1)%4] ;
		}
		meanVelocity_[3] = velocity_ ;
		meanDirection_.normalize() ;
	}
}

//-----------------------------------------------------------------
//-----------------------------------------------------------------

void Agent::computeNewVelocity2()
{
	VEC3 centroid ;
	VEC3 c ;
	VEC3 vel ;

	float cohesion = 1.0f ;

	centroid.zero() ;
	c.zero() ;
	vel.zero() ;

	for (std::vector<std::pair<float, Obstacle*> >::iterator it = obstacleNeighbors_.begin() ;it != obstacleNeighbors_.end() ; ++it)
	{
	}

	unsigned int nbA = 0 ;
	for (std::vector<std::pair<float, Agent*> >::iterator it = agentNeighbors_.begin() ;
		it != agentNeighbors_.end() && nbA < maxNeighbors_ ; ++it, ++nbA)
	{
		VEC3 mPos = getPosition() ;
		VEC3 oPos = (*it).second->getPosition() ;
		centroid += oPos ;

		VEC3 pOp(oPos - mPos) ;
		if (pOp.norm2() <= radius_ * radius_ * maxSpeed_ * maxSpeed_ * 2.0f)
		{
			c -= pOp ;
//			prefVelocity_.zero();
		}

		vel += (*it).second->prefVelocity_ ;
	}

	if (nbA > 0)
	{
		centroid /= nbA ;
		centroid -= getPosition() ;
		centroid /= 10 ;

		vel /= nbA ;
		vel -= prefVelocity_ ;
		vel /= 8 ;
	}

	newVelocity_ = prefVelocity_ + cohesion * centroid + c + vel ;
}



///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////comportement à modifier///////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////


//-----------------------------------------------------------------
// fonction d'évitement d'obstacles par les agents (calcul effectué
// avant de lancer RVO2 pour garantir la non-collision entre agents)
// En gros, on représente un obstacle :
// soit par un ensemble de segments orientés dans le sens direct (sim_->avoidance==0),
// soit par son centre (sim_->avoidance==1)
// Cette fonction parcourt les obstacles et calcule la norme de la force globale de
// répulsion. Ensuite, elle ajoute cette somme à l'objectif de façon
// à ce que l'agent change sa trajectoire.

void Agent::obstacle_priority(PFP::VEC3 * goalVector)
{
	float factor ;
	Obstacle* obst ;
	PFP::VEC3 x,y,vec ;
	PFP::VEC3 norm,sumNorm ;
	sumNorm.zero() ;

	// On parcourt les obstacles
	for(std::vector<std::pair<float, Obstacle*> >::iterator it = movingObstacleNeighbors_.begin() ;
		it != movingObstacleNeighbors_.end() ;
		++it)
	{
		// rangeSq = range * range (portee au carre)
		// les elements de l'iteration sont des obstacles.
		// chaque obstacle est une paire : float et *obstacle.
		// le float est la distance à l'obstacle et *obstacle
		// est un pointeur vers le segment.

		factor =(1.0f-(it->first/rangeSq_)) ;
		// En particulier ici, (*it).first (ou it->first)
		// represente la distance à l'agent (?)
		// factor est quelque chose qui faut 0
		// quand l'agent commence à interagir avec l'obstacle
		// et qui faut 1 quand l'agent se trouve colle sur l'obstacle.

//		CGoGNout<<factor<<" par rapport à "<<(*it).first<< " ayant un range de "<< rangeSq_<<CGoGNendl;
		obst=it->second ;
		x=obst->p1 ;
		y=obst->p2 ;
		vec=y-x ;
		vec.normalize() ;
		norm.zero() ;

		if (sim_->avoidance==0)// avoids with normal of obstacle side
		{
			norm[0]=vec[1] ;
			norm[1]=-vec[0] ;
		}
		else if (sim_->avoidance==1) // avoids with direction from center of the obstacle
		{
			MovingObstacle * mo = obst->mo;
			norm = this->part_.getPosition()-mo->center;
		}
//		else if (sim_->avoidance==2) // avoids with opposite direction of obstacle
//		{
//			MovingObstacle * mo = obst->mo;
//			norm = this->part_.getPosition()-mo->center;
//		}
		norm.normalize() ;
		norm*=5.0f*factor*factor*factor*factor*factor*factor*factor*factor ;
		// On met factor à la puissance 8 pour adoucir l'impact.
		sumNorm+=norm ;
	}

//	if (sumNorm.norm2()>1.0f) sumNorm.normalize();
//	if(sumNorm!=0)CGoGNout<<sumNorm<<"goal vector before :" <<(*goalVector)<< " goal vector after : "<<((*goalVector)+sumNorm)<<CGoGNendl;
	//(*goalVector)=((*goalVector)*9.0f+11.0f*sumNorm)/20.0f;
	(*goalVector)=((*goalVector)+sumNorm) ;
	//(*goalVector).normalize();
}

//-----------------------------------------------------------------
// Calcul vitesse optimale pour atteindre l'objectif (sans prendre
// en compte l'environnement).

void Agent::computePrefVelocity()
{
	VEC3 goalVector = goals_[curGoal_] - getPosition() ;
	float goalDist2 = goalVector.norm2() ;

	// Si l'agent arrive à proximité de l'objectif,
	// alors il passe à l'objectif suivant

	if (goalDist2 < radius_*radius_)
	{
		curGoal_ = (curGoal_ + 1) % goals_.size() ;
		goalVector = goals_[curGoal_] - getPosition() ;
		goalDist2 = goalVector.norm2() ;
	}

	// goadDist2 est le vecteur reliant l'agent à l'objectif.
	// Tant que ce vecteur a une norme supérieure à maxSpeed_
	// (i.e. tant qu'il est encore assez loin de l'objectif)
	// la vitesse sera de maxSpeed_. Sinon, s'il est plus
	// proche, la vitesse diminue.

	if (goalDist2 > maxSpeed_)
	{
		goalVector.normalize() ;
		goalVector *= maxSpeed_;
	}

	prefVelocity_ = goalVector ;
}

//-----------------------------------------------------------------

static int moAppend(MovingObstacle **moving_obstacles, int nb_mos, int max, MovingObstacle *mo)
{
	unsigned char already_processed=false;
	int mo_count;

	// if(nb_mos>=max-1)
	if(nb_mos>=max)
	{
		// cerr << "Agent::moPush : the maximum number of moving obstacles is assumed to be " << max << endl;
		// cerr << "There is visibly a need for more." << endl;
	}
	else
	{
		for(mo_count=0;
			(mo_count<nb_mos)&&(already_processed==false);
			mo_count++)
			if(moving_obstacles[mo_count] == mo)
				already_processed = true;

		if(mo_count==nb_mos)
		{
			moving_obstacles[mo_count] = mo;
			nb_mos++;
		}
	}
	return(nb_mos);
}

//-----------------------------------------------------------------
//-----------------------------------------------------------------
//-----------------------------------------------------------------
// Search for the best new velocity.

void Agent::computeNewVelocity()
{
	// The objective is to compute the sum of forces exerted on the agent.
	double collision_softening_factor;
	float ag_ambient_damping = 1.0;
	double mass_var = 0.95;
	double average_mass = 1.0;

	srand48(agentNo);
	double rand = 2.0*drand48()-1.0; // compris entre -1 et 1
	// double ag_mass = average_mass + rand*rand*rand*mass_var; // valeurs plus denses autour de la moyenne
	double ag_mass = average_mass + rand*mass_var; // valeurs uniformement réparties entre min et max

	//-------------

	VEC3 vec, forces, previous_pos;
	forces.zero();

	previous_pos = getPosition() - velocity_*sim_->timeStep_;

	//----- force due à l'attraction de l'objectif ----------

	float goal_attraction_force = 200.0;  // agent-goal interaction stiffness
	VEC3 p_goal = goals_[curGoal_];
	VEC3 u_goal = p_goal - getPosition();
	u_goal.normalize();

	cerr << agentNo << "---------------- position = " << getPosition() << endl;

	forces += goal_attraction_force * u_goal;

	cerr << "forces = " << forces << endl;

	//----- forces dues à la répulsion des obstacles en mouvement ----------

	VEC3 norm;
	// double obst_stiffness = 10000.0; // agent-obstacle interaction stiffness
	double obst_stiffness = 1000.0; // agent-obstacle interaction stiffness
	// double obst_damping = 1.0;  // agent-obstacle interaction damping
	int obst_power = 2;           // the power to which elevate the agent-obstacle distance
	Obstacle* obst ;

	nb_mos = 0;
	for(std::vector<std::pair<float, Obstacle*> >::iterator it = movingObstacleNeighbors_.begin() ;
		it != movingObstacleNeighbors_.end() ;
		++it)
	{
		obst=it->second;
		nb_mos=moAppend(movingObstacles_,nb_mos,maxMovingObstacles_,obst->mo);
	}

	MovingObstacle *mo;
	float force_value;
	int mo_count;
	for(mo_count=0;
		mo_count<nb_mos;
		mo_count++)
	{
		mo = movingObstacles_[mo_count];
		float dist = (getPosition()-mo->focus2).norm() + (getPosition()-mo->focus1).norm();
		// double effective_range = 3*mo->sum_dist_foci;
		double effective_range = mo->sum_dist_foci;
		if(dist <= effective_range)
		{
			collision_softening_factor = pow(1 - dist/effective_range,obst_power);
			force_value = obst_stiffness*collision_softening_factor*(effective_range-dist);

			norm.zero();
			// norm = (getPosition()-mo->focus2) + (getPosition()-mo->focus1);
			// norm.normalize();
			VEC3 vec1 = getPosition()-mo->focus1;
			vec1.normalize();
			VEC3 vec2 = getPosition()-mo->focus2;
			vec2.normalize();
			norm = vec1 + vec2;
			norm.normalize();

			forces += force_value * norm;
		}
	}

	//----- forces dues à la répulsion des obstacles fixes ----------

	// double fixed_obst_stiffness = 50000.0; // agent-obstacle interaction stiffness
	double fixed_obst_stiffness = 50000.0; // agent-obstacle interaction stiffness
	// double fixed_obst_damping = 1.0;  // agent-obstacle interaction damping
	int fixed_obst_power = 1;           // the power to which elevate the agent-obstacle distance
	Obstacle* fixed_obst ;

	int nobst=0;

	for(std::vector<std::pair<float, Obstacle*> >::iterator it = obstacleNeighbors_.begin() ;
		it != obstacleNeighbors_.end() ;
		++it)
	{
		double dist = it->first;
		// cerr << "nobst=" << nobst << "dist=" << dist << endl;
		// double effective_range = 50*range_;
		double effective_range = 10*range_;
		float force_value=0.0;
		if(dist < effective_range)
		{
			collision_softening_factor = pow(1 - dist/effective_range,fixed_obst_power);
			force_value = fixed_obst_stiffness*collision_softening_factor*(effective_range-dist);
		}

		fixed_obst=it->second ;
		VEC3 p1=fixed_obst->p1 ;
		VEC3 p2=fixed_obst->p2 ;
		vec=p2-p1;
		vec.normalize();
		norm.zero();

		norm[0]=vec[1] ;
		norm[1]=-vec[0] ;
		forces += force_value * norm;
		cerr << "obstacles fixes : nobst=" << nobst << " dist=" << dist << " force=" << force_value*norm << endl;
		// cerr << "force_value = " << force_value << endl;
		// cerr << "norm=" << norm << endl;

		nobst++;
	}

	//----- forces dues à la répulsion des autres agents -------

	// double ag_stiffness = 500.0;    // agent-agent interaction stiffness
	double ag_stiffness = 500.0;    // agent-agent interaction stiffness
	double ag_damping   = 1.0;     // agent-agent interaction damping
	double ag_phys_radius_coef = 20.0;
	double ag_power = 1;        // the power to which elevate the agent-agent distance

	unsigned int nbA = 0 ;

	for (std::vector<std::pair<float, Agent*> >::iterator it = agentNeighbors_.begin() ;
		it != agentNeighbors_.end() && nbA < maxNeighbors_ ; ++it, ++nbA)
	{
		Agent* other = it->second ;

		const VEC3 relativePosition(other->getPosition()-getPosition());
		const float distSq = relativePosition.norm2() ;
		const float dist   = sqrt(distSq);
		const float combinedRadius = ag_phys_radius_coef*(radius_ + other->radius_) ;
		// const float combinedRadiusSq = combinedRadius * combinedRadius ;

		VEC3 other_previous_pos = other->getPosition() - (other->velocity_*sim_->timeStep_);
		VEC3 previous_relativePosition = other_previous_pos - previous_pos;
		float previous_distSq = previous_relativePosition.norm2();
		float previous_dist = sqrt(previous_distSq);

		// const VEC3 u_other(relativePosition);
		VEC3 u_other(relativePosition);
		u_other = relativePosition;
		u_other.normalize();

		// cerr << "dist=" << dist << " combinedRadius=" << combinedRadius << endl;

		if(dist < combinedRadius)
		{
			collision_softening_factor = pow(1 - dist/combinedRadius,ag_power);
			float force_value = - ag_stiffness*collision_softening_factor*(combinedRadius-dist)
				- ag_damping * (dist - previous_dist) / sim_->timeStep_;

			cerr << "autres agents : force_value = " << force_value << endl;

			forces += force_value * u_other;
		}
	}

	//------- calcul de la trainee --------------------------------------

	// forces -= ag_ambient_damping * velocity_;

	//------- calcul de la nouvelle valeur de la vitesse ----------------

	VEC3 velocity_tmp;

	velocity_tmp = velocity_ + forces * (sim_->timeStep_ / ag_mass);
	if(velocity_tmp.norm2() > maxSpeed_*maxSpeed_)
	{
		velocity_tmp.normalize();
		velocity_tmp *= maxSpeed_;
	}
	newVelocity_ = velocity_tmp;

	//------- color depending on velocity -------------------------------

	double vmax = 0.5*maxSpeed_;
	VEC3 min_vx_color(1.0,0.0,0.0); // agents going towards the positive xs are red
	VEC3 max_vx_color(0.0,0.1,0.1); // agents going towards the negative xs are cyan
	VEC3 min_vy_color(0.0,1.0,0.0);
	VEC3 max_vy_color(1.0,0.0,1.1);

	double alpha_x = (newVelocity_[0]+vmax) / (2*vmax);
	double alpha_y = (newVelocity_[1]+vmax) / (2*vmax);

	double tmp_color1 =
		abs(alpha_x-0.5)*(alpha_x * max_vx_color[0] + (1-alpha_x)*min_vx_color[0]) +
		abs(alpha_y-0.5)*(alpha_y * max_vy_color[0] + (1-alpha_y)*min_vy_color[0]);
	double tmp_color2 =
		abs(alpha_x-0.5)*(alpha_x * max_vx_color[1] + (1-alpha_x)*min_vx_color[1]) +
		abs(alpha_y-0.5)*(alpha_y * max_vy_color[1] + (1-alpha_y)*min_vy_color[1]);
	double tmp_color3 =
		abs(alpha_x-0.5)*(alpha_x * max_vx_color[2] + (1-alpha_x)*min_vx_color[2]) +
		abs(alpha_y-0.5)*(alpha_y * max_vy_color[2] + (1-alpha_y)*min_vy_color[2]);

	float blend = 0.1;
	color1 = blend*tmp_color1 + (1-blend)*color1;
	color2 = blend*tmp_color2 + (1-blend)*color2;
	color3 = blend*tmp_color3 + (1-blend)*color3;
}

//-----------------------------------------------------------------

bool Agent::linearProgram1(const std::vector<Line>& lines, unsigned int lineNo, float radius, const VEC3& optVelocity, bool directionOpt, VEC3& result)
{
	return true ;
}

//-----------------------------------------------------------------

unsigned int Agent::linearProgram2(const std::vector<Line>& lines, float radius, const VEC3& optVelocity, bool directionOpt, VEC3& result)
{
	return 1;
}

//-----------------------------------------------------------------

void Agent::linearProgram3(const std::vector<Line>& lines, unsigned int numObstLines, unsigned int beginLine, float radius, VEC3& result)
{
}