moving_obstacle.cpp 44.3 KB
Newer Older
pitiot's avatar
maj    
pitiot committed
1
2
3
4
#include "moving_obstacle.h"
#include "obstacle.h"
#include "agent.h"
#include "simulator.h"
5
#include "moving_mesh.h"
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
		// 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
merging    
pitiot committed
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
		if ( parent==NULL) //départ face à la cible en cas d'obstacles pouvant effectuer des rotations
		{
//			VEC3 axeZ=VEC3 (0,0,1);
//#ifdef TWO_AND_HALF_DIM
//
//			VEC3 normale = Algo::Surface::Geometry::faceNormal<PFP>(sim->envMap_.map, parts_[nbVertices]->d, sim->envMap_.position);
//#else
//			VEC3 normale =axeZ;
//#endif
//			Geom::Matrix44f rot ;
//			rot.identity() ;
//			angle = Geom::angle(goals_[curGoal_] - center,front  - center);
//			Geom::rotate(axeZ,angle,rot);
//
//			float angle1 = Geom::angle(normale, VEC3 (0,0,1) ) ;
//		//	CGoGNout<<"angle : "<<angle<<CGoGNendl;
//			VEC3 axis = VEC3(0,0,1) ^ normale ;
//
//		//	Geom::translate(center[0],center[1],center[2],rot);
//			Geom::rotate(axis, angle1, rot) ;
//
225
226
227
228
229
230
//
////			std::cout<<" angle : "<< angle;
//
//			for (unsigned int i = 0; i < nbVertices; ++i)
//			{
////				std::cout<<" || pos[i] avant : "<< pos [i];
pitiot's avatar
merging    
pitiot committed
231
//				Geom::transform(pos[i],rot);
232
233
234
235
236
237
238
239
////				std::cout<<" || pos[i] APRES : "<< pos [i]<<std::endl;
//
//				parts_[i]->move(pos[i]);
//
//
//			}
//			angle=0;
//			front=(pos[0] + pos[1]) / 2;
pitiot's avatar
merging    
pitiot committed
240
		}
Thomas Jund's avatar
merge    
Thomas Jund committed
241

242
243
	groundFace = map.newFace(nbVertices);

pitiot's avatar
maj    
pitiot committed
244
245
	for (unsigned int i = 0; i < nbVertices; ++i)
	{
246
247
248
249
250
		float distance=(center-pos[i]).norm();
		if(distance>gravity_dist)
		{
			gravity_dist=distance;
		}
Thomas Jund's avatar
Thomas Jund committed
251
		Dart d = i;
252
253
		position[d] = pos[i];
		deformation[d] = VEC3(0);
254
255

		if(!rigid_)
Thomas Jund's avatar
Thomas Jund committed
256
		{
257
			velocity[d] = VEC3(0);
Thomas Jund's avatar
Thomas Jund committed
258
259
			forces[d] = VEC3(0);
		}
260

261

pitiot's avatar
pitiot committed
262
	}
263

264
265
	//extrude face to build a cage
	// compute edgeLength for mass-spring
pitiot's avatar
merging    
pitiot committed
266
	hight=rigid_ ? -10.0f : 10.0f;
pitiot's avatar
pitiot committed
267
	Algo::Surface::Modelisation::extrudeFace<PFP>(map, position, groundFace, hight) ;
pitiot's avatar
pitiot committed
268
269
270
271
272
//	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;

273
//	Algo::Surface::Modelisation::extrudeFace<PFP>(map, position, groundFace, 0.0f) ;
274
275
276
	map.fillHole(groundFace);
	groundFace = map.phi2(groundFace);

pitiot's avatar
pitiot committed
277

278
279
	if(!rigid_)
	{
pitiot's avatar
pitiot committed
280
281
282
283
284
285
286
287
288
289
290
291
		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);
		}

292
		Algo::Surface::Modelisation::EarTriangulation<PFP> et(map);
293
294
295
296
297
298
299
		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
300

