agent.cpp 23 KB
Newer Older
Arash HABIBI's avatar
Arash HABIBI committed
1
2
3
4
5
6
7
8
/*=====================================================================*\
  Friday December 7th 2012
  Arash HABIBI
  agent.cpp
  New behaviour algorithm based on a dynamic model
\*=====================================================================*/


Pierre Kraemer's avatar
Pierre Kraemer committed
9
10
11
#include "agent.h"
#include "simulator.h"

12
unsigned int Agent::maxNeighbors_ = 10 ;
Arash HABIBI's avatar
Arash HABIBI committed
13
unsigned int Agent::maxMovingObstacles_ = 10;
David Cazier's avatar
David Cazier committed
14
float Agent::averageMaxSpeed_ = 2.0f ;
Arash HABIBI's avatar
Arash HABIBI committed
15
// float Agent::averageMaxSpeed_ = 20.0f ;
pitiot's avatar
maj    
pitiot committed
16
float Agent::neighborDist_ = 10.0f ;
17
18
float Agent::neighborDistSq_ = neighborDist_ * neighborDist_ ;
float Agent::radius_ = 1.5f ;
Arash HABIBI's avatar
Arash HABIBI committed
19
20
//float Agent::timeHorizon_ = 10.0f ;
float Agent::timeHorizon_ = 100.0f ;
21
float Agent::timeHorizonObst_ = 10.0f ;
pitiot's avatar
merging    
pitiot committed
22
float Agent::range_ = 2*timeHorizonObst_ * averageMaxSpeed_ + radius_ ;
23
24
float Agent::rangeSq_ = range_ * range_ ;

Pierre Kraemer's avatar
bla    
Pierre Kraemer committed
25
26
unsigned int Agent::cptAgent = 0 ;

27
Agent::Agent(Simulator* sim, const VEC3& start, const VEC3& goal, Dart d) :
Pierre Kraemer's avatar
Pierre Kraemer committed
28
#ifdef SPATIAL_HASHING
pitiot's avatar
maj    
pitiot committed
29
	pos(position),
30
31
32
33
#else
	part_(sim->envMap_.map, d, start, sim->envMap_.position),
#endif
	curGoal_(0),
pitiot's avatar
maj    
pitiot committed
34
35
36
	velocity_(0),
	newVelocity_(0),
	prefVelocity_(0),
37
	meanDirection_(0),
pitiot's avatar
maj    
pitiot committed
38
39
	sim_(sim),
	alive(true)
Pierre Kraemer's avatar
Pierre Kraemer committed
40
{
41
	init(start,goal);
Pierre Kraemer's avatar
Pierre Kraemer committed
42
}
43

David Cazier's avatar
David Cazier committed
44
Agent::Agent(Simulator* sim, const VEC3& start, const VEC3& goal) :
Pierre Kraemer's avatar
Pierre Kraemer committed
45
46
47
#ifdef SPATIAL_HASHING
	pos(position),
#else
David Cazier's avatar
David Cazier committed
48
	part_(sim->envMap_.map, sim->envMap_.getBelongingCell(start), start, sim->envMap_.position),
David Cazier's avatar
David Cazier committed
49
50
#endif
	curGoal_(0),
David Cazier's avatar
David Cazier committed
51
52
53
	velocity_(0),
	newVelocity_(0),
	prefVelocity_(0),
David Cazier's avatar
David Cazier committed
54
	meanDirection_(0),
pitiot's avatar
maj    
pitiot committed
55
56
	sim_(sim),
	alive(true)
Pierre Kraemer's avatar
bla    
Pierre Kraemer committed
57
{
58
59
60
61
	init(start,goal);
}

