moving_obstacle.cpp 23.6 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"
pitiot's avatar
maj    
pitiot committed
6

7
8
#include "Algo/Modelisation/triangulation.h"

Pierre Kraemer's avatar
Pierre Kraemer committed
9
//float MovingObstacle::neighborDistSq_ = 5.0f * 5.0f;
pitiot's avatar
maj    
pitiot committed
10

pitiot's avatar
pitiot committed
11
float MovingObstacle::maxSpeed_ = 2.0f;
pitiot's avatar
pitiot committed
12
13
float MovingObstacle::neighborDist_ = 10.0f ;
float MovingObstacle::neighborDistSq_ = neighborDist_ * neighborDist_ ;
pitiot's avatar
maj    
pitiot committed
14
15
16
17
18
19
float MovingObstacle::timeHorizonObst_ = 10.0f;
unsigned int MovingObstacle::maxNeighbors_ = 20;
float MovingObstacle::detectionFixedObst = 50;

bool MovingObstacle::is_inside(VEC3 p)
{
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
	return false;
//	VEC3 vec, norm;
//	Obstacle * obst;
//
//	for (unsigned int i = 0; i < nbVertices; i++)
//	{
//		vec = VEC3(getPosition(i+1 % nbVertices)- getPosition(i));
//		norm[0] = vec[1];
//		norm[1] = -vec[0];
//
//		vec = VEC3(getPosition(i+1 % nbVertices) -p);
//		if (vec*norm < 0)
//			return false;
//	}
//
//	return true;
pitiot's avatar
maj    
pitiot committed
36
37
}

pitiot's avatar
pitiot committed
38
float get_angle(VEC3 v1, VEC3 v2) //renvoie l'angle entre [- pi ; Pi] du v2 à v1
pitiot's avatar
maj    
pitiot committed
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
{
	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
55
56
57
58
59
60
61
		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
62
63
64
	return flo;
}

pitiot's avatar
pitiot committed
65
VEC3 rotate(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
66
67
68
69
70
71
72
73
74
{
	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;
}

75
76
77

MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, std::vector<VEC3> goals, bool rigid, bool spin, Dart dInside, ArticulatedObstacle * art, int indParent) :
		nbVertices(pos.size()),
78
		center(0),
pitiot's avatar
maj    
pitiot committed
79
		index(ind),
80
		goals_(goals),
81
82
83
84
85
86
		curGoal_(1),
		velocity_factor(0.8f),
		color1(1.0f),
		color2(1.0f),
		color3(1.0f),
		seen(false),
pitiot's avatar
maj    
pitiot committed
87
		newVelocity_(0),
pitiot's avatar
merging    
pitiot committed
88
		sim_(sim),
89
90
		rigid_(rigid),
		spinning(spin),
pitiot's avatar
merging    
pitiot committed
91
		parent(art),
Thomas Jund's avatar
Thomas Jund committed
92
		ag_(NULL),
93
		index_parent(indParent)
