moving_obstacle.cpp 40.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
//	return false;
pitiot's avatar
up    
pitiot committed
66
	VEC3 vec, norm, p1,vec2,p2;
67
68
69
70
71


	for (unsigned int i = 0; i < nbVertices; i++)
	{
		 p1 = getPosition(i);
pitiot's avatar
up    
pitiot committed
72
		 p2 = center;
73
74
75

		vec = VEC3(p2 - p1);

pitiot's avatar
up    
pitiot committed
76
77
78

		vec2 = VEC3(p -p1);
		if (vec*vec2 < 0)
79
80
81
82
			return false;
	}

	return true;
pitiot's avatar
maj    
pitiot committed
83
84
}

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

pitiot's avatar
pitiot committed
112
VEC3 rotate2D(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
113
114
115
116
117
118
119
120
121
{
	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
122

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
#ifdef TWO_AND_HALF_DIM
pitiot's avatar
pitiot committed
150
	parts_ = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2DAndHalf<PFP>*[nbVertices+1];
151
#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
	{
#ifdef TWO_AND_HALF_DIM
pitiot's avatar
pitiot committed
174
175
		Dart d = dInside;/*sim_->envMap_.getBelongingCellOnSurface(pos[i]);  TO CHECK*/
//		CGoGNout<<" d trouvée :"<< d <<CGoGNendl;
176
		CGoGN::Algo::Surface::MovingObjects::ParticleCell2DAndHalf<PFP>* part = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2DAndHalf<PFP>(sim_->envMap_.map, d, pos[i], sim_->envMap_.position);
177

178
#else
179
		Dart d = sim_->envMap_.getBelongingCell(pos[i]);
180
181
182
183
184
185
186
187
		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;
	}
188
189
190

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

pitiot's avatar
pitiot committed
192
193
194
195
196
197

#ifdef TWO_AND_HALF_DIM
	CGoGN::Algo::Surface::MovingObjects::ParticleCell2DAndHalf<PFP>* part = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2DAndHalf<PFP>(sim_->envMap_.map, dInside, center, sim_->envMap_.position);
	parts_[nbVertices]=part;
#endif

198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
		// 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
223

224
225
	groundFace = map.newFace(nbVertices);

pitiot's avatar
maj    
pitiot committed
226
227
	for (unsigned int i = 0; i < nbVertices; ++i)
	{
228
229
230
231
232
		float distance=(center-pos[i]).norm();
		if(distance>gravity_dist)
		{
			gravity_dist=distance;
		}
Thomas Jund's avatar
Thomas Jund committed
233
		Dart d = i;
234
235
		position[d] = pos[i];
		deformation[d] = VEC3(0);
236
237

		if(!rigid_)
Thomas Jund's avatar
Thomas Jund committed
238
		{
239
			velocity[d] = VEC3(0);
Thomas Jund's avatar
Thomas Jund committed
240
241
			forces[d] = VEC3(0);
		}
242

243

pitiot's avatar
pitiot committed
244
	}
245

246
247
	//extrude face to build a cage
	// compute edgeLength for mass-spring
pitiot's avatar
pitiot committed
248
	hight=10.0f;
pitiot's avatar
pitiot committed
249
	Algo::Surface::Modelisation::extrudeFace<PFP>(map, position, groundFace, hight) ;
pitiot's avatar
pitiot committed
250
251
252
253
254
//	Dart dT = map.phi1(map.phi1(map.phi2(Dart(0))));
//	std::cout << __FUNCTION__ << " val " << dT << std::endl;
//	position[dT]= VEC3(0);
//	std::cout << __FUNCTION__ << " pos " << position[dT] << std::endl;

255
//	Algo::Surface::Modelisation::extrudeFace<PFP>(map, position, groundFace, 0.0f) ;
256
257
258
	map.fillHole(groundFace);
	groundFace = map.phi2(groundFace);

pitiot's avatar
pitiot committed
259

260
261
	if(!rigid_)
	{
pitiot's avatar
pitiot committed
262
263
264
265
266
267
268
269
270
271
272
273
		Dart d = Algo::Surface::Modelisation::trianguleFace<PFP>(map,groundFace);
		position[d]=parts_[nbVertices]->getPosition();


		for (unsigned int i = 0; i < nbVertices; ++i)
		{
			Dart e =map.phi<12>(d);
			map.splitFace(map.phi_1(e),map.phi1(e));

			d=map.phi<21>(d);
		}

274
		Algo::Surface::Modelisation::EarTriangulation<PFP> et(map);
275
276
277
278
279
280
281
		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
282

Thomas Jund's avatar
Thomas Jund committed
283
284
285
		DartMarker treated(map);
		for(Dart d = map.begin() ; d != map.end() ; map.next(d))
		{
286
			if(!map.isBoundaryMarked2(d) && !treated.isMarked(d))
Thomas Jund's avatar
Thomas Jund committed
287
288
			{
				treated.mark(d);
289
				vertexAngle[d] = Algo::Surface::Geometry::angle<PFP>(map,map.phi_1(d),map.phi2(map.phi1(d)),position);
Thomas Jund's avatar
Thomas Jund committed
290
291
292
			}
		}

293
		map.enableQuickTraversal<VERTEX>();
294

Thomas Jund's avatar
Thomas Jund committed
295
		dDir=dInside;
pitiot's avatar
pitiot committed
296
	}
297

pitiot's avatar
merging    
pitiot committed
298

299

pitiot's avatar
maj    
pitiot committed
300
301
	for (unsigned int i = 0; i < nbVertices; ++i)
	{
pitiot's avatar
pitiot committed
302
303
		Obstacle* o = new Obstacle(parts_[i]->getPosition(),
				parts_[(i + 1) % nbVertices]->getPosition(),
Thomas Jund's avatar
merge    
Thomas Jund committed
304
				parts_[(i - 1 + nbVertices) % nbVertices]->getPosition(),
Thomas Jund's avatar
Thomas Jund committed
305
				parts_[(i + 2) % nbVertices]->getPosition(), i, (i+1)% nbVertices, this, i);
pitiot's avatar
maj    
pitiot committed
306
		obstacles_[i] = o;
pitiot's avatar
pitiot committed
307
//		CGoGNout<<" obstacle :"<< i << " num : "<< o<<CGoGNendl;
pitiot's avatar
pitiot committed
308
		sim_->envMap_.pushObstacleInCells(o);
pitiot's avatar
maj    
pitiot committed
309
310
311
	}
}

312
313
314
315
316
317
318
319
320
321
322
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
323
	m_shader->setColor(Geom::Vec4f(0.,1.,0.,0.));
324
325
326
327
328
329
330
331
332
333
//	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
334
335

	m_ds = new Utils::Drawer();
336
337
}

