moving_obstacle.cpp 14.7 KB
Newer Older
pitiot's avatar
maj    
pitiot committed
1
2
3
4
5
#include "moving_obstacle.h"
#include "obstacle.h"
#include "agent.h"
#include "simulator.h"

Pierre Kraemer's avatar
Pierre Kraemer committed
6
//float MovingObstacle::neighborDistSq_ = 5.0f * 5.0f;
pitiot's avatar
maj    
pitiot committed
7

pitiot's avatar
pitiot committed
8
float MovingObstacle::maxSpeed_ = 2.0f;
pitiot's avatar
pitiot committed
9
10
float MovingObstacle::neighborDist_ = 10.0f ;
float MovingObstacle::neighborDistSq_ = neighborDist_ * neighborDist_ ;
pitiot's avatar
maj    
pitiot committed
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
float MovingObstacle::timeHorizonObst_ = 10.0f;
unsigned int MovingObstacle::maxNeighbors_ = 20;
float MovingObstacle::detectionFixedObst = 50;

bool MovingObstacle::is_inside(VEC3 p)
{
	VEC3 vec, norm;
	Obstacle * obst;

	for (unsigned int i = 0; i < nbVertices; i++)
	{
		obst = this->obstacles_[i];

		vec = VEC3(obst->p2 - obst->p1);
		norm[0] = vec[1];
		norm[1] = -vec[0];

		vec = VEC3(obst->p2 -p);
		if (vec*norm < 0)
			return false;
	}

	return true;
}

pitiot's avatar
pitiot committed
36
float get_angle(VEC3 v1, VEC3 v2) //renvoie l'angle entre [- pi ; Pi] du v2 à v1
pitiot's avatar
maj    
pitiot committed
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
{
	float flo = 0.0f;
	float nb = std::sqrt(v1.norm2() * v2.norm2());

	if (nb != 0)
	{
		nb = (v1*v2) / nb;

		if (nb > 1)
			nb = 1;
		else if (nb < -1)
			nb = -1;
		flo = std::acos(nb);
	}

	if ((v1[0] * v2[1] - v2[0] * v1[1]) > 0)
pitiot's avatar
pitiot committed
53
54
55
56
57
58
59
		flo = - flo;

	if(flo >M_PI)
		flo= flo-2*M_PI;
	if (flo<(-M_PI))
		flo=flo+2*M_PI;

pitiot's avatar
maj    
pitiot committed
60
61
62
	return flo;
}

pitiot's avatar
pitiot committed
63
VEC3 rotate(VEC3 pos1, VEC3 center, float angle) // renvoie le déplacement necessaire depuis pos1 pour effectuer la rotation centrée en center d'angle angle
pitiot's avatar
maj    
pitiot committed
64
65
66
67
68
69
70
71
72
{
	VEC3 pos2;
	PFP::REAL x = pos1[0] - center[0];
	PFP::REAL y = pos1[1] - center[1];
	pos2[0] = x * cos(angle) - y * sin(angle) - x;
	pos2[1] = x * sin(angle) + y * cos(angle) - y;
	return pos2;
}

pitiot's avatar
merging    
pitiot committed
73
MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, std::vector<VEC3> goals, bool spin, ArticulatedObstacle * art, int ind2) :
pitiot's avatar
maj    
pitiot committed
74
75
		index(ind),
		newVelocity_(0),
pitiot's avatar
merging    
pitiot committed
76
77
78
		sim_(sim),
		parent(art),
		index_parent(ind2)