void Agent::init(const VEC3& start, const VEC3& goal)
Pierre Kraemer's avatar
bla    
Pierre Kraemer committed
62
{
David Cazier's avatar
David Cazier committed
63
64
65
#ifdef SPATIAL_HASHING
	sim->envMap_.addAgentInGrid(this) ;
#endif
David Cazier's avatar
David Cazier committed
66
67
68
	goals_.clear() ;
	goals_.push_back(goal) ;
	goals_.push_back(start) ;
David Cazier's avatar
David Cazier committed
69
70
71
72
	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
pitiot's avatar
maj    
pitiot committed
73
	color3 = 0 ;
David Cazier's avatar
David Cazier committed
74
	for (unsigned int i=0 ; i<4 ; ++i) meanVelocity_[i].set(0) ;
75
76
	agentNeighbors_.reserve(maxNeighbors_ * 2) ;
	obstacleNeighbors_.reserve(maxNeighbors_ * 2) ;
pitiot's avatar
maj    
pitiot committed
77
	movingObstacleNeighbors_.reserve(maxNeighbors_ * 2) ;
Arash HABIBI's avatar
Arash HABIBI committed
78
79
	movingObstacles_ = new MovingObstacle* [maxMovingObstacles_];
	nb_mos=0;
80
	agentNo = cptAgent++ ;
Pierre Kraemer's avatar
bla    
Pierre Kraemer committed
81
}
Pierre Kraemer's avatar
Pierre Kraemer committed
82

pitiot's avatar
merging    
pitiot committed
83

pitiot's avatar
merge    
pitiot committed
84
//-----------------------------------------------------------------
Pierre Kraemer's avatar
Pierre Kraemer committed
85

pitiot's avatar
merging    
pitiot committed
86
//-----------------------------------------------------------------
Pierre Kraemer's avatar
Pierre Kraemer committed
87

Arash HABIBI's avatar
Arash HABIBI committed
88
89
//-----------------------------------------------------------------

90
91
VEC3 Agent::getPosition()
{
Pierre Kraemer's avatar
Pierre Kraemer committed
92
93
94
#ifdef SPATIAL_HASHING
	return pos ;
#else
pitiot's avatar
maj    
pitiot committed
95
	return part_.getPosition() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
96
#endif
97
98
}

Arash HABIBI's avatar
Arash HABIBI committed
99
100
101
//-----------------------------------------------------------------
//-----------------------------------------------------------------

pitiot's avatar
maj    
pitiot committed
102
bool agentSort(const std::pair<float, Agent*>& a1, const std::pair<float, Agent*>& a2)
Pierre Kraemer's avatar
Pierre Kraemer committed
103
{
104
	return a1.first < a2.first ;
Pierre Kraemer's avatar
Pierre Kraemer committed
105
106
}

Arash HABIBI's avatar
Arash HABIBI committed
107
108
//-----------------------------------------------------------------

Pierre Kraemer's avatar
Pierre Kraemer committed
109
bool obstacleSort(const std::pair<float, Obstacle*>& o1, const std::pair<float, Obstacle*>& o2)
Pierre Kraemer's avatar
Pierre Kraemer committed
110
{
111
	return o1.first < o2.first ;
Pierre Kraemer's avatar
Pierre Kraemer committed
112
113
}

Arash HABIBI's avatar
Arash HABIBI committed
114
115
//-----------------------------------------------------------------