Thomas Jund's avatar
Thomas Jund committed
338
void MovingObstacle::draw(bool showPath)
339
340
341
342
343
{
#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);
344
//	m_shader->setColor(Geom::Vec4f(movingObstacleNeighbors_.size()==0 ? 1.0f : 0,0.,0.,0.));
pitiot's avatar
merge    
pitiot committed
345

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

pitiot's avatar
up    
pitiot committed
348
349
//	if(movingObstacleNeighbors_.size()==0)
	if(index==12)
Arash HABIBI's avatar
Arash HABIBI committed
350
	// if(obstacleNeighbors_.size()==0)
351
352
353
		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
354

355
356
357
358
	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
359
360
361
362
363
364
365
366

	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]);
pitiot's avatar
up    
pitiot committed
367
368
		m_ds->vertex(center);
		for(unsigned int i = 0 ; i < goals_.size() ; i++)
Thomas Jund's avatar
Thomas Jund committed
369
		{
pitiot's avatar
up    
pitiot committed
370
			m_ds->vertex(goals_[(curGoal_+i)%(goals_.size())]);
Thomas Jund's avatar
Thomas Jund committed
371
372
373
374
375
		}

		m_ds->end();
		m_ds->endList();
	}
376
377
}

378
379
VEC3 MovingObstacle::getDilatedPosition(unsigned int ind)
{
380
	Dart d(ind); //WARNING : works only for one face created at start !
pitiot's avatar
pitiot committed
381
#ifndef TWO_AND_HALF_DIM
pitiot's avatar
up    
pitiot committed
382
383
384
385
386
387
//	return position[d]+deformation[d];
	if(!rigid_)
		{
			return position[d];
		}
	else return position[d]+deformation[d];
pitiot's avatar
pitiot committed
388
#else
Arash HABIBI's avatar
Arash HABIBI committed
389
	return position[d];
pitiot's avatar
pitiot committed
390
#endif
391
392
393
394
}

