moving_obstacle.cpp 17.5 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"

6
7
#include "Algo/Modelisation/triangulation.h"

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

pitiot's avatar
pitiot committed
10
float MovingObstacle::maxSpeed_ = 2.0f;
pitiot's avatar
pitiot committed
11
12
float MovingObstacle::neighborDist_ = 10.0f ;
float MovingObstacle::neighborDistSq_ = neighborDist_ * neighborDist_ ;
pitiot's avatar
maj    
pitiot committed
13
14
15
16
17
18
float MovingObstacle::timeHorizonObst_ = 10.0f;
unsigned int MovingObstacle::maxNeighbors_ = 20;
float MovingObstacle::detectionFixedObst = 50;

bool MovingObstacle::is_inside(VEC3 p)
{
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
	return false;
//	VEC3 vec, norm;
//	Obstacle * obst;
//
//	for (unsigned int i = 0; i < nbVertices; i++)
//	{
//		vec = VEC3(getPosition(i+1 % nbVertices)- getPosition(i));
//		norm[0] = vec[1];
//		norm[1] = -vec[0];
//
//		vec = VEC3(getPosition(i+1 % nbVertices) -p);
//		if (vec*norm < 0)
//			return false;
//	}
//
//	return true;
pitiot's avatar
maj    
pitiot committed
35
36
}

pitiot's avatar
pitiot committed
37
float get_angle(VEC3 v1, VEC3 v2) //renvoie l'angle entre [- pi ; Pi] du v2 à v1
pitiot's avatar
maj    
pitiot committed
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
{
	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
54
55
56
57
58
59
60
		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
61
62
63
	return flo;
}

pitiot's avatar
pitiot committed
64
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
65
66
67
68
69
70
71
72
73
{
	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;
}

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

pitiot's avatar
pitiot committed
91
//	movingObstNeighbors_.reserve(maxNeighbors_);
pitiot's avatar
pitiot committed
92
	curGoal_ = 1;
pitiot's avatar
maj    
pitiot committed
93
94
95
96
97
98
	nb_agents_voisins = 0;
	nb_register_cells = 0;
	float pos_max = 0;

	velocity_factor = 0.8f;
	nbVertices = pos.size();
99
100
	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
101
102

	obstacles_ = new Obstacle*[nbVertices];
103
104
	belonging_cells = new std::vector<Dart>[nbVertices];
	neighbor_cells = new std::vector<Dart>[nbVertices];
105
106
107
108
109
110
111
112
113
114
115
//	for (unsigned int i = 0; i < nbVertices; ++i)

	position = map.addAttribute<VEC3, VERTEX>("position") ;
	normal = map.addAttribute<VEC3, VERTEX>("normal") ;
	deformation = map.addAttribute<VEC3, VERTEX>("deformation") ;
	velocity = map.addAttribute<VEC3, VERTEX>("velocity") ;
	edgeLength = map.addAttribute<float, EDGE>("edgeLength") ;

	groundFace = map.newFace(nbVertices);

	d = groundFace;
pitiot's avatar
maj    
pitiot committed
116
117
	for (unsigned int i = 0; i < nbVertices; ++i)
	{
118
119
120
121
122
123
		position[d] = pos[i];
		deformation[d] = VEC3(0);
		velocity[d] = VEC3(0);
		center += pos[i];

		d = map.phi1(d);
pitiot's avatar
pitiot committed
124
	}
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145

	if(!rigid_)
	{
		//extrude face to build a cage
		// compute edgeLength for mass-spring
		Algo::Modelisation::extrudeFace<PFP>(map, position, d, 5.0f) ;
		map.fillHole(groundFace);
		groundFace = map.phi2(groundFace);

		Algo::Modelisation::EarTriangulation<PFP> et(map);
		et.triangule();

		TraversorE<PFP::MAP> tE(map);
		for(Dart d = tE.begin() ; d != tE.end() ; d = tE.next())
		{
			edgeLength[d] = VEC3(position[map.phi1(d)]-position[d]).norm();
		}
	}

	center /= nbVertices;
	front=(pos[1] + pos[2]) / 2;
pitiot's avatar
merging    
pitiot committed
146
147
148

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

149
150
	length = (pos[0]-pos[1]).norm();
	width  = (pos[1]-pos[2]).norm();
pitiot's avatar
merging    
pitiot committed
151
152
153
154
155
156
157
	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