pitiot's avatar
merging    
pitiot committed
301
302
303
304
		Dart centerDart =map.phi<11>(groundFace);
		d = centerDart;

		do
Thomas Jund's avatar
Thomas Jund committed
305
		{
pitiot's avatar
merging    
pitiot committed
306
307
308
309
310
			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);
Thomas Jund's avatar
Thomas Jund committed
311
		}
pitiot's avatar
merging    
pitiot committed
312
		while(d!=centerDart);
313
		map.enableQuickTraversal<VERTEX>();
314

Thomas Jund's avatar
Thomas Jund committed
315
		dDir=dInside;
pitiot's avatar
pitiot committed
316
	}
317

pitiot's avatar
merging    
pitiot committed
318

319

pitiot's avatar
maj    
pitiot committed
320
321
	for (unsigned int i = 0; i < nbVertices; ++i)
	{
pitiot's avatar
pitiot committed
322
323
		Obstacle* o = new Obstacle(parts_[i]->getPosition(),
				parts_[(i + 1) % nbVertices]->getPosition(),
Thomas Jund's avatar
merge    
Thomas Jund committed
324
				parts_[(i - 1 + nbVertices) % nbVertices]->getPosition(),
Thomas Jund's avatar
Thomas Jund committed
325
				parts_[(i + 2) % nbVertices]->getPosition(), i, (i+1)% nbVertices, this, i);
pitiot's avatar
maj    
pitiot committed
326
		obstacles_[i] = o;
pitiot's avatar
pitiot committed
327
//		CGoGNout<<" obstacle :"<< i << " num : "<< o<<CGoGNendl;
pitiot's avatar
pitiot committed
328
		sim_->envMap_.pushObstacleInCells(o);
pitiot's avatar
maj    
pitiot committed
329
330
331
	}
}

332
333
334
335
336
337
338
339
340
341
342
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
343
	m_shader->setColor(Geom::Vec4f(0.,1.,0.,0.));
344
345
346
347
348
349
350
351
352
353
//	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
354
355

	m_ds = new Utils::Drawer();
356
357
}

Thomas Jund's avatar
Thomas Jund committed
358
void MovingObstacle::draw(bool showPath)
359
360
361
362
363
{
#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);
364
//	m_shader->setColor(Geom::Vec4f(movingObstacleNeighbors_.size()==0 ? 1.0f : 0,0.,0.,0.));
pitiot's avatar
merge    
pitiot committed
365

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

pitiot's avatar
up    
pitiot committed
368
//	if(movingObstacleNeighbors_.size()==0)
pitiot's avatar
merging    
pitiot committed
369
370
371
372
//	if(index==12)
//	// if(obstacleNeighbors_.size()==0)
//		m_shader->setColor(Geom::Vec4f(col[0],col[1],col[2],0));
//	else
373
		m_shader->setColor(Geom::Vec4f(0.5,0.5,0.5,0));
pitiot's avatar
merge    
pitiot committed
374

375
376
377
378
	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
379
380
381
382
383
384
385
386

	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
387
388
		m_ds->vertex(center);
		for(unsigned int i = 0 ; i < goals_.size() ; i++)
Thomas Jund's avatar
Thomas Jund committed
389
		{
pitiot's avatar
up    
pitiot committed
390
			m_ds->vertex(goals_[(curGoal_+i)%(goals_.size())]);
Thomas Jund's avatar
Thomas Jund committed
391
392
393
394
395
		}

		m_ds->end();
		m_ds->endList();
	}
396
397
}

398
399
VEC3 MovingObstacle::getDilatedPosition(unsigned int ind)
{
400
	Dart d(ind); //WARNING : works only for one face created at start !
pitiot's avatar
pitiot committed
401
#ifndef TWO_AND_HALF_DIM
pitiot's avatar
up    
pitiot committed
402
403
404
405
406
407
//	return position[d]+deformation[d];
	if(!rigid_)
		{
			return position[d];
		}
	else return position[d]+deformation[d];
pitiot's avatar
pitiot committed
408
#else
Arash HABIBI's avatar
Arash HABIBI committed
409
	return position[d];
pitiot's avatar
pitiot committed
410
#endif
411
412
413
414
}

