moving_obstacle.cpp 29.3 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
36
37
38
//	return false;
	VEC3 vec, norm, p1,p2;


	for (unsigned int i = 0; i < nbVertices; i++)
	{
		 p1 = getPosition(i);
		 p2 = getPosition((i+1)%nbVertices);

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

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

	return true;
pitiot's avatar
maj    
pitiot committed
39
40
}

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

pitiot's avatar
pitiot committed
68
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
69
70
71
72
73
74
75
76
77
{
	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
pitiot committed
78
MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, std::vector<VEC3> goals, bool rigid, bool spin,int curGoal, Dart dInside, ArticulatedObstacle * art, int indParent) :
79
		nbVertices(pos.size()),
80
		center(0),
pitiot's avatar
maj    
pitiot committed
81
		index(ind),
82
		goals_(goals),
pitiot's avatar
pitiot committed
83
		curGoal_(curGoal),
84
85
86
87
88
		velocity_factor(0.8f),
		color1(1.0f),
		color2(1.0f),
		color3(1.0f),
		seen(false),
pitiot's avatar
maj    
pitiot committed
89
		newVelocity_(0),
pitiot's avatar
merging    
pitiot committed
90
		sim_(sim),
91
92
		rigid_(rigid),
		spinning(spin),
pitiot's avatar
merging    
pitiot committed
93
		parent(art),
pitiot's avatar
blah    
pitiot committed
94
		mm_(NULL),
Thomas Jund's avatar
Thomas Jund committed
95
		ag_(NULL),
96
97
		index_parent(indParent),
		gravity_dist(0)