pitiot's avatar
maj    
pitiot committed
94
95
96
{
	assert(pos.size() > 2);

97
98
	if(dInside==NIL)
		dInside = sim_->envMap_.getBelongingCell(pos[0]);
pitiot's avatar
maj    
pitiot committed
99

100
	registering_part = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2DMemo<PFP>(sim_->envMap_.map, dInside, pos[0], sim_->envMap_.position);
pitiot's avatar
maj    
pitiot committed
101
102

	obstacles_ = new Obstacle*[nbVertices];
103
104
	belonging_cells = new std::vector<Dart>[nbVertices];
	neighbor_cells = new std::vector<Dart>[nbVertices];
105
106
107
108

	position = map.addAttribute<VEC3, VERTEX>("position") ;
	normal = map.addAttribute<VEC3, VERTEX>("normal") ;
	deformation = map.addAttribute<VEC3, VERTEX>("deformation") ;
109
110
111
112

	if(!rigid_)
	{
		velocity = map.addAttribute<VEC3, VERTEX>("velocity") ;
Thomas Jund's avatar
Thomas Jund committed
113
		forces = map.addAttribute<VEC3, VERTEX>("force") ;
114
		edgeLength = map.addAttribute<float, EDGE>("edgeLength") ;
Thomas Jund's avatar
Thomas Jund committed
115
		vertexAngle = map.addAttribute<float, DART>("vertexAngle") ;
116
	}
117
118
119

	groundFace = map.newFace(nbVertices);

pitiot's avatar
maj    
pitiot committed
120
121
	for (unsigned int i = 0; i < nbVertices; ++i)
	{
Thomas Jund's avatar
Thomas Jund committed
122
		Dart d = i;
123
124
		position[d] = pos[i];
		deformation[d] = VEC3(0);
125
126

		if(!rigid_)
Thomas Jund's avatar
Thomas Jund committed
127
		{
128
			velocity[d] = VEC3(0);
Thomas Jund's avatar
Thomas Jund committed
129
130
			forces[d] = VEC3(0);
		}
131

132
		center += pos[i];
pitiot's avatar
pitiot committed
133
	}
134
135
136
137
138

	if(!rigid_)
	{
		//extrude face to build a cage
		// compute edgeLength for mass-spring
Thomas Jund's avatar
Thomas Jund committed
139
		Algo::Surface::Modelisation::extrudeFace<PFP>(map, position, groundFace, 25.0f) ;
140
141
142
		map.fillHole(groundFace);
		groundFace = map.phi2(groundFace);

143
		Algo::Surface::Modelisation::EarTriangulation<PFP> et(map);
144
145
146
147
148
149
150
		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
151

Thomas Jund's avatar
Thomas Jund committed
152
153
154
		DartMarker treated(map);
		for(Dart d = map.begin() ; d != map.end() ; map.next(d))
		{
155
			if(!map.isBoundaryMarked2(d) && !treated.isMarked(d))
Thomas Jund's avatar
Thomas Jund committed
156
157
			{
				treated.mark(d);
158
				vertexAngle[d] = Algo::Surface::Geometry::angle<PFP>(map,map.phi_1(d),map.phi1(d),position);
Thomas Jund's avatar
Thomas Jund committed
159
160
161
			}
		}

162
		map.enableQuickTraversal<VERTEX>();
163

Thomas Jund's avatar
Thomas Jund committed
164
		dDir=dInside;
pitiot's avatar
pitiot committed
165
	}
166
167
168

	center /= nbVertices;
	front=(pos[1] + pos[2]) / 2;
Arash HABIBI's avatar
Arash HABIBI committed
169

pitiot's avatar
merging    
pitiot committed
170
	if (spinning && parent==NULL) //départ face à la cible en cas d'obstacles pouvant effectuer des rotations
pitiot's avatar
pitiot committed
171
172
173
174
	{
		angle = get_angle(goals_[curGoal_] - center,front  - center);
		for (unsigned int i = 0; i < nbVertices; ++i)
		{
175
			position[i]+=rotate(position[i], center, angle);
pitiot's avatar
pitiot committed
176
177
178

		}
		angle=0;
pitiot's avatar
maj    
pitiot committed
179
	}
180

pitiot's avatar
maj    
pitiot committed
181
182
	for (unsigned int i = 0; i < nbVertices; ++i)
	{
pitiot's avatar
pitiot committed
183

184
185
186
187
		Obstacle* o = new Obstacle(position[i],
								position[(i + 1) % nbVertices],
								position[(i - 1 + nbVertices) % nbVertices],
								position[(i + 2) % nbVertices], this, i);
pitiot's avatar
maj    
pitiot committed
188
189
		obstacles_[i] = o;

pitiot's avatar
pitiot committed
190
		sim_->envMap_.pushObstacleInCells(o);
pitiot's avatar
maj    
pitiot committed
191
192
	}

193
194
195
196
197
198
199
200
201
202
//	float pos_max = 0;
//
//	for (unsigned int i = 0; i < nbVertices; ++i)
//	{
//		float it = (center - pos[i]).norm();
//		if (it > pos_max)
//			pos_max = it;
//	}
//	gravity_dist = pos_max + 5.0f;
//	obstacle_range = 15.0f * 15.0f;
pitiot's avatar
maj    
pitiot committed
203
204
}

