moving_obstacle.cpp 41.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
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
		// M appartient à l'ellipse ssi MF1 + MF2 = sum_dist_foci est une constante
		// où F1 et F2 sont les deux foyers.
	//	length = (pos[0]-pos[1]).norm();
	//	width  = (pos[1]-pos[2]).norm();
	//	sum_dist_foci_rest = 2*(length + width*(sqrt(2)-0.5));

//		if (spinning && parent==NULL) //départ face à la cible en cas d'obstacles pouvant effectuer des rotations
//		{
//
//			angle = get_angle(goals_[curGoal_] - center,front  - center);
////			std::cout<<" angle : "<< angle;
//
//			for (unsigned int i = 0; i < nbVertices; ++i)
//			{
////				std::cout<<" || pos[i] avant : "<< pos [i];
//				pos[i]+=rotate(pos[i], center, angle);
////				std::cout<<" || pos[i] APRES : "<< pos [i]<<std::endl;
//
//				parts_[i]->move(pos[i]);
//
//
//			}
//			angle=0;
//			front=(pos[0] + pos[1]) / 2;
//		}
Thomas Jund's avatar
merge    
Thomas Jund committed
223

224
225
	groundFace = map.newFace(nbVertices);

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

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

243

pitiot's avatar
pitiot committed
244
	}
245

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

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

pitiot's avatar
pitiot committed
259

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


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

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

274
		Algo::Surface::Modelisation::EarTriangulation<PFP> et(map);
275
276
277
278
279
280
281
		et.triangule();

		TraversorE<PFP::MAP> tE(map);
		for(Dart d = tE.begin() ; d != tE.end() ; d = tE.next())
		{
			edgeLength[d] = VEC3(position[map.phi1(d)]-position[d]).norm();
		}
Thomas Jund's avatar
Thomas Jund committed
282

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

293
		map.enableQuickTraversal<VERTEX>();
294

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

pitiot's avatar
merging    
pitiot committed
298

299

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

