moving_obstacle.cpp 38.8 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"
6
#include "Utils/colorMaps.h"
pitiot's avatar
maj    
pitiot committed
7

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

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

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

pitiot's avatar
pitiot committed
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
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));
	}
}
Thomas Jund's avatar
Thomas Jund committed
40

pitiot's avatar
pitiot committed
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
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
up    
pitiot committed
61
62


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

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

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

146
147
	if(dInside==NIL)
		dInside = sim_->envMap_.getBelongingCell(pos[0]);
pitiot's avatar
maj    
pitiot committed
148

149
150
151
#ifdef TWO_AND_HALF_DIM
	parts_ = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2DAndHalf<PFP>*[nbVertices];
#else
pitiot's avatar
pitiot committed
152
	parts_ = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2D<PFP>*[nbVertices];
153
154
#endif

pitiot's avatar
maj    
pitiot committed
155
	obstacles_ = new Obstacle*[nbVertices];
156
157
	belonging_cells = new std::vector<Dart>[nbVertices];
	neighbor_cells = new std::vector<Dart>[nbVertices];
158
159
160
161

	position = map.addAttribute<VEC3, VERTEX>("position") ;
	normal = map.addAttribute<VEC3, VERTEX>("normal") ;
	deformation = map.addAttribute<VEC3, VERTEX>("deformation") ;
162
163
164
165

	if(!rigid_)
	{
		velocity = map.addAttribute<VEC3, VERTEX>("velocity") ;
Thomas Jund's avatar
Thomas Jund committed
166
		forces = map.addAttribute<VEC3, VERTEX>("force") ;
167
		edgeLength = map.addAttribute<float, EDGE>("edgeLength") ;
Thomas Jund's avatar
Thomas Jund committed
168
		vertexAngle = map.addAttribute<float, DART>("vertexAngle") ;
169
	}
170

171
	for (unsigned int i = 0; i < nbVertices; ++i)
172
173
174
175
	{
		Dart d = sim_->envMap_.getBelongingCell(pos[i]);
#ifdef TWO_AND_HALF_DIM
		CGoGN::Algo::Surface::MovingObjects::ParticleCell2DAndHalf<PFP>* part = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2DAndHalf<PFP>(sim_->envMap_.map, d, pos[i], sim_->envMap_.position);
176

177
178
179
180
181
182
183
184
185
#else
		CGoGN::Algo::Surface::MovingObjects::ParticleCell2D<PFP>* part = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2D<PFP>(sim_->envMap_.map, d, pos[i], sim_->envMap_.position);
#endif
		parts_[i] = part;
		center += pos[i];

		if(i==0)
			dDir = d;
	}
186
187
188

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

190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
		// 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
215

216
217
	groundFace = map.newFace(nbVertices);

pitiot's avatar
maj    
pitiot committed
218
219
	for (unsigned int i = 0; i < nbVertices; ++i)
	{
220
221
222
223
224
		float distance=(center-pos[i]).norm();
		if(distance>gravity_dist)
		{
			gravity_dist=distance;
		}
Thomas Jund's avatar
Thomas Jund committed
225
		Dart d = i;
226
227
		position[d] = pos[i];
		deformation[d] = VEC3(0);
228
229

		if(!rigid_)
Thomas Jund's avatar
Thomas Jund committed
230
		{
231
			velocity[d] = VEC3(0);
Thomas Jund's avatar
Thomas Jund committed
232
233
			forces[d] = VEC3(0);
		}
234

235

pitiot's avatar
pitiot committed
236
	}
237

238
239
	//extrude face to build a cage
	// compute edgeLength for mass-spring
Thomas Jund's avatar
Thomas Jund committed
240
	Algo::Surface::Modelisation::extrudeFace<PFP>(map, position, groundFace, -10.0f) ;
241
//	Algo::Surface::Modelisation::extrudeFace<PFP>(map, position, groundFace, 0.0f) ;
242
243
244
	map.fillHole(groundFace);
	groundFace = map.phi2(groundFace);

245
246
	if(!rigid_)
	{
247
		Algo::Surface::Modelisation::EarTriangulation<PFP> et(map);
248
249
250
251
252
253
254
		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
255

Thomas Jund's avatar
Thomas Jund committed
256
257
258
		DartMarker treated(map);
		for(Dart d = map.begin() ; d != map.end() ; map.next(d))
		{
259
			if(!map.isBoundaryMarked2(d) && !treated.isMarked(d))
Thomas Jund's avatar
Thomas Jund committed
260
261
			{
				treated.mark(d);
262
				vertexAngle[d] = Algo::Surface::Geometry::angle<PFP>(map,map.phi_1(d),map.phi1(d),position);
Thomas Jund's avatar
Thomas Jund committed
263
264
265
			}
		}

266
		map.enableQuickTraversal<VERTEX>();
267

Thomas Jund's avatar
Thomas Jund committed
268
		dDir=dInside;
pitiot's avatar
pitiot committed
269
	}
270

pitiot's avatar
merging    
pitiot committed
271