205
206
VEC3 MovingObstacle::getDilatedPosition(unsigned int ind)
{
207
	Dart d(ind); //WARNING : works only for one face created at start !
Thomas Jund's avatar
Thomas Jund committed
208
209
//	return position[d]+deformation[d];
	return position[d];
210
211
212
213
}

VEC3 MovingObstacle::getPosition(unsigned int ind)
{
214
215
	Dart d(ind);
	return position[d];
216
217
}

pitiot's avatar
maj    
pitiot committed
218
219
220
221
bool MovingObstacle::test_opposition(VEC3 o, VEC3 p1, VEC3 p2)
{
	o.normalize();
	o *= -1;
222
223
224
	VEC3 vector(p1 - p2);
	vector.normalize();
	return (o - vector).norm2() < 0.1;
pitiot's avatar
maj    
pitiot committed
225
226
227
}

// TODO Check position
pitiot's avatar
pitiot committed
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
//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
282

pitiot's avatar
pitiot committed
283
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
284
{
Thomas Jund's avatar
Thomas Jund committed
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
	//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
338
339
340
341
342
//	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
343
//	{
pitiot's avatar
pitiot committed
344
345
346
347
348
349
//		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
350
//
pitiot's avatar
pitiot committed
351
352
353
354
//		for (std::vector<Agent*>::const_iterator it = agents.begin(); it != agents.end(); ++it)
//		{
//			if ((*it)->alive)
//			{
pitiot's avatar
pitiot committed
355
//
pitiot's avatar
pitiot committed
356
357
358
359
//				float distSq = (center - (*it)->getPosition()).norm2() ;
//				if ((agentNeighbors_.size() < maxNeighbors_ || distSq < maxDist)
//					&& distSq < neighborDistSq_)
//				{
pitiot's avatar
pitiot committed
360
//
pitiot's avatar
pitiot committed
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
//					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
396
//	}
pitiot's avatar
pitiot committed
397
398
399
400
401
402
}

void MovingObstacle::updateObstacleNeighbors() // obstacles voisins , distance par rapport aux centres des segments// a mettre en place si besoin
{
//	obstacleNeighbors_.clear() ;
//	movingObstacleNeighbors_.clear() ;
pitiot's avatar
pitiot committed
403
//
pitiot's avatar
pitiot committed
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
//	std::vector<Obstacle*>& obst = sim_->envMap_.obstvect[part_.d] ;
//	std::vector<Obstacle*>& neighborObst = sim_->envMap_.neighborObstvect[part_.d] ;
//	for(std::vector<Obstacle*>::const_iterator it = obst.begin() ; it != obst.end() ; ++it)
//	{
//		float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, part_.getPosition()) ;
//		if (distSq < rangeSq_)
//		{
//			if (Geom::testOrientation2D(part_.getPosition(), (*it)->p1, (*it)->p2) == Geom::RIGHT)
//			{
//				if ((*it)->mo==NULL)
//					obstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
//				else
//				{
//					movingObstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
//				}
//			}
pitiot's avatar
pitiot committed
420
//
pitiot's avatar
pitiot committed
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
//		}
//	}
//	for(std::vector<Obstacle*>::const_iterator it = neighborObst.begin() ; it != neighborObst.end() ; ++it)
//	{
//		float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, part_.getPosition()) ;
//		if(distSq < rangeSq_)
//		{
//			if(Geom::testOrientation2D(part_.getPosition(), (*it)->p1, (*it)->p2) == Geom::RIGHT)
//			{
//				if ((*it)->mo==NULL)
//					obstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
//				else
//					movingObstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
//			}
//		}
//	}
pitiot's avatar
maj    
pitiot committed
437
438
439
440
}

