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

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

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

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

bool MovingObstacle::is_inside(VEC3 p)
{
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
	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
36
37
}

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

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

75
76
77
78


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()),
79
		center(0),
pitiot's avatar
maj    
pitiot committed
80
		index(ind),
81
		goals_(goals),
82
83
84
85
86
87
		curGoal_(1),
		velocity_factor(0.8f),
		color1(1.0f),
		color2(1.0f),
		color3(1.0f),
		seen(false),
pitiot's avatar
maj    
pitiot committed
88
		newVelocity_(0),
pitiot's avatar
merging    
pitiot committed
89
		sim_(sim),
90
91
		rigid_(rigid),
		spinning(spin),
pitiot's avatar
merging    
pitiot committed
92
		parent(art),
Thomas Jund's avatar
Thomas Jund committed
93
		ag_(NULL),
94
		index_parent(indParent)
pitiot's avatar
maj    
pitiot committed
95
96
97
{
	assert(pos.size() > 2);

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

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

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

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

	if(!rigid_)
	{
		velocity = map.addAttribute<VEC3, VERTEX>("velocity") ;
Thomas Jund's avatar
Thomas Jund committed
114
		forces = map.addAttribute<VEC3, VERTEX>("force") ;
115
		edgeLength = map.addAttribute<float, EDGE>("edgeLength") ;
Thomas Jund's avatar
Thomas Jund committed
116
		vertexAngle = map.addAttribute<float, DART>("vertexAngle") ;
117
	}
118
119
120

	groundFace = map.newFace(nbVertices);

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

		if(!rigid_)
Thomas Jund's avatar
Thomas Jund committed
128
		{
129
			velocity[d] = VEC3(0);
Thomas Jund's avatar
Thomas Jund committed
130
131
			forces[d] = VEC3(0);
		}
132

133
134
135
		center += pos[i];

		d = map.phi1(d);
pitiot's avatar
pitiot committed
136
	}
137
138
139
140
141

	if(!rigid_)
	{
		//extrude face to build a cage
		// compute edgeLength for mass-spring
142
		Algo::Modelisation::extrudeFace<PFP>(map, position, d, 10.0f) ;
143
144
145
146
147
148
149
150
151
152
153
		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();
		}
Thomas Jund's avatar
Thomas Jund committed
154

Thomas Jund's avatar
Thomas Jund committed
155
156
157
158
159
160
161
162
163
164
		DartMarker treated(map);
		for(Dart d = map.begin() ; d != map.end() ; map.next(d))
		{
			if(!map.isBoundaryMarked(d) && !treated.isMarked(d))
			{
				treated.mark(d);
				vertexAngle[d] = Algo::Geometry::angle<PFP>(map,map.phi_1(d),map.phi1(d),position);
			}
		}

165
		map.enableQuickTraversal<VERTEX>();
166

Thomas Jund's avatar
Thomas Jund committed
167
		dDir=dInside;
pitiot's avatar
pitiot committed
168
	}
169
170
171

	center /= nbVertices;
	front=(pos[1] + pos[2]) / 2;
Arash HABIBI's avatar
Arash HABIBI committed
172
173
174

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

175
176
	length = (pos[0]-pos[1]).norm();
	width  = (pos[1]-pos[2]).norm();
Arash HABIBI's avatar
Arash HABIBI committed
177
178
179
180
181
182
	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 -------

pitiot's avatar
merging    
pitiot committed
183
	if (spinning && parent==NULL) //départ face à la cible en cas d'obstacles pouvant effectuer des rotations
pitiot's avatar
pitiot committed
184
185
186
187
	{
		angle = get_angle(goals_[curGoal_] - center,front  - center);
		for (unsigned int i = 0; i < nbVertices; ++i)
		{
188
			position[i]+=rotate(position[i], center, angle);
pitiot's avatar
pitiot committed
189
190
191

		}
		angle=0;
pitiot's avatar
maj    
pitiot committed
192
	}
193