272

pitiot's avatar
maj    
pitiot committed
273
274
	for (unsigned int i = 0; i < nbVertices; ++i)
	{
pitiot's avatar
pitiot committed
275
276
		Obstacle* o = new Obstacle(parts_[i]->getPosition(),
				parts_[(i + 1) % nbVertices]->getPosition(),
Thomas Jund's avatar
merge    
Thomas Jund committed
277
				parts_[(i - 1 + nbVertices) % nbVertices]->getPosition(),
Thomas Jund's avatar
Thomas Jund committed
278
				parts_[(i + 2) % nbVertices]->getPosition(), i, (i+1)% nbVertices, this, i);
pitiot's avatar
maj    
pitiot committed
279
		obstacles_[i] = o;
pitiot's avatar
pitiot committed
280
//		CGoGNout<<" obstacle :"<< i << " num : "<< o<<CGoGNendl;
pitiot's avatar
pitiot committed
281
		sim_->envMap_.pushObstacleInCells(o);
pitiot's avatar
maj    
pitiot committed
282
283
284
	}
}

285
286
287
288
289
290
291
292
293
294
295
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);
pitiot's avatar
pitiot committed
296
	m_shader->setColor(Geom::Vec4f(0.,1.,0.,0.));
297
298
299
300
301
302
303
304
305
306
//	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
Thomas Jund's avatar
Thomas Jund committed
307
308

	m_ds = new Utils::Drawer();
309
310
}

Thomas Jund's avatar
Thomas Jund committed
311
void MovingObstacle::draw(bool showPath)
312
313
314
315
316
{
#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);
317
//	m_shader->setColor(Geom::Vec4f(movingObstacleNeighbors_.size()==0 ? 1.0f : 0,0.,0.,0.));
pitiot's avatar
merge    
pitiot committed
318

319
	VEC3 col = Utils::color_map_BCGYR(float(index)/float(sim_->movingObstacles_.size()));
Arash HABIBI's avatar
Arash HABIBI committed
320

pitiot's avatar
up    
pitiot committed
321
322
//	if(movingObstacleNeighbors_.size()==0)
	if(index==12)
Arash HABIBI's avatar
Arash HABIBI committed
323
	// if(obstacleNeighbors_.size()==0)
324
325
326
		m_shader->setColor(Geom::Vec4f(col[0],col[1],col[2],0));
	else
		m_shader->setColor(Geom::Vec4f(0.5,0.5,0.5,0));
pitiot's avatar
merge    
pitiot committed
327

328
329
330
331
	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
Thomas Jund's avatar
Thomas Jund committed
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348

	if(showPath)
	{
		m_ds->newList(GL_COMPILE_AND_EXECUTE);
		m_ds->begin(GL_LINE_STRIP);

		VEC3 col = Utils::color_map_BCGYR(float(index)/float(sim_->movingObstacles_.size()));
		m_ds->color3f(col[0],col[1],col[2]);

		for(std::vector<VEC3>::iterator it = goals_.begin() ; it != goals_.end() ; ++it)
		{
			m_ds->vertex(*it);
		}

		m_ds->end();
		m_ds->endList();
	}
349
350
}

351
352
VEC3 MovingObstacle::getDilatedPosition(unsigned int ind)
{
353
	Dart d(ind); //WARNING : works only for one face created at start !
pitiot's avatar
up    
pitiot committed
354
355
	return position[d]+deformation[d];
//	return position[d];
356
357
358
359
}

VEC3 MovingObstacle::getPosition(unsigned int ind)
{
360
361
	Dart d(ind);
	return position[d];
362
363
}

pitiot's avatar
maj    
pitiot committed
364
365
366
367
bool MovingObstacle::test_opposition(VEC3 o, VEC3 p1, VEC3 p2)
{
	o.normalize();
	o *= -1;
368
369
370
	VEC3 vector(p1 - p2);
	vector.normalize();
	return (o - vector).norm2() < 0.1;
pitiot's avatar
maj    
pitiot committed
371
372
373
}

// TODO Check position
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
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
//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
428

pitiot's avatar
pitiot committed
429
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
430
{
Thomas Jund's avatar
Thomas Jund committed
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
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
	//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
484
485
486
487
488
//	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
489
//	{
pitiot's avatar
pitiot committed
490
491
492
493
494
495
//		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
496
//
pitiot's avatar
pitiot committed
497
498
499
500
//		for (std::vector<Agent*>::const_iterator it = agents.begin(); it != agents.end(); ++it)
//		{
//			if ((*it)->alive)
//			{
pitiot's avatar
pitiot committed
501
//
pitiot's avatar
pitiot committed
502
503
504
505
//				float distSq = (center - (*it)->getPosition()).norm2() ;
//				if ((agentNeighbors_.size() < maxNeighbors_ || distSq < maxDist)
//					&& distSq < neighborDistSq_)
//				{
pitiot's avatar
pitiot committed
506
//
pitiot's avatar
pitiot committed
507
508
509
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
//					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
542
//	}
pitiot's avatar
pitiot committed
543
544
545
546
}