Pierre Kraemer's avatar
Pierre Kraemer committed
116
117
void Agent::updateAgentNeighbors()
{
118
	agentNeighbors_.clear() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
119
120

#ifdef SPATIAL_HASHING
pitiot's avatar
maj    
pitiot committed
121
	const std::vector<Agent*>& agents = sim_->envMap_.getNeighbors(this) ;
David Cazier's avatar
David Cazier committed
122
	const std::vector<Agent*>& neighborAgents =	sim_->envMap_.getNeighborAgents(this);
Pierre Kraemer's avatar
Pierre Kraemer committed
123
#else
124
125
	const std::vector<Agent*>& agents = sim_->envMap_.agentvect[part_.d] ;
	const std::vector<Agent*>& neighborAgents = sim_->envMap_.neighborAgentvect[part_.d] ;
Pierre Kraemer's avatar
Pierre Kraemer committed
126
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
127

128
	float maxDist = 0.0f ;
pitiot's avatar
maj    
pitiot committed
129

David Cazier's avatar
David Cazier committed
130
131
	for (std::vector<Agent*>::const_iterator it = agents.begin(); it != agents.end(); ++it)
	{
pitiot's avatar
maj    
pitiot committed
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
		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)
David Cazier's avatar
David Cazier committed
150
		{
151
152
			float distSq = (getPosition() - (*it)->getPosition()).norm2() ;
			if ((agentNeighbors_.size() < maxNeighbors_ || distSq < maxDist)
David Cazier's avatar
David Cazier committed
153
154
			    && distSq < neighborDistSq_)
			{
155
156
				if (distSq > maxDist)
					maxDist = distSq ;
157
158
				agentNeighbors_.push_back(std::make_pair(distSq, *it)) ;
			}
159
160
		}

pitiot's avatar
maj    
pitiot committed
161
162
163
164
		sim_->nbUpdates++ ;
		sim_->nearNeighbors += agentNeighbors_.size() ;
		sim_->totalNeighbors += agents.size() + neighborAgents.size() ;

pitiot's avatar
pitiot committed
165
166
167
168
169
170

	}
	if (agentNeighbors_.size() > maxNeighbors_)
	{
		sim_->nbSorts++ ;
		std::sort(agentNeighbors_.begin(), agentNeighbors_.end(), agentSort) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
171
172
173
	}
}

Arash HABIBI's avatar
Arash HABIBI committed
174
175
176
//-----------------------------------------------------------------
//-----------------------------------------------------------------

Pierre Kraemer's avatar
Pierre Kraemer committed
177
void Agent::updateObstacleNeighbors()
Pierre Kraemer's avatar
Pierre Kraemer committed
178
{
Pierre Kraemer's avatar
Pierre Kraemer committed
179
	obstacleNeighbors_.clear() ;
pitiot's avatar
maj    
pitiot committed
180
	movingObstacleNeighbors_.clear() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
181

Pierre Kraemer's avatar
Pierre Kraemer committed
182
183
184
185
186
#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] ;
187
		for(std::vector<Obstacle*>::const_iterator it = obst.begin() ; it != obst.end() ; ++it)
Pierre Kraemer's avatar
Pierre Kraemer committed
188
189
190
191
192
		{
			float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, pos) ;
			if(distSq < rangeSq_)
			{
				if(Geom::testOrientation2D(pos, (*it)->p1, (*it)->p2) == Geom::RIGHT)
pitiot's avatar
maj    
pitiot committed
193
					obstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
194
195
196
197
			}
		}
	}
#else
198
	std::vector<Obstacle*>& obst = sim_->envMap_.obstvect[part_.d] ;
pitiot's avatar
maj    
pitiot committed
199
	std::vector<Obstacle*>& neighborObst = sim_->envMap_.neighborObstvect[part_.d] ;
pitiot's avatar
merging    
pitiot committed
200
201
202
	float maxDistObst = 0.0f ;
	float maxDistMovingObst = 0.0f ;

pitiot's avatar
maj    
pitiot committed
203
	for(std::vector<Obstacle*>::const_iterator it = obst.begin() ; it != obst.end() ; ++it)
David Cazier's avatar
David Cazier committed
204
	{
pitiot's avatar
merging    
pitiot committed
205
		if ((*it)->mo==NULL)
David Cazier's avatar
David Cazier committed
206
		{
pitiot's avatar
merging    
pitiot committed
207
208
			float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, part_.getPosition()) ;
			if ((obstacleNeighbors_.size() < maxNeighbors_ || distSq < maxDistObst)
209
				    && distSq < rangeSq_)
pitiot's avatar
maj    
pitiot committed
210
			{
pitiot's avatar
merging    
pitiot committed
211
				if (Geom::testOrientation2D(part_.getPosition(), (*it)->p1, (*it)->p2) == Geom::RIGHT)
pitiot's avatar
maj    
pitiot committed
212
				{
pitiot's avatar
merging    
pitiot committed
213

214
215
216
					if (distSq > maxDistObst)
						maxDistObst = distSq ;
					obstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
pitiot's avatar
merging    
pitiot committed
217

pitiot's avatar
maj    
pitiot committed
218
219
				}

pitiot's avatar
merging    
pitiot committed
220
			}
pitiot's avatar
maj    
pitiot committed
221
		}
