moving_obstacle.cpp 28.6 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
float MovingObstacle::timeHorizonObst_ = 10.0f;
unsigned int MovingObstacle::maxNeighbors_ = 20;
float MovingObstacle::detectionFixedObst = 50;

pitiot's avatar
pitiot committed
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
void  MovingObstacle::addGeneralCell ( Dart d)
{
	bool added = false;
	std::vector< std::pair<Dart,int> >::iterator it =general_belonging.begin();
	while (it != general_belonging.end() )
	{
		if(sim_->envMap_.map.sameFace((*it).first,d))
		{
			(*it).second ++;
			added= true;
			break;
		}
		else ++it;

	};

	if (!added)
	{
		general_belonging.push_back(std::make_pair(d,1));
	}
}
bool  MovingObstacle::removeGeneralCell (Dart d)
{
	std::vector< std::pair<Dart,int> >::iterator it =general_belonging.begin();
	while (it != general_belonging.end() )
	{
		if(sim_->envMap_.map.sameFace((*it).first,d))
		{
			if(--((*it).second)==0)
			{
				*it = general_belonging.back() ;
				general_belonging.pop_back() ;
			}
			return true;
		}
		else ++it;

	};
	return false;
}

pitiot's avatar
maj    
pitiot committed
59
60
bool MovingObstacle::is_inside(VEC3 p)
{
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
//	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
80
81
}

pitiot's avatar
pitiot committed
82
float get_angle(VEC3 v1, VEC3 v2) //renvoie l'angle entre [- pi ; Pi] du v2 à v1
pitiot's avatar
maj    
pitiot committed
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
{
	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
99
100
101
102
103
104
105
		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
106
107
108
	return flo;
}

pitiot's avatar
pitiot committed
109
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
110
111
112
113
114
115
116
117
118
{
	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
119
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) :
120
		nbVertices(pos.size()),
121
		center(0),
pitiot's avatar
maj    
pitiot committed
122
		index(ind),
123
		goals_(goals),
pitiot's avatar
pitiot committed
124
		curGoal_(curGoal),
125
126
127
128
129
		velocity_factor(0.8f),
		color1(1.0f),
		color2(1.0f),
		color3(1.0f),
		seen(false),
pitiot's avatar
maj    
pitiot committed
130
		newVelocity_(0),
pitiot's avatar
merging    
pitiot committed
131
		sim_(sim),
132
133
		rigid_(rigid),
		spinning(spin),
pitiot's avatar
merging    
pitiot committed
134
		parent(art),
pitiot's avatar
blah    
pitiot committed
135
		mm_(NULL),
Thomas Jund's avatar
Thomas Jund committed
136
		ag_(NULL),
137
138
		index_parent(indParent),
		gravity_dist(0)