void MovingObstacle::updateObstacleNeighbors() // obstacles voisins , distance par rapport aux centres des segments// a mettre en place si besoin
{
547
548
	obstacleNeighbors_.clear() ;
	movingObstacleNeighbors_.clear() ;
pitiot's avatar
pitiot committed
549
	for (std::vector< std::pair<Dart, int> >::iterator it2 = general_belonging.begin();it2 != general_belonging.end(); ++it2)
550
	{
pitiot's avatar
pitiot committed
551
552
		std::vector<Obstacle*>& obst = sim_->envMap_.obstvect[(*it2).first] ;
		std::vector<Obstacle*>& neighborObst = sim_->envMap_.neighborObstvect[(*it2).first] ;
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
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
//		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
631
632
}

Arash HABIBI's avatar
unidir    
Arash HABIBI committed
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
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
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
//-------------------------------------------------------------

int matrixInverse(VEC3 v1, VEC3 v2, VEC3 v3, VEC3 *inv1, VEC3 *inv2, VEC3 *inv3)
{
	int res = 0;
	float determinant = v1[0]*v2[1]*v3[2] + v2[0]*v3[1]*v1[2] + v3[0]*v1[1]*v2[2] - v3[0]*v2[1]*v1[2] - v2[0]*v1[1]*v3[2] - v1[0]*v3[1]*v2[2];

	if(determinant==0.0)
		res=1;

	else
	{
		float un_sur = 1.0 / determinant;

		*inv1 = VEC3( v2[1]*v3[2]-v3[1]*v2[2], v2[0]*v3[2]-v3[0]*v2[2], v2[0]*v3[1]-v3[0]*v2[1] );
		*inv2 = VEC3( v1[1]*v3[2]-v3[1]*v1[2], v1[0]*v3[2]-v3[0]*v1[2], v1[0]*v3[1]-v3[0]*v1[1] );
		*inv3 = VEC3( v1[1]*v2[2]-v2[1]*v1[2], v1[0]*v2[2]-v2[0]*v1[2], v1[0]*v2[1]-v2[0]*v1[1] );
		res=0;
		*inv1 = *inv1 * un_sur;
		*inv2 = *inv2 * un_sur;
		*inv3 = *inv3 * un_sur;
	}
	return res;
}

//-------------------------------------------------------------

float distToLine(VEC3 M, VEC3 P, VEC3 u)
{
	u.normalize();
	VEC3 MP = P - M;
	return(MP*(u^(MP^u)));
}

//-------------------------------------------------------------
// Première stratégie : appliquer à p1 et à p2 une force
// colinéaire à f, mais d'intensité différente. Dans ce cas,
// il est nécessaire de calculer la somme des moments et de
// l'annuler. *f1 = f*d2/(d1+d2) et *f2 = f*d1/(d1+d2).

void getResponse1(VEC3 f, VEC3 p, VEC3 p1, VEC3 p2, VEC3 *f1, VEC3 *f2)
{
	float d1  = distToLine(p1,p,f);
	float d2  = distToLine(p2,p,f);

	*f1 = f*(-d2/(d1+d2));
	*f2 = f*(-d1/(d1+d2));
}

//-------------------------------------------------------------
// Deuxième stratégie, les forces appliquées à p1 et p2
// doivent etre colinaires respectivement à pp1 et pp2.
// Donc la somme des moments est automatiquement nulle.
// Pour calculer les intensites, il faut décomposer f
// dans la base (pp1,pp2), ce qui donnera l'intensite de
// chaque force. Problème : on peut générer des forces
// d'étirement importantes.

void getResponse2(VEC3 f, VEC3 p, VEC3 p1, VEC3 p2, VEC3 *f1, VEC3 *f2)
{
	float d1  = (p1-p).norm();
	float d2  = (p2-p).norm();
	float d12 = (p2-p1).norm();

	VEC3 ortho = (p1-p) ^ (p2-p);
	if(ortho.norm()==0.0)
	{
		if(d12==0) // comprend le cas où d1==0 et d2==0
		{
			*f2 = (-0.5)*f;
			*f1 = (-0.5)*f;
		}
		else if(d1==0)
		{
			*f2 = VEC3(0,0,0);
			*f1 = -f;
			// meilleure idee : projeter f sur p1p2 -> f2 et f1 <- f-f2
		}
		else if(d2==0)
		{
			*f1 = VEC3(0,0,0);
			*f2 = -f;
			// meilleure idee : projeter f sur p1p2 -> f1 et f2 <- f-f1
		}
		else // trois points distincts, mais alignés
		{
			// A suivre
		}
	}
	// int res = matrixInverse(p1-p,p2-p,ortho, VEC3 *inv1, VEC3 *inv2, VEC3 *inv3);
	// inv1, inv2 et inv3 sont les composantes de la matrice inverse. L'image de
	// p1-p et p2-p par cette matrice donne la valeur des forces f1 et f2.
}


//-------------------------------------------------------------

void MovingObstacle::initForces()
{
732
	Dart dd = groundFace;
Arash HABIBI's avatar
unidir    
Arash HABIBI committed
733
	if(!rigid_)
734
		for (unsigned int i = 0; i < nbVertices; ++i)
Arash HABIBI's avatar
unidir    
Arash HABIBI committed
735
736
737
738
739
740
741
		{
			//initialisation of forces
			forces[dd] = VEC3(0.0);
			map.next(dd);
		}
}

Arash HABIBI's avatar
Arash HABIBI committed
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782

//-------------------------------------------------------------

VEC3 computeForce(VEC3 p, VEC3 p1, VEC3 p2, float obst_radius_infl, float obst_power, float obst_stiffness)
{
	double force_value=0.0;
	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)
	{
		double 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)/sum_of_dists);
		// 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();

		// force_value *= 10;
		/*
		VEC3 force_vector1, force_vector2;
		getResponse1(force_vector,p,p1,p2,&force_vector1,&force_vector2);
		Dart d1 = obst->d1;
		Dart d2 = obst->d2;
		obst->mo->forces[d1] += force_vector1;
		obst->mo->forces[d2] += force_vector2;
		*/
		return(force_value * normal);
	}
	else
		return(VEC3(0));
}