pitiot's avatar
merging    
pitiot committed
222
		else
pitiot's avatar
maj    
pitiot committed
223
		{
pitiot's avatar
merging    
pitiot committed
224
225
			float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, part_.getPosition()) ;
			if ((movingObstacleNeighbors_.size() < maxNeighbors_ || distSq < maxDistMovingObst)
226
					&& distSq < rangeSq_)
pitiot's avatar
maj    
pitiot committed
227
			{
228
//				if (Geom::testOrientation2D(part_.getPosition(), (*it)->p1, (*it)->p2) == Geom::RIGHT)
pitiot's avatar
merging    
pitiot committed
229
230
				{

231
232
					if (distSq > maxDistMovingObst)
						maxDistMovingObst = distSq ;
pitiot's avatar
maj    
pitiot committed
233
					movingObstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
pitiot's avatar
merging    
pitiot committed
234
235
236

				}

pitiot's avatar
maj    
pitiot committed
237
			}
238
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
239
	}
240

pitiot's avatar
merging    
pitiot committed
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
	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_)
					{
265
//						if (Geom::testOrientation2D(part_.getPosition(), (*it)->p1, (*it)->p2) == Geom::RIGHT)
pitiot's avatar
merging    
pitiot committed
266
267
268
269
270
271
272
273
274
275
						{

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

						}

					}
				}
	}
276
277
278
279
280
281
282
283
284
285
286
287

//	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) ;
//	}
pitiot's avatar
merging    
pitiot committed
288
289
290



Pierre Kraemer's avatar
Pierre Kraemer committed
291
#endif
Pierre Kraemer's avatar
Pierre Kraemer committed
292

pitiot's avatar
maj    
pitiot committed
293
294
//	if (obstacleNeighbors_.size() > maxNeighbors_)
//		std::sort(obstacleNeighbors_.begin(), obstacleNeighbors_.end(), obstacleSort) ;
pitiot's avatar
merging    
pitiot committed
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
//	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;
//		}
Pierre Kraemer's avatar
Pierre Kraemer committed
312
313
}

Arash HABIBI's avatar
Arash HABIBI committed
314
315
316
//-----------------------------------------------------------------
//-----------------------------------------------------------------

Pierre Kraemer's avatar
Pierre Kraemer committed
317
318
void Agent::update()
{
pitiot's avatar
maj    
pitiot committed
319
320
321
	if (alive)
	{
		finalGoal = goals_[curGoal_] ;
David Cazier's avatar
David Cazier committed
322
		velocity_ = newVelocity_ ;
Pierre Kraemer's avatar
Pierre Kraemer committed
323
#ifdef SPATIAL_HASHING
David Cazier's avatar
David Cazier committed
324
		pos = pos + (velocity_ * sim_->timeStep_) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
325
#else
pitiot's avatar
maj    
pitiot committed
326
327
		VEC3 target = part_.getPosition() + (velocity_ * sim_->timeStep_) ;
		part_.move(target) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
328
#endif
David Cazier's avatar
David Cazier committed
329
330
331
332
333
334
335
336

		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() ;
pitiot's avatar
maj    
pitiot committed
337
338
	}
}
pitiot's avatar
pitiot committed
339

Arash HABIBI's avatar
Arash HABIBI committed
340
341
342
//-----------------------------------------------------------------
//-----------------------------------------------------------------

pitiot's avatar
pitiot committed
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
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///////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

Arash HABIBI's avatar
Arash HABIBI committed
397
398
399
400
401
402
403
404
405
406
407
408