VEC3 MovingObstacle::getPosition(unsigned int ind)
{
395
396
	Dart d(ind);
	return position[d];
397
398
}

pitiot's avatar
maj    
pitiot committed
399
400
401
402
bool MovingObstacle::test_opposition(VEC3 o, VEC3 p1, VEC3 p2)
{
	o.normalize();
	o *= -1;
403
404
405
	VEC3 vector(p1 - p2);
	vector.normalize();
	return (o - vector).norm2() < 0.1;
pitiot's avatar
maj    
pitiot committed
406
407
408
}

// TODO Check position
pitiot's avatar
pitiot committed
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
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
//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
463

pitiot's avatar
pitiot committed
464
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
465
{
Thomas Jund's avatar
Thomas Jund committed
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
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
	//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
519
520
521
522
523
//	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
524
//	{
pitiot's avatar
pitiot committed
525
526
527
528
529
530
//		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
531
//
pitiot's avatar
pitiot committed
532
533
534
535
//		for (std::vector<Agent*>::const_iterator it = agents.begin(); it != agents.end(); ++it)
//		{
//			if ((*it)->alive)
//			{
pitiot's avatar
pitiot committed
536
//
pitiot's avatar
pitiot committed
537
538
539
540
//				float distSq = (center - (*it)->getPosition()).norm2() ;
//				if ((agentNeighbors_.size() < maxNeighbors_ || distSq < maxDist)
//					&& distSq < neighborDistSq_)
//				{
pitiot's avatar
pitiot committed
541
//
pitiot's avatar
pitiot committed
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
//					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
577
//	}
pitiot's avatar
pitiot committed
578
579
580
581
}