pitiot's avatar
maj    
pitiot committed
194
195
	for (unsigned int i = 0; i < nbVertices; ++i)
	{
pitiot's avatar
pitiot committed
196

197
198
199
200
		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
201
202
		obstacles_[i] = o;

pitiot's avatar
pitiot committed
203
		sim_->envMap_.pushObstacleInCells(o);
pitiot's avatar
maj    
pitiot committed
204
205
	}

206
207
208
209
210
211
212
213
214
215
//	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
216
217
}

218
219
VEC3 MovingObstacle::getDilatedPosition(unsigned int ind)
{
220
	Dart d(ind); //WARNING : works only for one face created at start !
Thomas Jund's avatar
Thomas Jund committed
221
222
	return position[d]+deformation[d];
//	return position[d];
223
224
225
226
}

VEC3 MovingObstacle::getPosition(unsigned int ind)
{
227
228
	Dart d(ind);
	return position[d];
229
230
}

pitiot's avatar
maj    
pitiot committed
231
232
233
234
bool MovingObstacle::test_opposition(VEC3 o, VEC3 p1, VEC3 p2)
{
	o.normalize();
	o *= -1;
235
236
237
	VEC3 vector(p1 - p2);
	vector.normalize();
	return (o - vector).norm2() < 0.1;
pitiot's avatar
maj    
pitiot committed
238
239
240
}

// TODO Check position
pitiot's avatar
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
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
//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
295

pitiot's avatar
pitiot committed
296
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
297
{
Thomas Jund's avatar
Thomas Jund committed
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
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
	//test for slugs : doesn't always work.. :)
//	if(!rigid_)
//	{
//		Dart d = registering_part->d;
//
//		std::list<MovingObstacle*> seen;
//		PFP::OBSTACLEVECT& obst = sim_->envMap_.obstvect[d] ;
//		PFP::OBSTACLEVECT& neighborObst = sim_->envMap_.neighborObstvect[d] ;
//
//		for (std::vector<Obstacle*>::iterator it = obst.begin(); it != obst.end(); ++it)
//		{
//
//			if ((*it)->mo!=NULL)
//			{
//				MovingObstacle* other = (*it)->mo;
//				if(other->velocity_ * velocity_<=0.0f) //stop when not in front of each other
//				{
//					if(velocity_*(other->registering_part->getPosition()-registering_part->getPosition())>0)
//					{
//						newVelocity_ = VEC3(newVelocity_[1], -newVelocity_[0], 0);
//						seen.push_back(other);
//					}
//				}
//				else if(velocity_*(other->registering_part->getPosition()-registering_part->getPosition())>0
//						&& (other->registering_part->getPosition()-registering_part->getPosition()).norm2()<2000.0f)
//					newVelocity_ *= 0.01f;
//			}
//		}
//
//		for (std::vector<Obstacle*>::iterator it = neighborObst.begin(); it != neighborObst.end(); ++it)
//		{
//
//			if ((*it)->mo!=NULL)
//			{
//				MovingObstacle* other = (*it)->mo;
//				if(std::find(seen.begin(),seen.end(), other)==seen.end())
//				{
//					if(other->velocity_ * velocity_<=0.0f
//							&& (other->registering_part->getPosition()-registering_part->getPosition()).norm2()<2000.0f)
//					{
//
//						if(velocity_*(other->registering_part->getPosition()-registering_part->getPosition())>0)
//						{
//							newVelocity_ = VEC3(newVelocity_[1], -newVelocity_[0], 0);
//						}
//					}
////					else if(velocity_*(other->registering_part->getPosition()-registering_part->getPosition())>0)
////						newVelocity_ *= 0.5f;
//				}
//			}
//		}
//	}

pitiot's avatar
pitiot committed
351
352
353
354
355
//	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
356
//	{
pitiot's avatar
pitiot committed
357
358
359
360
361
362
//		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
363
//
pitiot's avatar
pitiot committed
364
365
366
367
//		for (std::vector<Agent*>::const_iterator it = agents.begin(); it != agents.end(); ++it)
//		{
//			if ((*it)->alive)
//			{
pitiot's avatar
pitiot committed
368
//
pitiot's avatar
pitiot committed
369
370
371
372
//				float distSq = (center - (*it)->getPosition()).norm2() ;
//				if ((agentNeighbors_.size() < maxNeighbors_ || distSq < maxDist)
//					&& distSq < neighborDistSq_)
//				{
pitiot's avatar
pitiot committed
373
//
pitiot's avatar
pitiot committed
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
//					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
409
//	}
pitiot's avatar
pitiot committed
410
411
412
413
414
415
}

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
416
//
pitiot's avatar
pitiot committed
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
//	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
433
//
pitiot's avatar
pitiot committed
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
//		}
//	}
//	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
450
451
452
453
}