//-----------------------------------------------------------------
// 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)
pitiot's avatar
maj    
pitiot committed
409
410
{
	float factor ;
pitiot's avatar
pitiot committed
411
412
413
414
	Obstacle* obst ;
	PFP::VEC3 x,y,vec ;
	PFP::VEC3 norm,sumNorm ;
	sumNorm.zero() ;
pitiot's avatar
maj    
pitiot committed
415

Arash HABIBI's avatar
Arash HABIBI committed
416
	// On parcourt les obstacles
pitiot's avatar
pitiot committed
417
418
419
420
	for(std::vector<std::pair<float, Obstacle*> >::iterator it = movingObstacleNeighbors_.begin() ;
		it != movingObstacleNeighbors_.end() ;
		++it)
	{
Arash HABIBI's avatar
Arash HABIBI committed
421
422
423
424
425
426
427
428
429
430
431
432
433
		// 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.

pitiot's avatar
pitiot committed
434
//		CGoGNout<<factor<<" par rapport à "<<(*it).first<< " ayant un range de "<< rangeSq_<<CGoGNendl;
Arash HABIBI's avatar
Arash HABIBI committed
435
		obst=it->second ;
pitiot's avatar
pitiot committed
436
437
438
439
440
		x=obst->p1 ;
		y=obst->p2 ;
		vec=y-x ;
		vec.normalize() ;
		norm.zero() ;
Arash HABIBI's avatar
Arash HABIBI committed
441

pitiot's avatar
pitiot committed
442
		if (sim_->avoidance==0)// avoids with normal of obstacle side
pitiot's avatar
maj    
pitiot committed
443
		{
pitiot's avatar
pitiot committed
444
445
446
447
448
449
450
			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;
pitiot's avatar
maj    
pitiot committed
451
		}
pitiot's avatar
pitiot committed
452
453
454
455
456
457
458
//		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 ;
Arash HABIBI's avatar
Arash HABIBI committed
459
		// On met factor à la puissance 8 pour adoucir l'impact.
pitiot's avatar
pitiot committed
460
461
		sumNorm+=norm ;
	}
pitiot's avatar
maj    
pitiot committed
462

pitiot's avatar
pitiot committed
463
464
465
466
467
//	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();
Pierre Kraemer's avatar
Pierre Kraemer committed
468
469
}

Arash HABIBI's avatar
Arash HABIBI committed
470
471
472
473
474
//-----------------------------------------------------------------
// Calcul vitesse optimale pour atteindre l'objectif (sans prendre
// en compte l'environnement).