pitiot's avatar
maj    
pitiot committed
98
99
100
{
	assert(pos.size() > 2);

101
102
	if(dInside==NIL)
		dInside = sim_->envMap_.getBelongingCell(pos[0]);
pitiot's avatar
maj    
pitiot committed
103

pitiot's avatar
pitiot committed
104
	parts_ = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2D<PFP>*[nbVertices];
pitiot's avatar
maj    
pitiot committed
105
	obstacles_ = new Obstacle*[nbVertices];
106
107
	belonging_cells = new std::vector<Dart>[nbVertices];
	neighbor_cells = new std::vector<Dart>[nbVertices];
108
109
110
111

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

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

121
122
123
124
125
126
127
128
129
130
131
132
133
	for (unsigned int i = 0; i < nbVertices; ++i)
		{
			Dart d = sim_->envMap_.getBelongingCell(pos[i]);
			CGoGN::Algo::Surface::MovingObjects::ParticleCell2D<PFP>* part = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2D<PFP>(sim_->envMap_.map, d, pos[i], sim_->envMap_.position);
			parts_[i] = part;
			center += pos[i];

			if(i==0)
				dDir = d;
		}

		center /= nbVertices;
		front=(pos[0] + pos[1]) / 2;
Thomas Jund's avatar
merge    
Thomas Jund committed
134

135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
		// M appartient à l'ellipse ssi MF1 + MF2 = sum_dist_foci est une constante
		// où F1 et F2 sont les deux foyers.
	//	length = (pos[0]-pos[1]).norm();
	//	width  = (pos[1]-pos[2]).norm();
	//	sum_dist_foci_rest = 2*(length + width*(sqrt(2)-0.5));

//		if (spinning && parent==NULL) //départ face à la cible en cas d'obstacles pouvant effectuer des rotations
//		{
//
//			angle = get_angle(goals_[curGoal_] - center,front  - center);
////			std::cout<<" angle : "<< angle;
//
//			for (unsigned int i = 0; i < nbVertices; ++i)
//			{
////				std::cout<<" || pos[i] avant : "<< pos [i];
//				pos[i]+=rotate(pos[i], center, angle);
////				std::cout<<" || pos[i] APRES : "<< pos [i]<<std::endl;
//
//				parts_[i]->move(pos[i]);
//
//
//			}
//			angle=0;
//			front=(pos[0] + pos[1]) / 2;
//		}
Thomas Jund's avatar
merge    
Thomas Jund committed
160

161
162
	groundFace = map.newFace(nbVertices);

pitiot's avatar
maj    
pitiot committed
163
164
	for (unsigned int i = 0; i < nbVertices; ++i)
	{
165
166
167
168
169
		float distance=(center-pos[i]).norm();
		if(distance>gravity_dist)
		{
			gravity_dist=distance;
		}
Thomas Jund's avatar
Thomas Jund committed
170
		Dart d = i;
171
172
		position[d] = pos[i];
		deformation[d] = VEC3(0);
173
174

		if(!rigid_)
Thomas Jund's avatar
Thomas Jund committed
175
		{
176
			velocity[d] = VEC3(0);
Thomas Jund's avatar
Thomas Jund committed
177
178
			forces[d] = VEC3(0);
		}
179

180

pitiot's avatar
pitiot committed
181
	}
182

183
184
	//extrude face to build a cage
	// compute edgeLength for mass-spring
Thomas Jund's avatar
Thomas Jund committed
185
	Algo::Surface::Modelisation::extrudeFace<PFP>(map, position, groundFace, -10.0f) ;
186
187
188
	map.fillHole(groundFace);
	groundFace = map.phi2(groundFace);

189
190
	if(!rigid_)
	{
191
		Algo::Surface::Modelisation::EarTriangulation<PFP> et(map);
192
193
194
195
196
197
198
		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
199

Thomas Jund's avatar
Thomas Jund committed
200
201
202
		DartMarker treated(map);
		for(Dart d = map.begin() ; d != map.end() ; map.next(d))
		{
203
			if(!map.isBoundaryMarked2(d) && !treated.isMarked(d))
Thomas Jund's avatar
Thomas Jund committed
204
205
			{
				treated.mark(d);
206
				vertexAngle[d] = Algo::Surface::Geometry::angle<PFP>(map,map.phi_1(d),map.phi1(d),position);
Thomas Jund's avatar
Thomas Jund committed
207
208
209
			}
		}

210
		map.enableQuickTraversal<VERTEX>();
211

Thomas Jund's avatar
Thomas Jund committed
212
		dDir=dInside;
pitiot's avatar
pitiot committed
213
	}
214

pitiot's avatar
pitiot committed
215

216

pitiot's avatar
maj    
pitiot committed
217
218
	for (unsigned int i = 0; i < nbVertices; ++i)
	{
pitiot's avatar
pitiot committed
219
220
		Obstacle* o = new Obstacle(parts_[i]->getPosition(),
				parts_[(i + 1) % nbVertices]->getPosition(),
Thomas Jund's avatar
merge    
Thomas Jund committed
221
				parts_[(i - 1 + nbVertices) % nbVertices]->getPosition(),
pitiot's avatar
pitiot committed
222
				parts_[(i + 2) % nbVertices]->getPosition(), this, i);
pitiot's avatar
maj    
pitiot committed
223
		obstacles_[i] = o;
pitiot's avatar
pitiot committed
224
//		CGoGNout<<" obstacle :"<< i << " num : "<< o<<CGoGNendl;
pitiot's avatar
pitiot committed
225
		sim_->envMap_.pushObstacleInCells(o);
pitiot's avatar
maj    
pitiot committed
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
void MovingObstacle::initGL()
{
#ifdef EXPORTING_BOXES

	m_render = new Algo::Render::GL2::MapRender();

	m_positionVBO = new Utils::VBO();

	// using simple shader with color
	m_shader = new Utils::ShaderSimpleColor();
	m_shader->setAttributePosition(m_positionVBO);
	m_shader->setColor(Geom::Vec4f(0.,1.,0.,0.));
//	m_shader->setAmbiant(Geom::Vec4f(0.,1.,0.,0.));
//	m_shader->setDiffuse(Geom::Vec4f(0.,1.,0.,0.));

	m_positionVBO->updateData(position) ;

	m_render->initPrimitives<PFP>(map, Algo::Render::GL2::LINES,false) ;
	m_render->initPrimitives<PFP>(map, Algo::Render::GL2::TRIANGLES,false) ;

//	registerShader(m_shader);
#endif
}

void MovingObstacle::draw()
{
#ifdef EXPORTING_BOXES
//	m_render->initPrimitives<PFP>(map, Algo::Render::GL2::LINES,false) ;
//	m_render->initPrimitives<PFP>(map, Algo::Render::GL2::TRIANGLES,false) ;
	m_positionVBO->updateData(position);
259
	m_shader->setColor(Geom::Vec4f(movingObstacleNeighbors_.size()>1 ? 1.0f : 0,0.,0.,0.));
260
261
262
263
	m_render->draw(m_shader, Algo::Render::GL2::TRIANGLES);
	m_shader->setColor(Geom::Vec4f(0.,0.,0.,0.));
	m_render->draw(m_shader, Algo::Render::GL2::LINES);
#endif
pitiot's avatar
maj    
pitiot committed
264
265
}

266
267
VEC3 MovingObstacle::getDilatedPosition(unsigned int ind)
{
268
	Dart d(ind); //WARNING : works only for one face created at start !
269
270
	return position[d]+deformation[d];
//	return position[d];
271
272
273
274
}

VEC3 MovingObstacle::getPosition(unsigned int ind)
{
275
276
	Dart d(ind);
	return position[d];
277
278
}

pitiot's avatar
maj    
pitiot committed
279
280
281
282
bool MovingObstacle::test_opposition(VEC3 o, VEC3 p1, VEC3 p2)
{
	o.normalize();
	o *= -1;
283
284
285
	VEC3 vector(p1 - p2);
	vector.normalize();
	return (o - vector).norm2() < 0.1;
pitiot's avatar
maj    
pitiot committed
286
287
288
}

// TODO Check position
pitiot's avatar
pitiot committed
289
290
291
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
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
//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
343

pitiot's avatar
pitiot committed
344
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
345
{
Thomas Jund's avatar
Thomas Jund committed
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
	//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
399
400
401
402
403
//	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
404
//	{
pitiot's avatar
pitiot committed
405
406
407
408
409
410
//		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
411
//
pitiot's avatar
pitiot committed
412
413
414
415
//		for (std::vector<Agent*>::const_iterator it = agents.begin(); it != agents.end(); ++it)
//		{
//			if ((*it)->alive)
//			{
pitiot's avatar
pitiot committed
416
//
pitiot's avatar
pitiot committed
417
418
419
420
//				float distSq = (center - (*it)->getPosition()).norm2() ;
//				if ((agentNeighbors_.size() < maxNeighbors_ || distSq < maxDist)
//					&& distSq < neighborDistSq_)
//				{
pitiot's avatar
pitiot committed
421
//
pitiot's avatar
pitiot committed
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
//					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
457
//	}
pitiot's avatar
pitiot committed
458
459
460
461
}

void MovingObstacle::updateObstacleNeighbors() // obstacles voisins , distance par rapport aux centres des segments// a mettre en place si besoin
{
Arash HABIBI's avatar
03_2013    
Arash HABIBI committed
462
463
	obstacleNeighbors_.clear() ;
	movingObstacleNeighbors_.clear() ;
464
	for (std::set<Dart>::iterator it2 = general_belonging.begin();it2 != general_belonging.end(); ++it2)
Arash HABIBI's avatar
03_2013    
Arash HABIBI committed
465
	{
466
467
468
469
470
471
472
		std::vector<Obstacle*>& obst = sim_->envMap_.obstvect[*it2] ;
		std::vector<Obstacle*>& neighborObst = sim_->envMap_.neighborObstvect[*it2] ;
//		float maxDistObst = 0.0f ;
//		float maxDistMovingObst = 0.0f ;
		float distance_detection=2.5* gravity_dist;
		float distance_detectionSq=distance_detection*distance_detection;
		for(std::vector<Obstacle*>::const_iterator it = obst.begin() ; it != obst.end() ; ++it)
Arash HABIBI's avatar
03_2013    
Arash HABIBI committed
473
		{
474
			if ((*it)->mo==NULL)
Arash HABIBI's avatar
03_2013    
Arash HABIBI committed
475
			{
476
477
478
				float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, center) ;
				if (/*(obstacleNeighbors_.size() < maxMovingObstacles_|| distSq < maxDistObst)
						&&*/ distSq < distance_detectionSq)
Arash HABIBI's avatar
03_2013    
Arash HABIBI committed
479
				{
480
481
482
483
484
485
486
487


//						if (distSq > maxDistObst)
//							maxDistObst = distSq ;
						obstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;



Arash HABIBI's avatar
03_2013    
Arash HABIBI committed
488
489
				}
			}
490
491
492
493
494
495
496
497
498
499
500
501
502
			else
			{
				if((*it)->mo->index!=index)
				{
					float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, center) ;
					if (/*(movingObstacleNeighbors_.size() < maxMovingObstacles_ || distSq < maxDistMovingObst) &&*/
							 distSq < distance_detectionSq)
					{

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

Arash HABIBI's avatar
03_2013    
Arash HABIBI committed
503

504
505
506
507
508

					}

				}
			}
Arash HABIBI's avatar
03_2013    
Arash HABIBI committed
509
		}
510
511

		for(std::vector<Obstacle*>::const_iterator it = neighborObst.begin() ; it != neighborObst.end() ; ++it)
Arash HABIBI's avatar
03_2013    
Arash HABIBI committed
512
		{
513
			if ((*it)->mo==NULL)
Arash HABIBI's avatar
03_2013    
Arash HABIBI committed
514
			{
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
				float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, center) ;
				if (/*(obstacleNeighbors_.size() < maxMovingObstacles_|| distSq < maxDistObst)
						&&*/ distSq < distance_detectionSq)
				{


//						if (distSq > maxDistObst)
//							maxDistObst = distSq ;
						obstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;



				}
			}
			else
			{
				if((*it)->mo->index!=index)
				{
					float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, center) ;
					if (/*(movingObstacleNeighbors_.size() < maxMovingObstacles_ || distSq < maxDistMovingObst) &&*/
							 distSq < distance_detectionSq)
					{

//						if (distSq > maxDistMovingObst)
//							maxDistMovingObst = distSq ;
						movingObstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
					}
				}
Arash HABIBI's avatar
03_2013    
Arash HABIBI committed
543
544
545
			}
		}
	}
