moving_obstacle.cpp 50.7 KB
Newer Older
pitiot's avatar
pitiot committed
1
#include "articulated_obstacle.h"
pitiot's avatar
maj    
pitiot committed
2
3
4
#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; //  8 pour limaces ? 3 pour scn 0 et 4
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
tri    
pitiot committed
19
20
21
22
23
/*addGeneralCell
 * adds cell d to general belonging, if it's already present, increases the number of instance of the cell
 */


pitiot's avatar
pitiot committed
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
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));
	}
}
pitiot's avatar
tri    
pitiot committed
45
/*removeGeneralCell
pitiot's avatar
p    
pitiot committed
46
 * removes cell d to general belonging decreases the number of instance of the cell, if the number is 0 the celle is removed *forever*
pitiot's avatar
tri    
pitiot committed
47
 */
pitiot's avatar
pitiot committed
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
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
tri    
pitiot committed
68
69
70
71
/*
 * is_inside
 * tests if the point p is inside the obstacle (normal test, works only for CONVEX obstacles)
 */
pitiot's avatar
up    
pitiot committed
72
73


pitiot's avatar
maj    
pitiot committed
74
75
bool MovingObstacle::is_inside(VEC3 p)
{
76
//	return false;
pitiot's avatar
pitiot committed
77
78
	VEC3 vec, norm, p1,vec2,p2, dir;
	norm = VEC3 (0,0,1);
79
80
81

	for (unsigned int i = 0; i < nbVertices; i++)
	{
pitiot's avatar
pitiot committed
82
83
		 p1 = parts_[i]->getPosition();
		 p2 = parts_[(i+1)%nbVertices]->getPosition();
84
85

		vec = VEC3(p2 - p1);
pitiot's avatar
pitiot committed
86
87
		dir = vec ^norm;

88

pitiot's avatar
up    
pitiot committed
89
90

		vec2 = VEC3(p -p1);
pitiot's avatar
pitiot committed
91
		if (dir*vec2 < 0)
pitiot's avatar
pitiot committed
92
			return false;
93
94
95
	}

	return true;
pitiot's avatar
maj    
pitiot committed
96
97
}

pitiot's avatar
tri    
pitiot committed
98
99
100
101
102
/*
 * get_angle
 * pareil que Geom::getAngle ?
 */

pitiot's avatar
pitiot committed
103
float get_angle(VEC3 v1, VEC3 v2) //renvoie l'angle entre [- pi ; Pi] du v2 à v1
pitiot's avatar
maj    
pitiot committed
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
{
	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
120
121
122
123
124
125
126
		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
127
128
129
	return flo;
}

pitiot's avatar
tri    
pitiot committed
130
131
132
133
134
135
136
/*
 * rotate2D
 * // renvoie le déplacement necessaire depuis pos1 pour effectuer la rotation centrée en center d'angle angle
 * (moins compliqué que d'utiliser des matrices et la Geom::rotate ?)
 */

VEC3 rotate2D(VEC3 pos1, VEC3 center, float angle)
pitiot's avatar
maj    
pitiot committed
137
138
139
140
141
142
143
144
145
{
	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
146

pitiot's avatar
pitiot committed
147
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) :
148
		nbVertices(pos.size()),
pitiot's avatar
pitiot committed
149
		nbParticles(pos.size()),
150
		center(0),
pitiot's avatar
maj    
pitiot committed
151
		index(ind),
152
		goals_(goals),
pitiot's avatar
pitiot committed
153
		curGoal_(curGoal),
pitiot's avatar
pitiot committed
154
155
		velocity_factor(1.0f),
		color1(1.0f),			// couleur de l'obstacle
156
157
		color2(1.0f),
		color3(1.0f),
pitiot's avatar
pitiot committed
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
		seen(false),			// l'obstacle est il vu par des agents
		newVelocity_(0),		// vitesse actuelle
		sim_(sim),				// lien vers le simulateur
		rigid_(rigid),			// rigide ou déformable ?
		spinning(spin),			// l'obstacle peut il effectuer des rotations ?
		parent(art),			// obstacle articulé parent
		mm_(NULL),				// moving mesh associé
		ag_(NULL),				// agent suivi ?
		index_parent(indParent), // index de l'obstacle dans l'obstacle articulé
		gravity_dist(0),		// rayon de la boule englobante de l'obstacle
#ifdef LINEAR
		beta(0.60),
		alpha(0.95),
#else
		beta(0.60), // params shpaematching
		alpha(0.95),
#endif
pitiot's avatar
pitiot committed
175
		speed_detection_factor(1) // facteur qui détermine la taille du vecteur d'anticipation 8 pour scn 0 et 4
pitiot's avatar
pitiot committed
176