void MovingObstacle::updateObstacleNeighbors() // obstacles voisins , distance par rapport aux centres des segments// a mettre en place si besoin
{
582
583
	obstacleNeighbors_.clear() ;
	movingObstacleNeighbors_.clear() ;
pitiot's avatar
pitiot committed
584
	for (std::vector< std::pair<Dart, int> >::iterator it2 = general_belonging.begin();it2 != general_belonging.end(); ++it2)
585
	{
pitiot's avatar
pitiot committed
586
587
		std::vector<Obstacle*>& obst = sim_->envMap_.obstvect[(*it2).first] ;
		std::vector<Obstacle*>& neighborObst = sim_->envMap_.neighborObstvect[(*it2).first] ;
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
631
632
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
//		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
666
667
}

Arash HABIBI's avatar
unidir    
Arash HABIBI committed
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
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
//-------------------------------------------------------------

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()
{
pitiot's avatar
pitiot committed
767

Arash HABIBI's avatar
unidir    
Arash HABIBI committed
768
	if(!rigid_)
pitiot's avatar
pitiot committed
769
770
771
772
	{
		Dart centerDart =map.phi<11>(groundFace);
			Dart d = centerDart;
		do
Arash HABIBI's avatar
unidir    
Arash HABIBI committed
773
		{
pitiot's avatar
pitiot committed
774
			Dart e = map.phi1(d);
Arash HABIBI's avatar
unidir    
Arash HABIBI committed
775
			//initialisation of forces
pitiot's avatar
pitiot committed
776
777
			forces[e] = VEC3(0.0);
			d=map.phi<21>(d);
Arash HABIBI's avatar
unidir    
Arash HABIBI committed
778
		}
pitiot's avatar
pitiot committed
779
780
781
		while(d!=centerDart);
		forces[d] = VEC3(0.0);
	}
Arash HABIBI's avatar
unidir    
Arash HABIBI committed
782
783
}

Arash HABIBI's avatar
Arash HABIBI committed
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824

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

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
825
826
//-------------------------------------------------------------

827
void MovingObstacle::updateForces()
pitiot's avatar
maj    
pitiot committed
828
{
pitiot's avatar
pitiot committed
829
	assert(sim_->envMap_.map.getCurrentLevel() == sim_->envMap_.map.getMaxLevel()) ;
pitiot's avatar
pitiot committed
830

pitiot's avatar
pitiot committed
831
	//pour les tests de détection///////////////
pitiot's avatar
merging    
pitiot committed
832
833
834
835
836
837
838
	if(!seen)
	{
		color1=1.0f;
		color2=1.0f;
		color3=1.0f;
	}
	seen=false;
Thomas Jund's avatar
merge    
Thomas Jund committed
839

pitiot's avatar
pitiot committed
840
	//////////////////////////
Thomas Jund's avatar
merge    
Thomas Jund committed
841

842
	PFP::VEC3 bary(0);
pitiot's avatar
maj    
pitiot committed
843
844

	Dart d;
pitiot's avatar
pitiot committed
845
846
	velocity_ = newVelocity_* velocity_factor;

Arash HABIBI's avatar
Arash HABIBI committed
847
848
	// velocity_[0] = 0.0;
	// velocity_[1] = 0.0;
pitiot's avatar
maj    
pitiot committed
849
850

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

pitiot's avatar
merging    
pitiot committed
853
	float rotor=0;
854

pitiot's avatar
merging    
pitiot committed
855
856
857
858
	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
859

Arash HABIBI's avatar
03_2013    
Arash HABIBI committed
860
861
	// masse ressort pour la limace

862
	if(!rigid_)
pitiot's avatar
maj    
pitiot committed
863
	{
pitiot's avatar
pitiot committed
864
865
866
867
		Dart centerDart =map.phi<11>(groundFace);
		Dart d = centerDart;
		forces[d] += -0.9f*velocity[d];
		do
Thomas Jund's avatar
Thomas Jund committed
868
		{
pitiot's avatar
pitiot committed
869
			Dart e = map.phi1(d);
Thomas Jund's avatar
Thomas Jund committed
870
			//initialisation of forces with viscosity
pitiot's avatar
pitiot committed
871
			forces[e] += -0.9f*velocity[e];
Arash HABIBI's avatar
Arash HABIBI committed
872
			// forces[dd] = VEC3(0.0);
pitiot's avatar
pitiot committed
873
874
875
876
			VEC3 p1Next = position[map.phi1(e)]+(velocity[map.phi1(e)] * sim_->timeStep_);
			VEC3 p2Next = position[e]+(velocity[e] * sim_->timeStep_);
			// p1Next et p2Next sont la position des extremites de l'arete.
			VEC3 v1 = (p1Next-p2Next);
877

pitiot's avatar
pitiot committed
878
879
			//stretch spring : /!\ max rigidity relative to the timestep used (unstable otherwise)
			float norm = v1.norm();
pitiot's avatar
pitiot committed
880
			float rigidity = 50.0f;
Arash HABIBI's avatar
Arash HABIBI committed
881

pitiot's avatar
pitiot committed
882
883
884
885
			float stretch = rigidity*(edgeLength[e]-norm);
			if(norm>0.0f)
			{
				VEC3 f = stretch*(v1/norm);
Thomas Jund's avatar
Thomas Jund committed
886

pitiot's avatar
pitiot committed
887
888
889
890
				//apply force symmetrically
				forces[e] -= f;
				forces[map.phi1(e)] += f;
			}
Thomas Jund's avatar
Thomas Jund committed
891

pitiot's avatar
pitiot committed
892
893
894
895
			p1Next = position[map.phi1(d)]+(velocity[map.phi1(d)] * sim_->timeStep_);
			p2Next = position[d]+(velocity[d] * sim_->timeStep_);
			// p1Next et p2Next sont la position des extremites de l'arete.
			v1 = (p1Next-p2Next);
Thomas Jund's avatar
Thomas Jund committed
896

pitiot's avatar
pitiot committed
897
898
			//stretch spring : /!\ max rigidity relative to the timestep used (unstable otherwise)
			norm = v1.norm();
Thomas Jund's avatar
Thomas Jund committed
899

900

pitiot's avatar
pitiot committed
901
902
903
904
			stretch = rigidity*(edgeLength[d]-norm);
			if(norm>0.0f)
			{
				VEC3 f = stretch*(v1/norm);
905

pitiot's avatar
pitiot committed
906
907
908
909
				//apply force symmetrically
				forces[d] -= f;
				forces[map.phi1(d)] += f;
			}
910

911

Thomas Jund's avatar
Thomas Jund committed
912

913

pitiot's avatar
pitiot committed
914
915
916
			d=map.phi<21>(d);
		}
		while(d!=centerDart);
pitiot's avatar
maj    
pitiot committed
917

Arash HABIBI's avatar
03_2013    
Arash HABIBI committed
918
919
			//-------------------------------------------------------------------------
			// ARASH : A présent on calcule les interactions avec les autres obstacles.
pitiot's avatar
pitiot committed
920
921
922
		VEC3 norm;
		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
03_2013    
Arash HABIBI committed
923

pitiot's avatar
pitiot committed
924
925
926
927
928
		double obst_radius_infl;
		if(sim_->config==0)
			obst_radius_infl = 100.; // scenario 0
		else
			obst_radius_infl = 30.; // scenario 1 et 3
Arash HABIBI's avatar
comment    
Arash HABIBI committed
929

pitiot's avatar
pitiot committed
930
931
			do {
				Dart dd = map.phi1(d);
Arash HABIBI's avatar
03_2013    
Arash HABIBI committed
932
933
					VEC3 p = position[dd]+(velocity[dd] * sim_->timeStep_);

Arash HABIBI's avatar
Arash HABIBI committed
934
935
					// Evitement d'obstacles mobiles

Arash HABIBI's avatar
03_2013    
Arash HABIBI committed
936
937
938
939
940
941
					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
942

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

Arash HABIBI's avatar
Arash HABIBI committed
946
					// Evitement d'obstacles fixes
Arash HABIBI's avatar
03_2013    
Arash HABIBI committed
947

Arash HABIBI's avatar
Arash HABIBI committed
948
949
950
951
952
953
954
955
					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
956
957
					}

pitiot's avatar
pitiot committed
958
959
960
961
				d = map.phi<21>(d);
			} while(d!=centerDart);


962
963

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

967
		//apply force to each vertex
968
		/*
969
970
		d = groundFace;
		for (unsigned int i = 0; i < nbVertices; ++i)
pitiot's avatar
pitiot committed
971
		{
Thomas Jund's avatar
Thomas Jund committed
972
			velocity[d] += forces[d] * sim_->timeStep_;
973
974
975
976
			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
977
		}
978
		*/
979
	}
pitiot's avatar
pitiot committed
980
981
	else
	{
982
983
984
985
986
987
	//	CGoGNout << "Obstacle "<< index << CGoGNendl;
//		CGoGNout << "vitesse : "<< velocity_ << CGoGNendl;
		//	on fait tourner l'obstacle
		Dart d = groundFace;
		for (unsigned int i = 0; i < nbVertices; ++i)
		{
pitiot's avatar
pitiot committed
988
#ifdef TWO_AND_HALF_DIM
pitiot's avatar
up    
pitiot committed
989
990
991
992
993
994
995
996
997
998

			//// rotation pour les obstacles rectangulaires
			VEC3 x = parts_[i]->getPosition()-center;
			VEC3 normale = (parts_[(i+1)%nbVertices]->getPosition()-center)^(x);
			normale.normalize();
			VEC3 y = normale ^x;
			y.normalize();
			VEC3 nouvpos = rotate2D(VEC3 (1,0,0),(0,0,0),angle);

			position[d] += nouvpos[0]*x+nouvpos[1]*y;
pitiot's avatar
pitiot committed
999
1000
1001
#else
			position[d] += rotate2D(position[d], center, abs_angle*rotor);
#endif
1002
1003
			position[d] += (velocity_ * sim_->timeStep_);

1004
#ifdef EXPORTING_BOXES
1005
1006

#ifndef TWO_AND_HALF_DIM
pitiot's avatar
up    
pitiot committed
1007
			position[map.phi<211>(d)] += rotate2D(position[map.phi<211>(d)], center, abs_angle*rotor);
1008
1009
1010
			position[map.phi<211>(d)] += (velocity_ * sim_->timeStep_);
#endif

1011
1012
#endif

1013
1014
1015
//			position[map.phi<211>(d)] += (velocity_ * sim_->timeStep_);

			bary += position[d];
Thomas Jund's avatar
Thomas Jund committed
1016
1017


1018
1019
1020
			map.next(d);
		}

pitiot's avatar
pitiot committed
1021
		front=(position[0] + position[1]) / 2;
1022
1023
1024
1025
		if(angle >0)
			angle -= rotor;
		else
			angle += rotor;
pitiot's avatar
pitiot committed
1026
	}
pitiot's avatar
maj    
pitiot committed
1027

pitiot's avatar
pitiot committed
1028
	center = bary / nbVertices;
pitiot's avatar
maj    
pitiot committed
1029

1030
1031
	//computing deformation vector on vertices :
	//depending on the velocity, extend the perceived shape
1032
	VEC3 vel = velocity_.normalized();
1033
1034
1035

	d = groundFace;
	for (unsigned int i = 0; i < nbVertices; ++i)
1036
	{
1037
		VEC3 v(position[d] - center);
1038
1039
		VEC3 vN = v.normalized();
		float dot = vN * vel;
1040
1041
		deformation[d] = v * 2.0f * (dot+1.0f)/2.0f;
		map.next(d);
1042
1043
	}

Thomas Jund's avatar
Thomas Jund committed
1044
1045
//	if(!rigid_)
//		center = position[groundFace];
1046
1047
1048
}