pitiot's avatar
maj    
pitiot committed
546
547
548
549
}

void MovingObstacle::update()
{
pitiot's avatar
pitiot committed
550
	assert(sim_->envMap_.map.getCurrentLevel() == sim_->envMap_.map.getMaxLevel()) ;
pitiot's avatar
pitiot committed
551
	///////remise a zero
552
//	if(sim_->detect_agent_collision)
553
		general_belonging.clear();
pitiot's avatar
pitiot committed
554
	//pour les tests de détection///////////////
pitiot's avatar
merging    
pitiot committed
555
556
557
558
559
560
561
	if(!seen)
	{
		color1=1.0f;
		color2=1.0f;
		color3=1.0f;
	}
	seen=false;
Thomas Jund's avatar
merge    
Thomas Jund committed
562

pitiot's avatar
pitiot committed
563
	//////////////////////////
Thomas Jund's avatar
merge    
Thomas Jund committed
564

565
	PFP::VEC3 bary(0);
pitiot's avatar
maj    
pitiot committed
566
567

	Dart d;
pitiot's avatar
pitiot committed
568
569
	velocity_[0] = newVelocity_[0] * velocity_factor;
	velocity_[1] = newVelocity_[1] * velocity_factor;
Arash HABIBI's avatar
Arash HABIBI committed
570
571
	// velocity_[0] = 0.0;
	// velocity_[1] = 0.0;
pitiot's avatar
maj    
pitiot committed
572
573

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

pitiot's avatar
merging    
pitiot committed
576
	float rotor=0;
577

pitiot's avatar
merging    
pitiot committed
578
579
580
581
	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
582

Arash HABIBI's avatar
03_2013    
Arash HABIBI committed
583
584
	// masse ressort pour la limace

585
	if(!rigid_)
pitiot's avatar
maj    
pitiot committed
586
	{
587
588
		Dart d = groundFace;
		map.next(d);
pitiot's avatar
maj    
pitiot committed
589

Thomas Jund's avatar
Thomas Jund committed
590
591
592
593
594
595
596
597
		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);
		}

