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
76
77


MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, std::vector<VEC3> goals, bool rigid, bool spin, Dart dInside, ArticulatedObstacle * art, int indParent) :
		nbVertices(pos.size()),
78
		center(0),
pitiot's avatar
maj    
pitiot committed
79
		index(ind),
80
		goals_(goals),
81
82
83
84
85
86
		curGoal_(1),
		velocity_factor(0.8f),
		color1(1.0f),
		color2(1.0f),
		color3(1.0f),
		seen(false),
pitiot's avatar
maj    
pitiot committed
87
		newVelocity_(0),
pitiot's avatar
merging    
pitiot committed
88
		sim_(sim),
89
90
		rigid_(rigid),
		spinning(spin),
pitiot's avatar
merging    
pitiot committed
91
		parent(art),
92
		index_parent(indParent)
pitiot's avatar
maj    
pitiot committed
93
94
95
{
	assert(pos.size() > 2);

96
97
	if(dInside==NIL)
		dInside = sim_->envMap_.getBelongingCell(pos[0]);
pitiot's avatar
maj    
pitiot committed
98

99
	registering_part = new CGoGN::Algo::MovingObjects::ParticleCell2DMemo<PFP>(sim_->envMap_.map, dInside, pos[0], sim_->envMap_.position);
pitiot's avatar
maj    
pitiot committed
100
101

	obstacles_ = new Obstacle*[nbVertices];
102
103
	belonging_cells = new std::vector<Dart>[nbVertices];
	neighbor_cells = new std::vector<Dart>[nbVertices];
104
105
106
107

	position = map.addAttribute<VEC3, VERTEX>("position") ;
	normal = map.addAttribute<VEC3, VERTEX>("normal") ;
	deformation = map.addAttribute<VEC3, VERTEX>("deformation") ;
108
109
110
111
112
113

	if(!rigid_)
	{
		velocity = map.addAttribute<VEC3, VERTEX>("velocity") ;
		edgeLength = map.addAttribute<float, EDGE>("edgeLength") ;
	}
114
115
116

	groundFace = map.newFace(nbVertices);

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

		if(!rigid_)
			velocity[d] = VEC3(0);

126
127
128
		center += pos[i];

		d = map.phi1(d);
pitiot's avatar
pitiot committed
129
	}
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150

	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
151
152
153

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

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

		}
		angle=0;
pitiot's avatar
maj    
pitiot committed
171
	}
172

pitiot's avatar
maj    
pitiot committed
173
174
	for (unsigned int i = 0; i < nbVertices; ++i)
	{
pitiot's avatar
pitiot committed
175

176
177
178
179
		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
180
181
		obstacles_[i] = o;

pitiot's avatar
pitiot committed
182
		sim_->envMap_.pushObstacleInCells(o);
pitiot's avatar
maj    
pitiot committed
183
184
	}

185
186
187
188
189
190
191
192
193
194
//	float pos_max = 0;
//
//	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
maj    
pitiot committed
195
196
}

197
198
VEC3 MovingObstacle::getDilatedPosition(unsigned int ind)
{
199
	Dart d(ind); //WARNING : works only for one face created at start !
Thomas Jund's avatar
Thomas Jund committed
200
201
	return position[d]+deformation[d];
//	return position[d];
202
203
204
205
}

VEC3 MovingObstacle::getPosition(unsigned int ind)
{
206
207
	Dart d(ind);
	return position[d];
208
209
}

pitiot's avatar
maj    
pitiot committed
210
211
212
213
bool MovingObstacle::test_opposition(VEC3 o, VEC3 p1, VEC3 p2)
{
	o.normalize();
	o *= -1;
214
215
216
	VEC3 vector(p1 - p2);
	vector.normalize();
	return (o - vector).norm2() < 0.1;
pitiot's avatar
maj    
pitiot committed
217
218
219
}

// TODO Check position
pitiot's avatar
pitiot committed
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
266
267
268
269
270
271
272
273
//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
274