312
313
314
315
316
317
318
319
320
321
322
void MovingObstacle::initGL()
{
#ifdef EXPORTING_BOXES

	m_render = new Algo::Render::GL2::MapRender();

	m_positionVBO = new Utils::VBO();

	// using simple shader with color
	m_shader = new Utils::ShaderSimpleColor();
	m_shader->setAttributePosition(m_positionVBO);
pitiot's avatar
pitiot committed
323
	m_shader->setColor(Geom::Vec4f(0.,1.,0.,0.));
324
325
326
327
328
329
330
331
332
333
//	m_shader->setAmbiant(Geom::Vec4f(0.,1.,0.,0.));
//	m_shader->setDiffuse(Geom::Vec4f(0.,1.,0.,0.));

	m_positionVBO->updateData(position) ;

	m_render->initPrimitives<PFP>(map, Algo::Render::GL2::LINES,false) ;
	m_render->initPrimitives<PFP>(map, Algo::Render::GL2::TRIANGLES,false) ;

//	registerShader(m_shader);
#endif
Thomas Jund's avatar
Thomas Jund committed
334
335

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

Thomas Jund's avatar
Thomas Jund committed
338
void MovingObstacle::draw(bool showPath)
339
340
341
342
343
{
#ifdef EXPORTING_BOXES
//	m_render->initPrimitives<PFP>(map, Algo::Render::GL2::LINES,false) ;
//	m_render->initPrimitives<PFP>(map, Algo::Render::GL2::TRIANGLES,false) ;
	m_positionVBO->updateData(position);
344
//	m_shader->setColor(Geom::Vec4f(movingObstacleNeighbors_.size()==0 ? 1.0f : 0,0.,0.,0.));
pitiot's avatar
merge    
pitiot committed
345

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

pitiot's avatar
up    
pitiot committed
348
349
//	if(movingObstacleNeighbors_.size()==0)
	if(index==12)
Arash HABIBI's avatar
Arash HABIBI committed
350
	// if(obstacleNeighbors_.size()==0)
351
352
353
		m_shader->setColor(Geom::Vec4f(col[0],col[1],col[2],0));
	else
		m_shader->setColor(Geom::Vec4f(0.5,0.5,0.5,0));
pitiot's avatar
merge    
pitiot committed
354

355
356
357
358
	m_render->draw(m_shader, Algo::Render::GL2::TRIANGLES);
	m_shader->setColor(Geom::Vec4f(0.,0.,0.,0.));
	m_render->draw(m_shader, Algo::Render::GL2::LINES);
#endif
Thomas Jund's avatar
Thomas Jund committed
359
360
361
362
363
364
365
366

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

		VEC3 col = Utils::color_map_BCGYR(float(index)/float(sim_->movingObstacles_.size()));
		m_ds->color3f(col[0],col[1],col[2]);
pitiot's avatar
up    
pitiot committed
367
368
		m_ds->vertex(center);
		for(unsigned int i = 0 ; i < goals_.size() ; i++)
Thomas Jund's avatar
Thomas Jund committed
369
		{
pitiot's avatar
up    
pitiot committed
370
			m_ds->vertex(goals_[(curGoal_+i)%(goals_.size())]);
Thomas Jund's avatar
Thomas Jund committed
371
372
373
374
375
		}

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

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

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

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

// TODO Check position
pitiot's avatar
pitiot committed
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
//void MovingObstacle::contournerBatiment()
//{
//	PFP::VEC3 toto;
//	PFP::VEC3 toto_norm;
//	//for particles in the "front" of the object (to modify)
//	for(int k =0;k<2;k++)
//	{
//
//		registering_part->get_memo(vertices[k]);
//		std::vector<Obstacle*>& obst = sim_->envMap_.obstvect[registering_part->d];
//
//		//search all obstacles around
//		for(std::vector<Obstacle*>::const_iterator it = obst.begin(); it != obst.end(); ++it)
//		{
//			//only for fixed obstacles around
//			if ((*it)->mo==NULL)
//			{
//				float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, vertices[k]);
//				if(distSq < detectionFixedObst*detectionFixedObst)
//				{
//					toto= (*it)->p1 -(*it)->p2;
//					toto_norm=toto;
//					toto_norm[0]=-toto[1];
//					toto_norm[1]=toto[0];
//
//					if(test_opposition(toto_norm,front,center)) //// à changer ////////////
//					{
//						int co = rand() % 2 ;
//						if (toto[0]==0)
//						{
//							finalGoal[0]=front[0];
//							if (co == 0)
//								finalGoal[0]=sim_->envMap_.geometry.max()[1]-maxNeighbors_;
//							else
//								finalGoal[0]=sim_->envMap_.geometry.min()[1]+maxNeighbors_;
//						}
//						else
//						{
//							finalGoal[1]=front[1];
//							if (co == 0)
//								finalGoal[0]=sim_->envMap_.geometry.max()[1]-maxNeighbors_;
//							else
//								finalGoal[0]=sim_->envMap_.geometry.min()[1]+maxNeighbors_;
//						}
//
//						float angle =get_angle(finalGoal-center,front -center);
//						make_half_turn=angle*nbVertices;
//						break;
//					}
//				}
//			}
//		}
//	}
//}
pitiot's avatar
maj    
pitiot committed
463

pitiot's avatar
pitiot committed
464
void MovingObstacle::updateAgentNeighbors() // agents voisins avec distance au bord (distpointlinesq) de l'obstacle // a mettre en place si besoin
pitiot's avatar
maj    
pitiot committed
465
{
Thomas Jund's avatar
Thomas Jund committed
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
	//test for slugs : doesn't always work.. :)
//	if(!rigid_)
//	{
//		Dart d = registering_part->d;
//
//		std::list<MovingObstacle*> seen;
//		PFP::OBSTACLEVECT& obst = sim_->envMap_.obstvect[d] ;
//		PFP::OBSTACLEVECT& neighborObst = sim_->envMap_.neighborObstvect[d] ;
//
//		for (std::vector<Obstacle*>::iterator it = obst.begin(); it != obst.end(); ++it)
//		{
//
//			if ((*it)->mo!=NULL)
//			{
//				MovingObstacle* other = (*it)->mo;
//				if(other->velocity_ * velocity_<=0.0f) //stop when not in front of each other
//				{
//					if(velocity_*(other->registering_part->getPosition()-registering_part->getPosition())>0)
//					{
//						newVelocity_ = VEC3(newVelocity_[1], -newVelocity_[0], 0);
//						seen.push_back(other);
//					}
//				}
//				else if(velocity_*(other->registering_part->getPosition()-registering_part->getPosition())>0
//						&& (other->registering_part->getPosition()-registering_part->getPosition()).norm2()<2000.0f)
//					newVelocity_ *= 0.01f;
//			}
//		}
//
//		for (std::vector<Obstacle*>::iterator it = neighborObst.begin(); it != neighborObst.end(); ++it)
//		{
//
//			if ((*it)->mo!=NULL)
//			{
//				MovingObstacle* other = (*it)->mo;
//				if(std::find(seen.begin(),seen.end(), other)==seen.end())
//				{
//					if(other->velocity_ * velocity_<=0.0f
//							&& (other->registering_part->getPosition()-registering_part->getPosition()).norm2()<2000.0f)
//					{
//
//						if(velocity_*(other->registering_part->getPosition()-registering_part->getPosition())>0)
//						{
//							newVelocity_ = VEC3(newVelocity_[1], -newVelocity_[0], 0);
//						}
//					}
////					else if(velocity_*(other->registering_part->getPosition()-registering_part->getPosition())>0)
////						newVelocity_ *= 0.5f;
//				}
//			}
//		}
//	}

pitiot's avatar
pitiot committed
519
520
521
522
523
//	agentNeighbors_.clear() ;
//	Dart d;
//	float maxDist = 0.0f ;//distance max des agents eregistrés au centre
//	std::set::iterator agIt;//emplacement de l'agent en question
//	for(int i = 0;i< nbVertices;i++ )
pitiot's avatar
pitiot committed
524
//	{
pitiot's avatar
pitiot committed
525
526
527
528
529
530
//		registering_part->get_memo(vertices[i]);
//		d=registering_part->d;
//		const std::vector<Agent*>& agents = sim_->envMap_.agentvect[d] ;
//		const std::vector<Agent*>& neighborAgents = sim_->envMap_.neighborAgentvect[d] ;
//
//
pitiot's avatar
pitiot committed
531
//
pitiot's avatar
pitiot committed
532
533
534
535
//		for (std::vector<Agent*>::const_iterator it = agents.begin(); it != agents.end(); ++it)
//		{
//			if ((*it)->alive)
//			{
pitiot's avatar
pitiot committed
536
//
pitiot's avatar
pitiot committed
537
538
539
540
//				float distSq = (center - (*it)->getPosition()).norm2() ;
//				if ((agentNeighbors_.size() < maxNeighbors_ || distSq < maxDist)
//					&& distSq < neighborDistSq_)
//				{
pitiot's avatar
pitiot committed
541
//
pitiot's avatar
pitiot committed
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
//					if (distSq > maxDist)
//					{
//						maxDist = distSq ;
//						agIt=(agentNeighbors_.insert(std::make_pair(distSq, *it))).first() ;
//					}
//					else
//					{
//						agentNeighbors_.insert(std::make_pair(distSq, *it))
//					}
//				}
//
//			}
//		}
//
//		for (std::vector<Agent*>::const_iterator it = neighborAgents.begin(); it != neighborAgents.end(); ++it)
//		{
//			if ((*it)->alive)
//			{
//				float distSq = (getPosition() - (*it)->getPosition()).norm2() ;
//				if ((agentNeighbors_.size() < maxNeighbors_ || distSq < maxDist)
//					&& distSq < neighborDistSq_)
//				{
//					if (distSq > maxDist)
//					{
//						maxDist = distSq ;
//						agIt=(agentNeighbors_.insert(std::make_pair(distSq, *it))).first() ;
//					}
//					else
//					{
//						agentNeighbors_.insert(std::make_pair(distSq, *it))
//					}
//				}
//			}
//
//		}
pitiot's avatar
pitiot committed
577
//	}
pitiot's avatar
pitiot committed
578
579
580
581
}

void MovingObstacle::updateObstacleNeighbors() // obstacles voisins , distance par rapport aux centres des segments// a mettre en place si besoin
{
582
583
	obstacleNeighbors_.clear() ;
	movingObstacleNeighbors_.clear() ;
pitiot's avatar
pitiot committed
584
	for (std::vector< std::pair<Dart, int> >::iterator it2 = general_belonging.begin();it2 != general_belonging.end(); ++it2)
585
	{
pitiot's avatar
pitiot committed
586
587
		std::vector<Obstacle*>& obst = sim_->envMap_.obstvect[(*it2).first] ;
		std::vector<Obstacle*>& neighborObst = sim_->envMap_.neighborObstvect[(*it2).first] ;
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
//		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
618
619
620
621
622
623
//						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)) ;
						}
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658



					}

				}
			}
		}

		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