598
		CellMarkerStore<EDGE> cm(map);
Thomas Jund's avatar
Thomas Jund committed
599
		DartMarkerStore dm(map);
Arash HABIBI's avatar
03_2013    
Arash HABIBI committed
600
		// ARASH : On parcourt les sommets de la grande face
601
602
603
		for (unsigned int i = 1; i < nbVertices; ++i)
		{
			Dart dd = d;
Arash HABIBI's avatar
03_2013    
Arash HABIBI committed
604
605

			// ARASH : On parcourt les sous-faces triangulaire de la grande face
606
607
608
609
			do {
				if(!cm.isMarked(dd))
				{
					cm.mark(dd);
Thomas Jund's avatar
Thomas Jund committed
610
611
					VEC3 p1Next = position[map.phi1(dd)]+(velocity[map.phi1(dd)] * sim_->timeStep_);
					VEC3 p2Next = position[dd]+(velocity[dd] * sim_->timeStep_);
Arash HABIBI's avatar
03_2013    
Arash HABIBI committed
612
					// p1Next et p2Next sont la position des extremites de l'arete.
Thomas Jund's avatar
Thomas Jund committed
613
					VEC3 v1 = (p1Next-p2Next);
614
615
616

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

619
					float stretch = rigidity*(edgeLength[dd]-v1.norm());
620
621
622
					if(norm>0.0f)
					{
						VEC3 f = stretch*(v1/norm);
623

624
625
626
627
						//apply force symmetrically
						forces[dd] -= f;
						forces[map.phi1(dd)] += f;
					}
Arash HABIBI's avatar
Arash HABIBI committed
628
629
				}

Thomas Jund's avatar
Thomas Jund committed
630
631
632
				dd = map.phi1(dd);
			} while(dd!=d);

Arash HABIBI's avatar
03_2013    
Arash HABIBI committed
633
			// ARASH : les ressorts angulaires
Thomas Jund's avatar
Thomas Jund committed
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
			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)
					{
Thomas Jund's avatar
Thomas Jund committed
649
						float angularRig = 70.0f;
Thomas Jund's avatar
Thomas Jund committed
650

651
						float curAngle = Algo::Surface::Geometry::angle<PFP>(map, map.phi_1(dd),map.phi1(dd),position);
652

Thomas Jund's avatar
Thomas Jund committed
653
654
						float angularStretch = angularRig*(restAngle-curAngle);

655
656
657
658
659
660
661
						if(norm>0.0f && curAngle==curAngle) //test to avoid NaN
						{
							VEC3 f = angularStretch*(v1/norm);

							forces[dd] += f;
							forces[map.phi1(dd)] -= f;
						}
Thomas Jund's avatar
Thomas Jund committed
662
					}