void MovingObstacle::update()
{
pitiot's avatar
pitiot committed
441
	assert(sim_->envMap_.map.getCurrentLevel() == sim_->envMap_.map.getMaxLevel()) ;
442
443
444
	if(sim_->detect_agent_collision)
		general_belonging.clear();

pitiot's avatar
merging    
pitiot committed
445
446
447
448
449
450
451
	if(!seen)
	{
		color1=1.0f;
		color2=1.0f;
		color3=1.0f;
	}
	seen=false;
452
	PFP::VEC3 bary(0);
pitiot's avatar
maj    
pitiot committed
453
454

	Dart d;
pitiot's avatar
pitiot committed
455
456
	velocity_[0] = newVelocity_[0] * velocity_factor;
	velocity_[1] = newVelocity_[1] * velocity_factor;
Arash HABIBI's avatar
Arash HABIBI committed
457
458
	// velocity_[0] = 0.0;
	// velocity_[1] = 0.0;
pitiot's avatar
maj    
pitiot committed
459
460

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

pitiot's avatar
merging    
pitiot committed
463
	float rotor=0;
464

pitiot's avatar
merging    
pitiot committed
465
466
467
468
	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
469

470
	if(!rigid_)
pitiot's avatar
maj    
pitiot committed
471
	{
472
473
		Dart d = groundFace;
		map.next(d);
pitiot's avatar
maj    
pitiot committed
474

Thomas Jund's avatar
Thomas Jund committed
475
476
477
478
479
480
481
482
		Dart dd =d;
		for (unsigned int i = 1; i < nbVertices; ++i)
		{
			//initialisation of forces with viscosity
			forces[dd] = -0.2f*velocity[dd];
			map.next(dd);
		}

483
		CellMarkerStore<EDGE> cm(map);
Thomas Jund's avatar
Thomas Jund committed
484
		DartMarkerStore dm(map);
485
486
487
488
489
490
491
		for (unsigned int i = 1; i < nbVertices; ++i)
		{
			Dart dd = d;
			do {
				if(!cm.isMarked(dd))
				{
					cm.mark(dd);
Thomas Jund's avatar
Thomas Jund committed
492
493
494
					VEC3 p1Next = position[map.phi1(dd)]+(velocity[map.phi1(dd)] * sim_->timeStep_);
					VEC3 p2Next = position[dd]+(velocity[dd] * sim_->timeStep_);
					VEC3 v1 = (p1Next-p2Next);
495
496
497

					//stretch spring : /!\ max rigidity relative to the timestep used (unstable otherwise)
					float norm = v1.norm();
Thomas Jund's avatar
Thomas Jund committed
498
					float rigidity = 70.0f;
499

500
					float stretch = rigidity*(edgeLength[dd]-v1.norm());
501
502
503
					if(norm>0.0f)
					{
						VEC3 f = stretch*(v1/norm);
504

505
506
507
508
						//apply force symmetrically
						forces[dd] -= f;
						forces[map.phi1(dd)] += f;
					}
Arash HABIBI's avatar
Arash HABIBI committed
509
510
				}

Thomas Jund's avatar
Thomas Jund committed
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
				dd = map.phi1(dd);
			} while(dd!=d);

			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
529
						float angularRig = 70.0f;
Thomas Jund's avatar
Thomas Jund committed
530

531
						float curAngle = Algo::Surface::Geometry::angle<PFP>(map, map.phi_1(dd),map.phi1(dd),position);
532

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

535
536
537
538
539
540
541
						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
542
					}
543
				}
pitiot's avatar
maj    
pitiot committed
544

545
546
				dd = map.phi1(dd);
			} while(dd!=d);
pitiot's avatar
maj    
pitiot committed
547

548
549
550
551
			map.next(d);
		}

		//guiding vertex = first vertex (set the displacement)
Thomas Jund's avatar
Thomas Jund committed
552
		forces[groundFace] = VEC3(0);
553
		velocity[groundFace] = velocity_;
Arash HABIBI's avatar
Arash HABIBI committed
554

555
556
557
		//apply force to each vertex
		d = groundFace;
		for (unsigned int i = 0; i < nbVertices; ++i)
pitiot's avatar
pitiot committed
558
		{
Thomas Jund's avatar
Thomas Jund committed
559
			velocity[d] += forces[d] * sim_->timeStep_;
560
561
562
563
			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
564
		}