pitiot's avatar
maj    
pitiot committed
79
{
pitiot's avatar
merging    
pitiot committed
80
81
82
83
	seen = false;
	color1=1.0f;
	color2=1.0f;
	color3=1.0f;
pitiot's avatar
maj    
pitiot committed
84
85
	assert(pos.size() > 2);

pitiot's avatar
pitiot committed
86
//	movingObstNeighbors_.reserve(maxNeighbors_);
pitiot's avatar
pitiot committed
87
	spinning=spin;
pitiot's avatar
pitiot committed
88
89
	goals_=goals;
	curGoal_ = 1;
pitiot's avatar
maj    
pitiot committed
90
91
92
93
94
95
96
	VEC3 sum = 0;
	nb_agents_voisins = 0;
	nb_register_cells = 0;
	float pos_max = 0;

	velocity_factor = 0.8f;
	nbVertices = pos.size();
97
98
	Dart d = sim_->envMap_.getBelongingCell(pos[0]);
	registering_part = new CGoGN::Algo::MovingObjects::ParticleCell2DMemo<PFP>(sim_->envMap_.map, d, pos[0], sim_->envMap_.position);
pitiot's avatar
maj    
pitiot committed
99

100
	vertices.reserve(nbVertices);
pitiot's avatar
maj    
pitiot committed
101
	obstacles_ = new Obstacle*[nbVertices];
102
103
	belonging_cells = new std::vector<Dart>[nbVertices];
	neighbor_cells = new std::vector<Dart>[nbVertices];
pitiot's avatar
maj    
pitiot committed
104
105
	for (unsigned int i = 0; i < nbVertices; ++i)
	{
106
		vertices[i]=pos [i];
pitiot's avatar
pitiot committed
107
108
109
110
		sum += vertices[i];
	}
	center = sum / nbVertices;
	front=(vertices[1] + vertices[2]) / 2;
pitiot's avatar
merging    
pitiot committed
111
112
113
114
115
116
117
118
119
120
121
122

	//-------- code ajoute par Arash pour les obstacles rectangulaires --------------

	length = (vertices[0]-vertices[1]).norm();
	width  = (vertices[1]-vertices[2]).norm();
	sum_dist_foci_rest = 2*(length + width*(sqrt(2)-0.5));
	// M appartient à l'ellipse ssi MF1 + MF2 = sum_dist_foci est une constante
	// où F1 et F2 sont les deux foyers.

	//-------- fin du code ajoute par Arash pour les obstacles rectangulaires -------

	if (spinning && parent==NULL) //départ face à la cible en cas d'obstacles pouvant effectuer des rotations
pitiot's avatar
pitiot committed
123
124
125
126
127
128
129
130
131
	{
		angle = get_angle(goals_[curGoal_] - center,front  - center);
		for (unsigned int i = 0; i < nbVertices; ++i)
		{
			vertices[i]+=rotate(vertices[i], center, angle);

		}
		angle=0;
		front=(vertices[1] + vertices[2]) / 2;
pitiot's avatar
maj    
pitiot committed
132
133
134
	}
	for (unsigned int i = 0; i < nbVertices; ++i)
	{
pitiot's avatar
pitiot committed
135

136
137
138
139
		Obstacle* o = new Obstacle(vertices[i],
								   vertices[(i + 1) % nbVertices],
								   vertices[(i - 1 + nbVertices) % nbVertices],
								   vertices[(i + 2) % nbVertices], this, i);
pitiot's avatar
maj    
pitiot committed
140
141
		obstacles_[i] = o;

pitiot's avatar
pitiot committed
142
		sim_->envMap_.pushObstacleInCells(o);
pitiot's avatar
maj    
pitiot committed
143
144
145
	}


pitiot's avatar
pitiot committed
146

pitiot's avatar
maj    
pitiot committed
147
148
149
150
151
152
153
154
155
	for (unsigned int i = 0; i < nbVertices; ++i)
	{
		float it = (center - pos[i]).norm();
		if (it > pos_max)
			pos_max = it;
	}
	gravity_dist = pos_max + 5.0f;

	obstacle_range = 15.0f * 15.0f;
pitiot's avatar
pitiot committed
156

pitiot's avatar
pitiot committed
157

pitiot's avatar
pitiot committed
158
159
160



pitiot's avatar
maj    
pitiot committed
161
162
163
164
165
166
}

bool MovingObstacle::test_opposition(VEC3 o, VEC3 p1, VEC3 p2)
{
	o.normalize();
	o *= -1;
167
168
169
	VEC3 vector(p1 - p2);
	vector.normalize();
	return (o - vector).norm2() < 0.1;
pitiot's avatar
maj    
pitiot committed
170
171
172
}