void Agent::computePrefVelocity()
Pierre Kraemer's avatar
Pierre Kraemer committed
475
{
476
477
	VEC3 goalVector = goals_[curGoal_] - getPosition() ;
	float goalDist2 = goalVector.norm2() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
478

Arash HABIBI's avatar
Arash HABIBI committed
479
480
481
	// Si l'agent arrive à proximité de l'objectif,
	// alors il passe à l'objectif suivant

pitiot's avatar
maj    
pitiot committed
482
	if (goalDist2 < radius_*radius_)
David Cazier's avatar
David Cazier committed
483
	{
484
485
486
487
		curGoal_ = (curGoal_ + 1) % goals_.size() ;
		goalVector = goals_[curGoal_] - getPosition() ;
		goalDist2 = goalVector.norm2() ;
	}
Pierre Kraemer's avatar
yeah..    
Pierre Kraemer committed
488

Arash HABIBI's avatar
Arash HABIBI committed
489
490
491
492
493
494
	// 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.

pitiot's avatar
maj    
pitiot committed
495
496
497
498
499
	if (goalDist2 > maxSpeed_)
	{
		goalVector.normalize() ;
		goalVector *= maxSpeed_;
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
500

501
	prefVelocity_ = goalVector ;
Pierre Kraemer's avatar
Pierre Kraemer committed
502
503
}

Arash HABIBI's avatar
Arash HABIBI committed
504
505
506
507
508
509
510
//-----------------------------------------------------------------

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

Arash HABIBI's avatar
Arash HABIBI committed
511
	// if(nb_mos>=max-1)
pitiot's avatar
merging    
pitiot committed
512
	if(nb_mos>=max)
Arash HABIBI's avatar
Arash HABIBI committed
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
	{
		// 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);
}

Arash HABIBI's avatar
Arash HABIBI committed
534
535
536
537
538
//-----------------------------------------------------------------
//-----------------------------------------------------------------
//-----------------------------------------------------------------
// Search for the best new velocity.

Arash HABIBI's avatar
Arash HABIBI committed
539
void Agent::computeNewVelocity()
Pierre Kraemer's avatar
Pierre Kraemer committed
540
{
Arash HABIBI's avatar
Arash HABIBI committed
541
542
543
	// The objective is to compute the sum of forces exerted on the agent.
	double collision_softening_factor;
	float ag_ambient_damping = 1.0;
Arash HABIBI's avatar
Arash HABIBI committed
544
	double mass_var = 0.95;
Arash HABIBI's avatar
Arash HABIBI committed
545
546
547
548
549
550
	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
Pierre Kraemer's avatar
Pierre Kraemer committed
551

Arash HABIBI's avatar
Arash HABIBI committed
552
	//-------------
Pierre Kraemer's avatar
Pierre Kraemer committed
553

Arash HABIBI's avatar
Arash HABIBI committed
554
	VEC3 vec, forces, previous_pos;
Arash HABIBI's avatar
Arash HABIBI committed
555
	forces.zero();
Pierre Kraemer's avatar
Pierre Kraemer committed
556

Arash HABIBI's avatar
Arash HABIBI committed
557
	previous_pos = getPosition() - velocity_*sim_->timeStep_;
Pierre Kraemer's avatar
Pierre Kraemer committed
558

Arash HABIBI's avatar
Arash HABIBI committed
559
	//----- force due à l'attraction de l'objectif ----------
Pierre Kraemer's avatar
Pierre Kraemer committed
560

Arash HABIBI's avatar
Arash HABIBI committed
561
562
563
564
	float goal_attraction_force = 200.0;  // agent-goal interaction stiffness
	VEC3 p_goal = goals_[curGoal_];
	VEC3 u_goal = p_goal - getPosition();
	u_goal.normalize();
Pierre Kraemer's avatar
Pierre Kraemer committed
565

Arash HABIBI's avatar
Arash HABIBI committed
566
567
	cerr << agentNo << "---------------- position = " << getPosition() << endl;

Arash HABIBI's avatar
Arash HABIBI committed
568
	forces += goal_attraction_force * u_goal;
Pierre Kraemer's avatar
Pierre Kraemer committed
569

Arash HABIBI's avatar
Arash HABIBI committed
570
571
	cerr << "forces = " << forces << endl;

Arash HABIBI's avatar
Arash HABIBI committed
572
	//----- forces dues à la répulsion des obstacles en mouvement ----------
Pierre Kraemer's avatar
Pierre Kraemer committed
573

Arash HABIBI's avatar
Arash HABIBI committed
574
	VEC3 norm;
Arash HABIBI's avatar
Arash HABIBI committed
575
576
	// double obst_stiffness = 10000.0; // agent-obstacle interaction stiffness
	double obst_stiffness = 1000.0; // agent-obstacle interaction stiffness
Arash HABIBI's avatar
Arash HABIBI committed
577
	// double obst_damping = 1.0;  // agent-obstacle interaction damping
Arash HABIBI's avatar
Arash HABIBI committed
578
	int obst_power = 2;           // the power to which elevate the agent-obstacle distance
Arash HABIBI's avatar
Arash HABIBI committed
579
	Obstacle* obst ;
Pierre Kraemer's avatar
Pierre Kraemer committed
580

Arash HABIBI's avatar
Arash HABIBI committed
581
	nb_mos = 0;
Arash HABIBI's avatar
Arash HABIBI committed
582
583
584
585
	for(std::vector<std::pair<float, Obstacle*> >::iterator it = movingObstacleNeighbors_.begin() ;
		it != movingObstacleNeighbors_.end() ;
		++it)
	{
Arash HABIBI's avatar
Arash HABIBI committed
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
		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)
David Cazier's avatar
David Cazier committed
602
		{
Arash HABIBI's avatar
Arash HABIBI committed
603
604
			collision_softening_factor = pow(1 - dist/effective_range,obst_power);
			force_value = obst_stiffness*collision_softening_factor*(effective_range-dist);
Pierre Kraemer's avatar
Pierre Kraemer committed
605

Arash HABIBI's avatar
Arash HABIBI committed
606
607
608
609
610
611
612
613
614
615
616
			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;
Pierre Kraemer's avatar
Pierre Kraemer committed
617
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
618
619
	}

Arash HABIBI's avatar
Arash HABIBI committed
620
621
	//----- forces dues à la répulsion des obstacles fixes ----------

Arash HABIBI's avatar
Arash HABIBI committed
622
	// double fixed_obst_stiffness = 50000.0; // agent-obstacle interaction stiffness
Arash HABIBI's avatar
Arash HABIBI committed
623
624
625
626
627
628
629
630
631
632
633
634
	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;
Arash HABIBI's avatar
Arash HABIBI committed
635
		// cerr << "nobst=" << nobst << "dist=" << dist << endl;
Arash HABIBI's avatar
Arash HABIBI committed
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
		// 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;
Arash HABIBI's avatar
Arash HABIBI committed
655
		cerr << "obstacles fixes : nobst=" << nobst << " dist=" << dist << " force=" << force_value*norm << endl;
Arash HABIBI's avatar
Arash HABIBI committed
656
657
		// cerr << "force_value = " << force_value << endl;
		// cerr << "norm=" << norm << endl;
Arash HABIBI's avatar
Arash HABIBI committed
658
659
660
661

		nobst++;
	}

Arash HABIBI's avatar
Arash HABIBI committed
662
	//----- forces dues à la répulsion des autres agents -------
Pierre Kraemer's avatar
Pierre Kraemer committed
663

Arash HABIBI's avatar
Arash HABIBI committed
664
	// double ag_stiffness = 500.0;    // agent-agent interaction stiffness
Arash HABIBI's avatar
Arash HABIBI committed
665
	double ag_stiffness = 500.0;    // agent-agent interaction stiffness
Arash HABIBI's avatar
Arash HABIBI committed
666
667
	double ag_damping   = 1.0;     // agent-agent interaction damping
	double ag_phys_radius_coef = 20.0;
668
	double ag_power = 1;        // the power to which elevate the agent-agent distance
Pierre Kraemer's avatar
Pierre Kraemer committed
669

670
	unsigned int nbA = 0 ;
Arash HABIBI's avatar
Arash HABIBI committed
671

pitiot's avatar
maj    
pitiot committed
672
673
	for (std::vector<std::pair<float, Agent*> >::iterator it = agentNeighbors_.begin() ;
		it != agentNeighbors_.end() && nbA < maxNeighbors_ ; ++it, ++nbA)
David Cazier's avatar
David Cazier committed
674
	{
675
		Agent* other = it->second ;
Pierre Kraemer's avatar
Pierre Kraemer committed
676

Arash HABIBI's avatar
Arash HABIBI committed
677
		const VEC3 relativePosition(other->getPosition()-getPosition());
678
		const float distSq = relativePosition.norm2() ;
Arash HABIBI's avatar
Arash HABIBI committed
679
680
681
		const float dist   = sqrt(distSq);
		const float combinedRadius = ag_phys_radius_coef*(radius_ + other->radius_) ;
		// const float combinedRadiusSq = combinedRadius * combinedRadius ;
Pierre Kraemer's avatar
Pierre Kraemer committed
682

Arash HABIBI's avatar
Arash HABIBI committed
683
684
685
686
		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);
Pierre Kraemer's avatar
Pierre Kraemer committed
687

Arash HABIBI's avatar
Arash HABIBI committed
688
689
690
691
		// const VEC3 u_other(relativePosition);
		VEC3 u_other(relativePosition);
		u_other = relativePosition;
		u_other.normalize();
Pierre Kraemer's avatar
Pierre Kraemer committed
692

Arash HABIBI's avatar
Arash HABIBI committed
693
		// cerr << "dist=" << dist << " combinedRadius=" << combinedRadius << endl;
Pierre Kraemer's avatar
Pierre Kraemer committed
694

Arash HABIBI's avatar
Arash HABIBI committed
695
		if(dist < combinedRadius)
David Cazier's avatar
David Cazier committed
696
		{
Arash HABIBI's avatar
Arash HABIBI committed
697
698
699
			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_;
Pierre Kraemer's avatar
Pierre Kraemer committed
700

Arash HABIBI's avatar
Arash HABIBI committed
701
702
			cerr << "autres agents : force_value = " << force_value << endl;

Arash HABIBI's avatar
Arash HABIBI committed
703
			forces += force_value * u_other;
Pierre Kraemer's avatar
Pierre Kraemer committed
704
705
706
		}
	}