VEC3 MovingObstacle::getPosition(unsigned int ind)
{
415
416
	Dart d(ind);
	return position[d];
417
418
}

pitiot's avatar
maj    
pitiot committed
419
420
421
422
bool MovingObstacle::test_opposition(VEC3 o, VEC3 p1, VEC3 p2)
{
	o.normalize();
	o *= -1;
423
424
425
	VEC3 vector(p1 - p2);
	vector.normalize();
	return (o - vector).norm2() < 0.1;
pitiot's avatar
maj    
pitiot committed
426
427
428
}

// TODO Check position
pitiot's avatar
pitiot committed
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
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
//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
483

pitiot's avatar
pitiot committed
484
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
485
{
Thomas Jund's avatar
Thomas Jund committed
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
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
	//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
539
540
541
542
543
//	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
544
//	{
pitiot's avatar
pitiot committed
545
546
547
548
549
550
//		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
551
//
pitiot's avatar
pitiot committed
552
553
554
555
//		for (std::vector<Agent*>::const_iterator it = agents.begin(); it != agents.end(); ++it)
//		{
//			if ((*it)->alive)
//			{
pitiot's avatar
pitiot committed
556
//
pitiot's avatar
pitiot committed
557
558
559
560
//				float distSq = (center - (*it)->getPosition()).norm2() ;
//				if ((agentNeighbors_.size() < maxNeighbors_ || distSq < maxDist)
//					&& distSq < neighborDistSq_)
//				{
pitiot's avatar
pitiot committed
561
//
pitiot's avatar
pitiot committed
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
//					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
597
//	}
pitiot's avatar
pitiot committed
598
599
600
601
}

void MovingObstacle::updateObstacleNeighbors() // obstacles voisins , distance par rapport aux centres des segments// a mettre en place si besoin
{
602
603
	obstacleNeighbors_.clear() ;
	movingObstacleNeighbors_.clear() ;
pitiot's avatar
pitiot committed
604
	for (std::vector< std::pair<Dart, int> >::iterator it2 = general_belonging.begin();it2 != general_belonging.end(); ++it2)
605
	{
pitiot's avatar
pitiot committed
606
607
		std::vector<Obstacle*>& obst = sim_->envMap_.obstvect[(*it2).first] ;
		std::vector<Obstacle*>& neighborObst = sim_->envMap_.neighborObstvect[(*it2).first] ;
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
//		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
638
639
640
641
642
643
//						if (sim_->envMap_.testOrientation(parts_[nbVertices]->getPosition(), (*it)->p1, (*it)->p2, parts_[nbVertices]->d) == 1)
						{
	//						if (distSq > maxDistMovingObst)
	//							maxDistMovingObst = distSq ;
							movingObstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
						}
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



					}

				}
			}
		}

		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
679
680
681
682
683
684
//						if (sim_->envMap_.testOrientation(parts_[nbVertices]->getPosition(), (*it)->p1, (*it)->p2, parts_[nbVertices]->d) == 1)
						{
	//						if (distSq > maxDistMovingObst)
	//							maxDistMovingObst = distSq ;
							movingObstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
						}
685
686
687
688
689
					}
				}
			}
		}
	}
pitiot's avatar
maj    
pitiot committed
690
691
}

Arash HABIBI's avatar
unidir    
Arash HABIBI committed
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
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
//-------------------------------------------------------------

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
791

Arash HABIBI's avatar
unidir    
Arash HABIBI committed
792
	if(!rigid_)
pitiot's avatar
pitiot committed
793
794
795
796
	{
		Dart centerDart =map.phi<11>(groundFace);
			Dart d = centerDart;
		do
Arash HABIBI's avatar
unidir    
Arash HABIBI committed
797
		{
pitiot's avatar
pitiot committed
798
			Dart e = map.phi1(d);
Arash HABIBI's avatar
unidir    
Arash HABIBI committed
799
			//initialisation of forces
pitiot's avatar
pitiot committed
800
801
			forces[e] = VEC3(0.0);
			d=map.phi<21>(d);
Arash HABIBI's avatar
unidir    
Arash HABIBI committed
802
		}
pitiot's avatar
pitiot committed
803
804
805
		while(d!=centerDart);
		forces[d] = VEC3(0.0);
	}