663
				}
pitiot's avatar
maj    
pitiot committed
664

665
666
				dd = map.phi1(dd);
			} while(dd!=d);
pitiot's avatar
maj    
pitiot committed
667

Arash HABIBI's avatar
03_2013    
Arash HABIBI committed
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
			//-------------------------------------------------------------------------
			// ARASH : A présent on calcule les interactions avec les autres obstacles.

			do {
				if(!cm.isMarked(dd))
				{
					cm.mark(dd);

					VEC3 p = position[dd]+(velocity[dd] * sim_->timeStep_);

					for(std::vector<std::pair<float, Obstacle*> >::iterator it = movingObstacleNeighbors_.begin() ;
						it != movingObstacleNeighbors_.end() ; ++it)
					{
						Obstacle * obst = it->second;
						VEC3 p1=obst->p1 ;
						VEC3 p2=obst->p2 ;
						double longueur2 = (p1-p2).norm2();
						double rest_sum_of_dists = 2 * sqrt(obst_radius_infl*obst_radius_infl + longueur2/4);

						double d1 = (p-p1).norm();
						double d2 = (p-p2).norm();
						double sum_of_dists = d1+d2;
						if(sum_of_dists < rest_sum_of_dists)
						{
							collision_softening_factor = pow(1-sum_of_dists/rest_sum_of_dists,obst_power);
							force_value = obst_stiffness*collision_softening_factor*(rest_sum_of_dists - sum_of_dists);
							VEC3 v_obst = p2 - p1;
							VEC3 normal = VEC3(v_obst[1],-v_obst[0],0);
							// Ajouter une composante tangentielle
							normal += v_obst * ((d1-d2)/(5*sum_of_dists));
							// Le facteur 5 est là seulement pour diminuer l'influence de la composante tangentielle
							normal.normalize();
							forces[dd] -= force_value * normal;
							// This force is not symmetrically applied
							// We assume that a homologous vertex in the other obstacle
							// is also intersecting the current obstacle
						}
					}
				}
				dd = map.phi1(dd);
			} while(dd!=d);

710
711
712
713
			map.next(d);
		}

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

717
718
719
		//apply force to each vertex
		d = groundFace;
		for (unsigned int i = 0; i < nbVertices; ++i)