Arash HABIBI's avatar
unidir    
Arash HABIBI committed
783
784
//-------------------------------------------------------------

785
void MovingObstacle::updateForces()
pitiot's avatar
maj    
pitiot committed
786
{
pitiot's avatar
pitiot committed
787
	assert(sim_->envMap_.map.getCurrentLevel() == sim_->envMap_.map.getMaxLevel()) ;
pitiot's avatar
pitiot committed
788

pitiot's avatar
pitiot committed
789
	//pour les tests de détection///////////////
pitiot's avatar
merging    
pitiot committed
790
791
792
793
794
795
796
	if(!seen)
	{
		color1=1.0f;
		color2=1.0f;
		color3=1.0f;
	}
	seen=false;
Thomas Jund's avatar
merge    
Thomas Jund committed
797

pitiot's avatar
pitiot committed
798
	//////////////////////////
Thomas Jund's avatar
merge    
Thomas Jund committed
799

800
	PFP::VEC3 bary(0);
pitiot's avatar
maj    
pitiot committed
801
802

	Dart d;
pitiot's avatar
pitiot committed
803
804
	velocity_[0] = newVelocity_[0] * velocity_factor;
	velocity_[1] = newVelocity_[1] * velocity_factor;
Arash HABIBI's avatar
Arash HABIBI committed
805
806
	// velocity_[0] = 0.0;
	// velocity_[1] = 0.0;
pitiot's avatar
maj    
pitiot committed
807
808

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

pitiot's avatar
merging    
pitiot committed
811
	float rotor=0;
812

pitiot's avatar
merging    
pitiot committed
813
814
815
816
	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
817

Arash HABIBI's avatar
03_2013    
Arash HABIBI committed
818
819
	// masse ressort pour la limace

820
	if(!rigid_)
pitiot's avatar
maj    
pitiot committed
821
	{
822
		Dart d = groundFace;
823
		// map.next(d);
pitiot's avatar
maj    
pitiot committed
824

Thomas Jund's avatar
Thomas Jund committed
825
		Dart dd =d;
826
		for (unsigned int i = 0; i < nbVertices; ++i)
Thomas Jund's avatar
Thomas Jund committed
827
828
		{
			//initialisation of forces with viscosity
Arash HABIBI's avatar
unidir    
Arash HABIBI committed
829
			forces[dd] += -0.2f*velocity[dd];
Arash HABIBI's avatar
Arash HABIBI committed
830
			// forces[dd] = VEC3(0.0);
Thomas Jund's avatar
Thomas Jund committed
831
832
833
			map.next(dd);
		}

834
		CellMarkerStore<EDGE> cm(map);
Arash HABIBI's avatar
Arash HABIBI committed
835
		CellMarkerStore<VERTEX> cmV(map);
Thomas Jund's avatar
Thomas Jund committed
836
		DartMarkerStore dm(map);
Arash HABIBI's avatar
03_2013    
Arash HABIBI committed
837
		// ARASH : On parcourt les sommets de la grande face
838
		for (unsigned int i = 0; i < nbVertices; ++i)
839
840
		{
			Dart dd = d;
Arash HABIBI's avatar
03_2013    
Arash HABIBI committed
841
842

			// ARASH : On parcourt les sous-faces triangulaire de la grande face
Arash HABIBI's avatar
Arash HABIBI committed
843

844
845
846
847
			do {
				if(!cm.isMarked(dd))
				{
					cm.mark(dd);
Thomas Jund's avatar
Thomas Jund committed
848
849
					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
850
					// p1Next et p2Next sont la position des extremites de l'arete.
Thomas Jund's avatar
Thomas Jund committed
851
					VEC3 v1 = (p1Next-p2Next);
852
853
854

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

857
					float stretch = rigidity*(edgeLength[dd]-v1.norm());
858
859
					if(norm>0.0f)
					{
Arash HABIBI's avatar
Arash HABIBI committed
860
						VEC3 f = 1.5*stretch*(v1/norm);
861

862
863
864
865
						//apply force symmetrically
						forces[dd] -= f;
						forces[map.phi1(dd)] += f;
					}
Arash HABIBI's avatar
Arash HABIBI committed
866
867
				}

Thomas Jund's avatar
Thomas Jund committed
868
869
870
				dd = map.phi1(dd);
			} while(dd!=d);

Arash HABIBI's avatar
03_2013    
Arash HABIBI committed
871
			// ARASH : les ressorts angulaires
Thomas Jund's avatar
Thomas Jund committed
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
			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
887
						float angularRig = 70.0f;
Thomas Jund's avatar
Thomas Jund committed
888

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

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

893
894
895
896
897
898
899
						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
900
					}
901
				}
pitiot's avatar
maj    
pitiot committed
902

903
904
				dd = map.phi1(dd);
			} while(dd!=d);