Arash HABIBI's avatar
unidir    
Arash HABIBI committed
806
807
}

Arash HABIBI's avatar
Arash HABIBI committed
808
809
810

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

pitiot's avatar
pitiot committed
811
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
812
813
814
815
816
817
818
819
820
821
822
823
824
{
	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
825
		VEC3 normal = normFace^v_obst;
Arash HABIBI's avatar
Arash HABIBI committed
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
		// 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
849
850
//-------------------------------------------------------------

851
void MovingObstacle::updateForces()
pitiot's avatar
maj    
pitiot committed
852
{
pitiot's avatar
pitiot committed
853
	assert(sim_->envMap_.map.getCurrentLevel() == sim_->envMap_.map.getMaxLevel()) ;
pitiot's avatar
pitiot committed
854

pitiot's avatar
pitiot committed
855
	//pour les tests de détection///////////////
pitiot's avatar
merging    
pitiot committed
856
857
858
859
860
861
862
	if(!seen)
	{
		color1=1.0f;
		color2=1.0f;
		color3=1.0f;
	}
	seen=false;
Thomas Jund's avatar
merge    
Thomas Jund committed
863

pitiot's avatar
pitiot committed
864
	//////////////////////////
Thomas Jund's avatar
merge    
Thomas Jund committed
865

866
	PFP::VEC3 bary(0);
pitiot's avatar
maj    
pitiot committed
867
868

	Dart d;
pitiot's avatar
pitiot committed
869
870
	velocity_ = newVelocity_* velocity_factor;

Arash HABIBI's avatar
Arash HABIBI committed
871
872
	// velocity_[0] = 0.0;
	// velocity_[1] = 0.0;
pitiot's avatar
maj    
pitiot committed
873
874

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

pitiot's avatar
merging    
pitiot committed
877
	float rotor=0;
878

pitiot's avatar
merging    
pitiot committed
879
880
881
882
	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
883

Arash HABIBI's avatar
03_2013    
Arash HABIBI committed
884
885
	// masse ressort pour la limace

886
	if(!rigid_)
pitiot's avatar
maj    
pitiot committed
887
	{
pitiot's avatar
pitiot committed
888
889
890
891
		Dart centerDart =map.phi<11>(groundFace);
		Dart d = centerDart;
		forces[d] += -0.9f*velocity[d];
		do
Thomas Jund's avatar
Thomas Jund committed
892
		{
pitiot's avatar
pitiot committed
893
			Dart e = map.phi1(d);
Thomas Jund's avatar
Thomas Jund committed
894
			//initialisation of forces with viscosity
pitiot's avatar
pitiot committed
895
			forces[e] += -0.9f*velocity[e];
Arash HABIBI's avatar
Arash HABIBI committed
896
			// forces[dd] = VEC3(0.0);
pitiot's avatar
merging    
pitiot committed
897
			VEC3 p1Next = position[map.phi1(e)]+(velocity[map.phi1(e)] * sim_->timeStep_); // ressorts sur le bord
pitiot's avatar
pitiot committed
898
899
900
			VEC3 p2Next = position[e]+(velocity[e] * sim_->timeStep_);
			// p1Next et p2Next sont la position des extremites de l'arete.
			VEC3 v1 = (p1Next-p2Next);
901

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

pitiot's avatar
merging    
pitiot committed
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
			float stretch = 4*rigidity*(edgeLength[e]-norm);
			float angularStretch = 0, angularStretch2 = 0;
			float restAngle = vertexAngle[e];
			if(restAngle!=0.0f)
			{
				float angularRig = 2*rigidity;

				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);
			}


pitiot's avatar
pitiot committed
928
929
			if(norm>0.0f)
			{
pitiot's avatar
merging    
pitiot committed
930
				VEC3 f = (stretch+angularStretch)*(v1/norm);
Thomas Jund's avatar
Thomas Jund committed
931

pitiot's avatar
pitiot committed
932
933
934
935
				//apply force symmetrically
				forces[e] -= f;
				forces[map.phi1(e)] += f;
			}
Thomas Jund's avatar
Thomas Jund committed
936

pitiot's avatar
merging    
pitiot committed
937
938

			p1Next = position[e]+(velocity[e] * sim_->timeStep_); /// ressorts vers le centre de la face
pitiot's avatar
pitiot committed
939
940
941
			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
942

pitiot's avatar
pitiot committed
943
944
			//stretch spring : /!\ max rigidity relative to the timestep used (unstable otherwise)
			norm = v1.norm();
pitiot's avatar
merging    
pitiot committed
945
946
947
948
949
950
951
952
953
954
955
956
			restAngle = vertexAngle[d];
			if(restAngle!=0.0f)
			{
				float angularRig = 2*rigidity;

				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);
Thomas Jund's avatar
Thomas Jund committed
957

pitiot's avatar
merging    
pitiot committed
958
959
				if(v3 * vPl < 0.0f)
					curAngle *= -1.0f;
960

pitiot's avatar
merging    
pitiot committed
961
962
963
964
				angularStretch = angularRig*(restAngle-curAngle);
			}

			stretch = 4*rigidity*(edgeLength[d]-norm);
pitiot's avatar
pitiot committed
965
966
			if(norm>0.0f)
			{
pitiot's avatar
merging    
pitiot committed
967
				VEC3 f = (stretch+angularStretch)*(v1/norm);
968

pitiot's avatar
pitiot committed
969
970
				//apply force symmetrically
				forces[d] -= f;
pitiot's avatar
merging    
pitiot committed
971
				forces[e] += f;
pitiot's avatar
pitiot committed
972
			}
973

pitiot's avatar
merging    
pitiot committed
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
			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 = 2*rigidity;

				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;
994

pitiot's avatar
merging    
pitiot committed
995
996
997
998
999
				angularStretch2 = angularRig*(restAngle-curAngle);
			}
			if(norm>0.0f)
			{
				VEC3 f = (angularStretch2)*(v1/norm);
Thomas Jund's avatar
Thomas Jund committed
1000

pitiot's avatar
merging    
pitiot committed
1001
1002
1003
1004
				//apply force symmetrically
				forces[map.phi1(e)] -= f;
				forces[d] += f;
			}
1005

pitiot's avatar
pitiot committed
1006
1007
1008
			d=map.phi<21>(d);
		}
		while(d!=centerDart);