pitiot's avatar
maj    
pitiot committed
177
{
pitiot's avatar
pitiot committed
178
	if (index_parent>0) velocity_factor=2.0f;
pitiot's avatar
pitiot committed
179
180
	stiff_agent=1000000;
	radius_agent=17;
pitiot's avatar
pop    
pitiot committed
181

pitiot's avatar
pitiot committed
182
183
	stiff_obst=0.02;
	radius_obst=20;
pitiot's avatar
pop    
pitiot committed
184

pitiot's avatar
pitiot committed
185
	long_radius_agent=20;
pitiot's avatar
pitiot committed
186
	long_radius_obst=5;
pitiot's avatar
pitiot committed
187
188
189
190
191
192

	for (unsigned int i = 0; i < nbVertices; ++i)
			{
				center += pos[i];
			}
		center /= nbVertices;
pitiot's avatar
maj    
pitiot committed
193
194
	assert(pos.size() > 2);

195
196
	if(dInside==NIL)
		dInside = sim_->envMap_.getBelongingCell(pos[0]);
pitiot's avatar
pitiot committed
197
	if(index_parent<1)
pitiot's avatar
pitiot committed
198
199
200
201
202
	{
			pos.push_back(center);
			pos.push_back(center);
			nbParticles=nbVertices+2;
	}
pitiot's avatar
pitiot committed
203
204
//	if(!rigid);
//	  nbVertices +=1; //a center particle for the mass-spring
pitiot's avatar
pitiot committed
205
	
206
#ifdef TWO_AND_HALF_DIM
pitiot's avatar
pitiot committed
207
	parts_ = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2DAndHalf<PFP>*[nbParticles];
208
#else
pitiot's avatar
pitiot committed
209
210
211
212
213
#ifdef SECURED
	parts_ = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2DSecured<PFP>*[nbParticles];
#else
	parts_ = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2D<PFP>*[nbParticles];
#endif
214
215
#endif

pitiot's avatar
pitiot committed
216

pitiot's avatar
pitiot committed
217
	if(index_parent<1)
pitiot's avatar
pitiot committed
218
219
220
221
222
223
224
225
226
		{
			belonging_cells = new std::vector<Dart>[nbParticles-1];
			neighbor_cells = new std::vector<Dart>[nbParticles-1];
		}
	else
	{
		belonging_cells = new std::vector<Dart>[nbParticles];
		neighbor_cells = new std::vector<Dart>[nbParticles];
	}
227
228
	position = map.addAttribute<VEC3, VERTEX>("position") ;
	normal = map.addAttribute<VEC3, VERTEX>("normal") ;
pitiot's avatar
pitiot committed
229
	velocity = map.addAttribute<VEC3, VERTEX>("velocity") ;
230
	deformation = map.addAttribute<VEC3, VERTEX>("deformation") ;
231
232
233

	if(!rigid_)
	{
pitiot's avatar
pitiot committed
234

Thomas Jund's avatar
Thomas Jund committed
235
		forces = map.addAttribute<VEC3, VERTEX>("force") ;
236
		edgeLength = map.addAttribute<float, EDGE>("edgeLength") ;
Thomas Jund's avatar
Thomas Jund committed
237
		vertexAngle = map.addAttribute<float, DART>("vertexAngle") ;
238
	}
pitiot's avatar
pitiot committed
239
240

	for (unsigned int i = 0; i < nbParticles; ++i)
241
242
	{
#ifdef TWO_AND_HALF_DIM
pitiot's avatar
pitiot committed
243
		Dart d = dInside;
244

pitiot's avatar
up    
pitiot committed
245
246
		parts_[i] = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2DAndHalf<PFP>(sim_->envMap_.map, d, center, sim_->envMap_.position);
		parts_[i]->move(pos[i]);
247
#else
248
		Dart d = sim_->envMap_.getBelongingCell(pos[i]);
pitiot's avatar
pitiot committed
249
250
251
252
253
254
#ifdef SECURED
		parts_[i]  = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2DSecured<PFP>(sim_->envMap_.map, d, pos[i], sim_->envMap_.position);
#else

		parts_[i]  = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2D<PFP>(sim_->envMap_.map, d, pos[i], sim_->envMap_.position);
#endif
255
256
257
258
#endif
		if(i==0)
			dDir = d;
	}
pitiot's avatar
pitiot committed
259
	front=(parts_[0]->getPosition() + parts_[nbVertices-1]->getPosition()) / 2;
pitiot's avatar
pitiot committed
260
261
262



Thomas Jund's avatar
Thomas Jund committed
263

264
265
266
267
268
		// 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));
pitiot's avatar
pitiot committed
269
270
271
272
273
#ifndef TWO_AND_HALF_DIM
	if ( parent==NULL && spinning) //départ face à la cible en cas d'obstacles pouvant effectuer des rotations
	{
			VEC3 axeZ=VEC3 (0,0,1);
#ifdef TWO_AND_HALF_DIM
274

pitiot's avatar
pitiot committed
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
			VEC3 normale = Algo::Surface::Geometry::faceNormal<PFP>(sim->envMap_.map, parts_[0]->d, sim->envMap_.position);
#else
			VEC3 normale =axeZ;
#endif
			Geom::Matrix44f rot ;
			rot.identity() ;
//			Geom::translate(center[0],center[1],center[2],rot);
			angle =get_angle(goals_[curGoal_] - center,front  - center);
			Geom::rotate(axeZ,angle,rot);

			float angle1 = Geom::angle(normale, VEC3 (0,0,1) ) ;
//			CGoGNout<<"angle : "<<angle << " ||  angle1 : "<< angle1<<CGoGNendl;
			if(angle1!=0)
			{
				VEC3 axis = VEC3(0,0,1) ^ normale ;

			//	Geom::translate(center[0],center[1],center[2],rot);
				Geom::rotate(axis, angle1, rot) ;
			}
//			Geom::translate(center[0],center[1],center[2],rot);

//			std::cout<<" angle : "<< angle;
Thomas Jund's avatar
merge    
Thomas Jund committed
297

pitiot's avatar
pitiot committed
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
			for (unsigned int i = 0; i < nbVertices; ++i)
			{
//				std::cout<<" || pos[i] avant : "<< pos [i];
				pos[i]=center + Geom::transform(pos[i]-center,rot);
//				pos[i]+=rotate2D(pos[i],center,angle);
//				std::cout<<" || pos[i] APRES : "<< pos [i]<<std::endl;

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


			}
			angle=0;
			front=(pos[0] + pos[nbVertices-1]) / 2;
	}
#endif
313
314
	groundFace = map.newFace(nbVertices);

pitiot's avatar
maj    
pitiot committed
315
316
	for (unsigned int i = 0; i < nbVertices; ++i)
	{
317
318
319
320
321
		float distance=(center-pos[i]).norm();
		if(distance>gravity_dist)
		{
			gravity_dist=distance;
		}
Thomas Jund's avatar
Thomas Jund committed
322
		Dart d = i;
323
324
		position[d] = pos[i];
		deformation[d] = VEC3(0);
pitiot's avatar
pitiot committed
325
		velocity[d] = VEC3(0);
326
327

		if(!rigid_)
Thomas Jund's avatar
Thomas Jund committed
328
		{
pitiot's avatar
pitiot committed
329

Thomas Jund's avatar
Thomas Jund committed
330
331
			forces[d] = VEC3(0);
		}
pitiot's avatar
pitiot committed
332
	}
333

334
335
	//extrude face to build a cage
	// compute edgeLength for mass-spring
pitiot's avatar
pitiot committed
336
337
338
	height=rigid_ ? 10.0f : 10.0f;
	Algo::Surface::Modelisation::extrudeFace<PFP>(map, position, groundFace, height) ;
	
339
340
341
	map.fillHole(groundFace);
	groundFace = map.phi2(groundFace);

pitiot's avatar
pitiot committed
342
	std::vector<Dart> bord;
343
344
	if(!rigid_)
	{
pitiot's avatar
pitiot committed
345

pitiot's avatar
pitiot committed
346
347
348
//////////////////Mass SPRING
//		Dart d = Algo::Surface::Modelisation::trianguleFace<PFP>(map,groundFace);
//		position[d]=parts_[nbVertices]->getPosition();
pitiot's avatar
pitiot committed
349

pitiot's avatar
pitiot committed
350
		Dart d=groundFace;
pitiot's avatar
pitiot committed
351
352
		for (unsigned int i = 0; i < nbVertices; ++i)
		{
pitiot's avatar
pitiot committed
353
			Dart e =map.phi<2111>(d);
pitiot's avatar
pitiot committed
354
355
			map.splitFace(map.phi_1(e),map.phi1(e));

pitiot's avatar
pitiot committed
356
			d=map.phi_1(d);
pitiot's avatar
pitiot committed
357
358
		}

pitiot's avatar
pitiot committed
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
//		Algo::Surface::Modelisation::EarTriangulation<PFP> et(map);
//		et.triangule();
//
//		TraversorE<PFP::MAP> tE(map);
//		for(Dart d = tE.begin() ; d != tE.end() ; d = tE.next())
//		{
//			edgeLength[d] = VEC3(position[map.phi1(d)]-position[d]).norm();
//		}
//
//		Dart centerDart =map.phi<11>(groundFace);
//		d = centerDart;
//
//		do
//		{
//			Dart e = map.phi1(d);
//			vertexAngle[e] = Algo::Surface::Geometry::angle<PFP>(map,d,map.phi2(map.phi_1(d)),position);
//			vertexAngle[d] = Algo::Surface::Geometry::angle<PFP>(map,map.phi_1(d),map.phi2(e),position);
//			vertexAngle[map.phi_1(d)]= Algo::Surface::Geometry::angle<PFP>(map,e,map.phi2(d),position);
//			d=map.phi<21>(d);
//		}
//		while(d!=centerDart);
		///////////////////:
		////////////////////SHAPE
382

pitiot's avatar
pitiot committed
383
384
		d = groundFace;
		do
385
		{
pitiot's avatar
pitiot committed
386
387
388
389
			bord.push_back(d);
			bord.push_back(map.phi_1(map.phi2(d)));
			d=map.phi_1(d);
		}while ( d !=groundFace);
Thomas Jund's avatar
Thomas Jund committed
390

pitiot's avatar
merging    
pitiot committed
391

pitiot's avatar
pitiot committed
392
393

		///////////////////
394
		map.enableQuickTraversal<VERTEX>();
395

Thomas Jund's avatar
Thomas Jund committed
396
		dDir=dInside;
pitiot's avatar
pitiot committed
397
#ifdef LINEAR
pitiot's avatar
pitiot committed
398
	shape_= new ShapeMatchingLinear<PFP>(map,position,bord,beta);
pitiot's avatar
pitiot committed
399
400
401
#else
	shape_= new ShapeMatchingQuadratic<PFP>(map,position,bord,beta);
#endif
pitiot's avatar
pitiot committed
402
	shape_->initialize();
pitiot's avatar
pitiot committed
403
404
	}

405

pitiot's avatar
maj    
pitiot committed
406
407
	for (unsigned int i = 0; i < nbVertices; ++i)
	{
pitiot's avatar
pitiot committed
408
409
		Obstacle* o = new Obstacle(parts_[i]->getPosition(),
				parts_[(i + 1) % nbVertices]->getPosition(),
pitiot's avatar
pop    
pitiot committed
410
				 this, i,stiff_agent,stiff_obst,radius_agent,radius_obst);
pitiot's avatar
pitiot committed
411
		obstacles_.push_back(o);
pitiot's avatar
pitiot committed
412
413
414
		/////definition du troisieme point
		if (rigid_) o->p3=parts_[(i + 2) % nbVertices]->getPosition();

pitiot's avatar
pitiot committed
415
//		CGoGNout<<" obstacle :"<< i << " num : "<< o<<CGoGNendl;
pitiot's avatar
pitiot committed
416
		sim_->envMap_.pushObstacleInCells(o);
pitiot's avatar
maj    
pitiot committed
417
	}
pitiot's avatar
pitiot committed
418

pitiot's avatar
pitiot committed
419
	/////obstacles lointain
pitiot's avatar
pitiot committed
420
	if(index_parent<1)
pitiot's avatar
pitiot committed
421
422
423
	{
		Obstacle* o = new Obstacle(parts_[nbVertices]->getPosition(),
						parts_[nbVertices+1]->getPosition(),
pitiot's avatar
pop    
pitiot committed
424
						 this, nbVertices,stiff_agent,stiff_obst,long_radius_agent,long_radius_obst);
pitiot's avatar
pitiot committed
425

pitiot's avatar
pitiot committed
426
427
428
429
430
431
432
433
				obstacles_.push_back(o);

		//		CGoGNout<<" obstacle :"<< i << " num : "<< o<<CGoGNendl;
				sim_->envMap_.pushObstacleInCells(o);


					/// fin ajout
	}
pitiot's avatar
pitiot committed
434

pitiot's avatar
maj    
pitiot committed
435
436
}