void MovingObstacle::update()
{
pitiot's avatar
pitiot committed
454
	assert(sim_->envMap_.map.getCurrentLevel() == sim_->envMap_.map.getMaxLevel()) ;
455
456
457
	if(sim_->detect_agent_collision)
		general_belonging.clear();

pitiot's avatar
merging    
pitiot committed
458
459
460
461
462
463
464
	if(!seen)
	{
		color1=1.0f;
		color2=1.0f;
		color3=1.0f;
	}
	seen=false;
465
	PFP::VEC3 bary(0);
pitiot's avatar
maj    
pitiot committed
466
467

	Dart d;
pitiot's avatar
pitiot committed
468
469
	velocity_[0] = newVelocity_[0] * velocity_factor;
	velocity_[1] = newVelocity_[1] * velocity_factor;
Arash HABIBI's avatar
Arash HABIBI committed
470
471
	// velocity_[0] = 0.0;
	// velocity_[1] = 0.0;
pitiot's avatar
maj    
pitiot committed
472
473

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

pitiot's avatar
merging    
pitiot committed
476
	float rotor=0;
477

pitiot's avatar
merging    
pitiot committed
478
479
480
481
	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
482

483
	if(!rigid_)
pitiot's avatar
maj    
pitiot committed
484
	{
485
486
		Dart d = groundFace;
		map.next(d);
pitiot's avatar
maj    
pitiot committed
487

Thomas Jund's avatar
Thomas Jund committed
488
489
490
491
492
493
494
495
		Dart dd =d;
		for (unsigned int i = 1; i < nbVertices; ++i)
		{
			//initialisation of forces with viscosity
			forces[dd] = -0.2f*velocity[dd];
			map.next(dd);
		}

496
		CellMarkerStore<EDGE> cm(map);
Thomas Jund's avatar
Thomas Jund committed
497
		DartMarkerStore dm(map);
498
499
500
501
502
503
504
		for (unsigned int i = 1; i < nbVertices; ++i)
		{
			Dart dd = d;
			do {
				if(!cm.isMarked(dd))
				{
					cm.mark(dd);
Thomas Jund's avatar
Thomas Jund committed
505
506
507
					VEC3 p1Next = position[map.phi1(dd)]+(velocity[map.phi1(dd)] * sim_->timeStep_);
					VEC3 p2Next = position[dd]+(velocity[dd] * sim_->timeStep_);
					VEC3 v1 = (p1Next-p2Next);
508
509
510

					//stretch spring : /!\ max rigidity relative to the timestep used (unstable otherwise)
					float norm = v1.norm();
Thomas Jund's avatar
Thomas Jund committed
511
					float rigidity = 40.0f;
512

513
					float stretch = rigidity*(edgeLength[dd]-v1.norm());
Thomas Jund's avatar
Thomas Jund committed
514
					VEC3 f = stretch*(v1/norm);
515

Thomas Jund's avatar
Thomas Jund committed
516
517
518
					//apply force symmetrically
					forces[dd] -= f;
					forces[map.phi1(dd)] += f;
Arash HABIBI's avatar
Arash HABIBI committed
519
520
				}

Thomas Jund's avatar
Thomas Jund committed
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
				dd = map.phi1(dd);
			} while(dd!=d);

			do {
				if(!dm.isMarked(dd))
				{
					dm.mark(dd);
//					VEC3 v1 = (position[map.phi1(dd)]-position[dd]);
					VEC3 p1Next = position[map.phi1(dd)]+(velocity[map.phi1(dd)] * sim_->timeStep_);
					VEC3 p2Next = position[dd]+(velocity[dd] * sim_->timeStep_);
					VEC3 v1 = (p1Next-p2Next);

					float norm = v1.norm();

					//bend spring: /!\ max rigidity relative to the timestep used (unstable otherwise)
					float restAngle = vertexAngle[dd];
					if(restAngle!=0.0f)
					{
						float angularRig = 40.0f;

						float curAngle = Algo::Geometry::angle<PFP>(map, map.phi_1(dd),map.phi1(dd),position);
						float angularStretch = angularRig*(restAngle-curAngle);

						VEC3 f = angularStretch*(v1/norm);
						forces[dd] += f;
						forces[map.phi1(dd)] -= f;
					}
548
				}
pitiot's avatar
maj    
pitiot committed
549

550
551
				dd = map.phi1(dd);
			} while(dd!=d);
pitiot's avatar
maj    
pitiot committed
552

553
554
555
556
			map.next(d);
		}

		//guiding vertex = first vertex (set the displacement)
Thomas Jund's avatar
Thomas Jund committed
557
		forces[groundFace] = VEC3(0);
558
		velocity[groundFace] = velocity_;
Arash HABIBI's avatar
Arash HABIBI committed
559

560
561
562
		//apply force to each vertex
		d = groundFace;
		for (unsigned int i = 0; i < nbVertices; ++i)
pitiot's avatar
pitiot committed
563
		{
Thomas Jund's avatar
Thomas Jund committed
564
			velocity[d] += forces[d] * sim_->timeStep_;
565
566
567
568
			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
569
		}
570
	}
pitiot's avatar
pitiot committed
571
572
	else
	{
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
	//	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
pitiot committed
594
	}
pitiot's avatar
maj    
pitiot committed
595

pitiot's avatar
pitiot committed
596
	center = bary / nbVertices;
pitiot's avatar
maj    
pitiot committed
597

598
599
	//computing deformation vector on vertices :
	//depending on the velocity, extend the perceived shape
600
	VEC3 vel = velocity_.normalized();
601
602
603

	d = groundFace;
	for (unsigned int i = 0; i < nbVertices; ++i)
604
	{
605
		VEC3 v(position[d] - center);
606
607
		VEC3 vN = v.normalized();
		float dot = vN * vel;
608
609
		deformation[d] = v * 2.0f * (dot+1.0f)/2.0f;
		map.next(d);
610
611
	}

Thomas Jund's avatar
Thomas Jund committed
612
613
//	if(!rigid_)
//		center = position[groundFace];
614

Arash HABIBI's avatar
Arash HABIBI committed
615
616
	//-------- code ajoute par Arash pour les obstacles rectangulaires --------------

617
	VEC3 P0_P1 = position[1] - position[0];
pitiot's avatar
merging    
pitiot committed
618
	float velocity_coef = 10.0;
Arash HABIBI's avatar
Arash HABIBI committed
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633

	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
634
635
636
637
638
	// MAJ des obstacles
	for (unsigned int i = 0; i < nbVertices; ++i)
	{
		//		CGoGNout << "avant une etape : Obstacle "<< i << CGoGNendl;
		Obstacle* o = obstacles_[i];
639
640
641
642
		o->p1 = getDilatedPosition(i);
		o->p2 = getDilatedPosition((i+1) % nbVertices);
		o->prevP = getDilatedPosition((i - 1 + nbVertices) % nbVertices);
		o->nextP = getDilatedPosition((i + 2 + nbVertices) % nbVertices);
Thomas Jund's avatar
Thomas Jund committed
643
644
645
646
		Dart d = sim_->envMap_.popAndPushObstacleInCells(o,i);

		if(i==0)
			dDir = d;
pitiot's avatar
pitiot committed
647

648
649
650
651
652
653
654
		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
655
656
657
658
659
660
		//		CGoGNout << "Apres une etape : Obstacle "<< i << CGoGNendl;
	}

	//
	//		envMap_->addMovingObstAsNeighbor(this,general_belonging,&(general_neighbors));
	//		CGoGNout << "nouvel update : "<< CGoGNendl;
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
	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)
			{
				if (this->is_inside((*it)->part_.getPosition()))
				{
					(*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
677

678
679
		}
	}
pitiot's avatar
maj    
pitiot committed
680
681
682

}