pitiot's avatar
maj    
pitiot committed
139
140
141
{
	assert(pos.size() > 2);

142
143
	if(dInside==NIL)
		dInside = sim_->envMap_.getBelongingCell(pos[0]);
pitiot's avatar
maj    
pitiot committed
144

pitiot's avatar
pitiot committed
145
	parts_ = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2D<PFP>*[nbVertices];
pitiot's avatar
maj    
pitiot committed
146
	obstacles_ = new Obstacle*[nbVertices];
147
148
	belonging_cells = new std::vector<Dart>[nbVertices];
	neighbor_cells = new std::vector<Dart>[nbVertices];
149
150
151
152

	position = map.addAttribute<VEC3, VERTEX>("position") ;
	normal = map.addAttribute<VEC3, VERTEX>("normal") ;
	deformation = map.addAttribute<VEC3, VERTEX>("deformation") ;
153
154
155
156

	if(!rigid_)
	{
		velocity = map.addAttribute<VEC3, VERTEX>("velocity") ;
Thomas Jund's avatar
Thomas Jund committed
157
		forces = map.addAttribute<VEC3, VERTEX>("force") ;
158
		edgeLength = map.addAttribute<float, EDGE>("edgeLength") ;
Thomas Jund's avatar
Thomas Jund committed
159
		vertexAngle = map.addAttribute<float, DART>("vertexAngle") ;
160
	}
161

162
163
164
165
166
167
168
169
170
171
172
173
174
	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
175

176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
		// 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
201

202
203
	groundFace = map.newFace(nbVertices);

pitiot's avatar
maj    
pitiot committed
204
205
	for (unsigned int i = 0; i < nbVertices; ++i)
	{
206
207
208
209
210
		float distance=(center-pos[i]).norm();
		if(distance>gravity_dist)
		{
			gravity_dist=distance;
		}
Thomas Jund's avatar
Thomas Jund committed
211
		Dart d = i;
212
213
		position[d] = pos[i];
		deformation[d] = VEC3(0);
214
215

		if(!rigid_)
Thomas Jund's avatar
Thomas Jund committed
216
		{
217
			velocity[d] = VEC3(0);
Thomas Jund's avatar
Thomas Jund committed
218
219
			forces[d] = VEC3(0);
		}
220

221

pitiot's avatar
pitiot committed
222
	}
223

224
225
	//extrude face to build a cage
	// compute edgeLength for mass-spring
Thomas Jund's avatar
Thomas Jund committed
226
	Algo::Surface::Modelisation::extrudeFace<PFP>(map, position, groundFace, -10.0f) ;
227
228
229
	map.fillHole(groundFace);
	groundFace = map.phi2(groundFace);

230
231
	if(!rigid_)
	{
232
		Algo::Surface::Modelisation::EarTriangulation<PFP> et(map);
233
234
235
236
237
238
239
		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
240

Thomas Jund's avatar
Thomas Jund committed
241
242
243
		DartMarker treated(map);
		for(Dart d = map.begin() ; d != map.end() ; map.next(d))
		{
244
			if(!map.isBoundaryMarked2(d) && !treated.isMarked(d))
Thomas Jund's avatar
Thomas Jund committed
245
246
			{
				treated.mark(d);
247
				vertexAngle[d] = Algo::Surface::Geometry::angle<PFP>(map,map.phi_1(d),map.phi1(d),position);
Thomas Jund's avatar
Thomas Jund committed
248
249
250
			}
		}

251
		map.enableQuickTraversal<VERTEX>();
252

Thomas Jund's avatar
Thomas Jund committed
253
		dDir=dInside;
pitiot's avatar
pitiot committed
254
	}
255

pitiot's avatar
merging    
pitiot committed
256

257

pitiot's avatar
maj    
pitiot committed
258
259
	for (unsigned int i = 0; i < nbVertices; ++i)
	{
pitiot's avatar
pitiot committed
260
261
		Obstacle* o = new Obstacle(parts_[i]->getPosition(),
				parts_[(i + 1) % nbVertices]->getPosition(),
Thomas Jund's avatar
merge    
Thomas Jund committed
262
				parts_[(i - 1 + nbVertices) % nbVertices]->getPosition(),
pitiot's avatar
pitiot committed
263
				parts_[(i + 2) % nbVertices]->getPosition(), this, i);
pitiot's avatar
maj    
pitiot committed
264
		obstacles_[i] = o;
pitiot's avatar
pitiot committed
265
//		CGoGNout<<" obstacle :"<< i << " num : "<< o<<CGoGNendl;
pitiot's avatar
pitiot committed
266
		sim_->envMap_.pushObstacleInCells(o);
pitiot's avatar
maj    
pitiot committed
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
295
296
297
298
299
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);
300
301
//	m_shader->setColor(Geom::Vec4f(movingObstacleNeighbors_.size()==0 ? 1.0f : 0,0.,0.,0.));
	m_shader->setColor(Geom::Vec4f(1.0f,0.,0.,0.0));
302
303
304
305
306
307
	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
}