437
438
439
440
441
442
443
444
445
446
447
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
448
	m_shader->setColor(Geom::Vec4f(0.,1.,0.,0.));
449
450
451
452
453
454
455
456
457
458
//	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
459
460

	m_ds = new Utils::Drawer();
461
462
}

pitiot's avatar
pitiot committed
463
void MovingObstacle::draw(bool showPath, bool showPrediction)
464
465
466
467
468
{
#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);
469
//	m_shader->setColor(Geom::Vec4f(movingObstacleNeighbors_.size()==0 ? 1.0f : 0,0.,0.,0.));
pitiot's avatar
merge    
pitiot committed
470

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

pitiot's avatar
up    
pitiot committed
473
//	if(movingObstacleNeighbors_.size()==0)
pitiot's avatar
merging    
pitiot committed
474
475
476
477
//	if(index==12)
//	// if(obstacleNeighbors_.size()==0)
//		m_shader->setColor(Geom::Vec4f(col[0],col[1],col[2],0));
//	else
pitiot's avatar
pitiot committed
478
		m_shader->setColor(Geom::Vec4f(0,0.3,0,0)); // vert
pitiot's avatar
merge    
pitiot committed
479

480
481
482
483
	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
484
485
486
487
488
489
490
491

	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
pitiot committed
492
		m_ds->vertex(position[groundFace]);
pitiot's avatar
up    
pitiot committed
493
		for(unsigned int i = 0 ; i < goals_.size() ; i++)
Thomas Jund's avatar
Thomas Jund committed
494
		{
pitiot's avatar
up    
pitiot committed
495
			m_ds->vertex(goals_[(curGoal_+i)%(goals_.size())]);
Thomas Jund's avatar
Thomas Jund committed
496
497
		}

pitiot's avatar
pitiot committed
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
		m_ds->end();
		m_ds->endList();
	}
	if(false)//show vertices velocity
	{
		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]);
		m_ds->vertex(center);
		Dart d =groundFace;
		for(unsigned int i = 0 ; i < nbVertices ; i++)
		{
			m_ds->vertex(position[d]);
			VEC3 speed = velocity[d];
			m_ds->vertex(speed+position[d]);
			m_ds->vertex(position[d]);
			map.next(d);
		}