pitiot's avatar
maj    
pitiot committed
1009

Arash HABIBI's avatar
03_2013    
Arash HABIBI committed
1010
1011
			//-------------------------------------------------------------------------
			// ARASH : A présent on calcule les interactions avec les autres obstacles.
pitiot's avatar
pitiot committed
1012
1013
1014
		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
1015

pitiot's avatar
pitiot committed
1016
		double obst_radius_infl;
pitiot's avatar
merging    
pitiot committed
1017

pitiot's avatar
pitiot committed
1018
1019
			obst_radius_infl = 10.; // scenario 1 et 3

pitiot's avatar
merging    
pitiot committed
1020
		float fixed_obst_factor = 5.0f;
Arash HABIBI's avatar
comment    
Arash HABIBI committed
1021

pitiot's avatar
merging    
pitiot committed
1022
1023
1024
		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
1025
				Dart dd = map.phi1(d);
Arash HABIBI's avatar
03_2013    
Arash HABIBI committed
1026
1027
					VEC3 p = position[dd]+(velocity[dd] * sim_->timeStep_);

Arash HABIBI's avatar
Arash HABIBI committed
1028
1029
					// Evitement d'obstacles mobiles

Arash HABIBI's avatar
03_2013    
Arash HABIBI committed
1030
1031
1032
1033
1034
1035
					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
1036