//-------------------------------------------------------------------------
1049

1050
1051
void MovingObstacle::applyForces()
{
pitiot's avatar
pitiot committed
1052
1053

	Dart centerDart =map.phi<11>(groundFace);
1054
1055
1056

	if(!rigid_)
	{
pitiot's avatar
pitiot committed
1057
		Dart d = centerDart;
1058
1059
		for (unsigned int i = 0; i < nbVertices; ++i)
		{
pitiot's avatar
pitiot committed
1060
1061
1062
1063
1064
1065
1066
1067
1068
			Dart e = map.phi_1(d);
			velocity[e] += forces[e] * sim_->timeStep_;
			position[e] += (velocity[e] * sim_->timeStep_);





			d=map.phi<21>(d);
1069
		}
pitiot's avatar
pitiot committed
1070
1071
1072
1073
1074
1075
1076
1077

		velocity[centerDart] +=forces[centerDart] * sim_->timeStep_;
		position[centerDart] += (velocity[centerDart] * sim_->timeStep_);





1078
// MAJ des obstacles
1079

pitiot's avatar
pitiot committed
1080

pitiot's avatar
pitiot committed
1081
	}
1082
1083
1084



1085
1086


pitiot's avatar
pitiot committed
1087
}
1088

pitiot's avatar
pitiot committed
1089
1090
1091
1092
1093
1094
1095
1096
1097
1098
1099
1100
1101
1102
1103
1104
1105
1106
1107
1108
1109
1110
1111
1112
1113
1114
1115
1116
1117
1118
1119
1120
1121
1122
1123
1124
1125
1126
1127
1128
1129
1130
1131
1132
1133
1134
1135
1136
1137
1138
1139
1140
1141
1142
1143
1144
1145
1146
1147
1148
1149
1150
1151
1152
void MovingObstacle::updateRegistration()
{
	PFP::VEC3 bary(0);
		Dart centerDart =map.phi<11>(groundFace);

		if(!rigid_)
		{
			Dart d = centerDart;
			for (unsigned int i = 0; i < nbVertices; ++i)
			{
				Dart e = map.phi_1(d);


				parts_[i]->move(position[e]);
	#ifdef TWO_AND_HALF_DIM

				position[e] = parts_[i]->getPosition(); //recalage de l'obstacle sur ses particules (qui elles ont bien suivi la carte au sol)

					PFP::VEC3 normal = CGoGN::Algo::Surface::Geometry::faceNormal<PFP>(sim_->envMap_.map, parts_[i]->d, sim_->envMap_.position);
						normal *= hight;
	//					std::cout << " phi11 d : " <<  map.phi<112>(d) <<" || d : "<<d<< std::endl;
					position[map.phi_1(map.phi<12>(d))] = position[e]+normal;

	#endif
					bary += position[e];
				d=map.phi<21>(d);
			}


			center = bary / nbVertices;
			parts_[nbVertices]->move(position[centerDart]);

		#ifdef TWO_AND_HALF_DIM
			position[centerDart] = parts_[nbVertices]->getPosition(); //recalage de l'obstacle sur ses particules (qui elles ont bien suivi la carte au sol)
		#endif

	// MAJ des obstacles


		}
	else
		{
			for (unsigned int i = 0; i < nbVertices+1; ++i)
					{


							parts_[i]->move(getDilatedPosition(i));

				#ifdef TWO_AND_HALF_DIM


							Dart d(i);
							position[d] = parts_[i]->getPosition(); //recalage de l'obstacle sur ses particules (qui elles ont bien suivi la carte au sol)

								PFP::VEC3 normal = CGoGN::Algo::Surface::Geometry::faceNormal<PFP>(sim_->envMap_.map, parts_[i]->d, sim_->envMap_.position);
									normal *= hight;
				//					std::cout << " phi11 d : " <<  map.phi<112>(d) <<" || d : "<<d<< std::endl;
								position[map.phi_1(d)] = position[d]+normal;

				#endif
					}
		}
	for (unsigned int i = 0; i < nbVertices; ++i)
		{
pitiot's avatar
maj    
pitiot committed
1153
1154
		//		CGoGNout << "avant une etape : Obstacle "<< i << CGoGNendl;
		Obstacle* o = obstacles_[i];
1155
1156
		o->p1 = getDilatedPosition(i);
		o->p2 = getDilatedPosition((i+1) % nbVertices);
Thomas Jund's avatar
Thomas Jund committed
1157

pitiot's avatar
pitiot committed
1158

1159
1160
		o->prevP = getDilatedPosition((i - 1 + nbVertices) % nbVertices);
		o->nextP = getDilatedPosition((i + 2 + nbVertices) % nbVertices);
pitiot's avatar
pitiot committed
1161

Thomas Jund's avatar
Thomas Jund committed
1162

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

pitiot's avatar
pitiot committed
1166
		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
1167
		{
Thomas Jund's avatar
Thomas Jund committed
1168
			sim_->envMap_.popAndPushObstacleInCells(o,i);
pitiot's avatar
pitiot committed
1169
		}
Thomas Jund's avatar
Thomas Jund committed
1170

pitiot's avatar
pitiot committed
1171
1172
1173
1174
1175
1176
1177
1178
		/////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
1179
1180
		//		CGoGNout << "Apres une etape : Obstacle "<< i << CGoGNendl;
	}
pitiot's avatar
pitiot committed
1181
1182
1183
1184
1185
1186
1187
1188
	/////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
1189
1190
1191
	//
	//		envMap_->addMovingObstAsNeighbor(this,general_belonging,&(general_neighbors));
	//		CGoGNout << "nouvel update : "<< CGoGNendl;
1192
1193
	if(sim_->detect_agent_collision)
	{
pitiot's avatar
pitiot committed
1194
		for (std::vector< std::pair<Dart, int> >::iterator it2 = general_belonging.begin(); it2 != general_belonging.end(); ++ it2)
1195
		{
pitiot's avatar
pitiot committed
1196
			std::vector<Agent*> vector =sim_->envMap_.agentvect[(*it2).first];
1197
1198
1199
1200
1201
1202
1203
1204
1205
1206
1207
			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
1208

1209
1210
		}
	}
pitiot's avatar
maj    
pitiot committed
1211
1212
}