683
std::vector<Dart> MovingObstacle::getMemoCross(const VEC3& pos, const VEC3& dest, Dart& d1)
pitiot's avatar
maj    
pitiot committed
684
{
pitiot's avatar
pitiot committed
685
686
	registering_part->move(pos);

687
	d1=registering_part->d;
Thomas Jund's avatar
Thomas Jund committed
688

689
//	CGoGNout<<"d1 : "<< *d1<<"|| d2 : "<< *d2<<"|| start : "<< pos<<"|| stop : "<< dest<<CGoGNendl;
pitiot's avatar
pitiot committed
690
	return (registering_part->move(dest));
pitiot's avatar
maj    
pitiot committed
691
692
}

693
void resetPartSubdiv(Obstacle* o)
pitiot's avatar
maj    
pitiot committed
694
695
{
	MovingObstacle * mo = o->mo;
696

pitiot's avatar
maj    
pitiot committed
697
698
	if (mo != NULL)
	{
699
		VEC3 pos =mo->registering_part->getPosition();
Arash HABIBI's avatar
Arash HABIBI committed
700
701
		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)) ;

702
		mo->registering_part->setState(FACE) ;
pitiot's avatar
pitiot committed
703
		mo->registering_part->move(pos) ;
pitiot's avatar
maj    
pitiot committed
704
705
706
707
708
709
	}
}

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