// TODO Check position
pitiot's avatar
pitiot committed
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
//void MovingObstacle::contournerBatiment()
//{
//	PFP::VEC3 toto;
//	PFP::VEC3 toto_norm;
//	//for particles in the "front" of the object (to modify)
//	for(int k =0;k<2;k++)
//	{
//
//		registering_part->get_memo(vertices[k]);
//		std::vector<Obstacle*>& obst = sim_->envMap_.obstvect[registering_part->d];
//
//		//search all obstacles around
//		for(std::vector<Obstacle*>::const_iterator it = obst.begin(); it != obst.end(); ++it)
//		{
//			//only for fixed obstacles around
//			if ((*it)->mo==NULL)
//			{
//				float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, vertices[k]);
//				if(distSq < detectionFixedObst*detectionFixedObst)
//				{
//					toto= (*it)->p1 -(*it)->p2;
//					toto_norm=toto;
//					toto_norm[0]=-toto[1];
//					toto_norm[1]=toto[0];
//
//					if(test_opposition(toto_norm,front,center)) //// à changer ////////////
//					{
//						int co = rand() % 2 ;
//						if (toto[0]==0)
//						{
//							finalGoal[0]=front[0];
//							if (co == 0)
//								finalGoal[0]=sim_->envMap_.geometry.max()[1]-maxNeighbors_;
//							else
//								finalGoal[0]=sim_->envMap_.geometry.min()[1]+maxNeighbors_;
//						}
//						else
//						{
//							finalGoal[1]=front[1];
//							if (co == 0)
//								finalGoal[0]=sim_->envMap_.geometry.max()[1]-maxNeighbors_;
//							else
//								finalGoal[0]=sim_->envMap_.geometry.min()[1]+maxNeighbors_;
//						}
//
//						float angle =get_angle(finalGoal-center,front -center);
//						make_half_turn=angle*nbVertices;
//						break;
//					}
//				}
//			}
//		}
//	}
//}
pitiot's avatar
maj    
pitiot committed
227

pitiot's avatar
pitiot committed
228
void MovingObstacle::updateAgentNeighbors() // agents voisins avec distance au bord (distpointlinesq) de l'obstacle // a mettre en place si besoin
pitiot's avatar
maj    
pitiot committed
229
{
pitiot's avatar
pitiot committed
230
231
232
233
234
//	agentNeighbors_.clear() ;
//	Dart d;
//	float maxDist = 0.0f ;//distance max des agents eregistrés au centre
//	std::set::iterator agIt;//emplacement de l'agent en question
//	for(int i = 0;i< nbVertices;i++ )
pitiot's avatar
pitiot committed
235
//	{
pitiot's avatar
pitiot committed
236
237
238
239
240
241
//		registering_part->get_memo(vertices[i]);
//		d=registering_part->d;
//		const std::vector<Agent*>& agents = sim_->envMap_.agentvect[d] ;
//		const std::vector<Agent*>& neighborAgents = sim_->envMap_.neighborAgentvect[d] ;
//
//
pitiot's avatar
pitiot committed
242
//
pitiot's avatar
pitiot committed
243
244
245
246
//		for (std::vector<Agent*>::const_iterator it = agents.begin(); it != agents.end(); ++it)
//		{
//			if ((*it)->alive)
//			{
pitiot's avatar
pitiot committed
247
//
pitiot's avatar
pitiot committed
248
249
250
251
//				float distSq = (center - (*it)->getPosition()).norm2() ;
//				if ((agentNeighbors_.size() < maxNeighbors_ || distSq < maxDist)
//					&& distSq < neighborDistSq_)
//				{
pitiot's avatar
pitiot committed
252
//
pitiot's avatar
pitiot committed
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
//					if (distSq > maxDist)
//					{
//						maxDist = distSq ;
//						agIt=(agentNeighbors_.insert(std::make_pair(distSq, *it))).first() ;
//					}
//					else
//					{
//						agentNeighbors_.insert(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 ;
//						agIt=(agentNeighbors_.insert(std::make_pair(distSq, *it))).first() ;
//					}
//					else
//					{
//						agentNeighbors_.insert(std::make_pair(distSq, *it))
//					}
//				}
//			}
//
//		}
pitiot's avatar
pitiot committed
288
//	}
pitiot's avatar
pitiot committed
289
290
291
292
293
294
}