pitiot's avatar
maj    
pitiot committed
905

Arash HABIBI's avatar
03_2013    
Arash HABIBI committed
906
907
908
909
			//-------------------------------------------------------------------------
			// ARASH : A présent on calcule les interactions avec les autres obstacles.

			do {
Arash HABIBI's avatar
Arash HABIBI committed
910
				if(!cmV.isMarked(dd))
Arash HABIBI's avatar
03_2013    
Arash HABIBI committed
911
				{
Arash HABIBI's avatar
Arash HABIBI committed
912
					cmV.mark(dd);
Arash HABIBI's avatar
03_2013    
Arash HABIBI committed
913

Arash HABIBI's avatar
comment    
Arash HABIBI committed
914
					VEC3 norm;
Arash HABIBI's avatar
Arash HABIBI committed
915
916
					double obst_stiffness = 5.0; // agent-obstacle interaction stiffness
					int obst_power = 2  ;           // the power to which elevate the agent-obstacle distance
Arash HABIBI's avatar
comment    
Arash HABIBI committed
917
918
919
920
921

					double obst_radius_infl;
					if(sim_->config==0)
						obst_radius_infl = 100.; // scenario 0
					else
Arash HABIBI's avatar
Arash HABIBI committed
922
						obst_radius_infl = 30.; // scenario 1 et 3
Arash HABIBI's avatar
comment    
Arash HABIBI committed
923

Arash HABIBI's avatar
03_2013    
Arash HABIBI committed
924
925
					VEC3 p = position[dd]+(velocity[dd] * sim_->timeStep_);

Arash HABIBI's avatar
Arash HABIBI committed
926
927
					// Evitement d'obstacles mobiles

Arash HABIBI's avatar
03_2013    
Arash HABIBI committed
928
929
930
931
932
933
					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 ;
Arash HABIBI's avatar
unidir    
Arash HABIBI committed
934

Arash HABIBI's avatar
Arash HABIBI committed
935
936
						forces[dd] += computeForce(p,p1,p2,obst_radius_infl,obst_power,obst_stiffness);
					}
Arash HABIBI's avatar
unidir    
Arash HABIBI committed
937

Arash HABIBI's avatar
Arash HABIBI committed
938
					// Evitement d'obstacles fixes
Arash HABIBI's avatar
03_2013    
Arash HABIBI committed
939

Arash HABIBI's avatar
Arash HABIBI committed
940
941
942
943
944
945
946
947
					for(std::vector<std::pair<float, Obstacle*> >::iterator it = obstacleNeighbors_.begin() ;
						it != obstacleNeighbors_.end() ; ++it)
					{
						Obstacle * obst = it->second;
						VEC3 p1=obst->p1 ;
						VEC3 p2=obst->p2 ;

						forces[dd] += computeForce(p,p1,p2,obst_radius_infl,obst_power,10*obst_stiffness);
Arash HABIBI's avatar
03_2013    
Arash HABIBI committed
948
949
950
951
952
					}
				}
				dd = map.phi1(dd);
			} while(dd!=d);

953
954
955
956
			map.next(d);
		}

		//guiding vertex = first vertex (set the displacement)
957
		// forces[groundFace] = VEC3(0);
958
		velocity[groundFace] = velocity_;
Arash HABIBI's avatar
Arash HABIBI committed
959

960
		//apply force to each vertex
961
		/*
962
963
		d = groundFace;
		for (unsigned int i = 0; i < nbVertices; ++i)
pitiot's avatar
pitiot committed
964
		{
Thomas Jund's avatar
Thomas Jund committed
965
			velocity[d] += forces[d] * sim_->timeStep_;
966
967
968
969
			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
970
		}
971
		*/
972
	}
pitiot's avatar
pitiot committed
973
974
	else
	{
975
976
977
978
979
980
981
982
983
	//	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_);

984
985
986
987
988
#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

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

			bary += position[d];
Thomas Jund's avatar
Thomas Jund committed
992
993


994
995
996
			map.next(d);
		}