565
	}
pitiot's avatar
pitiot committed
566
567
	else
	{
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
	//	CGoGNout << "Obstacle "<< index << CGoGNendl;
//		CGoGNout << "vitesse : "<< velocity_ << CGoGNendl;
		//	on fait tourner l'obstacle
		Dart d = groundFace;
		for (unsigned int i = 0; i < nbVertices; ++i)
		{
			position[d] += rotate(position[d], center, abs_angle*rotor);
			position[d] += (velocity_ * sim_->timeStep_);

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

			bary += position[d];

			map.next(d);
		}

		front=(position[1] + position[2]) / 2;
		if(angle >0)
			angle -= rotor;
		else
			angle += rotor;
pitiot's avatar
pitiot committed
589
	}
pitiot's avatar
maj    
pitiot committed
590

pitiot's avatar
pitiot committed
591
	center = bary / nbVertices;
pitiot's avatar
maj    
pitiot committed
592

593
594
	//computing deformation vector on vertices :
	//depending on the velocity, extend the perceived shape
595
	VEC3 vel = velocity_.normalized();
596
597
598

	d = groundFace;
	for (unsigned int i = 0; i < nbVertices; ++i)
599
	{
600
		VEC3 v(position[d] - center);
601
602
		VEC3 vN = v.normalized();
		float dot = vN * vel;
603
604
		deformation[d] = v * 2.0f * (dot+1.0f)/2.0f;
		map.next(d);
605
606
	}

Thomas Jund's avatar
Thomas Jund committed
607
608
//	if(!rigid_)
//		center = position[groundFace];
609

pitiot's avatar
maj    
pitiot committed
610
611
612
613
614
	// MAJ des obstacles
	for (unsigned int i = 0; i < nbVertices; ++i)
	{
		//		CGoGNout << "avant une etape : Obstacle "<< i << CGoGNendl;
		Obstacle* o = obstacles_[i];
615
616
617
618
		o->p1 = getDilatedPosition(i);
		o->p2 = getDilatedPosition((i+1) % nbVertices);
		o->prevP = getDilatedPosition((i - 1 + nbVertices) % nbVertices);
		o->nextP = getDilatedPosition((i + 2 + nbVertices) % nbVertices);
Thomas Jund's avatar
Thomas Jund committed
619
620
621
622
		Dart d = sim_->envMap_.popAndPushObstacleInCells(o,i);

		if(i==0)
			dDir = d;
pitiot's avatar
pitiot committed
623

624
625
626
627
628
629
630
		if(sim_->detect_agent_collision)
		{
			for (std::vector<Dart>::iterator it = belonging_cells[i].begin(); it != belonging_cells[i].end(); ++it)
			{
				general_belonging.insert(*it);
			}
		}
pitiot's avatar
maj    
pitiot committed
631
632
633
634
635
636
		//		CGoGNout << "Apres une etape : Obstacle "<< i << CGoGNendl;
	}

	//
	//		envMap_->addMovingObstAsNeighbor(this,general_belonging,&(general_neighbors));
	//		CGoGNout << "nouvel update : "<< CGoGNendl;
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
	if(sim_->detect_agent_collision)
	{
		for (std::set<Dart>::iterator it2 = general_belonging.begin(); it2 != general_belonging.end(); ++ it2)
		{
			std::vector<Agent*> vector =sim_->envMap_.agentvect[(*it2)];
			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
653

654
655
		}
	}
pitiot's avatar
maj    
pitiot committed
656
657
658

}

659
std::vector<Dart> MovingObstacle::getMemoCross(const VEC3& pos, const VEC3& dest, Dart& d1)
pitiot's avatar
maj    
pitiot committed
660
{
pitiot's avatar
pitiot committed
661
662
	registering_part->move(pos);

663
	d1=registering_part->d;
Thomas Jund's avatar
Thomas Jund committed
664

665
//	CGoGNout<<"d1 : "<< *d1<<"|| d2 : "<< *d2<<"|| start : "<< pos<<"|| stop : "<< dest<<CGoGNendl;
pitiot's avatar
pitiot committed
666
	return (registering_part->move(dest));
pitiot's avatar
maj    
pitiot committed
667
668
}