711
712
	if (mo != NULL)
	{
713
		VEC3 pos1 = mo->registering_part->getPosition();
714
		if (Algo::Geometry::isPointInConvexFace2D <PFP> (mo->sim_->envMap_.map, d1, mo->sim_->envMap_.position, pos1, true))
715
			mo->registering_part->d = d1;
pitiot's avatar
maj    
pitiot committed
716
717
718
	}
}

719
void resetPart(MovingObstacle * mo, Dart d1)
pitiot's avatar
maj    
pitiot committed
720
{
721
722
	if (mo->registering_part->d == mo->sim_->envMap_.map.phi1(d1))
		mo->registering_part->d = d1;
pitiot's avatar
maj    
pitiot committed
723
724
725
726
727
728
}

void displayMO(Obstacle * o)
{
	CGoGNout << "obstacle problematique : " << (o->mo->index) << CGoGNendl;
}
pitiot's avatar
pitiot committed
729
730
731
732
733
734
735
736
737
738

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////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
739
740
	VEC3 goalVector;
	if (index_parent<1)
pitiot's avatar
pitiot committed
741
	{
Thomas Jund's avatar
Thomas Jund committed
742
		if(rigid_ || goals_.size()>1)
pitiot's avatar
merging    
pitiot committed
743
744
		{
			goalVector = goals_[curGoal_] - center ;
Thomas Jund's avatar
Thomas Jund committed
745
	//		goalVector = goals_[curGoal_] - position[groundFace] ;
pitiot's avatar
merging    
pitiot committed
746

Thomas Jund's avatar
Thomas Jund committed
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
			float goalDist2 = goalVector.norm2() ;

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

			if (goalDist2 > maxSpeed_)
			{
				goalVector.normalize() ;
				goalVector *= maxSpeed_;
			}
		}
		else
pitiot's avatar
merging    
pitiot committed
764
		{
Thomas Jund's avatar
Thomas Jund committed
765
766
767
768
769
770
771
772
773
774
			if(ag_!=NULL)
			{
				goalVector = ag_->getPosition() - center ;
				if (goalVector.norm2() > maxSpeed_)
				{
					goalVector.normalize() ;
					goalVector *= maxSpeed_;
				}
			}
			else
Thomas Jund's avatar
Thomas Jund committed
775
			{
Thomas Jund's avatar
Thomas Jund committed
776
777
	//			if(sim_->envMap_.pedWayMark.isMarked(sim_->envMap_.map.phi2(dDir)))
	//				dDir = sim_->envMap_.map.phi1(sim_->envMap_.map.phi1(dDir));
Thomas Jund's avatar
Thomas Jund committed
778

Thomas Jund's avatar
Thomas Jund committed
779
				VEC3 mid = (sim_->envMap_.position[dDir]+sim_->envMap_.position[sim_->envMap_.map.phi1(dDir)])*0.5f;
Thomas Jund's avatar
Thomas Jund committed
780
781
				goalVector = mid - position[0] ;

Thomas Jund's avatar
Thomas Jund committed
782
783
784
785
786
787
788
				Dart dStart=dDir;
				VEC3 dir = center - position[0];
				while(goalVector.norm2()==0 || sim_->envMap_.pedWayMark.isMarked(sim_->envMap_.map.phi2(dDir)) || ((dir*goalVector)>0))
				{
	//				std::cout << "d " << dDir << std::endl;
	//				std::cout << "dir*goalVector " << dir*goalVector << std::endl;
	//				std::cout << "goal vec " << goalVector << std::endl;
Thomas Jund's avatar
Thomas Jund committed
789

Thomas Jund's avatar
Thomas Jund committed
790
791
792
					dDir = sim_->envMap_.map.phi_1(dDir);
					mid = (sim_->envMap_.position[dDir]+sim_->envMap_.position[sim_->envMap_.map.phi1(dDir)])*0.5f;
					goalVector = mid - position[0] ;
Thomas Jund's avatar
Thomas Jund committed
793

Thomas Jund's avatar
Thomas Jund committed
794
795
796
					if(dDir==dStart)
						break;
				}
Thomas Jund's avatar
Thomas Jund committed
797

Thomas Jund's avatar
Thomas Jund committed
798
799
				if(sim_->envMap_.map.isBoundaryEdge(sim_->envMap_.map.phi2(dDir)))
					goalVector = VEC3(0);
Thomas Jund's avatar
Thomas Jund committed
800

Thomas Jund's avatar
Thomas Jund committed
801
802
803
804
805
806
807
808
809
810
	//			goalVector = mid - center ;

	//			std::cout << "dDir " << dDir << " goalVec" << goalVector << std::endl;

	//			float goalDist2 = goalVector.norm2() ;
	//			if (goalDist2 > maxSpeed_)
				{
					goalVector.normalize() ;
					goalVector *= maxSpeed_*maxSpeed_;
				}
Thomas Jund's avatar
Thomas Jund committed
811
			}
pitiot's avatar
merging    
pitiot committed
812
813
814
		}
	}
	else
