moving_obstacle.cpp 40.1 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
150
151
#ifdef TWO_AND_HALF_DIM
	parts_ = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2DAndHalf<PFP>*[nbVertices];
#else
pitiot's avatar
pitiot committed
152
	parts_ = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2D<PFP>*[nbVertices];
153
154
#endif

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

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

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

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

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

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

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

217
218
	groundFace = map.newFace(nbVertices);

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

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

236

pitiot's avatar
pitiot committed
237
	}
238

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

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

Thomas Jund's avatar
Thomas Jund committed
258
259
260
		DartMarker treated(map);
		for(Dart d = map.begin() ; d != map.end() ; map.next(d))
		{
261
			if(!map.isBoundaryMarked2(d) && !treated.isMarked(d))
Thomas Jund's avatar
Thomas Jund committed
262
263
			{
				treated.mark(d);
264
				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
265
266
267
			}
		}

268
		map.enableQuickTraversal<VERTEX>();
269

Thomas Jund's avatar
Thomas Jund committed
270
		dDir=dInside;
pitiot's avatar
pitiot committed
271
	}
272

pitiot's avatar
merging    
pitiot committed
273

274

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

287
288
289
290
291
292
293
294
295
296
297
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
298
	m_shader->setColor(Geom::Vec4f(0.,1.,0.,0.));
299
300
301
302
303
304
305
306
307
308
//	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
309
310

	m_ds = new Utils::Drawer();
311
312
}

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

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

pitiot's avatar
up    
pitiot committed
323
324
//	if(movingObstacleNeighbors_.size()==0)
	if(index==12)
Arash HABIBI's avatar
Arash HABIBI committed
325
	// if(obstacleNeighbors_.size()==0)
326
327
328
		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
329

330
331
332
333
	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
334
335
336
337
338
339
340
341

	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
342
343
		m_ds->vertex(center);
		for(unsigned int i = 0 ; i < goals_.size() ; i++)
Thomas Jund's avatar
Thomas Jund committed
344
		{
pitiot's avatar
up    
pitiot committed
345
			m_ds->vertex(goals_[(curGoal_+i)%(goals_.size())]);
Thomas Jund's avatar
Thomas Jund committed
346
347
348
349
350
		}

		m_ds->end();
		m_ds->endList();
	}
351
352
}

353
354
VEC3 MovingObstacle::getDilatedPosition(unsigned int ind)
{
355
	Dart d(ind); //WARNING : works only for one face created at start !
pitiot's avatar
pitiot committed
356
#ifndef TWO_AND_HALF_DIM
pitiot's avatar
up    
pitiot committed
357
358
359
360
361
362
//	return position[d]+deformation[d];
	if(!rigid_)
		{
			return position[d];
		}
	else return position[d]+deformation[d];
pitiot's avatar
pitiot committed
363
#else
Arash HABIBI's avatar
Arash HABIBI committed
364
	return position[d];
pitiot's avatar
pitiot committed
365
#endif
366
367
368
369
}

VEC3 MovingObstacle::getPosition(unsigned int ind)
{
370
371
	Dart d(ind);
	return position[d];
372
373
}

pitiot's avatar
maj    
pitiot committed
374
375
376
377
bool MovingObstacle::test_opposition(VEC3 o, VEC3 p1, VEC3 p2)
{
	o.normalize();
	o *= -1;
378
379
380
	VEC3 vector(p1 - p2);
	vector.normalize();
	return (o - vector).norm2() < 0.1;
pitiot's avatar
maj    
pitiot committed
381
382
383
}

// TODO Check position
pitiot's avatar
pitiot committed
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
//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
438

pitiot's avatar
pitiot committed
439
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
440
{
Thomas Jund's avatar
Thomas Jund committed
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
	//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
494
495
496
497
498
//	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
499
//	{
pitiot's avatar
pitiot committed
500
501
502
503
504
505
//		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
506
//
pitiot's avatar
pitiot committed
507
508
509
510
//		for (std::vector<Agent*>::const_iterator it = agents.begin(); it != agents.end(); ++it)
//		{
//			if ((*it)->alive)
//			{
pitiot's avatar
pitiot committed
511
//
pitiot's avatar
pitiot committed
512
513
514
515
//				float distSq = (center - (*it)->getPosition()).norm2() ;
//				if ((agentNeighbors_.size() < maxNeighbors_ || distSq < maxDist)
//					&& distSq < neighborDistSq_)
//				{
pitiot's avatar
pitiot committed
516
//
pitiot's avatar
pitiot committed
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
//					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
552
//	}
pitiot's avatar
pitiot committed
553
554
555
556
}