659
660
661
662
663
664
//						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)) ;
						}
665
666
667
668
669
					}
				}
			}
		}
	}
pitiot's avatar
maj    
pitiot committed
670
671
}

Arash HABIBI's avatar
unidir    
Arash HABIBI committed
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
//-------------------------------------------------------------

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
771

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

Arash HABIBI's avatar
Arash HABIBI committed
788
789
790

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

pitiot's avatar
pitiot committed
791
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
792
793
794
795
796
797
798
799
800
801
802
803
804
{
	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
805
		VEC3 normal = normFace^v_obst;
Arash HABIBI's avatar
Arash HABIBI committed
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
		// 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
829
830
//-------------------------------------------------------------

831
void MovingObstacle::updateForces()
pitiot's avatar
maj    
pitiot committed
832
{
pitiot's avatar
pitiot committed
833
	assert(sim_->envMap_.map.getCurrentLevel() == sim_->envMap_.map.getMaxLevel()) ;
pitiot's avatar
pitiot committed
834

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

pitiot's avatar
pitiot committed
844
	//////////////////////////
Thomas Jund's avatar
merge    
Thomas Jund committed
845

846
	PFP::VEC3 bary(0);
pitiot's avatar
maj    
pitiot committed
847
848

	Dart d;
pitiot's avatar
pitiot committed
849
850
	velocity_ = newVelocity_* velocity_factor;

Arash HABIBI's avatar
Arash HABIBI committed
851
852
	// velocity_[0] = 0.0;
	// velocity_[1] = 0.0;
pitiot's avatar
maj    
pitiot committed
853
854

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

pitiot's avatar
merging    
pitiot committed
857
	float rotor=0;
858

pitiot's avatar
merging    
pitiot committed
859
860
861
862
	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
863

Arash HABIBI's avatar
03_2013    
Arash HABIBI committed
864
865
	// masse ressort pour la limace

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

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

pitiot's avatar
pitiot committed
886
887
888
889
			float stretch = rigidity*(edgeLength[e]-norm);
			if(norm>0.0f)
			{
				VEC3 f = stretch*(v1/norm);
Thomas Jund's avatar
Thomas Jund committed
890

pitiot's avatar
pitiot committed
891
892
893
894
				//apply force symmetrically
				forces[e] -= f;
				forces[map.phi1(e)] += f;
			}
Thomas Jund's avatar
Thomas Jund committed
895

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

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

904

pitiot's avatar
pitiot committed
905
906
907
908
			stretch = rigidity*(edgeLength[d]-norm);
			if(norm>0.0f)
			{
				VEC3 f = stretch*(v1/norm);
909

pitiot's avatar
pitiot committed
910
911
912
913
				//apply force symmetrically
				forces[d] -= f;
				forces[map.phi1(d)] += f;
			}
914

915

Thomas Jund's avatar
Thomas Jund committed
916

917

pitiot's avatar
pitiot committed
918
919
920
			d=map.phi<21>(d);
		}
		while(d!=centerDart);
pitiot's avatar
maj    
pitiot committed
921

Arash HABIBI's avatar
03_2013    
Arash HABIBI committed
922
923
			//-------------------------------------------------------------------------
			// ARASH : A présent on calcule les interactions avec les autres obstacles.
pitiot's avatar
pitiot committed
924
925
926
		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
927

pitiot's avatar
pitiot committed
928
929
930
931
		double obst_radius_infl;
		if(sim_->config==0)
			obst_radius_infl = 100.; // scenario 0
		else
pitiot's avatar
pitiot committed
932
933
			obst_radius_infl = 10.; // scenario 1 et 3

Arash HABIBI's avatar
comment    
Arash HABIBI committed
934

pitiot's avatar
pitiot committed
935
		PFP::VEC3 normFace = CGoGN::Algo::Surface::Geometry::faceNormal<PFP>(sim_->envMap_.map, parts_[nbVertices]->d, sim_->envMap_.position);
pitiot's avatar
pitiot committed
936
937
			do {
				Dart dd = map.phi1(d);
Arash HABIBI's avatar
03_2013    
Arash HABIBI committed
938
939
					VEC3 p = position[dd]+(velocity[dd] * sim_->timeStep_);

Arash HABIBI's avatar
Arash HABIBI committed
940
941
					// Evitement d'obstacles mobiles

Arash HABIBI's avatar
03_2013    
Arash HABIBI committed
942
943
944
945
946
947
					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
948

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

pitiot's avatar
pitiot committed
952
					//	Evitement d'obstacles fixes
Arash HABIBI's avatar
03_2013    
Arash HABIBI committed
953

Arash HABIBI's avatar
Arash HABIBI committed
954
955
956
957
					for(std::vector<std::pair<float, Obstacle*> >::iterator it = obstacleNeighbors_.begin() ;
						it != obstacleNeighbors_.end() ; ++it)
					{
						Obstacle * obst = it->second;
pitiot's avatar
pitiot committed
958
959
						VEC3 p1=obst->p2 ;
						VEC3 p2=obst->p1 ;
Arash HABIBI's avatar
Arash HABIBI committed
960

pitiot's avatar
pitiot committed
961
						forces[dd] += computeForce(p,p1,p2,obst_radius_infl,obst_power,1*obst_stiffness,normFace);
Arash HABIBI's avatar
03_2013    
Arash HABIBI committed
962
963
					}

pitiot's avatar
pitiot committed
964
965
966
967
				d = map.phi<21>(d);
			} while(d!=centerDart);


968
969

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

973
		//apply force to each vertex
974
		/*
975
976
		d = groundFace;
		for (unsigned int i = 0; i < nbVertices; ++i)
pitiot's avatar
pitiot committed
977
		{
Thomas Jund's avatar
Thomas Jund committed
978
			velocity[d] += forces[d] * sim_->timeStep_;
979
980
981
982
			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
983
		}
984
		*/
985
	}