pitiot's avatar
pitiot committed
997
		front=(position[0] + position[1]) / 2;
998
999
1000
1001
		if(angle >0)
			angle -= rotor;
		else
			angle += rotor;
pitiot's avatar
pitiot committed
1002
	}
pitiot's avatar
maj    
pitiot committed
1003

pitiot's avatar
pitiot committed
1004
	center = bary / nbVertices;
pitiot's avatar
maj    
pitiot committed
1005

1006
1007
	//computing deformation vector on vertices :
	//depending on the velocity, extend the perceived shape
1008
	VEC3 vel = velocity_.normalized();
1009
1010
1011

	d = groundFace;
	for (unsigned int i = 0; i < nbVertices; ++i)
1012
	{
1013
		VEC3 v(position[d] - center);
1014
1015
		VEC3 vN = v.normalized();
		float dot = vN * vel;
1016
1017
		deformation[d] = v * 2.0f * (dot+1.0f)/2.0f;
		map.next(d);
1018
1019
	}

Thomas Jund's avatar
Thomas Jund committed
1020
1021
//	if(!rigid_)
//		center = position[groundFace];
1022
1023
1024
}

//-------------------------------------------------------------------------
1025

1026
1027
1028
1029
1030
1031
1032
1033
1034
1035
1036
1037
1038
1039
1040
1041
1042
1043
void MovingObstacle::applyForces()
{
	PFP::VEC3 bary(0);

	if(!rigid_)
	{
		Dart d = groundFace;
		for (unsigned int i = 0; i < nbVertices; ++i)
		{
			velocity[d] += forces[d] * sim_->timeStep_;
			position[d] += (velocity[d] * sim_->timeStep_);
			position[map.phi<211>(d)] += (velocity[d] * sim_->timeStep_);
			bary += position[d];
			map.next(d);
		}
		center = bary / nbVertices;
	}
// MAJ des obstacles
pitiot's avatar
maj    
pitiot committed
1044
1045
	for (unsigned int i = 0; i < nbVertices; ++i)
	{
1046
1047
1048
1049
1050
1051
1052
1053
1054
1055
#ifdef TWO_AND_HALF_DIM
			std::cout << " dist supp " << (getDilatedPosition(i)-parts_[i]->getPosition()).norm() << std::endl;
#endif
			parts_[i]->move(getDilatedPosition(i));
#ifdef TWO_AND_HALF_DIM
			std::cout << " dist " << parts_[i]->getDistance() << std::endl;

			position[d] = parts_[i]->getPosition();
#endif

pitiot's avatar
maj    
pitiot committed
1056
1057
		//		CGoGNout << "avant une etape : Obstacle "<< i << CGoGNendl;
		Obstacle* o = obstacles_[i];
1058
1059
		o->p1 = getDilatedPosition(i);
		o->p2 = getDilatedPosition((i+1) % nbVertices);
Thomas Jund's avatar
Thomas Jund committed
1060
1061

#ifndef POTENTIAL
1062
1063
		o->prevP = getDilatedPosition((i - 1 + nbVertices) % nbVertices);
		o->nextP = getDilatedPosition((i + 2 + nbVertices) % nbVertices);
Thomas Jund's avatar
Thomas Jund committed
1064
1065
#endif

pitiot's avatar
pitiot committed
1066
1067
		Dart d1 = parts_[i]->d;
		Dart d2 = parts_[(i+1)%nbVertices]->d;
Thomas Jund's avatar
Thomas Jund committed
1068

pitiot's avatar
pitiot committed
1069
		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
1070
		{
Thomas Jund's avatar
Thomas Jund committed
1071
			sim_->envMap_.popAndPushObstacleInCells(o,i);
pitiot's avatar
pitiot committed
1072
		}
Thomas Jund's avatar
Thomas Jund committed
1073

pitiot's avatar
pitiot committed
1074
1075
1076
1077
1078
1079
1080
1081
		/////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)
//		{
//			CGoGNout<<(*ite).index<<" ; ";
//		}
//		CGoGNout<< CGoGNendl;
pitiot's avatar
maj    
pitiot committed
1082
1083
		//		CGoGNout << "Apres une etape : Obstacle "<< i << CGoGNendl;
	}
pitiot's avatar
pitiot committed
1084
1085
1086
1087
1088
1089
1090
1091
	/////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
1092
1093
1094
	//
	//		envMap_->addMovingObstAsNeighbor(this,general_belonging,&(general_neighbors));
	//		CGoGNout << "nouvel update : "<< CGoGNendl;
1095
1096
	if(sim_->detect_agent_collision)
	{
pitiot's avatar
pitiot committed
1097
		for (std::vector< std::pair<Dart, int> >::iterator it2 = general_belonging.begin(); it2 != general_belonging.end(); ++ it2)
1098
		{
pitiot's avatar
pitiot committed
1099
			std::vector<Agent*> vector =sim_->envMap_.agentvect[(*it2).first];
1100
1101
1102
1103
1104
1105
1106
1107
1108
1109
1110
			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
1111

1112
1113
		}
	}
pitiot's avatar
maj    
pitiot committed
1114
1115
}