Arash HABIBI's avatar
Arash HABIBI committed
707
	//------- calcul de la trainee --------------------------------------
Pierre Kraemer's avatar
Pierre Kraemer committed
708

Arash HABIBI's avatar
Arash HABIBI committed
709
	// forces -= ag_ambient_damping * velocity_;
Pierre Kraemer's avatar
Pierre Kraemer committed
710

Arash HABIBI's avatar
Arash HABIBI committed
711
	//------- calcul de la nouvelle valeur de la vitesse ----------------
Pierre Kraemer's avatar
Pierre Kraemer committed
712

Arash HABIBI's avatar
Arash HABIBI committed
713
	VEC3 velocity_tmp;
Pierre Kraemer's avatar
Pierre Kraemer committed
714

Arash HABIBI's avatar
Arash HABIBI committed
715
716
	velocity_tmp = velocity_ + forces * (sim_->timeStep_ / ag_mass);
	if(velocity_tmp.norm2() > maxSpeed_*maxSpeed_)
David Cazier's avatar
David Cazier committed
717
	{
Arash HABIBI's avatar
Arash HABIBI committed
718
719
		velocity_tmp.normalize();
		velocity_tmp *= maxSpeed_;
Pierre Kraemer's avatar
Pierre Kraemer committed
720
	}