pitiot's avatar
pitiot committed
986
987
	else
	{
988
989
990
991
992
993
	//	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
994
#ifdef TWO_AND_HALF_DIM
pitiot's avatar
up    
pitiot committed
995
996
997
998
999
1000
1001
1002
1003
1004

			//// 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
1005
1006
1007
#else
			position[d] += rotate2D(position[d], center, abs_angle*rotor);
#endif
1008
1009
			position[d] += (velocity_ * sim_->timeStep_);

1010
#ifdef EXPORTING_BOXES
1011
1012

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

1017
1018
#endif

1019
1020
1021
//			position[map.phi<211>(d)] += (velocity_ * sim_->timeStep_);

			bary += position[d];
Thomas Jund's avatar
Thomas Jund committed
1022
1023


1024
1025
1026
			map.next(d);
		}

pitiot's avatar
pitiot committed
1027
		front=(position[0] + position[1]) / 2;
1028
1029
1030
1031
		if(angle >0)
			angle -= rotor;
		else
			angle += rotor;
pitiot's avatar
pitiot committed
1032
	}
pitiot's avatar
maj    
pitiot committed
1033

pitiot's avatar
pitiot committed
1034
	center = bary / nbVertices;
pitiot's avatar
maj    
pitiot committed
1035

1036
1037
	//computing deformation vector on vertices :
	//depending on the velocity, extend the perceived shape