158
159
160
161
	{
		angle = get_angle(goals_[curGoal_] - center,front  - center);
		for (unsigned int i = 0; i < nbVertices; ++i)
		{
162
			position[i]+=rotate(position[i], center, angle);
pitiot's avatar
pitiot committed
163
164
165

		}
		angle=0;
pitiot's avatar
maj    
pitiot committed
166
167
168
	}
	for (unsigned int i = 0; i < nbVertices; ++i)
	{
pitiot's avatar
pitiot committed
169

170
171
172
173
		Obstacle* o = new Obstacle(position[i],
								position[(i + 1) % nbVertices],
								position[(i - 1 + nbVertices) % nbVertices],
								position[(i + 2) % nbVertices], this, i);
pitiot's avatar
maj    
pitiot committed
174
175
		obstacles_[i] = o;

pitiot's avatar
pitiot committed
176
		sim_->envMap_.pushObstacleInCells(o);
pitiot's avatar
maj    
pitiot committed
177
178
179
180
181
182
183
184
185
186
187
188
189
	}

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

190
191
VEC3 MovingObstacle::getDilatedPosition(unsigned int ind)
{
192
193
	Dart d(ind); //WARNING : works only for one face created at start !
	return position[d]+deformation[d];
194
195
196
197
}

VEC3 MovingObstacle::getPosition(unsigned int ind)
{
198
199
	Dart d(ind);
	return position[d];
200
201
}

pitiot's avatar
maj    
pitiot committed
202
203
204
205
bool MovingObstacle::test_opposition(VEC3 o, VEC3 p1, VEC3 p2)
{
	o.normalize();
	o *= -1;
206
207
208
	VEC3 vector(p1 - p2);
	vector.normalize();
	return (o - vector).norm2() < 0.1;
pitiot's avatar
maj    
pitiot committed
209
210
211
}

// TODO Check position
pitiot's avatar
pitiot committed
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
//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
266

pitiot's avatar
pitiot committed
267
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
268
{
pitiot's avatar
pitiot committed
269
270
271
272
273
//	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
274
//	{
pitiot's avatar
pitiot committed
275
276
277
278
279
280
//		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
281
//
pitiot's avatar
pitiot committed
282
283
284
285
//		for (std::vector<Agent*>::const_iterator it = agents.begin(); it != agents.end(); ++it)
//		{
//			if ((*it)->alive)
//			{
pitiot's avatar
pitiot committed
286
//
pitiot's avatar
pitiot committed
287
288
289
290
//				float distSq = (center - (*it)->getPosition()).norm2() ;
//				if ((agentNeighbors_.size() < maxNeighbors_ || distSq < maxDist)
//					&& distSq < neighborDistSq_)
//				{
pitiot's avatar
pitiot committed
291
//
pitiot's avatar
pitiot committed
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
//					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
327
//	}
pitiot's avatar
pitiot committed
328
329
330
331
332
333
}

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
334
//
pitiot's avatar
pitiot committed
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
//	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
351
//
pitiot's avatar
pitiot committed
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
//		}
//	}
//	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
368
369
370
371
}