308
309
VEC3 MovingObstacle::getDilatedPosition(unsigned int ind)
{
310
	Dart d(ind); //WARNING : works only for one face created at start !
311
312
	return position[d]+deformation[d];
//	return position[d];
313
314
315
316
}

VEC3 MovingObstacle::getPosition(unsigned int ind)
{
317
318
	Dart d(ind);
	return position[d];
319
320
}

pitiot's avatar
maj    
pitiot committed
321
322
323
324
bool MovingObstacle::test_opposition(VEC3 o, VEC3 p1, VEC3 p2)
{
	o.normalize();
	o *= -1;
325
326
327
	VEC3 vector(p1 - p2);
	vector.normalize();
	return (o - vector).norm2() < 0.1;
pitiot's avatar
maj    
pitiot committed
328
329
330
}

// TODO Check position
pitiot's avatar
pitiot committed
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
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
//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
385

pitiot's avatar
pitiot committed
386
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
387
{
Thomas Jund's avatar
Thomas Jund committed
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
	//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
441
442
443
444
445
//	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
446
//	{
pitiot's avatar
pitiot committed
447
448
449
450
451
452
//		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
453
//
pitiot's avatar
pitiot committed
454
455
456
457
//		for (std::vector<Agent*>::const_iterator it = agents.begin(); it != agents.end(); ++it)
//		{
//			if ((*it)->alive)
//			{
pitiot's avatar
pitiot committed
458
//
pitiot's avatar
pitiot committed
459
460
461
462
//				float distSq = (center - (*it)->getPosition()).norm2() ;
//				if ((agentNeighbors_.size() < maxNeighbors_ || distSq < maxDist)
//					&& distSq < neighborDistSq_)
//				{
pitiot's avatar
pitiot committed
463
//
pitiot's avatar
pitiot committed
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
//					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
499
//	}
pitiot's avatar
pitiot committed
500
501
502
503
}

void MovingObstacle::updateObstacleNeighbors() // obstacles voisins , distance par rapport aux centres des segments// a mettre en place si besoin
{
504
505
	obstacleNeighbors_.clear() ;
	movingObstacleNeighbors_.clear() ;
pitiot's avatar
pitiot committed
506
	for (std::vector< std::pair<Dart, int> >::iterator it2 = general_belonging.begin();it2 != general_belonging.end(); ++it2)
507
	{
pitiot's avatar
pitiot committed
508
509
		std::vector<Obstacle*>& obst = sim_->envMap_.obstvect[(*it2).first] ;
		std::vector<Obstacle*>& neighborObst = sim_->envMap_.neighborObstvect[(*it2).first] ;
510
511
512
513
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
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
//		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)
		{
			if ((*it)->mo==NULL)
			{
				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)) ;



					}

				}
			}
		}

		for(std::vector<Obstacle*>::const_iterator it = neighborObst.begin() ; it != neighborObst.end() ; ++it)
		{
			if ((*it)->mo==NULL)
			{
				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)) ;



					}
				}
			}
		}
	}
pitiot's avatar
maj    
pitiot committed
591
592
593
594
}