1038
	VEC3 vel = velocity_.normalized();
1039
1040
1041

	d = groundFace;
	for (unsigned int i = 0; i < nbVertices; ++i)
1042
	{
1043
		VEC3 v(position[d] - center);
1044
1045
		VEC3 vN = v.normalized();
		float dot = vN * vel;
1046
1047
		deformation[d] = v * 2.0f * (dot+1.0f)/2.0f;
		map.next(d);
1048
1049
	}

Thomas Jund's avatar
Thomas Jund committed
1050
1051
//	if(!rigid_)
//		center = position[groundFace];
1052
1053
1054
}

//-------------------------------------------------------------------------
1055

1056
1057
void MovingObstacle::applyForces()
{
pitiot's avatar
pitiot committed
1058
1059

	Dart centerDart =map.phi<11>(groundFace);
1060
1061
1062

	if(!rigid_)
	{
pitiot's avatar
pitiot committed
1063
		Dart d = centerDart;
1064
1065
		for (unsigned int i = 0; i < nbVertices; ++i)
		{
pitiot's avatar
pitiot committed
1066
1067
1068
1069
1070
1071
1072
1073
1074
			Dart e = map.phi_1(d);
			velocity[e] += forces[e] * sim_->timeStep_;
			position[e] += (velocity[e] * sim_->timeStep_);





			d=map.phi<21>(d);
1075
		}
pitiot's avatar
pitiot committed
1076
1077
1078
1079
1080
1081
1082
1083

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





1084
// MAJ des obstacles
1085

pitiot's avatar
pitiot committed
1086

pitiot's avatar
pitiot committed
1087
	}
1088
1089
1090



1091
1092


pitiot's avatar
pitiot committed
1093
}
1094