Thomas Jund's avatar
Thomas Jund committed
519
520
521
		m_ds->end();
		m_ds->endList();
	}
pitiot's avatar
pitiot committed
522
	if(showPrediction)// affichage anticipation
pitiot's avatar
pitiot committed
523
524
525
	{

		m_ds->newList(GL_COMPILE_AND_EXECUTE);
pitiot's avatar
pitiot committed
526
		m_ds->lineWidth(10.0f);
pitiot's avatar
pitiot committed
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
		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(unsigned int i = nbVertices ; i < nbParticles ; i++)
		{

			m_ds->vertex(parts_[i]->getPosition());

		}

		m_ds->end();
		m_ds->endList();
pitiot's avatar
pop    
pitiot committed
542
543

	}
pitiot's avatar
pitiot committed
544
	glLineWidth(1.0f);
545
546
}

547
548
VEC3 MovingObstacle::getDilatedPosition(unsigned int ind)
{
549
	Dart d(ind); //WARNING : works only for one face created at start !
pitiot's avatar
pitiot committed
550
#ifndef TWO_AND_HALF_DIM
pitiot's avatar
up    
pitiot committed
551
552
553
554
555
//	return position[d]+deformation[d];
	if(!rigid_)
		{
			return position[d];
		}
pitiot's avatar
pitiot committed
556
	else return position[d];
pitiot's avatar
pitiot committed
557
#else
Arash HABIBI's avatar
Arash HABIBI committed
558
	return position[d];
pitiot's avatar
pitiot committed
559
#endif
560
561
562
563
}