void MovingObstacle::updateObstacleNeighbors() // obstacles voisins , distance par rapport aux centres des segments// a mettre en place si besoin
{
//	obstacleNeighbors_.clear() ;
//	movingObstacleNeighbors_.clear() ;
pitiot's avatar
pitiot committed
295
//
pitiot's avatar
pitiot committed
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
//	std::vector<Obstacle*>& obst = sim_->envMap_.obstvect[part_.d] ;
//	std::vector<Obstacle*>& neighborObst = sim_->envMap_.neighborObstvect[part_.d] ;
//	for(std::vector<Obstacle*>::const_iterator it = obst.begin() ; it != obst.end() ; ++it)
//	{
//		float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, part_.getPosition()) ;
//		if (distSq < rangeSq_)
//		{
//			if (Geom::testOrientation2D(part_.getPosition(), (*it)->p1, (*it)->p2) == Geom::RIGHT)
//			{
//				if ((*it)->mo==NULL)
//					obstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
//				else
//				{
//					movingObstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
//				}
//			}
pitiot's avatar
pitiot committed
312
//
pitiot's avatar
pitiot committed
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
//		}
//	}
//	for(std::vector<Obstacle*>::const_iterator it = neighborObst.begin() ; it != neighborObst.end() ; ++it)
//	{
//		float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, part_.getPosition()) ;
//		if(distSq < rangeSq_)
//		{
//			if(Geom::testOrientation2D(part_.getPosition(), (*it)->p1, (*it)->p2) == Geom::RIGHT)
//			{
//				if ((*it)->mo==NULL)
//					obstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
//				else
//					movingObstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
//			}
//		}
//	}
pitiot's avatar
maj    
pitiot committed
329
330
}

pitiot's avatar
pitiot committed
331