pitiot's avatar
pitiot committed
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
1154
1155
1156
1157
1158
void MovingObstacle::updateRegistration()
{
	PFP::VEC3 bary(0);
		Dart centerDart =map.phi<11>(groundFace);

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


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

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

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

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


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

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

	// MAJ des obstacles


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


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

				#ifdef TWO_AND_HALF_DIM


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

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

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

pitiot's avatar
pitiot committed
1164

1165
1166
		o->prevP = getDilatedPosition((i - 1 + nbVertices) % nbVertices);
		o->nextP = getDilatedPosition((i + 2 + nbVertices) % nbVertices);
pitiot's avatar
pitiot committed
1167

Thomas Jund's avatar
Thomas Jund committed
1168

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

pitiot's avatar
pitiot committed
1172
		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
1173
		{
Thomas Jund's avatar
Thomas Jund committed
1174
			sim_->envMap_.popAndPushObstacleInCells(o,i);
pitiot's avatar
pitiot committed
1175
		}
Thomas Jund's avatar
Thomas Jund committed
1176

pitiot's avatar
pitiot committed
1177
1178
1179
1180
1181
1182
1183
1184
		/////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
1185
1186
		//		CGoGNout << "Apres une etape : Obstacle "<< i << CGoGNendl;
	}
pitiot's avatar
pitiot committed
1187
1188
1189
1190
1191
1192
1193
1194
	/////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
1195
1196
1197
	//
	//		envMap_->addMovingObstAsNeighbor(this,general_belonging,&(general_neighbors));
	//		CGoGNout << "nouvel update : "<< CGoGNendl;
1198
1199
	if(sim_->detect_agent_collision)
	{
pitiot's avatar
pitiot committed
1200
		for (std::vector< std::pair<Dart, int> >::iterator it2 = general_belonging.begin(); it2 != general_belonging.end(); ++ it2)
1201
		{
pitiot's avatar
pitiot committed
1202
			std::vector<Agent*> vector =sim_->envMap_.agentvect[(*it2).first];
1203
1204
1205
1206
1207
1208
1209
1210
1211
1212
1213
			for(std::vector<Agent*>::iterator it=vector.begin();it!=vector.end(); ++it)
			{
				if (this->is_inside((*it)->part_.getPosition()))
				{
					(*it)->alive=false;
					(*it)->color1=0.0f;
					(*it)->color2=0.4f;
					(*it)->color3=0.0f;
					sim_->envMap_.popAgentInCells(*it,(*it)->part_.d);
				}
			}
pitiot's avatar
maj    
pitiot committed
1214

1215
1216
		}
	}