pitiot's avatar
pitiot committed
1037
						forces[dd] += computeForce(p,p1,p2,obst_radius_infl,obst_power,obst_stiffness,normFace);
Arash HABIBI's avatar
Arash HABIBI committed
1038
					}
Arash HABIBI's avatar
unidir    
Arash HABIBI committed
1039

pitiot's avatar
pitiot committed
1040
					//	Evitement d'obstacles fixes
Arash HABIBI's avatar
03_2013    
Arash HABIBI committed
1041

Arash HABIBI's avatar
Arash HABIBI committed
1042
1043
1044
1045
					for(std::vector<std::pair<float, Obstacle*> >::iterator it = obstacleNeighbors_.begin() ;
						it != obstacleNeighbors_.end() ; ++it)
					{
						Obstacle * obst = it->second;
pitiot's avatar
pitiot committed
1046
1047
						VEC3 p1=obst->p2 ;
						VEC3 p2=obst->p1 ;
Arash HABIBI's avatar
Arash HABIBI committed
1048

pitiot's avatar
merging    
pitiot committed
1049
						forces[dd] += computeForce(p,p1,p2,fixed_obst_factor*obst_radius_infl,fixed_obst_factor*obst_power,fixed_obst_factor*obst_stiffness,normFace);
Arash HABIBI's avatar
03_2013    
Arash HABIBI committed
1050
1051
					}

pitiot's avatar
pitiot committed
1052
				d = map.phi<21>(d);
pitiot's avatar
merging    
pitiot committed
1053
			}
pitiot's avatar
pitiot committed
1054
1055


1056
1057

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

1061
		//apply force to each vertex
1062
		/*
1063
1064
		d = groundFace;
		for (unsigned int i = 0; i < nbVertices; ++i)
pitiot's avatar
pitiot committed
1065
		{
Thomas Jund's avatar
Thomas Jund committed
1066
			velocity[d] += forces[d] * sim_->timeStep_;
1067
1068
1069
1070
			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
1071
		}
1072
		*/
1073
	}
pitiot's avatar
pitiot committed
1074
1075
	else
	{
1076
1077
1078
1079
1080
1081
	//	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
1082
#ifdef TWO_AND_HALF_DIM
pitiot's avatar
up    
pitiot committed
1083
1084
1085
1086
1087
1088
1089
1090
1091
1092

			//// 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
1093
1094
1095
#else
			position[d] += rotate2D(position[d], center, abs_angle*rotor);
#endif
1096
1097
			position[d] += (velocity_ * sim_->timeStep_);

1098
#ifdef EXPORTING_BOXES
1099
1100

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

1105
1106
#endif

1107
1108
1109
//			position[map.phi<211>(d)] += (velocity_ * sim_->timeStep_);

			bary += position[d];
Thomas Jund's avatar
Thomas Jund committed
1110
1111


1112
1113
1114
			map.next(d);
		}

pitiot's avatar
pitiot committed
1115
		front=(position[0] + position[1]) / 2;
1116
1117
1118
1119
		if(angle >0)
			angle -= rotor;
		else
			angle += rotor;
pitiot's avatar
pitiot committed
1120
	}
pitiot's avatar
maj    
pitiot committed
1121

pitiot's avatar
pitiot committed
1122
	center = bary / nbVertices;
pitiot's avatar
maj    
pitiot committed
1123

1124
1125
	//computing deformation vector on vertices :
	//depending on the velocity, extend the perceived shape
1126
	VEC3 vel = velocity_.normalized();
1127
1128
1129

	d = groundFace;
	for (unsigned int i = 0; i < nbVertices; ++i)
1130
	{
1131
		VEC3 v(position[d] - center);
1132
1133
		VEC3 vN = v.normalized();
		float dot = vN * vel;
1134
1135
		deformation[d] = v * 2.0f * (dot+1.0f)/2.0f;
		map.next(d);
1136
1137
	}

Thomas Jund's avatar
Thomas Jund committed
1138
1139
//	if(!rigid_)
//		center = position[groundFace];
1140
1141
1142
}