1213
//-------------------------------------------------------------------------
Thomas Jund's avatar
Thomas Jund committed
1214

pitiot's avatar
pitiot committed
1215
std::vector<Dart> MovingObstacle::getMemoCross(const VEC3& pos, const VEC3& dest, Dart& d1, Dart& d2)
pitiot's avatar
maj    
pitiot committed
1216
{
1217
1218
1219
#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
1220
	CGoGN::Algo::Surface::MovingObjects::ParticleCell2DMemo<PFP> * registering_part = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2DMemo<PFP>(sim_->envMap_.map, d1,pos,sim_->envMap_.position);
1221
#endif
pitiot's avatar
pitiot committed
1222
1223
	std::vector<Dart> result =(registering_part->move(dest));
	d2=registering_part->d;
1224
//	CGoGNout<<"d1 : "<< *d1<<"|| d2 : "<< *d2<<"|| start : "<< pos<<"|| stop : "<< dest<<CGoGNendl;
pitiot's avatar
pitiot committed
1225
	return result;
pitiot's avatar
maj    
pitiot committed
1226
1227
}

1228
void resetPartSubdiv(Obstacle* o)
pitiot's avatar
maj    
pitiot committed
1229
1230
{
	MovingObstacle * mo = o->mo;
1231

pitiot's avatar
maj    
pitiot committed
1232
1233
	if (mo != NULL)
	{
pitiot's avatar
up    
pitiot committed
1234
1235
		unsigned int n =o->index;
		unsigned int n2 =(n+1)%(mo->nbVertices);
pitiot's avatar
merging    
pitiot committed
1236

pitiot's avatar
up    
pitiot committed
1237
1238
		VEC3 pos = mo->parts_[n]->getPosition();
		VEC3 pos2 = mo->parts_[n2]->getPosition();
pitiot's avatar
pitiot committed
1239
		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
1240

pitiot's avatar
pitiot committed
1241
1242
1243
		mo->parts_[n]->setState(FACE) ;
		mo->parts_[n]->move(pos) ;

pitiot's avatar
up    
pitiot committed
1244
1245
1246
1247
1248
		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
1249
		mo->dDir = mo->parts_[0]->d;
pitiot's avatar
maj    
pitiot committed
1250
1251
	}
}
pitiot's avatar
up    
pitiot committed
1252
//, unsigned int fLevel
pitiot's avatar
maj    
pitiot committed
1253
1254
1255
void resetObstPartInFace(Obstacle* o, Dart d1)
{
	MovingObstacle * mo = o->mo;
1256

Thomas Jund's avatar
Thomas Jund committed
1257
1258
	if (mo != NULL)
	{
pitiot's avatar
pitiot committed
1259
		unsigned int n =o->index;
pitiot's avatar
up    
pitiot committed
1260
		unsigned int n2 =(n+1)%(mo->nbVertices);
pitiot's avatar
pitiot committed
1261
		VEC3 pos1 = mo->parts_[n]->getPosition();
pitiot's avatar
up    
pitiot committed
1262
1263
1264
		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
1265
			mo->parts_[n]->d = d1;
pitiot's avatar
up    
pitiot committed
1266
1267
1268
//		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
1269
		if(n==0)
pitiot's avatar
pitiot committed
1270
			mo->dDir = mo->parts_[0]->d;
pitiot's avatar
maj    
pitiot committed
1271
	}
pitiot's avatar
up    
pitiot committed
1272
1273
1274
1275
1276
1277
1278
1279
1280
1281
1282
1283
1284
1285
1286
1287
1288
1289