pitiot's avatar
maj    
pitiot committed
332
333
334
// TODO Check position
void MovingObstacle::update()
{
pitiot's avatar
pitiot committed
335
	assert(sim_->envMap_.map.getCurrentLevel() == sim_->envMap_.map.getMaxLevel()) ;
336
337
338
	if(sim_->detect_agent_collision)
		general_belonging.clear();

pitiot's avatar
merging    
pitiot committed
339
340
341
342
343
344
345
	if(!seen)
	{
		color1=1.0f;
		color2=1.0f;
		color3=1.0f;
	}
	seen=false;
pitiot's avatar
maj    
pitiot committed
346
347
348
	PFP::VEC3 bary = 0;

	Dart d;
pitiot's avatar
pitiot committed
349
350
	velocity_[0] = newVelocity_[0] * velocity_factor;
	velocity_[1] = newVelocity_[1] * velocity_factor;
pitiot's avatar
maj    
pitiot committed
351
352

	// MAJ des particules
pitiot's avatar
pitiot committed
353
	float abs_angle= angle > 0 ? 1 : -1;
pitiot's avatar
merging    
pitiot committed
354
355
356
357
358
359
360
361
362
	float rotor=0;
	if (index_parent==0)
	{
		rotor = abs_angle*angle > 0.04f ? 0.04f : abs_angle*angle ;
	}
	else
	{
		rotor = abs_angle*angle ;
	}
pitiot's avatar
pitiot committed
363
364
365
//	CGoGNout << "Obstacle "<< index << CGoGNendl;
//	CGoGNout << "vitesse : "<< velocity_ << CGoGNendl;
	//	on fait tourner l'obstacle
pitiot's avatar
maj    
pitiot committed
366
367
368
	for (unsigned int i = 0; i < nbVertices; ++i)
	{

pitiot's avatar
pitiot committed
369
		VEC3 target =vertices[i];
pitiot's avatar
maj    
pitiot committed
370

pitiot's avatar
pitiot committed
371
		target += rotate(vertices[i], center, abs_angle*rotor);
pitiot's avatar
maj    
pitiot committed
372

pitiot's avatar
pitiot committed
373
		target += (velocity_ * sim_->timeStep_);
374
		vertices[i]=target;
pitiot's avatar
maj    
pitiot committed
375
376
		bary += target;
	}
pitiot's avatar
pitiot committed
377
378
379
380
381
382
383
384
385
	front=(vertices[1] + vertices[2]) / 2;
	if(angle >0)
		{
		angle -= rotor;
		}
	else
	{
		angle += rotor;
	}
pitiot's avatar
maj    
pitiot committed
386

pitiot's avatar
pitiot committed
387
	center = bary / nbVertices;
pitiot's avatar
maj    
pitiot committed
388

pitiot's avatar
merging    
pitiot committed
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
	//-------- code ajoute par Arash pour les obstacles rectangulaires --------------

	VEC3 P0_P1 = vertices[1] - vertices[0];
	float velocity_coef = 10.0;

	if(P0_P1 * velocity_ > 0) 	// P0_P1 est dans le sens de la vitesse
	{
		focus1  = center - P0_P1*(1-(width/length)*(sqrt(2)-0.5));
		focus2 = center + P0_P1*(1-(width/length)*(sqrt(2)-0.5)) + (velocity_coef*velocity_);
	}
	else
	{
		focus1  = center - P0_P1*(1-(width/length)*(sqrt(2)-0.5)) + (velocity_coef*velocity_);
		focus2 = center + P0_P1*(1-(width/length)*(sqrt(2)-0.5));
	}
	sum_dist_foci = sum_dist_foci_rest + velocity_coef*velocity_.norm();

	//-------- fin du code ajoute par Arash pour les obstacles rectangulaires -------

pitiot's avatar
maj    
pitiot committed
408
409
410
411
412
413
414
415
416
	// MAJ des obstacles
	for (unsigned int i = 0; i < nbVertices; ++i)
	{
		//		CGoGNout << "avant une etape : Obstacle "<< i << CGoGNendl;
		Obstacle* o = obstacles_[i];
//		o->p1 = parts_[i]->getPosition();
//		o->p2 = parts_[(i + 1) % nbVertices]->getPosition();
//		o->prevP = parts_[(i - 1 + nbVertices) % nbVertices]->getPosition();
//		o->nextP = parts_[(i + 2) % nbVertices]->getPosition();
pitiot's avatar
pitiot committed
417
418
419
		sim_->envMap_.popAndPushObstacleInCells(o,i);


420
421
422
423
424
425
426
		if(sim_->detect_agent_collision)
		{
			for (std::vector<Dart>::iterator it = belonging_cells[i].begin(); it != belonging_cells[i].end(); ++it)
			{
				general_belonging.insert(*it);
			}
		}
pitiot's avatar
maj    
pitiot committed
427
428
429
430
431
432
		//		CGoGNout << "Apres une etape : Obstacle "<< i << CGoGNendl;
	}

	//
	//		envMap_->addMovingObstAsNeighbor(this,general_belonging,&(general_neighbors));
	//		CGoGNout << "nouvel update : "<< CGoGNendl;
433
434
435
436
437
438
439
	if(sim_->detect_agent_collision)
	{
		for (std::set<Dart>::iterator it2 = general_belonging.begin(); it2 != general_belonging.end(); ++ it2)
		{
			std::vector<Agent*> vector =sim_->envMap_.agentvect[(*it2)];
			for(std::vector<Agent*>::iterator it=vector.begin();it!=vector.end(); ++it)
			{
pitiot's avatar
merging    
pitiot committed
440
				if (this->is_inside((*it)->part_.getPosition()))
441
442
443
444
445
446
447
448
				{
					(*it)->alive=false;
					(*it)->color1=0.0f;
					(*it)->color2=0.4f;
					(*it)->color3=0.0f;
					sim_->envMap_.popAgentInCells(*it,(*it)->part_.d);
				}
			}
pitiot's avatar
maj    
pitiot committed
449

450
451
		}
	}
pitiot's avatar
maj    
pitiot committed
452
453
454

}

pitiot's avatar
merging    
pitiot committed
455

456
std::vector<Dart> MovingObstacle::getMemoCross(const VEC3& pos, const VEC3& dest, Dart& d1)
pitiot's avatar
maj    
pitiot committed
457
{
pitiot's avatar
pitiot committed
458
459
	registering_part->move(pos);

460
	d1=registering_part->d;
461
//	CGoGNout<<"d1 : "<< *d1<<"|| d2 : "<< *d2<<"|| start : "<< pos<<"|| stop : "<< dest<<CGoGNendl;
pitiot's avatar
pitiot committed
462
	return (registering_part->move(dest));
pitiot's avatar
maj    
pitiot committed
463
464
}