void MovingObstacle::updateObstacleNeighbors() // obstacles voisins , distance par rapport aux centres des segments// a mettre en place si besoin
{
557
558
	obstacleNeighbors_.clear() ;
	movingObstacleNeighbors_.clear() ;
pitiot's avatar
pitiot committed
559
	for (std::vector< std::pair<Dart, int> >::iterator it2 = general_belonging.begin();it2 != general_belonging.end(); ++it2)
560
	{
pitiot's avatar
pitiot committed
561
562
		std::vector<Obstacle*>& obst = sim_->envMap_.obstvect[(*it2).first] ;
		std::vector<Obstacle*>& neighborObst = sim_->envMap_.neighborObstvect[(*it2).first] ;
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
//		float maxDistObst = 0.0f ;
//		float maxDistMovingObst = 0.0f ;
		float distance_detection=2.5* gravity_dist;
		float distance_detectionSq=distance_detection*distance_detection;
		for(std::vector<Obstacle*>::const_iterator it = obst.begin() ; it != obst.end() ; ++it)
		{
			if ((*it)->mo==NULL)
			{
				float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, center) ;
				if (/*(obstacleNeighbors_.size() < maxMovingObstacles_|| distSq < maxDistObst)
						&&*/ distSq < distance_detectionSq)
				{


//						if (distSq > maxDistObst)
//							maxDistObst = distSq ;
						obstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;



				}
			}
			else
			{
				if((*it)->mo->index!=index)
				{
					float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, center) ;
					if (/*(movingObstacleNeighbors_.size() < maxMovingObstacles_ || distSq < maxDistMovingObst) &&*/
							 distSq < distance_detectionSq)
					{

//						if (distSq > maxDistMovingObst)
//							maxDistMovingObst = distSq ;
						movingObstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;



					}

				}
			}
		}

		for(std::vector<Obstacle*>::const_iterator it = neighborObst.begin() ; it != neighborObst.end() ; ++it)
		{
			if ((*it)->mo==NULL)
			{
				float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, center) ;
				if (/*(obstacleNeighbors_.size() < maxMovingObstacles_|| distSq < maxDistObst)
						&&*/ distSq < distance_detectionSq)
				{


//						if (distSq > maxDistObst)
//							maxDistObst = distSq ;
						obstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;



				}
			}
			else
			{
				if((*it)->mo->index!=index)
				{
					float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, center) ;
					if (/*(movingObstacleNeighbors_.size() < maxMovingObstacles_ || distSq < maxDistMovingObst) &&*/
							 distSq < distance_detectionSq)
					{

//						if (distSq > maxDistMovingObst)
//							maxDistMovingObst = distSq ;
						movingObstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
					}
				}
			}
		}
	}
pitiot's avatar
maj    
pitiot committed
641
642
}

Arash HABIBI's avatar
unidir    
Arash HABIBI committed
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
//-------------------------------------------------------------

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()
{
742
	Dart dd = groundFace;
Arash HABIBI's avatar
unidir    
Arash HABIBI committed
743
	if(!rigid_)
744
		for (unsigned int i = 0; i < nbVertices; ++i)
Arash HABIBI's avatar
unidir    
Arash HABIBI committed
745
746
747
748
749
750
751
		{
			//initialisation of forces
			forces[dd] = VEC3(0.0);
			map.next(dd);
		}
}

Arash HABIBI's avatar
Arash HABIBI committed
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
791
792

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