void MovingObstacle::update()
{
pitiot's avatar
pitiot committed
372
	assert(sim_->envMap_.map.getCurrentLevel() == sim_->envMap_.map.getMaxLevel()) ;
373
374
375
	if(sim_->detect_agent_collision)
		general_belonging.clear();

pitiot's avatar
merging    
pitiot committed
376
377
378
379
380
381
382
	if(!seen)
	{
		color1=1.0f;
		color2=1.0f;
		color3=1.0f;
	}
	seen=false;
383
	PFP::VEC3 bary(0);
pitiot's avatar
maj    
pitiot committed
384
385

	Dart d;
pitiot's avatar
pitiot committed
386
387
	velocity_[0] = newVelocity_[0] * velocity_factor;
	velocity_[1] = newVelocity_[1] * velocity_factor;
pitiot's avatar
maj    
pitiot committed
388
389

	// MAJ des particules
pitiot's avatar
pitiot committed
390
	float abs_angle= angle > 0 ? 1 : -1;
pitiot's avatar
merge    
pitiot committed
391

pitiot's avatar
merging    
pitiot committed
392
	float rotor=0;
393

pitiot's avatar
merging    
pitiot committed
394
395
396
397
	if (index_parent==0)
		rotor = abs_angle*angle > 0.04f ? 0.04f : abs_angle*angle ;
	else
		rotor = abs_angle*angle ;
pitiot's avatar
merge    
pitiot committed
398

399
	if(!rigid_)
pitiot's avatar
maj    
pitiot committed
400
	{
401
402
		Dart d = groundFace;
		map.next(d);
403

404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
		CellMarkerStore<EDGE> cm(map);
		for (unsigned int i = 1; i < nbVertices; ++i)
		{
			Dart dd = d;
			do {
				if(!cm.isMarked(dd))
				{
					cm.mark(dd);
					VEC3 v1 = (position[map.phi1(dd)]-position[dd]);

					//inertie
					velocity[dd] = velocity[dd]*0.9f;

					//stretch spring : /!\ max rigidity relative to the timestep used (unstable otherwise)
					float norm = v1.norm();
					float rigidity = 500.0f;
					float stretch = rigidity*(edgeLength[dd]-v1.norm());
					VEC3 f = stretch*(v1/norm)*sim_->timeStep_;

					//apply force symmetrically
					velocity[dd] -= f;
					velocity[map.phi1(dd)] += f;
				}
427

428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
				dd = map.phi1(dd);
			} while(dd!=d);

			map.next(d);
		}

		//guiding vertex = first vertex (set the displacement)
		velocity[groundFace] = velocity_;

		//apply force to each vertex
		d = groundFace;
		for (unsigned int i = 0; i < nbVertices; ++i)
		{
			position[d] += (velocity[d] * sim_->timeStep_);
			position[map.phi<211>(d)] += (velocity[d] * sim_->timeStep_);
			bary += position[d];
			map.next(d);
		}
	}
pitiot's avatar
pitiot committed
447
	else
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
	{
	//	CGoGNout << "Obstacle "<< index << CGoGNendl;
//		CGoGNout << "vitesse : "<< velocity_ << CGoGNendl;
		//	on fait tourner l'obstacle
		Dart d = groundFace;
		for (unsigned int i = 0; i < nbVertices; ++i)
		{
			position[d] += rotate(position[d], center, abs_angle*rotor);
			position[d] += (velocity_ * sim_->timeStep_);

//			position[map.phi<211>(d)] += (velocity_ * sim_->timeStep_);

			bary += position[d];

			map.next(d);
		}

		front=(position[1] + position[2]) / 2;
		if(angle >0)
			angle -= rotor;
		else
			angle += rotor;
	}
pitiot's avatar
maj    
pitiot committed
471

pitiot's avatar
pitiot committed
472
	center = bary / nbVertices;
pitiot's avatar
maj    
pitiot committed
473

474
475
	//computing deformation vector on vertices :
	//depending on the velocity, extend the perceived shape
476
	VEC3 vel = velocity_.normalized();
477
478
479

	d = groundFace;
	for (unsigned int i = 0; i < nbVertices; ++i)
480
	{
481
		VEC3 v(position[d] - center);
482
483
		VEC3 vN = v.normalized();
		float dot = vN * vel;
484
485
		deformation[d] = v * 2.0f * (dot+1.0f)/2.0f;
		map.next(d);
486
487
	}

488
489
490
	if(!rigid_)
		center = position[groundFace];

pitiot's avatar
merging    
pitiot committed
491
492
	//-------- code ajoute par Arash pour les obstacles rectangulaires --------------

493
	VEC3 P0_P1 = position[1] - position[0];
pitiot's avatar
merging    
pitiot committed
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
	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
510
511
512
513
514
	// MAJ des obstacles
	for (unsigned int i = 0; i < nbVertices; ++i)
	{
		//		CGoGNout << "avant une etape : Obstacle "<< i << CGoGNendl;
		Obstacle* o = obstacles_[i];
515
516
517
518
		o->p1 = getDilatedPosition(i);
		o->p2 = getDilatedPosition((i+1) % nbVertices);
		o->prevP = getDilatedPosition((i - 1 + nbVertices) % nbVertices);
		o->nextP = getDilatedPosition((i + 2 + nbVertices) % nbVertices);
pitiot's avatar
pitiot committed
519
520
		sim_->envMap_.popAndPushObstacleInCells(o,i);

521
522
523
524
525
526
527
		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
528
529
530
531
532
533
		//		CGoGNout << "Apres une etape : Obstacle "<< i << CGoGNendl;
	}

	//
	//		envMap_->addMovingObstAsNeighbor(this,general_belonging,&(general_neighbors));
	//		CGoGNout << "nouvel update : "<< CGoGNendl;
534
535
536
537
538
539
540
	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
541
				if (this->is_inside((*it)->part_.getPosition()))
542
543
544
545
546
547
548
549
				{
					(*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
550

551
552
		}
	}
pitiot's avatar
maj    
pitiot committed
553
554
555

}