465
void resetPartSubdiv(Obstacle* o)
pitiot's avatar
maj    
pitiot committed
466
467
{
	MovingObstacle * mo = o->mo;
468

pitiot's avatar
maj    
pitiot committed
469
470
	if (mo != NULL)
	{
471
		VEC3 pos =mo->registering_part->getPosition();
pitiot's avatar
merging    
pitiot committed
472
473
		mo->registering_part->CGoGN::Algo::MovingObjects::ParticleBase<PFP>::move(Algo::Geometry::faceCentroid<PFP>(mo->sim_->envMap_.map, mo->registering_part->d, mo->sim_->envMap_.position)) ;

474
		mo->registering_part->setState(FACE) ;
pitiot's avatar
pitiot committed
475
		mo->registering_part->move(pos) ;
pitiot's avatar
maj    
pitiot committed
476
477
478
479
480
481
	}
}

void resetObstPartInFace(Obstacle* o, Dart d1)
{
	MovingObstacle * mo = o->mo;
482

pitiot's avatar
maj    
pitiot committed
483
484
	if (mo != NULL) {

485
		VEC3 pos1 = mo->registering_part->getPosition();
pitiot's avatar
maj    
pitiot committed
486
487
		if (Algo::Geometry::isPointInConvexFace2D < PFP
				> (mo->sim_->envMap_.map, d1, mo->sim_->envMap_.position, pos1, true)) {
488
			mo->registering_part->d = d1;
pitiot's avatar
maj    
pitiot committed
489
490
491
492
493
		}

	}
}

494
void resetPart(MovingObstacle * mo, Dart d1)
pitiot's avatar
maj    
pitiot committed
495
{
496
497
	if (mo->registering_part->d == mo->sim_->envMap_.map.phi1(d1))
		mo->registering_part->d = d1;
pitiot's avatar
pitiot committed
498

pitiot's avatar
maj    
pitiot committed
499
500
501
502
503
504
}

void displayMO(Obstacle * o)
{
	CGoGNout << "obstacle problematique : " << (o->mo->index) << CGoGNendl;
}
pitiot's avatar
pitiot committed
505
506
507
508
509
510
511
512
513
514

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



// TODO Check position
void MovingObstacle::computePrefVelocity() //calcul du vecteur optimal pour atteindre l'objectif // changer pour 2.5 ?
{
pitiot's avatar
merging    
pitiot committed
515
516
	VEC3 goalVector;
	if (index_parent<1)
pitiot's avatar
pitiot committed
517
518
519
	{
		goalVector = goals_[curGoal_] - center ;

pitiot's avatar
merging    
pitiot committed
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
		float goalDist2 = goalVector.norm2() ;

		if (goalDist2 < 5.0f)
		{
			curGoal_ = (curGoal_ + 1) % goals_.size() ;
			goalVector = goals_[curGoal_] - center ;
			goalDist2 = goalVector.norm2() ;
		}

		if (goalDist2 > maxSpeed_)
		{
			goalVector.normalize() ;
			goalVector *= maxSpeed_;
		}
	}
	else
pitiot's avatar
pitiot committed
536
	{
pitiot's avatar
merging    
pitiot committed
537
538
539
540
541
542
543
		goalVector = get_center(parent,index_parent-1) -center;
		float goalDist2 = goalVector.norm2() ;
		if (goalDist2 > maxSpeed_)
		{
			goalVector.normalize() ;
			goalVector *= maxSpeed_;
		}
pitiot's avatar
pitiot committed
544
	}
pitiot's avatar
pitiot committed
545
	if (spinning) angle =get_angle(goalVector,front-center);
pitiot's avatar
merging    
pitiot committed
546

pitiot's avatar
pitiot committed
547
548
549
550
551
	prefVelocity_ = goalVector ;

}
void MovingObstacle::computeNewVelocity()  //comportement des obstacles en tenant compte de l'environnement.
{
pitiot's avatar
pitiot committed
552
553
554
555
556
557
558
559
560
561
	if (spinning)
	{
		VEC3 forward = front-center;
		float goalDist2 = forward.norm2() ;
		float objective = prefVelocity_.norm2();
		if (goalDist2 > objective)
			{
				forward.normalize() ;
				forward *= objective;
			}
pitiot's avatar
merging    
pitiot committed
562
		if(index_parent==0 && angle>0.01f) forward/=10;
pitiot's avatar
pitiot committed
563
564
565
566
567
568
		newVelocity_=forward;
	}
	else
	{
		newVelocity_=prefVelocity_;
	}
pitiot's avatar
pitiot committed
569
}