pitiot's avatar
maj    
pitiot committed
1217
1218
}

1219
//-------------------------------------------------------------------------
Thomas Jund's avatar
Thomas Jund committed
1220

pitiot's avatar
pitiot committed
1221
std::vector<Dart> MovingObstacle::getMemoCross(const VEC3& pos, const VEC3& dest, Dart& d1, Dart& d2)
pitiot's avatar
maj    
pitiot committed
1222
{
1223
1224
1225
#ifdef TWO_AND_HALF_DIM
	CGoGN::Algo::Surface::MovingObjects::ParticleCell2DAndHalfMemo<PFP> * registering_part = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2DAndHalfMemo<PFP>(sim_->envMap_.map, d1,pos,sim_->envMap_.position);
#else
pitiot's avatar
pitiot committed
1226
	CGoGN::Algo::Surface::MovingObjects::ParticleCell2DMemo<PFP> * registering_part = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2DMemo<PFP>(sim_->envMap_.map, d1,pos,sim_->envMap_.position);
1227
#endif
pitiot's avatar
pitiot committed
1228
1229
	std::vector<Dart> result =(registering_part->move(dest));
	d2=registering_part->d;
1230
//	CGoGNout<<"d1 : "<< *d1<<"|| d2 : "<< *d2<<"|| start : "<< pos<<"|| stop : "<< dest<<CGoGNendl;
pitiot's avatar
pitiot committed
1231
	return result;
pitiot's avatar
maj    
pitiot committed
1232
1233
}

1234
void resetPartSubdiv(Obstacle* o)
pitiot's avatar
maj    
pitiot committed
1235
1236
{
	MovingObstacle * mo = o->mo;
1237

pitiot's avatar
maj    
pitiot committed
1238
1239
	if (mo != NULL)
	{
pitiot's avatar
up    
pitiot committed
1240
1241
		unsigned int n =o->index;
		unsigned int n2 =(n+1)%(mo->nbVertices);
pitiot's avatar
merging    
pitiot committed
1242

pitiot's avatar
up    
pitiot committed
1243
1244
		VEC3 pos = mo->parts_[n]->getPosition();
		VEC3 pos2 = mo->parts_[n2]->getPosition();
pitiot's avatar
pitiot committed
1245
		mo->parts_[n]->CGoGN::Algo::MovingObjects::ParticleBase<PFP>::move(Algo::Surface::Geometry::faceCentroid<PFP>(mo->sim_->envMap_.map, mo->parts_[n]->d, mo->sim_->envMap_.position)) ;
Arash HABIBI's avatar
Arash HABIBI committed
1246

pitiot's avatar
pitiot committed
1247
1248
1249
		mo->parts_[n]->setState(FACE) ;
		mo->parts_[n]->move(pos) ;

pitiot's avatar
up    
pitiot committed
1250
1251
1252
1253
1254
		mo->parts_[n2]->CGoGN::Algo::MovingObjects::ParticleBase<PFP>::move(Algo::Surface::Geometry::faceCentroid<PFP>(mo->sim_->envMap_.map, mo->parts_[n]->d, mo->sim_->envMap_.position)) ;

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

pitiot's avatar
pitiot committed
1255
		mo->dDir = mo->parts_[0]->d;
pitiot's avatar
maj    
pitiot committed
1256
1257
	}
}
pitiot's avatar
up    
pitiot committed
1258
//, unsigned int fLevel
pitiot's avatar
maj    
pitiot committed
1259
1260
1261
void resetObstPartInFace(Obstacle* o, Dart d1)
{
	MovingObstacle * mo = o->mo;
1262

Thomas Jund's avatar
Thomas Jund committed
1263
1264
	if (mo != NULL)
	{