VEC3 MovingObstacle::getPosition(unsigned int ind)
{
564
565
	Dart d(ind);
	return position[d];
566
567
}

pitiot's avatar
maj    
pitiot committed
568
569
570
571
bool MovingObstacle::test_opposition(VEC3 o, VEC3 p1, VEC3 p2)
{
	o.normalize();
	o *= -1;
572
573
574
	VEC3 vector(p1 - p2);
	vector.normalize();
	return (o - vector).norm2() < 0.1;
pitiot's avatar
maj    
pitiot committed
575
576
577
}

// TODO Check position
pitiot's avatar
pitiot committed
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
631
//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
632

pitiot's avatar
pitiot committed
633
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
634
{
Thomas Jund's avatar
Thomas Jund committed
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
	//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
688
689
690
691
692
//	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
693
//	{
pitiot's avatar
pitiot committed
694
695
696
697
698
699
//		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
700
//
pitiot's avatar
pitiot committed
701
702
703
704
//		for (std::vector<Agent*>::const_iterator it = agents.begin(); it != agents.end(); ++it)
//		{
//			if ((*it)->alive)
//			{
pitiot's avatar
pitiot committed
705
//
pitiot's avatar
pitiot committed
706
707
708
709
//				float distSq = (center - (*it)->getPosition()).norm2() ;
//				if ((agentNeighbors_.size() < maxNeighbors_ || distSq < maxDist)
//					&& distSq < neighborDistSq_)
//				{
pitiot's avatar
pitiot committed
710
//
pitiot's avatar
pitiot committed
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
//					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
746
//	}
pitiot's avatar
pitiot committed
747
748
749
750
}

void MovingObstacle::updateObstacleNeighbors() // obstacles voisins , distance par rapport aux centres des segments// a mettre en place si besoin
{
751
752
	obstacleNeighbors_.clear() ;
	movingObstacleNeighbors_.clear() ;
pitiot's avatar
pitiot committed
753
	for (std::vector< std::pair<Dart, int> >::iterator it2 = general_belonging.begin();it2 != general_belonging.end(); ++it2)
754
	{
pitiot's avatar
pitiot committed
755
756
		std::vector<Obstacle*>& obst = sim_->envMap_.obstvect[(*it2).first] ;
		std::vector<Obstacle*>& neighborObst = sim_->envMap_.neighborObstvect[(*it2).first] ;
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
783
784
785
786
//		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)
					{
pitiot's avatar
pitiot committed
787

pitiot's avatar
pitiot committed
788
						{
pitiot's avatar
pitiot committed
789

pitiot's avatar
pitiot committed
790
791
							movingObstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
						}
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
825
826



					}

				}
			}
		}

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

pitiot's avatar
pitiot committed
828
						{
pitiot's avatar
pitiot committed
829

pitiot's avatar
pitiot committed
830
831
							movingObstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
						}
832
833
834
835
836
					}
				}
			}
		}
	}
pitiot's avatar
maj    
pitiot committed
837
838
}

Arash HABIBI's avatar
unidir    
Arash HABIBI committed
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
//-------------------------------------------------------------

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
938

Arash HABIBI's avatar
unidir    
Arash HABIBI committed
939
	if(!rigid_)
pitiot's avatar
pitiot committed
940
	{
pitiot's avatar
pitiot committed
941
942

			Dart d = groundFace;
pitiot's avatar
pitiot committed
943
		do
Arash HABIBI's avatar
unidir    
Arash HABIBI committed
944
		{
pitiot's avatar
pitiot committed
945

Arash HABIBI's avatar
unidir    
Arash HABIBI committed
946
			//initialisation of forces
pitiot's avatar
pitiot committed
947
948
			forces[d] = VEC3(0.0);
			d=map.phi_1(d);;
Arash HABIBI's avatar
unidir    
Arash HABIBI committed
949
		}
pitiot's avatar
pitiot committed
950
951
		while(d!=groundFace);

pitiot's avatar
pitiot committed
952
	}
Arash HABIBI's avatar
unidir    
Arash HABIBI committed
953
954
}

Arash HABIBI's avatar
Arash HABIBI committed
955
956
957

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