1116
//-------------------------------------------------------------------------
Thomas Jund's avatar
Thomas Jund committed
1117

pitiot's avatar
pitiot committed
1118
std::vector<Dart> MovingObstacle::getMemoCross(const VEC3& pos, const VEC3& dest, Dart& d1, Dart& d2)
pitiot's avatar
maj    
pitiot committed
1119
{
1120
1121
1122
#ifdef TWO_AND_HALF_DIM
	CGoGN::Algo::Surface::MovingObjects::ParticleCell2DAndHalfMemo<PFP> * registering_part = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2DAndHalfMemo<PFP>(sim_->envMap_.map, d1,pos,sim_->envMap_.position);
#else
pitiot's avatar
pitiot committed
1123
	CGoGN::Algo::Surface::MovingObjects::ParticleCell2DMemo<PFP> * registering_part = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2DMemo<PFP>(sim_->envMap_.map, d1,pos,sim_->envMap_.position);
1124
#endif
pitiot's avatar
pitiot committed
1125
1126
	std::vector<Dart> result =(registering_part->move(dest));
	d2=registering_part->d;
1127
//	CGoGNout<<"d1 : "<< *d1<<"|| d2 : "<< *d2<<"|| start : "<< pos<<"|| stop : "<< dest<<CGoGNendl;
pitiot's avatar
pitiot committed
1128
	return result;
pitiot's avatar
maj    
pitiot committed
1129
1130
}

1131
void resetPartSubdiv(Obstacle* o)
pitiot's avatar
maj    
pitiot committed
1132
1133
{
	MovingObstacle * mo = o->mo;
1134

pitiot's avatar
maj    
pitiot committed
1135
1136
	if (mo != NULL)
	{
pitiot's avatar
up    
pitiot committed
1137
1138
		unsigned int n =o->index;
		unsigned int n2 =(n+1)%(mo->nbVertices);
pitiot's avatar
merging    
pitiot committed
1139

pitiot's avatar
up    
pitiot committed
1140
1141
		VEC3 pos = mo->parts_[n]->getPosition();
		VEC3 pos2 = mo->parts_[n2]->getPosition();
pitiot's avatar
pitiot committed
1142
		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
1143

pitiot's avatar
pitiot committed
1144
1145
1146
		mo->parts_[n]->setState(FACE) ;
		mo->parts_[n]->move(pos) ;

pitiot's avatar
up    
pitiot committed
1147
1148
1149
1150
1151
		mo->parts_[n2]->CGoGN::Algo::MovingObjects::ParticleBase<PFP>::move(Algo::Surface::Geometry::faceCentroid<PFP>(mo->sim_->envMap_.map, mo->parts_[n]->d, mo->sim_->envMap_.position)) ;

		mo->parts_[n2]->setState(FACE) ;
		mo->parts_[n2]->move(pos2) ;

pitiot's avatar
pitiot committed
1152
		mo->dDir = mo->parts_[0]->d;
pitiot's avatar
maj    
pitiot committed
1153
1154
	}
}
pitiot's avatar
up    
pitiot committed
1155
//, unsigned int fLevel
pitiot's avatar
maj    
pitiot committed
1156
1157
1158
void resetObstPartInFace(Obstacle* o, Dart d1)
{
	MovingObstacle * mo = o->mo;
1159

Thomas Jund's avatar
Thomas Jund committed
1160
1161
	if (mo != NULL)
	{
pitiot's avatar
pitiot committed
1162
		unsigned int n =o->index;
pitiot's avatar
up    
pitiot committed
1163
		unsigned int n2 =(n+1)%(mo->nbVertices);
pitiot's avatar
pitiot committed
1164
		VEC3 pos1 = mo->parts_[n]->getPosition();
pitiot's avatar
up    
pitiot committed
1165
1166
1167
		VEC3 pos2 = mo->parts_[n2]->getPosition();
//		if (Algo::Surface::Geometry::isPointInConvexFace2D <PFP> (mo->sim_->envMap_.map, d1, mo->sim_->envMap_.position, pos1, true))
		if ( mo->sim_->envMap_.is_insideConvexCell2D(pos1, d1))
pitiot's avatar
pitiot committed
1168
			mo->parts_[n]->d = d1;
pitiot's avatar
up    
pitiot committed
1169
1170
1171
//		if (Algo::Surface::Geometry::isPointInConvexFace2D <PFP> (mo->sim_->envMap_.map, d1, mo->sim_->envMap_.position, pos2, true))
		if ( mo->sim_->envMap_.is_insideConvexCell2D(pos2, d1))
					mo->parts_[n2]->d = d1;
pitiot's avatar
pitiot committed
1172
		if(n==0)
pitiot's avatar
pitiot committed
1173
			mo->dDir = mo->parts_[0]->d;
pitiot's avatar
maj    
pitiot committed
1174
	}
pitiot's avatar
up    
pitiot committed
1175
1176
1177
1178
1179
1180
1181
1182
1183
1184
1185
1186
1187
1188
1189
1190
1191
1192
1193
1194
1195
1196
1197
1198
//
//	MovingObstacle * mo = o->mo;
//
//	if (mo != NULL)
//	{
//
//		unsigned int n =o->index;
//		unsigned int n2 =(n+1)%mo->nbVertices;
//		Dart d2 =mo->sim_->envMap_.map.faceOldestDart(mo->parts_[n]->d);
//		Dart d3 =mo->sim_->envMap_.map.faceOldestDart(mo->parts_[n2]->d);
//		mo->sim_->envMap_.map.setCurrentLevel(fLevel - 1) ;
//		if ( mo->sim_->envMap_.map.sameFace(d1,d2))
//		{
//			CGoGNout<<"particule : "<< n<<" du MovingObstacle "<<mo->index<< " changée de " << mo->parts_[n]->d <<" en "<<d2<<CGoGNendl;
//			mo->parts_[n]->d = d2;
//
//		}
//		if ( mo->sim_->envMap_.map.sameFace(d1,d3))
//		{
//			CGoGNout<<"particule : "<< n2<<" du MovingObstacle "<<mo->index<< " changée de " << mo->parts_[n2]->d <<" en "<<d3<<CGoGNendl;
//			mo->parts_[n2]->d = d3;
//		}
//		mo->sim_->envMap_.map.setCurrentLevel(mo->sim_->envMap_.map.getMaxLevel()) ;
//	}
pitiot's avatar
maj    
pitiot committed
1199
1200
}