Arash HABIBI's avatar
Arash HABIBI committed
721
	newVelocity_ = velocity_tmp;
Arash HABIBI's avatar
Arash HABIBI committed
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747

	//------- 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;
Arash HABIBI's avatar
Arash HABIBI committed
748
}
Pierre Kraemer's avatar
Pierre Kraemer committed
749

Arash HABIBI's avatar
Arash HABIBI committed
750
//-----------------------------------------------------------------
Pierre Kraemer's avatar
Pierre Kraemer committed
751

Arash HABIBI's avatar
Arash HABIBI committed
752
753
bool Agent::linearProgram1(const std::vector<Line>& lines, unsigned int lineNo, float radius, const VEC3& optVelocity, bool directionOpt, VEC3& result)
{
754
	return true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
755
756
}

Arash HABIBI's avatar
Arash HABIBI committed
757
//-----------------------------------------------------------------
Pierre Kraemer's avatar
Pierre Kraemer committed
758

Arash HABIBI's avatar
Arash HABIBI committed
759
unsigned int Agent::linearProgram2(const std::vector<Line>& lines, float radius, const VEC3& optVelocity, bool directionOpt, VEC3& result)
Pierre Kraemer's avatar
Pierre Kraemer committed
760
{
Arash HABIBI's avatar
Arash HABIBI committed
761
762
	return 1;
}
Pierre Kraemer's avatar
Pierre Kraemer committed
763

Arash HABIBI's avatar
Arash HABIBI committed
764
//-----------------------------------------------------------------
765

Arash HABIBI's avatar
Arash HABIBI committed
766
767
void Agent::linearProgram3(const std::vector<Line>& lines, unsigned int numObstLines, unsigned int beginLine, float radius, VEC3& result)
{
Pierre Kraemer's avatar
Pierre Kraemer committed
768
}