pitiot's avatar
pitiot committed
275
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
276
{
pitiot's avatar
pitiot committed
277
278
279
280
281
//	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
282
//	{
pitiot's avatar
pitiot committed
283
284
285
286
287
288
//		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
289
//
pitiot's avatar
pitiot committed
290
291
292
293
//		for (std::vector<Agent*>::const_iterator it = agents.begin(); it != agents.end(); ++it)
//		{
//			if ((*it)->alive)
//			{
pitiot's avatar
pitiot committed
294
//
pitiot's avatar
pitiot committed
295
296
297
298
//				float distSq = (center - (*it)->getPosition()).norm2() ;
//				if ((agentNeighbors_.size() < maxNeighbors_ || distSq < maxDist)
//					&& distSq < neighborDistSq_)
//				{
pitiot's avatar
pitiot committed
299
//
pitiot's avatar
pitiot committed
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
327
328
329
330
331
332
333
334
//					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
335
//	}
pitiot's avatar
pitiot committed
336
337
338
339
340
341
}

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
342
//
pitiot's avatar
pitiot committed
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
//	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
359
//
pitiot's avatar
pitiot committed
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
//		}
//	}
//	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
376
377
378
379
}

void MovingObstacle::update()
{
pitiot's avatar
pitiot committed
380
	assert(sim_->envMap_.map.getCurrentLevel() == sim_->envMap_.map.getMaxLevel()) ;
381
382
383
	if(sim_->detect_agent_collision)
		general_belonging.clear();

pitiot's avatar
merging    
pitiot committed
384
385
386
387
388
389
390
	if(!seen)
	{
		color1=1.0f;
		color2=1.0f;
		color3=1.0f;
	}
	seen=false;
391
	PFP::VEC3 bary(0);
pitiot's avatar
maj    
pitiot committed
392
393

	Dart d;
pitiot's avatar
pitiot committed
394
395
	velocity_[0] = newVelocity_[0] * velocity_factor;
	velocity_[1] = newVelocity_[1] * velocity_factor;
Arash HABIBI's avatar
Arash HABIBI committed
396
397
	// velocity_[0] = 0.0;
	// velocity_[1] = 0.0;
pitiot's avatar
maj    
pitiot committed
398
399

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

pitiot's avatar
merging    
pitiot committed
402
	float rotor=0;
403

pitiot's avatar
merging    
pitiot committed
404
405
406
407
	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
408

409
	if(!rigid_)
pitiot's avatar
maj    
pitiot committed
410
	{
411
412
		Dart d = groundFace;
		map.next(d);
413

414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
		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;
				}
437

438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
				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
457
	else
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
	{
	//	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
481

pitiot's avatar
pitiot committed
482
	center = bary / nbVertices;
pitiot's avatar
maj    
pitiot committed
483

484
485
	//computing deformation vector on vertices :
	//depending on the velocity, extend the perceived shape
486
	VEC3 vel = velocity_.normalized();
487
488
489

	d = groundFace;
	for (unsigned int i = 0; i < nbVertices; ++i)
490
	{
491
		VEC3 v(position[d] - center);
492
493
		VEC3 vN = v.normalized();
		float dot = vN * vel;
494
495
		deformation[d] = v * 2.0f * (dot+1.0f)/2.0f;
		map.next(d);
496
497
	}

498
499
500
	if(!rigid_)
		center = position[groundFace];

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

503
	VEC3 P0_P1 = position[1] - position[0];
pitiot's avatar
merging    
pitiot committed
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
	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
520
521
522
523
524
	// MAJ des obstacles
	for (unsigned int i = 0; i < nbVertices; ++i)
	{
		//		CGoGNout << "avant une etape : Obstacle "<< i << CGoGNendl;
		Obstacle* o = obstacles_[i];
525
526
527
528
		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
529
530
		sim_->envMap_.popAndPushObstacleInCells(o,i);

531
532
533
534
535
536
537
		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
538
539
540
541
542
543
		//		CGoGNout << "Apres une etape : Obstacle "<< i << CGoGNendl;
	}

	//
	//		envMap_->addMovingObstAsNeighbor(this,general_belonging,&(general_neighbors));
	//		CGoGNout << "nouvel update : "<< CGoGNendl;
544
545
546
547
548
549
550
	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
551
				if (this->is_inside((*it)->part_.getPosition()))
552
553
554
555
556
557
558
559
				{
					(*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
560

561
562
		}
	}
pitiot's avatar
maj    
pitiot committed
563
564
565

}

566
std::vector<Dart> MovingObstacle::getMemoCross(const VEC3& pos, const VEC3& dest, Dart& d1)
pitiot's avatar
maj    
pitiot committed
567
{
pitiot's avatar
pitiot committed
568
569
	registering_part->move(pos);

570
	d1=registering_part->d;
571
//	CGoGNout<<"d1 : "<< *d1<<"|| d2 : "<< *d2<<"|| start : "<< pos<<"|| stop : "<< dest<<CGoGNendl;
pitiot's avatar
pitiot committed
572
	return (registering_part->move(dest));
pitiot's avatar
maj    
pitiot committed
573
574
}

575
void resetPartSubdiv(Obstacle* o)
pitiot's avatar
maj    
pitiot committed
576
577
{
	MovingObstacle * mo = o->mo;
578

pitiot's avatar
maj    
pitiot committed
579
580
	if (mo != NULL)
	{
581
		VEC3 pos =mo->registering_part->getPosition();
pitiot's avatar
merging    
pitiot committed
582
583
		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)) ;

584
		mo->registering_part->setState(FACE) ;
pitiot's avatar
pitiot committed
585
		mo->registering_part->move(pos) ;
pitiot's avatar
maj    
pitiot committed
586
587
588
589
590
591
	}
}

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