669
void resetPartSubdiv(Obstacle* o)
pitiot's avatar
maj    
pitiot committed
670
671
{
	MovingObstacle * mo = o->mo;
672

pitiot's avatar
maj    
pitiot committed
673
674
	if (mo != NULL)
	{
675
		VEC3 pos =mo->registering_part->getPosition();
Thomas Jund's avatar
Thomas Jund committed
676
		mo->registering_part->CGoGN::Algo::MovingObjects::ParticleBase<PFP>::move(Algo::Surface::Geometry::faceCentroid<PFP>(mo->sim_->envMap_.map, mo->registering_part->d, mo->sim_->envMap_.position)) ;
Arash HABIBI's avatar
Arash HABIBI committed
677

678
		mo->registering_part->setState(FACE) ;
pitiot's avatar
pitiot committed
679
		mo->registering_part->move(pos) ;
pitiot's avatar
maj    
pitiot committed
680
681
682
683
684
685
	}
}

void resetObstPartInFace(Obstacle* o, Dart d1)
{
	MovingObstacle * mo = o->mo;
686

687
688
	if (mo != NULL)
	{
689
		VEC3 pos1 = mo->registering_part->getPosition();
690
		if (Algo::Surface::Geometry::isPointInConvexFace2D <PFP> (mo->sim_->envMap_.map, d1, mo->sim_->envMap_.position, pos1, true))
691
			mo->registering_part->d = d1;
pitiot's avatar
maj    
pitiot committed
692
693
694
	}
}

695
void resetPart(MovingObstacle * mo, Dart d1)
pitiot's avatar
maj    
pitiot committed
696
{
697
698
	if (mo->registering_part->d == mo->sim_->envMap_.map.phi1(d1))
		mo->registering_part->d = d1;
pitiot's avatar
maj    
pitiot committed
699
700
701
702
703
704
}

void displayMO(Obstacle * o)
{
	CGoGNout << "obstacle problematique : " << (o->mo->index) << CGoGNendl;
}
pitiot's avatar
pitiot committed
705
706
707
708
709
710
711
712
713
714

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////comportement à modifier///////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////