VEC3 computeForce(VEC3 p, VEC3 p1, VEC3 p2, float obst_radius_infl, float obst_power, float obst_stiffness)
{
	double force_value=0.0;
	double longueur2 = (p1-p2).norm2();
	double rest_sum_of_dists = 2 * sqrt(obst_radius_infl*obst_radius_infl + longueur2/4);

	double d1 = (p-p1).norm();
	double d2 = (p-p2).norm();
	double sum_of_dists = d1+d2;
	if(sum_of_dists < rest_sum_of_dists)
	{
		double collision_softening_factor = pow(1-sum_of_dists/rest_sum_of_dists,obst_power);
		force_value = obst_stiffness*collision_softening_factor*(rest_sum_of_dists - sum_of_dists);
		VEC3 v_obst = p2 - p1;
		VEC3 normal = VEC3(v_obst[1],-v_obst[0],0);
		// Ajouter une composante tangentielle

		normal += v_obst * ((d1-d2)/sum_of_dists);
		// normal += v_obst * ((d1-d2)/(5*sum_of_dists));
		// Le facteur 5 est là seulement pour diminuer l'influence de la composante tangentielle
		normal.normalize();

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


Arash HABIBI's avatar
unidir    
Arash HABIBI committed
793
794
//-------------------------------------------------------------

795
void MovingObstacle::updateForces()
pitiot's avatar
maj    
pitiot committed
796
{
pitiot's avatar
pitiot committed
797
	assert(sim_->envMap_.map.getCurrentLevel() == sim_->envMap_.map.getMaxLevel()) ;
pitiot's avatar
pitiot committed
798

pitiot's avatar
pitiot committed
799
	//pour les tests de détection///////////////
pitiot's avatar
merging    
pitiot committed
800
801
802
803
804
805
806
	if(!seen)
	{
		color1=1.0f;
		color2=1.0f;
		color3=1.0f;
	}
	seen=false;
Thomas Jund's avatar
merge    
Thomas Jund committed
807

pitiot's avatar
pitiot committed
808
	//////////////////////////
Thomas Jund's avatar
merge    
Thomas Jund committed
809

810
	PFP::VEC3 bary(0);
pitiot's avatar
maj    
pitiot committed
811
812

	Dart d;
pitiot's avatar
pitiot committed
813
814
	velocity_[0] = newVelocity_[0] * velocity_factor;
	velocity_[1] = newVelocity_[1] * velocity_factor;
pitiot's avatar
pitiot committed
815
	velocity_[2] = newVelocity_[2] * velocity_factor;
Arash HABIBI's avatar
Arash HABIBI committed
816
817
	// velocity_[0] = 0.0;
	// velocity_[1] = 0.0;
pitiot's avatar
maj    
pitiot committed
818
819

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

pitiot's avatar
merging    
pitiot committed
822
	float rotor=0;
823

pitiot's avatar
merging    
pitiot committed
824
825
826
827
	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
828

Arash HABIBI's avatar
03_2013    
Arash HABIBI committed
829
830
	// masse ressort pour la limace

831
	if(!rigid_)
pitiot's avatar
maj    
pitiot committed
832
	{
833
		Dart d = groundFace;
834
		// map.next(d);
pitiot's avatar
maj    
pitiot committed
835

Thomas Jund's avatar
Thomas Jund committed
836
		Dart dd =d;
837
		for (unsigned int i = 0; i < nbVertices; ++i)
Thomas Jund's avatar
Thomas Jund committed
838
839
		{
			//initialisation of forces with viscosity
840
			forces[dd] += -0.9f*velocity[dd];
Arash HABIBI's avatar
Arash HABIBI committed
841
			// forces[dd] = VEC3(0.0);
Thomas Jund's avatar
Thomas Jund committed
842
843
844
			map.next(dd);
		}

845
		CellMarkerStore<EDGE> cm(map);
Arash HABIBI's avatar
Arash HABIBI committed
846
		CellMarkerStore<VERTEX> cmV(map);
Thomas Jund's avatar
Thomas Jund committed
847
		DartMarkerStore dm(map);
Arash HABIBI's avatar
03_2013    
Arash HABIBI committed
848
		// ARASH : On parcourt les sommets de la grande face
849
		for (unsigned int i = 0; i < nbVertices; ++i)
850
851
		{
			Dart dd = d;
Arash HABIBI's avatar
03_2013    
Arash HABIBI committed
852
853

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

855
856
857
858
			do {
				if(!cm.isMarked(dd))
				{
					cm.mark(dd);
Thomas Jund's avatar
Thomas Jund committed
859
860
					VEC3 p1Next = position[map.phi1(dd)]+(velocity[map.phi1(dd)] * sim_->timeStep_);
					VEC3 p2Next = position[dd]+(velocity[dd] * sim_->timeStep_);
Arash HABIBI's avatar
03_2013    
Arash HABIBI committed
861
					// p1Next et p2Next sont la position des extremites de l'arete.
Thomas Jund's avatar
Thomas Jund committed
862
					VEC3 v1 = (p1Next-p2Next);
863
864
865

					//stretch spring : /!\ max rigidity relative to the timestep used (unstable otherwise)
					float norm = v1.norm();
866
					float rigidity = 30.0f;
867

868
					float stretch = rigidity*(edgeLength[dd]-v1.norm());
869
870
					if(norm>0.0f)
					{
Arash HABIBI's avatar
Arash HABIBI committed
871
						VEC3 f = 1.5*stretch*(v1/norm);
872

873
874
875
876
						//apply force symmetrically
						forces[dd] -= f;
						forces[map.phi1(dd)] += f;
					}
Arash HABIBI's avatar
Arash HABIBI committed
877
878
				}

Thomas Jund's avatar
Thomas Jund committed
879
880
881
				dd = map.phi1(dd);
			} while(dd!=d);

Arash HABIBI's avatar
03_2013    
Arash HABIBI committed
882
			// ARASH : les ressorts angulaires
Thomas Jund's avatar
Thomas Jund committed
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
			do {
				if(!dm.isMarked(dd))
				{
					dm.mark(dd);
//					VEC3 v1 = (position[map.phi1(dd)]-position[dd]);
					VEC3 p1Next = position[map.phi1(dd)]+(velocity[map.phi1(dd)] * sim_->timeStep_);
					VEC3 p2Next = position[dd]+(velocity[dd] * sim_->timeStep_);
					VEC3 v1 = (p1Next-p2Next);

					float norm = v1.norm();

					//bend spring: /!\ max rigidity relative to the timestep used (unstable otherwise)
					float restAngle = vertexAngle[dd];
					if(restAngle!=0.0f)
					{
Thomas Jund's avatar
Thomas Jund committed
898
						float angularRig = 70.0f;
Thomas Jund's avatar
Thomas Jund committed
899

900
901
902
903
904
905
906
907
908
909
						float curAngle = Algo::Surface::Geometry::angle<PFP>(map, map.phi_1(dd),map.phi2(map.phi1(dd)),position);

						VEC3 v1 = Algo::Surface::Geometry::vectorOutOfDart<PFP>(map, map.phi_1(dd), position);
						VEC3 v2 = Algo::Surface::Geometry::vectorOutOfDart<PFP>(map, map.phi2(map.phi1(dd)), position);

						VEC3 v3 = v1 ^ v2;
						VEC3 vPl = Algo::Surface::Geometry::faceNormal<PFP>(map, dd, position);

						if(v3 * vPl < 0.0f)
							curAngle *= -1.0f;
910

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

913
914
915
916
917
918
919
						if(norm>0.0f && curAngle==curAngle) //test to avoid NaN
						{
							VEC3 f = angularStretch*(v1/norm);

							forces[dd] += f;
							forces[map.phi1(dd)] -= f;
						}
Thomas Jund's avatar
Thomas Jund committed
920
					}
921
				}
pitiot's avatar
maj    
pitiot committed
922

923
924
				dd = map.phi1(dd);
			} while(dd!=d);
pitiot's avatar
maj    
pitiot committed
925

Arash HABIBI's avatar
03_2013    
Arash HABIBI committed
926
927
928
929
			//-------------------------------------------------------------------------
			// ARASH : A présent on calcule les interactions avec les autres obstacles.

			do {
Arash HABIBI's avatar
Arash HABIBI committed
930
				if(!cmV.isMarked(dd))
Arash HABIBI's avatar
03_2013    
Arash HABIBI committed
931
				{
Arash HABIBI's avatar
Arash HABIBI committed
932
					cmV.mark(dd);
Arash HABIBI's avatar
03_2013    
Arash HABIBI committed
933

Arash HABIBI's avatar
comment    
Arash HABIBI committed
934
					VEC3 norm;
Arash HABIBI's avatar
Arash HABIBI committed
935
936
					double obst_stiffness = 5.0; // agent-obstacle interaction stiffness
					int obst_power = 2  ;           // the power to which elevate the agent-obstacle distance
Arash HABIBI's avatar
comment    
Arash HABIBI committed
937
938
939
940
941

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

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

Arash HABIBI's avatar
Arash HABIBI committed
946
947
					// Evitement d'obstacles mobiles

Arash HABIBI's avatar
03_2013    
Arash HABIBI committed
948
949
950
951
952
953
					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
954

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

Arash HABIBI's avatar
Arash HABIBI committed
958
					// Evitement d'obstacles fixes
Arash HABIBI's avatar
03_2013    
Arash HABIBI committed
959

Arash HABIBI's avatar
Arash HABIBI committed
960
961
962
963
964
965
966
967
					for(std::vector<std::pair<float, Obstacle*> >::iterator it = obstacleNeighbors_.begin() ;
						it != obstacleNeighbors_.end() ; ++it)
					{
						Obstacle * obst = it->second;
						VEC3 p1=obst->p1 ;
						VEC3 p2=obst->p2 ;

						forces[dd] += computeForce(p,p1,p2,obst_radius_infl,obst_power,10*obst_stiffness);
Arash HABIBI's avatar
03_2013    
Arash HABIBI committed
968
969
970
971
972
					}
				}
				dd = map.phi1(dd);
			} while(dd!=d);

973
974
975
976
			map.next(d);
		}

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

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

			//// 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
1012
1013
1014
#else
			position[d] += rotate2D(position[d], center, abs_angle*rotor);
#endif
1015
1016
			position[d] += (velocity_ * sim_->timeStep_);

1017
#ifdef EXPORTING_BOXES
1018
1019

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

1024
1025
#endif

1026
1027
1028
//			position[map.phi<211>(d)] += (velocity_ * sim_->timeStep_);

			bary += position[d];
Thomas Jund's avatar
Thomas Jund committed
1029
1030


1031
1032
1033
			map.next(d);
		}

pitiot's avatar
pitiot committed
1034
		front=(position[0] + position[1]) / 2;
1035
1036
1037
1038
		if(angle >0)
			angle -= rotor;
		else
			angle += rotor;
pitiot's avatar
pitiot committed
1039
	}
pitiot's avatar
maj    
pitiot committed
1040

pitiot's avatar
pitiot committed
1041
	center = bary / nbVertices;
pitiot's avatar
maj    
pitiot committed
1042

1043
1044
	//computing deformation vector on vertices :
	//depending on the velocity, extend the perceived shape
1045
	VEC3 vel = velocity_.normalized();
1046
1047
1048

	d = groundFace;
	for (unsigned int i = 0; i < nbVertices; ++i)
1049
	{
1050
		VEC3 v(position[d] - center);
1051
1052
		VEC3 vN = v.normalized();
		float dot = vN * vel;
1053
1054
		deformation[d] = v * 2.0f * (dot+1.0f)/2.0f;
		map.next(d);
1055
1056
	}

Thomas Jund's avatar
Thomas Jund committed
1057
1058
//	if(!rigid_)
//		center = position[groundFace];
1059
1060
1061
}

//-------------------------------------------------------------------------
1062

1063
1064
1065
1066
1067
1068
1069
1070
1071
1072
1073
1074
1075
1076
1077
1078
1079
1080
void MovingObstacle::applyForces()
{
	PFP::VEC3 bary(0);

	if(!rigid_)
	{
		Dart d = groundFace;
		for (unsigned int i = 0; i < nbVertices; ++i)
		{
			velocity[d] += forces[d] * sim_->timeStep_;
			position[d] += (velocity[d] * sim_->timeStep_);
			position[map.phi<211>(d)] += (velocity[d] * sim_->timeStep_);
			bary += position[d];
			map.next(d);
		}
		center = bary / nbVertices;
	}
// MAJ des obstacles
pitiot's avatar
maj    
pitiot committed
1081
1082
	for (unsigned int i = 0; i < nbVertices; ++i)
	{
1083
#ifdef TWO_AND_HALF_DIM
1084

pitiot's avatar
pitiot committed
1085
1086

//			std::cout << " dist prevu " << (getDilatedPosition(i) - parts_[i]->getPosition()).norm() << std::endl;
1087

1088
#endif
1089

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

1092
#ifdef TWO_AND_HALF_DIM
pitiot's avatar
pitiot committed
1093
//			std::cout << " dist " << parts_[i]->getDistance() << std::endl;
1094

1095
			Dart d(i);
pitiot's avatar
pitiot committed
1096
1097
1098
			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;
1099

pitiot's avatar
pitiot committed
1100
			position[map.phi<112>(d)] = position[d]+normal;
1101
1102
#endif

pitiot's avatar
maj    
pitiot committed
1103
1104
		//		CGoGNout << "avant une etape : Obstacle "<< i << CGoGNendl;
		Obstacle* o = obstacles_[i];
1105
1106
		o->p1 = getDilatedPosition(i);
		o->p2 = getDilatedPosition((i+1) % nbVertices);
Thomas Jund's avatar
Thomas Jund committed
1107

pitiot's avatar
pitiot committed
1108

1109
1110
		o->prevP = getDilatedPosition((i - 1 + nbVertices) % nbVertices);
		o->nextP = getDilatedPosition((i + 2 + nbVertices) % nbVertices);
pitiot's avatar
pitiot committed
1111

Thomas Jund's avatar
Thomas Jund committed
1112

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

pitiot's avatar
pitiot committed
1116
		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
1117
		{
Thomas Jund's avatar
Thomas Jund committed
1118
			sim_->envMap_.popAndPushObstacleInCells(o,i);
pitiot's avatar
pitiot committed
1119
		}
Thomas Jund's avatar
Thomas Jund committed
1120

pitiot's avatar
pitiot committed
1121
1122
1123
1124
1125
1126
1127
1128
		/////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
1129
1130
		//		CGoGNout << "Apres une etape : Obstacle "<< i << CGoGNendl;
	}
pitiot's avatar
pitiot committed
1131
1132
1133
1134
1135
1136
1137
1138
	/////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
1139
1140
1141
	//
	//		envMap_->addMovingObstAsNeighbor(this,general_belonging,&(general_neighbors));
	//		CGoGNout << "nouvel update : "<< CGoGNendl;
1142
1143
	if(sim_->detect_agent_collision)
	{
pitiot's avatar
pitiot committed
1144
		for (std::vector< std::pair<Dart, int> >::iterator it2 = general_belonging.begin(); it2 != general_belonging.end(); ++ it2)
1145
		{
pitiot's avatar
pitiot committed
1146
			std::vector<Agent*> vector =sim_->envMap_.agentvect[(*it2).first];
1147
1148
1149
1150
1151
1152
1153
1154
1155
1156
1157
			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
1158

1159
1160
		}
	}
pitiot's avatar
maj    
pitiot committed
1161
1162
}

1163
//-------------------------------------------------------------------------
Thomas Jund's avatar
Thomas Jund committed
1164

pitiot's avatar
pitiot committed
1165
std::vector<Dart> MovingObstacle::getMemoCross(const VEC3& pos, const VEC3& dest, Dart& d1, Dart& d2)
pitiot's avatar
maj    
pitiot committed
1166
{
1167
1168
1169
#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
1170
	CGoGN::Algo::Surface::MovingObjects::ParticleCell2DMemo<PFP> * registering_part = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2DMemo<PFP>(sim_->envMap_.map, d1,pos,sim_->envMap_.position);
1171
#endif
pitiot's avatar
pitiot committed
1172
1173
	std::vector<Dart> result =(registering_part->move(dest));
	d2=registering_part->d;
1174
//	CGoGNout<<"d1 : "<< *d1<<"|| d2 : "<< *d2<<"|| start : "<< pos<<"|| stop : "<< dest<<CGoGNendl;
pitiot's avatar
pitiot committed
1175
	return result;
pitiot's avatar
maj    
pitiot committed
1176
1177
}

1178
void resetPartSubdiv(Obstacle* o)
pitiot's avatar
maj    
pitiot committed
1179
1180
{
	MovingObstacle * mo = o->mo;
1181

pitiot's avatar
maj    
pitiot committed
1182
1183
	if (mo != NULL)
	{
pitiot's avatar
up    
pitiot committed
1184
1185
		unsigned int n =o->index;
		unsigned int n2 =(n+1)%(mo->nbVertices);
pitiot's avatar
merging    
pitiot committed
1186

pitiot's avatar
up    
pitiot committed
1187
1188
		VEC3 pos = mo->parts_[n]->getPosition();
		VEC3 pos2 = mo->parts_[n2]->getPosition();
pitiot's avatar
pitiot committed
1189
		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
1190

pitiot's avatar
pitiot committed
1191
1192
1193
		mo->parts_[n]->setState(FACE) ;
		mo->parts_[n]->move(pos) ;

pitiot's avatar
up    
pitiot committed
1194
1195
1196
1197
1198
		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
1199
		mo->dDir = mo->parts_[0]->d;
pitiot's avatar
maj    
pitiot committed
1200
1201
	}
}
pitiot's avatar
up    
pitiot committed
1202
//, unsigned int fLevel
pitiot's avatar
maj    
pitiot committed
1203
1204
1205
void resetObstPartInFace(Obstacle* o, Dart d1)
{
	MovingObstacle * mo = o->mo;
1206

Thomas Jund's avatar
Thomas Jund committed
1207
1208
	if (mo != NULL)
	{
pitiot's avatar
pitiot committed
1209
		unsigned int n =o->index;
pitiot's avatar
up    
pitiot committed
1210
		unsigned int n2 =(n+1)%(mo->nbVertices);
pitiot's avatar
pitiot committed
1211
		VEC3 pos1 = mo->parts_[n]->getPosition();
pitiot's avatar
up    
pitiot committed
1212
1213
1214
		VEC3 pos2 = mo->parts_[n2]->getPosition();
//		if (Algo::Surface::Geometry::isPointInConvexFace2D <PFP> (mo->sim_->envMap_.map, d1, mo->sim_->envMap_.position, pos1, true))
		if ( mo->sim_->envMap_.is_insideConvexCell2D(pos1, d1))
pitiot's avatar
pitiot committed
1215
			mo->parts_[n]->d = d1;
pitiot's avatar
up    
pitiot committed
1216
1217
1218
//		if (Algo::Surface::Geometry::isPointInConvexFace2D <PFP> (mo->sim_->envMap_.map, d1, mo->sim_->envMap_.position, pos2, true))
		if ( mo->sim_->envMap_.is_insideConvexCell2D(pos2, d1))
					mo->parts_[n2]->d = d1;
pitiot's avatar
pitiot committed
1219
		if(n==0)
pitiot's avatar
pitiot committed
1220
			mo->dDir = mo->parts_[0]->d;
pitiot's avatar
maj    
pitiot committed
1221
	}
pitiot's avatar
up    
pitiot committed
1222
1223
1224
1225
1226
1227
1228
1229
1230
1231
1232
1233
1234
1235
1236
1237
1238
1239
1240
1241
1242
1243
1244
1245
//
//	MovingObstacle * mo = o->mo;
//
//	if (mo != NULL)
//	{
//
//		unsigned int n =o->index;
//		unsigned int n2 =(n+1)%mo->nbVertices;
//		Dart d2 =mo->sim_->envMap_.map.faceOldestDart(mo->parts_[n]->d);
//		Dart d3 =mo->sim_->envMap_.map.faceOldestDart(mo->parts_[n2]->d);
//		mo->sim_->envMap_.map.setCurrentLevel(fLevel - 1) ;
//		if ( mo->sim_->envMap_.map.sameFace(d1,d2))
//		{
//			CGoGNout<<"particule : "<< n<<" du MovingObstacle "<<mo->index<< " changée de " << mo->parts_[n]->d <<" en "<<d2<<CGoGNendl;
//			mo->parts_[n]->d = d2