pitiot's avatar
pitiot committed
958
VEC3 computeForce(VEC3 p, VEC3 p1, VEC3 p2, float obst_radius_infl, float obst_power, float obst_stiffness, VEC3 normFace)
Arash HABIBI's avatar
Arash HABIBI committed
959
960
961
962
963
964
965
966
967
968
969
970
971
{
	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;
pitiot's avatar
pitiot committed
972
973
974
975
		VEC3 normal;
		///new vector tangente à l'ellipse
		VEC3 bc = p-p2;
		normal =((bc.norm())/sum_of_dists)*v_obst+bc;
Arash HABIBI's avatar
Arash HABIBI committed
976

pitiot's avatar
pitiot committed
977
978
979
980
981
//		//vector arash
//		normal= normFace^v_obst;
//		// Ajouter une composante tangentielle
//
//		normal += v_obst * ((d1-d2)/sum_of_dists);
Arash HABIBI's avatar
Arash HABIBI committed
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
1001
		// 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
1002
1003
//-------------------------------------------------------------

1004
void MovingObstacle::updateForces()
pitiot's avatar
maj    
pitiot committed
1005
{
pitiot's avatar
pitiot committed
1006
	assert(sim_->envMap_.map.getCurrentLevel() == sim_->envMap_.map.getMaxLevel()) ;
pitiot's avatar
pitiot committed
1007

pitiot's avatar
pitiot committed
1008
	//pour les tests de détection///////////////
pitiot's avatar
merging    
pitiot committed
1009
1010
1011
1012
1013
1014
1015
	if(!seen)
	{
		color1=1.0f;
		color2=1.0f;
		color3=1.0f;
	}
	seen=false;
Thomas Jund's avatar
merge    
Thomas Jund committed
1016

pitiot's avatar
pitiot committed
1017
	//////////////////////////
Thomas Jund's avatar
merge    
Thomas Jund committed
1018

1019
	PFP::VEC3 bary(0);
pitiot's avatar
maj    
pitiot committed
1020
1021

	Dart d;
pitiot's avatar
pitiot committed
1022
1023
	velocity_ = newVelocity_* velocity_factor;

pitiot's avatar
maj    
pitiot committed
1024
	// MAJ des particules
pitiot's avatar
merge    
pitiot committed
1025

1026
	if(!rigid_)
pitiot's avatar
maj    
pitiot committed
1027
	{
pitiot's avatar
pitiot committed
1028
1029
//	// masse ressort pour la limace
//
1030

pitiot's avatar
pitiot committed
1031
1032
1033
1034
1035
1036
1037
1038
1039
1040
1041
1042
1043
1044
1045
1046
1047
1048
1049
1050
1051
1052
1053
1054
1055
1056
1057
1058
1059
1060
1061
1062
1063
1064
1065
1066
1067
1068
1069
1070
1071
1072
1073
1074
1075
1076
1077
1078
1079
1080
1081
1082
1083
1084
1085
1086
1087
1088
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
1153
//		Dart centerDart =map.phi<11>(groundFace);
//		Dart d = centerDart;
//		forces[d] += -0.9f*velocity[d];
//		float rigidity = 10.0f;
//		float bend_factor=1.0f;// limace 1 ||
//		float angular_factor=2.0f;// limace 2
//		do
//		{
//			Dart e = map.phi1(d);
//			//initialisation of forces with viscosity
//			forces[e] += -0.9f*velocity[e];
//			VEC3 p1Next = position[map.phi1(e)]+(velocity[map.phi1(e)] * sim_->timeStep_); // ressorts sur le bord
//			VEC3 p2Next = position[e]+(velocity[e] * sim_->timeStep_);
//			// p1Next et p2Next sont la position des extremites de l'arete.
//			VEC3 v1 = (p1Next-p2Next);
//
//			//stretch spring : /!\ max rigidity relative to the timestep used (unstable otherwise)
//			float norm = v1.norm();
//
//
//			float stretch =  bend_factor*rigidity*(edgeLength[e]-norm);//4
//			float angularStretch = 0, angularStretch2 = 0;
//			float restAngle = vertexAngle[e];
//			if(restAngle!=0.0f)
//			{
//				float angularRig = angular_factor*rigidity;//2
//
//				float curAngle = Algo::Surface::Geometry::angle<PFP>(map, d,map.phi2(map.phi_1(d)),position);
//
//				VEC3 v = Algo::Surface::Geometry::vectorOutOfDart<PFP>(map, d, position);
//				VEC3 v2 = Algo::Surface::Geometry::vectorOutOfDart<PFP>(map, map.phi2(map.phi_1(d)), position);
//
//				VEC3 v3 = v ^ v2;
//				VEC3 vPl = Algo::Surface::Geometry::faceNormal<PFP>(map, d, position);
//
//				if(v3 * vPl < 0.0f)
//					curAngle *= -1.0f;
//
//				angularStretch = angularRig*(restAngle-curAngle);
//			}
//
//
//			if(norm>0.0f)
//			{
//				VEC3 f = (stretch+angularStretch)*(v1/norm);
//
//				//apply force symmetrically
//				forces[e] -= f;
//				forces[map.phi1(e)] += f;
//			}
//
//
//			p1Next = position[e]+(velocity[e] * sim_->timeStep_); /// ressorts vers le centre de la face
//			p2Next = position[d]+(velocity[d] * sim_->timeStep_);
//			// p1Next et p2Next sont la position des extremites de l'arete.
//			v1 = (p1Next-p2Next);
//
//			//stretch spring : /!\ max rigidity relative to the timestep used (unstable otherwise)
//			norm = v1.norm();
//			restAngle = vertexAngle[d];
//			if(restAngle!=0.0f)
//			{
//				float angularRig = angular_factor*rigidity;//2
//
//				float curAngle = Algo::Surface::Geometry::angle<PFP>(map, map.phi_1(d),map.phi2(map.phi1(d)),position);
//
//				VEC3 v = Algo::Surface::Geometry::vectorOutOfDart<PFP>(map, map.phi_1(d), position);
//				VEC3 v2 = Algo::Surface::Geometry::vectorOutOfDart<PFP>(map, map.phi2(map.phi1(d)), position);
//
//				VEC3 v3 = v ^ v2;
//				VEC3 vPl = Algo::Surface::Geometry::faceNormal<PFP>(map, d, position);
//
//				if(v3 * vPl < 0.0f)
//					curAngle *= -1.0f;
//
//				angularStretch = angularRig*(restAngle-curAngle);
//			}
//
//			stretch =  bend_factor*rigidity*(edgeLength[d]-norm);//4
//			if(norm>0.0f)
//			{
//				VEC3 f = (stretch+angularStretch)*(v1/norm);
//
//				//apply force symmetrically
//				forces[d] -= f;
//				forces[e] += f;
//			}
//
//			p1Next = position[d]+(velocity[d] * sim_->timeStep_); // ressorts sur le bord
//			p2Next = position[map.phi1(e)]+(velocity[map.phi1(e)] * sim_->timeStep_);
//			// p1Next et p2Next sont la position des extremites de l'arete.
//			v1 = (p1Next-p2Next);
//			norm = v1.norm();
//			restAngle = vertexAngle[map.phi_1(d)];
//			if(restAngle!=0.0f)
//			{
//				float angularRig = angular_factor*rigidity;//2
//
//				float curAngle = Algo::Surface::Geometry::angle<PFP>(map, e,map.phi2(d),position);
//
//				VEC3 v = Algo::Surface::Geometry::vectorOutOfDart<PFP>(map, e, position);
//				VEC3 v2 = Algo::Surface::Geometry::vectorOutOfDart<PFP>(map, map.phi2(d), position);
//
//				VEC3 v3 = v ^ v2;
//				VEC3 vPl = Algo::Surface::Geometry::faceNormal<PFP>(map, d, position);
//
//				if(v3 * vPl < 0.0f)
//					curAngle *= -1.0f;
//
//				angularStretch2 = angularRig*(restAngle-curAngle);
//			}
//			if(norm>0.0f)
//			{
//				VEC3 f = (angularStretch2)*(v1/norm);
//
//				//apply force symmetrically
//				forces[map.phi1(e)] -= f;
//				forces[d] += f;
//			}
//
//			d=map.phi<21>(d);
//		}
//		while(d!=centerDart);
pitiot's avatar
maj    
pitiot committed
1154

Arash HABIBI's avatar
03_2013    
Arash HABIBI committed
1155
1156
			//-------------------------------------------------------------------------
			// ARASH : A présent on calcule les interactions avec les autres obstacles.
pitiot's avatar
pitiot committed
1157
		VEC3 norm;
pitiot's avatar
pitiot committed
1158

pitiot's avatar
pitiot committed
1159
		int obst_power = 2  ;           // the power to which elevate the agent-obstacle distance
Arash HABIBI's avatar
03_2013    
Arash HABIBI committed
1160

pitiot's avatar
merging    
pitiot committed
1161

pitiot's avatar
pitiot committed
1162

pitiot's avatar
merging    
pitiot committed
1163
		float fixed_obst_factor = 5.0f;
pitiot's avatar
pitiot committed
1164
		d=groundFace;
pitiot's avatar
merging    
pitiot committed
1165
1166
1167
		for (unsigned int i = 0; i < nbVertices; ++i)
				{
			PFP::VEC3 normFace = CGoGN::Algo::Surface::Geometry::faceNormal<PFP>(sim_->envMap_.map, parts_[i]->d, sim_->envMap_.position);
pitiot's avatar
pitiot committed
1168
				Dart dd = map.phi_1(d);
pitiot's avatar
pitiot committed
1169

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

Arash HABIBI's avatar
Arash HABIBI committed
1172
1173
					// Evitement d'obstacles mobiles

Arash HABIBI's avatar
03_2013    
Arash HABIBI committed
1174
1175
1176
1177
1178
1179
					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
1180

pitiot's avatar
pitiot committed
1181
						forces[dd] += computeForce(p,p1,p2,obst->obst_radius_infl_obst,obst_power,obst->obst_stiffness_obst,normFace);
Arash HABIBI's avatar
Arash HABIBI committed
1182
					}
Arash HABIBI's avatar
unidir    
Arash HABIBI committed
1183

pitiot's avatar
pitiot committed
1184
					//	Evitement d'obstacles fixes
Arash HABIBI's avatar
03_2013    
Arash HABIBI committed
1185

Arash HABIBI's avatar
Arash HABIBI committed
1186
1187
1188
1189
					for(std::vector<std::pair<float, Obstacle*> >::iterator it = obstacleNeighbors_.begin() ;
						it != obstacleNeighbors_.end() ; ++it)
					{
						Obstacle * obst = it->second;
pitiot's avatar
pitiot committed
1190
1191
						VEC3 p1=obst->p2 ;
						VEC3 p2=obst->p1 ;
Arash HABIBI's avatar
Arash HABIBI committed
1192

pitiot's avatar
pitiot committed
1193
						forces[dd] += computeForce(p,p1,p2,obst->obst_radius_infl_obst,fixed_obst_factor*obst_power,fixed_obst_factor*obst->obst_stiffness_obst,normFace);
Arash HABIBI's avatar
03_2013    
Arash HABIBI committed
1194
1195
					}

pitiot's avatar
pitiot committed
1196
				d = map.phi_1(d);
pitiot's avatar
merging    
pitiot committed
1197
			}
pitiot's avatar
pitiot committed
1198
1199


1200
1201

		//guiding vertex = first vertex (set the displacement)
1202
		// forces[groundFace] = VEC3(0);
1203
		velocity[groundFace] = velocity_;
pitiot's avatar
pitiot committed
1204
		velocity[map.phi_1(groundFace)] = velocity_;
1205
		//apply force to each vertex
1206
		/*
1207
1208
		d = groundFace;
		for (unsigned int i = 0; i < nbVertices; ++i)
pitiot's avatar
pitiot committed
1209
		{
Thomas Jund's avatar
Thomas Jund committed
1210
			velocity[d] += forces[d] * sim_->timeStep_;
1211
1212
1213
1214
			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
1215
		}
1216
		*/
1217
	}
pitiot's avatar
pitiot committed
1218
1219
	else
	{
pitiot's avatar
pitiot committed
1220
1221
		float abs_angle= angle > 0 ? 1 : -1;

pitiot's avatar
pitiot committed
1222
1223
		rotor=0;
		float max_angle = maxSpeed_*sim_->timeStep_;
pitiot's avatar
pitiot committed
1224
		if (index_parent==0)
pitiot's avatar
pitiot committed
1225
1226
1227
1228
1229
		{

			if (sim_->config ==4) rotor = abs_angle*angle > max_angle/10 ? abs_angle *max_angle/10 : angle ;
			else rotor = abs_angle*angle > 10*max_angle ? abs_angle *10*max_angle : angle ;
		}
pitiot's avatar
pitiot committed
1230
		else
pitiot's avatar
pitiot committed
1231
1232
			rotor = abs_angle*angle >  max_angle ? abs_angle * max_angle : angle ;

1233
1234
1235
1236
1237
1238
	//	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
1239
1240
			VEC3 oldPos= position[d];

pitiot's avatar
pitiot committed
1241
#ifdef TWO_AND_HALF_DIM
pitiot's avatar
up    
pitiot committed
1242
1243
1244
1245
1246
1247
1248

			//// 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();
Thomas Jund's avatar
Thomas Jund committed
1249
			VEC3 nouvpos = rotate2D(VEC3(1,0,0),VEC3(0,0,0),angle);
pitiot's avatar
up    
pitiot committed
1250

pitiot's avatar
pitiot committed
1251
			velocity[d] += nouvpos[0]*x+nouvpos[1]*y;
pitiot's avatar
pitiot committed
1252
#else
pitiot's avatar
pitiot committed
1253

pitiot's avatar
pitiot committed
1254
			position[d] += rotate2D(position[d], center, rotor);
pitiot's avatar
pitiot committed
1255
#endif
1256
			position[d] += (velocity_ * sim_->timeStep_);
pitiot's avatar
pitiot committed
1257
1258
			velocity[d]=position[d]-oldPos;

1259

1260
#ifdef EXPORTING_BOXES
1261
1262

#ifndef TWO_AND_HALF_DIM
pitiot's avatar
pitiot committed
1263
			position[map.phi<211>(d)] += rotate2D(position[map.phi<211>(d)], center, rotor);
1264
1265
1266
			position[map.phi<211>(d)] += (velocity_ * sim_->timeStep_);
#endif

1267
1268
#endif

1269
1270
1271
//			position[map.phi<211>(d)] += (velocity_ * sim_->timeStep_);

			bary += position[d];
Thomas Jund's avatar
Thomas Jund committed
1272
1273


1274
1275
1276
			map.next(d);
		}

pitiot's avatar
pitiot committed
1277
		front=(position[0] + position[1]) / 2;
pitiot's avatar
pitiot committed
1278
1279
1280

			angle -= abs_angle*rotor;

pitiot's avatar
pitiot committed
1281
	}
pitiot's avatar
maj    
pitiot committed
1282

pitiot's avatar
pitiot committed
1283
	center = bary / nbVertices;
pitiot's avatar
maj    
pitiot committed
1284

1285
1286
	//computing deformation vector on vertices :
	//depending on the velocity, extend the perceived shape
1287
	VEC3 vel = velocity_.normalized();
1288
1289
1290

	d = groundFace;
	for (unsigned int i = 0; i < nbVertices; ++i)
1291
	{
1292
		VEC3 v(position[d] - center);
1293
1294
		VEC3 vN = v.normalized();
		float dot = vN * vel;
1295
1296
		deformation[d] = v * 2.0f * (dot+1.0f)/2.0f;
		map.next(d);
1297
1298
	}

Thomas Jund's avatar
Thomas Jund committed
1299
1300
//	if(!rigid_)
//		center = position[groundFace];
1301
1302
1303
}

//-------------------------------------------------------------------------
1304

1305
1306
void MovingObstacle::applyForces()
{
pitiot's avatar
pitiot committed
1307

pitiot's avatar
pitiot committed
1308
//	Dart centerDart =map.phi<11>(groundFace);
1309
1310
1311

	if(!rigid_)
	{
pitiot's avatar