// TODO Check position
void MovingObstacle::computePrefVelocity() //calcul du vecteur optimal pour atteindre l'objectif // changer pour 2.5 ?
{
pitiot's avatar
merging    
pitiot committed
715
716
	VEC3 goalVector;
	if (index_parent<1)
pitiot's avatar
pitiot committed
717
	{
Thomas Jund's avatar
Thomas Jund committed
718
		if(rigid_ || goals_.size()>1)
pitiot's avatar
merging    
pitiot committed
719
720
		{
			goalVector = goals_[curGoal_] - center ;
Thomas Jund's avatar
Thomas Jund committed
721
	//		goalVector = goals_[curGoal_] - position[groundFace] ;
pitiot's avatar
merging    
pitiot committed
722

Thomas Jund's avatar
Thomas Jund committed
723
724
			float goalDist2 = goalVector.norm2() ;

725
			if (goalDist2 < 1000.0f)
Thomas Jund's avatar
Thomas Jund committed
726
727
728
729
730
731
732
733
734
735
736
737
738
739
			{
				curGoal_ = (curGoal_ + 1) % goals_.size() ;
				goalVector = goals_[curGoal_] - center ;
	//			goalVector = goals_[curGoal_] - position[groundFace] ;
				goalDist2 = goalVector.norm2() ;
			}

			if (goalDist2 > maxSpeed_)
			{
				goalVector.normalize() ;
				goalVector *= maxSpeed_;
			}
		}
		else
pitiot's avatar
merging    
pitiot committed
740
		{
Thomas Jund's avatar
Thomas Jund committed
741
742
743
744
745
746
747
748
749
750
			if(ag_!=NULL)
			{
				goalVector = ag_->getPosition() - center ;
				if (goalVector.norm2() > maxSpeed_)
				{
					goalVector.normalize() ;
					goalVector *= maxSpeed_;
				}
			}
			else
Thomas Jund's avatar
Thomas Jund committed
751
			{
Thomas Jund's avatar
Thomas Jund committed
752
753
	//			if(sim_->envMap_.pedWayMark.isMarked(sim_->envMap_.map.phi2(dDir)))
	//				dDir = sim_->envMap_.map.phi1(sim_->envMap_.map.phi1(dDir));
Thomas Jund's avatar
Thomas Jund committed
754

Thomas Jund's avatar
Thomas Jund committed
755
				VEC3 mid = (sim_->envMap_.position[dDir]+sim_->envMap_.position[sim_->envMap_.map.phi1(dDir)])*0.5f;
Thomas Jund's avatar
Thomas Jund committed
756
757
				goalVector = mid - position[0] ;

Thomas Jund's avatar
Thomas Jund committed
758
759
760
761
762
763
764
				Dart dStart=dDir;
				VEC3 dir = center - position[0];
				while(goalVector.norm2()==0 || sim_->envMap_.pedWayMark.isMarked(sim_->envMap_.map.phi2(dDir)) || ((dir*goalVector)>0))
				{
	//				std::cout << "d " << dDir << std::endl;
	//				std::cout << "dir*goalVector " << dir*goalVector << std::endl;
	//				std::cout << "goal vec " << goalVector << std::endl;
Thomas Jund's avatar
Thomas Jund committed
765

Thomas Jund's avatar
Thomas Jund committed
766
767
768
					dDir = sim_->envMap_.map.phi_1(dDir);
					mid = (sim_->envMap_.position[dDir]+sim_->envMap_.position[sim_->envMap_.map.phi1(dDir)])*0.5f;
					goalVector = mid - position[0] ;
Thomas Jund's avatar
Thomas Jund committed
769

Thomas Jund's avatar
Thomas Jund committed
770
771
772
					if(dDir==dStart)
						break;
				}
Thomas Jund's avatar
Thomas Jund committed
773

Thomas Jund's avatar
Thomas Jund committed
774
775
				if(sim_->envMap_.map.isBoundaryEdge(sim_->envMap_.map.phi2(dDir)))
					goalVector = VEC3(0);
Thomas Jund's avatar
Thomas Jund committed
776

Thomas Jund's avatar
Thomas Jund committed
777
778
779
780
781
782
783
784
785
786
	//			goalVector = mid - center ;

	//			std::cout << "dDir " << dDir << " goalVec" << goalVector << std::endl;

	//			float goalDist2 = goalVector.norm2() ;
	//			if (goalDist2 > maxSpeed_)
				{
					goalVector.normalize() ;
					goalVector *= maxSpeed_*maxSpeed_;
				}
Thomas Jund's avatar
Thomas Jund committed
787
			}
pitiot's avatar
merging    
pitiot committed
788
789
790
		}
	}
	else
pitiot's avatar
pitiot committed
791
	{
pitiot's avatar
merging    
pitiot committed
792
793
794
795
796
797
798
		goalVector = get_center(parent,index_parent-1) -center;
		float goalDist2 = goalVector.norm2() ;
		if (goalDist2 > maxSpeed_)
		{
			goalVector.normalize() ;
			goalVector *= maxSpeed_;
		}
pitiot's avatar
pitiot committed
799
	}
800
801
802

	if (spinning)
		angle =get_angle(goalVector,front-center);
pitiot's avatar
merging    
pitiot committed
803

pitiot's avatar
pitiot committed
804
805
806
807
808
	prefVelocity_ = goalVector ;

}
void MovingObstacle::computeNewVelocity()  //comportement des obstacles en tenant compte de l'environnement.
{
pitiot's avatar
pitiot committed
809
810
811
812
813
	if (spinning)
	{
		VEC3 forward = front-center;
		float goalDist2 = forward.norm2() ;
		float objective = prefVelocity_.norm2();
814

pitiot's avatar
pitiot committed
815
		if (goalDist2 > objective)
816
817
818
819
820
821
822
823
		{
			forward.normalize() ;
			forward *= objective;
		}

		if(index_parent==0 && angle>0.01f)
			forward/=10;

pitiot's avatar
pitiot committed
824
825
826
827
828
829
		newVelocity_=forward;
	}
	else
	{
		newVelocity_=prefVelocity_;
	}
pitiot's avatar
pitiot committed
830
}
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895