//-------------------------------------------------------------------------
1143

1144
1145
void MovingObstacle::applyForces()
{
pitiot's avatar
pitiot committed
1146
1147

	Dart centerDart =map.phi<11>(groundFace);
1148
1149
1150

	if(!rigid_)
	{
pitiot's avatar
pitiot committed
1151
		Dart d = centerDart;
1152
1153
		for (unsigned int i = 0; i < nbVertices; ++i)
		{
pitiot's avatar
pitiot committed
1154
1155
1156
1157
1158
1159
1160
1161
1162
			Dart e = map.phi_1(d);
			velocity[e] += forces[e] * sim_->timeStep_;
			position[e] += (velocity[e] * sim_->timeStep_);





			d=map.phi<21>(d);
1163
		}
pitiot's avatar
pitiot committed
1164
1165
1166
1167
1168
1169
1170
1171

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





1172
// MAJ des obstacles
1173

pitiot's avatar
pitiot committed
1174

pitiot's avatar
pitiot committed
1175
	}
1176
1177
1178



1179
1180


pitiot's avatar
pitiot committed
1181
}
1182

pitiot's avatar
pitiot committed
1183
1184
1185
1186
1187
1188
1189
1190
1191
1192
1193
1194
1195
1196
1197
1198
1199
1200
1201
1202
1203
1204
1205
1206
1207
1208
1209
1210
1211
1212
1213
1214
1215
1216
1217
1218
1219
1220
1221
1222
1223
1224
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
		{
pitiot's avatar
merging    
pitiot committed
1225
			for (unsigned int i = 0; i < nbVertices; ++i)
pitiot's avatar
pitiot committed
1226
1227
					{

pitiot's avatar
merging    
pitiot committed
1228
1229
					VEC3 pos = getDilatedPosition(i);
							parts_[i]->move(pos);
pitiot's avatar
pitiot committed
1230
1231
1232
1233
1234
1235
1236
1237
1238
1239
1240
1241
1242
1243
1244
1245
1246

				#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
1247
1248
		//		CGoGNout << "avant une etape : Obstacle "<< i << CGoGNendl;
		Obstacle* o = obstacles_[i];
1249
1250
		o->p1 = getDilatedPosition(i);
		o->p2 = getDilatedPosition((i+1) % nbVertices);
Thomas Jund's avatar
Thomas Jund committed
1251

pitiot's avatar
pitiot committed
1252

1253
1254
		o->prevP = getDilatedPosition((i - 1 + nbVertices) % nbVertices);
		o->nextP = getDilatedPosition((i + 2 + nbVertices) % nbVertices);
pitiot's avatar
pitiot committed
1255

Thomas Jund's avatar
Thomas Jund committed
1256

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

pitiot's avatar
pitiot committed
1260
		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
1261
		{
Thomas Jund's avatar
Thomas Jund committed
1262
			sim_->envMap_.popAndPushObstacleInCells(o,i);
pitiot's avatar
pitiot committed
1263
		}
Thomas Jund's avatar
Thomas Jund committed
1264

pitiot's avatar
pitiot committed
1265
1266
1267
1268
1269
1270
1271
1272
		/////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
1273
1274
		//		CGoGNout << "Apres une etape : Obstacle "<< i << CGoGNendl;
	}
pitiot's avatar
pitiot committed
1275
1276
1277
1278
1279
1280
1281
1282
	/////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
1283
1284
1285
	//
	//		envMap_->addMovingObstAsNeighbor(this,general_belonging,&(general_neighbors));
	//		CGoGNout << "nouvel update : "<< CGoGNendl;
1286
1287
	if(sim_->detect_agent_collision)
	{
pitiot's avatar
pitiot committed
1288
		for (std::vector< std::pair<Dart, int> >::iterator it2 = general_belonging.begin(); it2 != general_belonging.end(); ++ it2)
1289
		{
pitiot's avatar
pitiot committed
1290
			std::vector<Agent*> vector =sim_->envMap_.agentvect[(*it2).first];
1291