pitiot's avatar
pitiot committed
815
	{
pitiot's avatar
merging    
pitiot committed
816
817
818
819
820
821
822
		goalVector = get_center(parent,index_parent-1) -center;
		float goalDist2 = goalVector.norm2() ;
		if (goalDist2 > maxSpeed_)
		{
			goalVector.normalize() ;
			goalVector *= maxSpeed_;
		}
pitiot's avatar
pitiot committed
823
	}
824
825
826

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

pitiot's avatar
pitiot committed
828
829
830
831
832
	prefVelocity_ = goalVector ;

}
void MovingObstacle::computeNewVelocity()  //comportement des obstacles en tenant compte de l'environnement.
{
pitiot's avatar
pitiot committed
833
834
835
836
837
	if (spinning)
	{
		VEC3 forward = front-center;
		float goalDist2 = forward.norm2() ;
		float objective = prefVelocity_.norm2();
838

pitiot's avatar
pitiot committed
839
		if (goalDist2 > objective)
840
841
842
843
844
845
846
847
		{
			forward.normalize() ;
			forward *= objective;
		}

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

pitiot's avatar
pitiot committed
848
849
850
851
852
853
		newVelocity_=forward;
	}
	else
	{
		newVelocity_=prefVelocity_;
	}
pitiot's avatar
pitiot committed
854
}
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924