556
std::vector<Dart> MovingObstacle::getMemoCross(const VEC3& pos, const VEC3& dest, Dart& d1)
pitiot's avatar
maj    
pitiot committed
557
{
pitiot's avatar
pitiot committed
558
559
	registering_part->move(pos);

560
	d1=registering_part->d;
561
//	CGoGNout<<"d1 : "<< *d1<<"|| d2 : "<< *d2<<"|| start : "<< pos<<"|| stop : "<< dest<<CGoGNendl;
pitiot's avatar
pitiot committed
562
	return (registering_part->move(dest));
pitiot's avatar
maj    
pitiot committed
563
564
}

565
void resetPartSubdiv(Obstacle* o)
pitiot's avatar
maj    
pitiot committed
566
567
{
	MovingObstacle * mo = o->mo;
568

pitiot's avatar
maj    
pitiot committed
569
570
	if (mo != NULL)
	{
571
		VEC3 pos =mo->registering_part->getPosition();
pitiot's avatar
merging    
pitiot committed
572
573
		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)) ;

574
		mo->registering_part->setState(FACE) ;
pitiot's avatar
pitiot committed
575
		mo->registering_part->move(pos) ;
pitiot's avatar
maj    
pitiot committed
576
577
578
579
580
581
	}
}

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

583
584
	if (mo != NULL)
	{
585
		VEC3 pos1 = mo->registering_part->getPosition();
586
		if (Algo::Geometry::isPointInConvexFace2D <PFP> (mo->sim_->envMap_.map, d1, mo->sim_->envMap_.position, pos1, true))
587
			mo->registering_part->d = d1;
pitiot's avatar
maj    
pitiot committed
588
589
590
	}
}

591
void resetPart(MovingObstacle * mo, Dart d1)
pitiot's avatar
maj    
pitiot committed
592
{
593
594
	if (mo->registering_part->d == mo->sim_->envMap_.map.phi1(d1))
		mo->registering_part->d = d1;
pitiot's avatar
maj    
pitiot committed
595
596
597
598
599
600
}

void displayMO(Obstacle * o)
{
	CGoGNout << "obstacle problematique : " << (o->mo->index) << CGoGNendl;
}
pitiot's avatar
pitiot committed
601
602
603
604
605
606
607
608
609
610

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////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
611
612
	VEC3 goalVector;
	if (index_parent<1)
pitiot's avatar
pitiot committed
613
614
	{
		goalVector = goals_[curGoal_] - center ;
615
//		goalVector = goals_[curGoal_] - position[groundFace] ;
pitiot's avatar
pitiot committed
616

pitiot's avatar
merging    
pitiot committed
617
618
619
620
621
622
		float goalDist2 = goalVector.norm2() ;

		if (goalDist2 < 5.0f)
		{
			curGoal_ = (curGoal_ + 1) % goals_.size() ;
			goalVector = goals_[curGoal_] - center ;
623
//			goalVector = goals_[curGoal_] - position[groundFace] ;
pitiot's avatar
merging    
pitiot committed
624
625
626
627
628
629
630
631
632
633
			goalDist2 = goalVector.norm2() ;
		}

		if (goalDist2 > maxSpeed_)
		{
			goalVector.normalize() ;
			goalVector *= maxSpeed_;
		}
	}
	else
pitiot's avatar
pitiot committed
634
	{
pitiot's avatar
merging    
pitiot committed
635
636
637
638
639
640
641
		goalVector = get_center(parent,index_parent-1) -center;
		float goalDist2 = goalVector.norm2() ;
		if (goalDist2 > maxSpeed_)
		{
			goalVector.normalize() ;
			goalVector *= maxSpeed_;
		}
pitiot's avatar
pitiot committed
642
	}
643
644
645

	if (spinning)
		angle =get_angle(goalVector,front-center);
pitiot's avatar
merging    
pitiot committed
646

pitiot's avatar
pitiot committed
647
648
649
650
651
	prefVelocity_ = goalVector ;

}
void MovingObstacle::computeNewVelocity()  //comportement des obstacles en tenant compte de l'environnement.
{
pitiot's avatar
pitiot committed
652
653
654
655
656
	if (spinning)
	{
		VEC3 forward = front-center;
		float goalDist2 = forward.norm2() ;
		float objective = prefVelocity_.norm2();
657

pitiot's avatar
pitiot committed
658
		if (goalDist2 > objective)
659
660
661
662
663
664
665
666
		{
			forward.normalize() ;
			forward *= objective;
		}

		if(index_parent==0 && angle>0.01f)
			forward/=10;

pitiot's avatar
pitiot committed
667
668
669
670
671
672
		newVelocity_=forward;
	}
	else
	{
		newVelocity_=prefVelocity_;
	}
pitiot's avatar
pitiot committed
673
}