void MovingObstacle::update()
{
pitiot's avatar
pitiot committed
595
	assert(sim_->envMap_.map.getCurrentLevel() == sim_->envMap_.map.getMaxLevel()) ;
pitiot's avatar
pitiot committed
596

pitiot's avatar
pitiot committed
597
	//pour les tests de détection///////////////
pitiot's avatar
merging    
pitiot committed
598
599
600
601
602
603
604
	if(!seen)
	{
		color1=1.0f;
		color2=1.0f;
		color3=1.0f;
	}
	seen=false;
Thomas Jund's avatar
merge    
Thomas Jund committed
605

pitiot's avatar
pitiot committed
606
	//////////////////////////
Thomas Jund's avatar
merge    
Thomas Jund committed
607

608
	PFP::VEC3 bary(0);
pitiot's avatar
maj    
pitiot committed
609
610

	Dart d;
pitiot's avatar
pitiot committed
611
612
	velocity_[0] = newVelocity_[0] * velocity_factor;
	velocity_[1] = newVelocity_[1] * velocity_factor;
Arash HABIBI's avatar
Arash HABIBI committed
613
614
	// velocity_[0] = 0.0;
	// velocity_[1] = 0.0;
pitiot's avatar
maj    
pitiot committed
615
616

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

pitiot's avatar
merging    
pitiot committed
619
	float rotor=0;
620

pitiot's avatar
merging    
pitiot committed
621
622
623
624
	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
625

626
	if(!rigid_)
pitiot's avatar
maj    
pitiot committed
627
	{
628
629
		Dart d = groundFace;
		map.next(d);
pitiot's avatar
maj    
pitiot committed
630

Thomas Jund's avatar
Thomas Jund committed
631
632
633
634
635
636
637
638
		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);
		}

639
		CellMarkerStore<EDGE> cm(map);
Thomas Jund's avatar
Thomas Jund committed
640
		DartMarkerStore dm(map);
641
642
643
644
645
646
647
		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
648
649
650
					VEC3 p1Next = position[map.phi1(dd)]+(velocity[map.phi1(dd)] * sim_->timeStep_);
					VEC3 p2Next = position[dd]+(velocity[dd] * sim_->timeStep_);
					VEC3 v1 = (p1Next-p2Next);
651
652
653

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

656
					float stretch = rigidity*(edgeLength[dd]-v1.norm());
657
658
659
					if(norm>0.0f)
					{
						VEC3 f = stretch*(v1/norm);
660

661
662
663
664
						//apply force symmetrically
						forces[dd] -= f;
						forces[map.phi1(dd)] += f;
					}
Arash HABIBI's avatar
Arash HABIBI committed
665
666
				}

Thomas Jund's avatar
Thomas Jund committed
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
				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)
					{
Thomas Jund's avatar
Thomas Jund committed
685
						float angularRig = 70.0f;
Thomas Jund's avatar
Thomas Jund committed
686

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

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

691
692
693
694
695
696
697
						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
698
					}
699
				}
pitiot's avatar
maj    
pitiot committed
700

701
702
				dd = map.phi1(dd);
			} while(dd!=d);
pitiot's avatar
maj    
pitiot committed
703

704
705
706
707
			map.next(d);
		}

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

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

733
734
735
736
737
#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

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

			bary += position[d];
pitiot's avatar
pitiot committed
741
			parts_[i]->move(getDilatedPosition(i));
742
743
744
			map.next(d);
		}

pitiot's avatar
pitiot committed
745
		front=(position[0] + position[1]) / 2;
746
747
748
749
		if(angle >0)
			angle -= rotor;
		else
			angle += rotor;
pitiot's avatar
pitiot committed
750
	}
pitiot's avatar
maj    
pitiot committed
751

pitiot's avatar
pitiot committed
752
	center = bary / nbVertices;
pitiot's avatar
maj    
pitiot committed
753

754
755
	//computing deformation vector on vertices :
	//depending on the velocity, extend the perceived shape
756
	VEC3 vel = velocity_.normalized();
757
758
759

	d = groundFace;
	for (unsigned int i = 0; i < nbVertices; ++i)
760
	{
761
		VEC3 v(position[d] - center);
762
763
		VEC3 vN = v.normalized();
		float dot = vN * vel;
764
765
		deformation[d] = v * 2.0f * (dot+1.0f)/2.0f;
		map.next(d);
766
767
	}

Thomas Jund's avatar
Thomas Jund committed
768
769
//	if(!rigid_)
//		center = position[groundFace];
770