pitiot's avatar
pitiot committed
1201
void resetPart(Obstacle * o, Dart d1)
pitiot's avatar
maj    
pitiot committed
1202
{
pitiot's avatar
pitiot committed
1203
1204
1205
1206
	MovingObstacle * mo = o->mo;
	if(mo!=NULL)
	{
		int n =o->index;
pitiot's avatar
pitiot committed
1207
1208
1209

		if (mo->parts_[n]->d == mo->sim_->envMap_.map.phi1(d1))
			mo->parts_[n]->d = d1;
Thomas Jund's avatar
Thomas Jund committed
1210

pitiot's avatar
up    
pitiot committed
1211
1212
1213
		if (mo->parts_[(n+1)%mo->nbVertices]->d == mo->sim_->envMap_.map.phi1(d1))
					mo->parts_[(n+1)%mo->nbVertices]->d = d1;

pitiot's avatar
pitiot committed
1214
		if(n==0)
pitiot's avatar
pitiot committed
1215
			mo->dDir = mo->parts_[n]->d;
pitiot's avatar
pitiot committed
1216
	}
pitiot's avatar
maj    
pitiot committed
1217
1218
1219
1220
}

void displayMO(Obstacle * o)
{
pitiot's avatar
pitiot committed
1221
	CGoGNout << "obstacle problematique : " <<(o->index) <<"du MO n° : "<< (o->mo->index) << CGoGNendl;
pitiot's avatar
maj    
pitiot committed
1222
}
pitiot's avatar
pitiot committed
1223
1224
1225
1226
1227
1228
1229
1230
1231
1232

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////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
1233
	VEC3 goalVector;
Thomas Jund's avatar
Thomas Jund committed
1234
	if (index_parent<1) //not articulated
pitiot's avatar
pitiot committed
1235
	{
Thomas Jund's avatar
Thomas Jund committed
1236
		if(rigid_ || goals_.size()>1)
pitiot's avatar
merging    
pitiot committed
1237
1238
		{
			goalVector = goals_[curGoal_] - center ;
Thomas Jund's avatar
Thomas Jund committed
1239
	//		goalVector = goals_[curGoal_] - position[groundFace] ;
pitiot's avatar
merging    
pitiot committed
1240

Thomas Jund's avatar
Thomas Jund committed
1241
1242
			float goalDist2 = goalVector.norm2() ;

1243
			if (goalDist2 < 1000.0f)
Thomas Jund's avatar
Thomas Jund committed
1244
1245
1246
1247
1248
1249
1250
1251
1252
1253
1254
1255
1256
1257
			{
				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
1258
		{
Thomas Jund's avatar
Thomas Jund committed
1259
1260
1261
1262
1263
1264
1265
1266
1267
1268
			if(ag_!=NULL)
			{
				goalVector = ag_->getPosition() - center ;
				if (goalVector.norm2() > maxSpeed_)
				{
					goalVector.normalize() ;
					goalVector *= maxSpeed_;
				}
			}
			else
Thomas Jund's avatar
Thomas Jund committed
1269
			{
Thomas Jund's avatar
Thomas Jund committed
1270
1271
	//			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
1272

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

Thomas Jund's avatar
Thomas Jund committed
1276
1277
1278
1279
1280
1281
1282
				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
1283

Thomas Jund's avatar
Thomas Jund committed
1284
1285
1286
					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] ;