pitiot's avatar
pitiot committed
720
		{
Thomas Jund's avatar
Thomas Jund committed
721
			velocity[d] += forces[d] * sim_->timeStep_;
722
723
724
725
			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
726
		}
727
	}
pitiot's avatar
pitiot committed
728
729
	else
	{
730
731
732
733
734
735
736
737
738
	//	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_);

739
740
741
742
743
#ifdef EXPORTING_BOXES
			position[map.phi<211>(d)] += rotate(position[map.phi<211>(d)], center, abs_angle*rotor);
			position[map.phi<211>(d)] += (velocity_ * sim_->timeStep_);
#endif

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

			bary += position[d];
pitiot's avatar
pitiot committed
747
			parts_[i]->move(getDilatedPosition(i));
748
749
750
			map.next(d);
		}

pitiot's avatar
pitiot committed
751
		front=(position[0] + position[1]) / 2;
752
753
754
755
		if(angle >0)
			angle -= rotor;
		else
			angle += rotor;
pitiot's avatar
pitiot committed
756
	}
pitiot's avatar
maj    
pitiot committed
757

pitiot's avatar
pitiot committed
758
	center = bary / nbVertices;
pitiot's avatar
maj    
pitiot committed
759

760
761
	//computing deformation vector on vertices :
	//depending on the velocity, extend the perceived shape
762
	VEC3 vel = velocity_.normalized();
763
764
765

	d = groundFace;
	for (unsigned int i = 0; i < nbVertices; ++i)
766
	{
767
		VEC3 v(position[d] - center);
768
769
		VEC3 vN = v.normalized();
		float dot = vN * vel;
770
771
		deformation[d] = v * 2.0f * (dot+1.0f)/2.0f;
		map.next(d);
772
773
	}

Thomas Jund's avatar
Thomas Jund committed
774
775
//	if(!rigid_)
//		center = position[groundFace];
776

pitiot's avatar
maj    
pitiot committed
777
778
779
780
781
	// MAJ des obstacles
	for (unsigned int i = 0; i < nbVertices; ++i)
	{
		//		CGoGNout << "avant une etape : Obstacle "<< i << CGoGNendl;
		Obstacle* o = obstacles_[i];
782
783
784
785
		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
786
787
		Dart d1 = parts_[i]->d;
		Dart d2 = parts_[(i+1)%nbVertices]->d;
pitiot's avatar
pitiot committed
788
		if(!((sim_->envMap_.map.sameFace(d1,d2))&& (parts_[i]->crossCell==CGoGN::Algo::Surface::MovingObjects::NO_CROSS && parts_[(i+1)%nbVertices]->crossCell==CGoGN::Algo::Surface::MovingObjects::NO_CROSS)))
pitiot's avatar
pitiot committed
789
		{
pitiot's avatar
pitiot committed
790
		sim_->envMap_.popAndPushObstacleInCells(o,i);
Thomas Jund's avatar
Thomas Jund committed
791

pitiot's avatar
pitiot committed
792
		}
pitiot's avatar
pitiot committed
793

794
795
//		if(sim_->detect_agent_collision)
//		{
796
797
798
799
			for (std::vector<Dart>::iterator it = belonging_cells[i].begin(); it != belonging_cells[i].end(); ++it)
			{
				general_belonging.insert(*it);
			}
800
//		}
pitiot's avatar
maj    
pitiot committed
801
802
803
804
805
806
		//		CGoGNout << "Apres une etape : Obstacle "<< i << CGoGNendl;
	}

	//
	//		envMap_->addMovingObstAsNeighbor(this,general_belonging,&(general_neighbors));
	//		CGoGNout << "nouvel update : "<< CGoGNendl;
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
	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
823

824
825
		}
	}
pitiot's avatar
maj    
pitiot committed
826
827
828

}

Thomas Jund's avatar
Thomas Jund committed
829