PFP::REAL MovingObstacle::computeMVC(PFP::VEC3 p, Dart vertex)
{
	PFP::REAL r = (position[vertex]-p).norm();

	PFP::REAL sumU(0.);
	Dart it = vertex;
	do
	{
		PFP::VEC3 vi = position[it];
		PFP::VEC3 vj = position[map.phi1(it)];
		PFP::VEC3 vk = position[map.phi_1(it)];

		PFP::REAL Bjk = Geom::angle(vj - p, vk - p);
		PFP::REAL Bij = Geom::angle(vi - p, vj - p);
		PFP::REAL Bki = Geom::angle(vk - p, vi - p);

		PFP::VEC3 ei = (vi - p) / ((vi - p).norm());
		PFP::VEC3 ej = (vj - p) / ((vj - p).norm());
		PFP::VEC3 ek = (vk - p) / ((vk - p).norm());

		PFP::VEC3 nij = (ei ^ ej) / ((ei ^ ej).norm());
		PFP::VEC3 njk = (ej ^ ek) / ((ej ^ ek).norm());
		PFP::VEC3 nki = (ek ^ ei) / ((ek ^ ei).norm());

		PFP::REAL ui= (Bjk + (Bij*(nij*njk)) + (Bki*(nki*njk))) / (2.0f*ei*njk);

		sumU += ui;

		it = map.phi<21>(it);
	}
	while(it != vertex);

	return (1.0f / r) * sumU;
}

void MovingObstacle::computePointMVC(PFP::VEC3 point, std::vector<PFP::REAL>& coordinates)
{
	DartMarker mark_vertex(map);

	coordinates.clear();

	PFP::REAL sumMVC(0);
	unsigned int j = 0;
	TraversorV<PFP::MAP> t(map) ;
	for(Dart it = t.begin(); it != t.end(); it = t.next())
	{
		PFP::REAL c = computeMVC(point, it);
		coordinates.push_back(c);
		sumMVC += c;
		j++;
	}

	for(unsigned int i = 0; i < j; i++)
		coordinates[i] /= sumMVC;
}

void MovingObstacle::attachMesh(MovingMesh* mm)
{
	mm_ = mm;
	mvc_ = mm->map.getAttribute<NoMathIONameAttribute<std::vector<PFP::REAL> >, VERTEX>("mvc");
	if(!mvc_.isValid())
		mvc_ = mm->map.addAttribute<NoMathIONameAttribute<std::vector<PFP::REAL> >, VERTEX>("mvc");
Thomas Jund's avatar
Thomas Jund committed
896
	TraversorV<PFP2::MAP> t(mm_->map);
897
898
899
900
	for(Dart it = t.begin(); it != t.end(); it = t.next())
		computePointMVC(mm->position[it], mvc_[it]);
}

Thomas Jund's avatar
Thomas Jund committed
901
902
903
904
905
void MovingObstacle::attachAgent(Agent* ag)
{
	ag_ = ag;
}

906
907
908
909
void MovingObstacle::updateMesh()
{
	if(mm_)
	{
Thomas Jund's avatar
Thomas Jund committed
910
		TraversorV<PFP2::MAP> t(mm_->map);
911
912
913
914
915
916
917
918
919
920
921
922
923
924
		for(Dart it = t.begin(); it != t.end(); it = t.next())
		{
			PFP::VEC3 newPos;
			unsigned int j = 0;
			TraversorV<PFP::MAP> tt(map) ;
			for(Dart itt = tt.begin(); itt != tt.end(); itt = tt.next())
			{
				newPos += mvc_[it][j] * position[itt];
				++j;
			}
			mm_->position[it] = newPos;
		}
	}
}