593
594
	if (mo != NULL)
	{
595
		VEC3 pos1 = mo->registering_part->getPosition();
596
		if (Algo::Geometry::isPointInConvexFace2D <PFP> (mo->sim_->envMap_.map, d1, mo->sim_->envMap_.position, pos1, true))
597
			mo->registering_part->d = d1;
pitiot's avatar
maj    
pitiot committed
598
599
600
	}
}

601
void resetPart(MovingObstacle * mo, Dart d1)
pitiot's avatar
maj    
pitiot committed
602
{
603
604
	if (mo->registering_part->d == mo->sim_->envMap_.map.phi1(d1))
		mo->registering_part->d = d1;
pitiot's avatar
maj    
pitiot committed
605
606
607
608
609
610
}

void displayMO(Obstacle * o)
{
	CGoGNout << "obstacle problematique : " << (o->mo->index) << CGoGNendl;
}
pitiot's avatar
pitiot committed
611
612
613
614
615
616
617
618
619
620

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////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
621
622
	VEC3 goalVector;
	if (index_parent<1)
pitiot's avatar
pitiot committed
623
624
	{
		goalVector = goals_[curGoal_] - center ;
625
//		goalVector = goals_[curGoal_] - position[groundFace] ;
pitiot's avatar
pitiot committed
626

pitiot's avatar
merging    
pitiot committed
627
628
629
630
631
632
		float goalDist2 = goalVector.norm2() ;

		if (goalDist2 < 5.0f)
		{
			curGoal_ = (curGoal_ + 1) % goals_.size() ;
			goalVector = goals_[curGoal_] - center ;
633
//			goalVector = goals_[curGoal_] - position[groundFace] ;
pitiot's avatar
merging    
pitiot committed
634
635
636
637
638
639
640
641
642
643
			goalDist2 = goalVector.norm2() ;
		}

		if (goalDist2 > maxSpeed_)
		{
			goalVector.normalize() ;
			goalVector *= maxSpeed_;
		}
	}
	else
pitiot's avatar
pitiot committed
644
	{
pitiot's avatar
merging    
pitiot committed
645
646
647
648
649
650
651
		goalVector = get_center(parent,index_parent-1) -center;
		float goalDist2 = goalVector.norm2() ;
		if (goalDist2 > maxSpeed_)
		{
			goalVector.normalize() ;
			goalVector *= maxSpeed_;
		}
pitiot's avatar
pitiot committed
652
	}
653
654
655

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

pitiot's avatar
pitiot committed
657
658
659
660
661
	prefVelocity_ = goalVector ;

}
void MovingObstacle::computeNewVelocity()  //comportement des obstacles en tenant compte de l'environnement.
{
pitiot's avatar
pitiot committed
662
663
664
665
666
	if (spinning)
	{
		VEC3 forward = front-center;
		float goalDist2 = forward.norm2() ;
		float objective = prefVelocity_.norm2();
667

pitiot's avatar
pitiot committed
668
		if (goalDist2 > objective)
669
670
671
672
673
674
675
676
		{
			forward.normalize() ;
			forward *= objective;
		}

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

pitiot's avatar
pitiot committed
677
678
679
680
681
682
		newVelocity_=forward;
	}
	else
	{
		newVelocity_=prefVelocity_;
	}
pitiot's avatar
pitiot committed
683
}