PFP::REAL MovingObstacle::computeMVC(PFP::VEC3 p, Dart vertex)
{
	PFP::REAL r = (position[vertex]-p).norm();

	PFP::REAL sumU(0.);
	Dart it = vertex;
	do
	{
		PFP::VEC3 vi = position[it];
		PFP::VEC3 vj = position[map.phi1(it)];
		PFP::VEC3 vk = position[map.phi_1(it)];

		PFP::REAL Bjk = Geom::angle(vj - p, vk - p);
		PFP::REAL Bij = Geom::angle(vi - p, vj - p);
		PFP::REAL Bki = Geom::angle(vk - p, vi - p);

		PFP::VEC3 ei = (vi - p) / ((vi - p).norm());
		PFP::VEC3 ej = (vj - p) / ((vj - p).norm());
		PFP::VEC3 ek = (vk - p) / ((vk - p).norm());

		PFP::VEC3 nij = (ei ^ ej) / ((ei ^ ej).norm());
		PFP::VEC3 njk = (ej ^ ek) / ((ej ^ ek).norm());
		PFP::VEC3 nki = (ek ^ ei) / ((ek ^ ei).norm());

		PFP::REAL ui= (Bjk + (Bij*(nij*njk)) + (Bki*(nki*njk))) / (2.0f*ei*njk);

		sumU += ui;

		it = map.phi<21>(it);
	}
	while(it != vertex);

	return (1.0f / r) * sumU;
}

void MovingObstacle::computePointMVC(PFP::VEC3 point, std::vector<PFP::REAL>& coordinates)
{
	DartMarker mark_vertex(map);

	coordinates.clear();

	PFP::REAL sumMVC(0);
	unsigned int j = 0;
	TraversorV<PFP::MAP> t(map) ;
	for(Dart it = t.begin(); it != t.end(); it = t.next())
	{
		PFP::REAL c = computeMVC(point, it);
		coordinates.push_back(c);
		sumMVC += c;
		j++;
	}

	for(unsigned int i = 0; i < j; i++)
		coordinates[i] /= sumMVC;
}

void MovingObstacle::attachMesh(MovingMesh* mm)
{
	mm_ = mm;
	mvc_ = mm->map.getAttribute<NoMathIONameAttribute<std::vector<PFP::REAL> >, VERTEX>("mvc");
	if(!mvc_.isValid())
		mvc_ = mm->map.addAttribute<NoMathIONameAttribute<std::vector<PFP::REAL> >, VERTEX>("mvc");
	TraversorV<PFP::MAP> t(mm_->map);
	for(Dart it = t.begin(); it != t.end(); it = t.next())
		computePointMVC(mm->position[it], mvc_[it]);
}

Thomas Jund's avatar
Thomas Jund committed
925
926
927
928
929
void MovingObstacle::attachAgent(Agent* ag)
{
	ag_ = ag;
}

930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
void MovingObstacle::updateMesh()
{
	if(mm_)
	{
		TraversorV<PFP::MAP> t(mm_->map);
		for(Dart it = t.begin(); it != t.end(); it = t.next())
		{
			PFP::VEC3 newPos;
			unsigned int j = 0;
			TraversorV<PFP::MAP> tt(map) ;
			for(Dart itt = tt.begin(); itt != tt.end(); itt = tt.next())
			{
				newPos += mvc_[it][j] * position[itt];
				++j;
			}
			mm_->position[it] = newPos;
		}
	}
}