pitiot's avatar
maj    
pitiot committed
771
772
773
774
775
	// MAJ des obstacles
	for (unsigned int i = 0; i < nbVertices; ++i)
	{
		//		CGoGNout << "avant une etape : Obstacle "<< i << CGoGNendl;
		Obstacle* o = obstacles_[i];
776
777
778
779
		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
780
781
		Dart d1 = parts_[i]->d;
		Dart d2 = parts_[(i+1)%nbVertices]->d;
pitiot's avatar
pitiot committed
782
		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
783
		{
pitiot's avatar
pitiot committed
784
		sim_->envMap_.popAndPushObstacleInCells(o,i);
Thomas Jund's avatar
Thomas Jund committed
785

pitiot's avatar
pitiot committed
786
		}
pitiot's avatar
pitiot committed
787
788
789
790
		/////affichage des belonging cells
//		CGoGNout<< CGoGNendl;
//		CGoGNout << "Obstacle "<< i << ": ";
//		for(std::vector<Dart>::iterator ite = belonging_cells[i].begin();ite!=belonging_cells[i].end(); ++ite)
791
//		{
pitiot's avatar
pitiot committed
792
//			CGoGNout<<(*ite).index<<" ; ";
793
//		}
pitiot's avatar
pitiot committed
794
//		CGoGNout<< CGoGNendl;
pitiot's avatar
maj    
pitiot committed
795
796
		//		CGoGNout << "Apres une etape : Obstacle "<< i << CGoGNendl;
	}
pitiot's avatar
pitiot committed
797
798
799
800
801
802
803
804
	/////affichage du general_belonging
//	CGoGNout<< CGoGNendl;
//	CGoGNout << "General : ";
//	for(std::vector<std::pair<Dart , int> >::iterator ite = general_belonging.begin();ite!=general_belonging.end(); ++ite)
//	{
//		CGoGNout<<"< "<<(*ite).first.index<<" , "<<(*ite).second<<"> ; ";
//	}
//	CGoGNout<< CGoGNendl;
pitiot's avatar
maj    
pitiot committed
805
806
807
	//
	//		envMap_->addMovingObstAsNeighbor(this,general_belonging,&(general_neighbors));
	//		CGoGNout << "nouvel update : "<< CGoGNendl;
808
809
	if(sim_->detect_agent_collision)
	{
pitiot's avatar
pitiot committed
810
		for (std::vector< std::pair<Dart, int> >::iterator it2 = general_belonging.begin(); it2 != general_belonging.end(); ++ it2)
811
		{
pitiot's avatar
pitiot committed
812
			std::vector<Agent*> vector =sim_->envMap_.agentvect[(*it2).first];
813
814
815
816
817
818
819
820
821
822
823
			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
824

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

}

Thomas Jund's avatar
Thomas Jund committed
830

pitiot's avatar
pitiot committed
831
std::vector<Dart> MovingObstacle::getMemoCross(const VEC3& pos, const VEC3& dest, Dart& d1, Dart& d2)
pitiot's avatar
maj    
pitiot committed
832
{
pitiot's avatar
pitiot committed
833
	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
834
835
	std::vector<Dart> result =(registering_part->move(dest));
	d2=registering_part->d;
836
//	CGoGNout<<"d1 : "<< *d1<<"|| d2 : "<< *d2<<"|| start : "<< pos<<"|| stop : "<< dest<<CGoGNendl;
pitiot's avatar
pitiot committed
837
	return result;
pitiot's avatar
maj    
pitiot committed
838
839
}

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

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

pitiot's avatar
pitiot committed
848
		VEC3 pos =mo->parts_[n]->getPosition();
pitiot's avatar
pitiot committed
849
		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
850

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

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

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

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

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

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


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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

pitiot's avatar
pitiot committed
1013
1014
1015
1016
1017
1018
		newVelocity_=forward;
	}
	else
	{
		newVelocity_=prefVelocity_;
	}
pitiot's avatar
pitiot committed
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
1084



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

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

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