pitiot's avatar
pitiot committed
830
std::vector<Dart> MovingObstacle::getMemoCross(const VEC3& pos, const VEC3& dest, Dart& d1, Dart& d2)
pitiot's avatar
maj    
pitiot committed
831
{
pitiot's avatar
pitiot committed
832
	CGoGN::Algo::Surface::MovingObjects::ParticleCell2DMemo<PFP> * registering_part = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2DMemo<PFP>(sim_->envMap_.map, d1,pos,sim_->envMap_.position);
pitiot's avatar
pitiot committed
833
834
	std::vector<Dart> result =(registering_part->move(dest));
	d2=registering_part->d;
835
//	CGoGNout<<"d1 : "<< *d1<<"|| d2 : "<< *d2<<"|| start : "<< pos<<"|| stop : "<< dest<<CGoGNendl;
pitiot's avatar
pitiot committed
836
	return result;
pitiot's avatar
maj    
pitiot committed
837
838
}

839
void resetPartSubdiv(Obstacle* o)
pitiot's avatar
maj    
pitiot committed
840
841
{
	MovingObstacle * mo = o->mo;
842

pitiot's avatar
maj    
pitiot committed
843
844
	if (mo != NULL)
	{
pitiot's avatar
pitiot committed
845
		int n =o->index;
Arash HABIBI's avatar
Arash HABIBI committed
846

pitiot's avatar
pitiot committed
847
		VEC3 pos =mo->parts_[n]->getPosition();
pitiot's avatar
pitiot committed
848
		mo->parts_[n]->CGoGN::Algo::MovingObjects::ParticleBase<PFP>::move(Algo::Surface::Geometry::faceCentroid<PFP>(mo->sim_->envMap_.map, mo->parts_[n]->d, mo->sim_->envMap_.position)) ;
Arash HABIBI's avatar
Arash HABIBI committed
849

pitiot's avatar
pitiot committed
850
851
852
853
		mo->parts_[n]->setState(FACE) ;
		mo->parts_[n]->move(pos) ;

		mo->dDir = mo->parts_[0]->d;
pitiot's avatar
maj    
pitiot committed
854
855
856
857
858
859
	}
}

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

pitiot's avatar
maj    
pitiot committed
861
	if (mo != NULL) {
pitiot's avatar
pitiot committed
862
863
		unsigned int n =o->index;
		VEC3 pos1 = mo->parts_[n]->getPosition();
pitiot's avatar
pitiot committed
864
		if (Algo::Surface::Geometry::isPointInConvexFace2D < PFP
pitiot's avatar
maj    
pitiot committed
865
				> (mo->sim_->envMap_.map, d1, mo->sim_->envMap_.position, pos1, true)) {
pitiot's avatar
pitiot committed
866
867
			mo->parts_[n]->d = d1;
		}
pitiot's avatar
pitiot committed
868
		if(n==0)
pitiot's avatar
pitiot committed
869
			mo->dDir = mo->parts_[0]->d;
pitiot's avatar
maj    
pitiot committed
870
871
872
	}
}

pitiot's avatar
pitiot committed
873
void resetPart(Obstacle * o, Dart d1)
pitiot's avatar
maj    
pitiot committed
874
{
pitiot's avatar
pitiot committed
875
876
877
878
	MovingObstacle * mo = o->mo;
	if(mo!=NULL)
	{
		int n =o->index;
pitiot's avatar
pitiot committed
879
880
881

		if (mo->parts_[n]->d == mo->sim_->envMap_.map.phi1(d1))
			mo->parts_[n]->d = d1;
pitiot's avatar
pitiot committed
882
		if(n==0)
pitiot's avatar
pitiot committed
883
884
885
			mo->dDir = mo->parts_[n]->d;


pitiot's avatar
pitiot committed
886
	}
pitiot's avatar
maj    
pitiot committed
887
888
889
890
891
892
}

void displayMO(Obstacle * o)
{
	CGoGNout << "obstacle problematique : " << (o->mo->index) << CGoGNendl;
}
pitiot's avatar
pitiot committed
893
894
895
896
897
898
899
900
901
902

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////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
903
904
	VEC3 goalVector;
	if (index_parent<1)
pitiot's avatar
pitiot committed
905
	{
Thomas Jund's avatar
Thomas Jund committed
906
		if(rigid_ || goals_.size()>1)
pitiot's avatar
merging    
pitiot committed
907
908
		{
			goalVector = goals_[curGoal_] - center ;
Thomas Jund's avatar
Thomas Jund committed
909
	//		goalVector = goals_[curGoal_] - position[groundFace] ;
pitiot's avatar
merging    
pitiot committed
910

Thomas Jund's avatar
Thomas Jund committed
911
912
			float goalDist2 = goalVector.norm2() ;

913
			if (goalDist2 < 1000.0f)
Thomas Jund's avatar
Thomas Jund committed
914
915
916
917
918
919
920
921
922
923
924
925
926
927
			{
				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
928
		{
Thomas Jund's avatar
Thomas Jund committed
929
930
931
932
933
934
935
936
937
938
			if(ag_!=NULL)
			{
				goalVector = ag_->getPosition() - center ;
				if (goalVector.norm2() > maxSpeed_)
				{
					goalVector.normalize() ;
					goalVector *= maxSpeed_;
				}
			}
			else
Thomas Jund's avatar
Thomas Jund committed
939
			{
Thomas Jund's avatar
Thomas Jund committed
940
941
	//			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
942

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

Thomas Jund's avatar
Thomas Jund committed
946
947
948
949
950
951
952
				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
953

Thomas Jund's avatar
Thomas Jund committed
954
955
956
					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
957

Thomas Jund's avatar
Thomas Jund committed
958
959
960
					if(dDir==dStart)
						break;
				}
Thomas Jund's avatar
Thomas Jund committed
961

Thomas Jund's avatar
Thomas Jund committed
962
963
				if(sim_->envMap_.map.isBoundaryEdge(sim_->envMap_.map.phi2(dDir)))
					goalVector = VEC3(0);
Thomas Jund's avatar
Thomas Jund committed
964

Thomas Jund's avatar
Thomas Jund committed
965
966
967
968
969
970
971
972
973
974
	//			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
975
			}
pitiot's avatar
merging    
pitiot committed
976
977
978
		}
	}
	else
pitiot's avatar
pitiot committed
979
	{
pitiot's avatar
merging    
pitiot committed
980
981
982
983
984
985
986
		goalVector = get_center(parent,index_parent-1) -center;
		float goalDist2 = goalVector.norm2() ;
		if (goalDist2 > maxSpeed_)
		{
			goalVector.normalize() ;
			goalVector *= maxSpeed_;
		}
pitiot's avatar
pitiot committed
987
	}
988
989
990

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

pitiot's avatar
pitiot committed
992
993
994
995
996
	prefVelocity_ = goalVector ;

}
void MovingObstacle::computeNewVelocity()  //comportement des obstacles en tenant compte de l'environnement.
{
pitiot's avatar
pitiot committed
997
998
999
1000
1001
	if (spinning)
	{
		VEC3 forward = front-center;
		float goalDist2 = forward.norm2() ;
		float objective = prefVelocity_.norm2();
1002

pitiot's avatar
pitiot committed
1003
		if (goalDist2 > objective)
1004
1005
1006
1007
1008
1009
1010
1011
		{
			forward.normalize() ;
			forward *= objective;
		}

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

pitiot's avatar
pitiot committed
1012
1013
1014
1015
1016
1017
		newVelocity_=forward;
	}
	else
	{
		newVelocity_=prefVelocity_;
	}
pitiot's avatar
pitiot committed
1018
}
1019
1020
1021
1022
1023
1024
1025
1026
1027
1028
1029
1030
1031
1032
1033
1034
1035
1036
1037
1038
1039
1040
1041
1042
1043
1044
1045
1046
1047
1048
1049
1050
1051
1052
1053
1054
1055
1056
1057
1058
1059
1060
1061
1062
1063
1064
1065
1066
1067
1068
1069
1070
1071
1072
1073
1074
1075
1076
1077
1078
1079
1080
1081
1082
1083



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");
Thomas Jund's avatar
Thomas Jund committed
1084
	TraversorV<PFP2::MAP> t(mm_->map);
1085
1086
1087
1088
	for(Dart it = t.begin(); it != t.end(); it = t.next())
		computePointMVC(mm->position[it], mvc_[it]);
}

Thomas Jund's avatar
Thomas Jund committed
1089
1090
1091
1092
1093
void MovingObstacle::attachAgent(Agent* ag)
{
	ag_ = ag;
}

1094
1095
1096
1097
void MovingObstacle::updateMesh()
{
	if(mm_)
	{
Thomas Jund's avatar
Thomas Jund committed
1098
		TraversorV<PFP2::MAP> t(mm_->map);
1099
1100
1101
1102
1103
1104
1105
1106
1107
1108
1109
1110
1111
1112
		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;
		}
	}
}