moving_obstacle.cpp 13 KB
Newer Older
pitiot's avatar
maj    
pitiot committed
1
2
3
4
5
#include "moving_obstacle.h"
#include "obstacle.h"
#include "agent.h"
#include "simulator.h"

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

pitiot's avatar
pitiot committed
8
float MovingObstacle::maxSpeed_ = 2.0f;
pitiot's avatar
pitiot committed
9
10
float MovingObstacle::neighborDist_ = 10.0f ;
float MovingObstacle::neighborDistSq_ = neighborDist_ * neighborDist_ ;
pitiot's avatar
maj    
pitiot committed
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
float MovingObstacle::timeHorizonObst_ = 10.0f;
unsigned int MovingObstacle::maxNeighbors_ = 20;
float MovingObstacle::detectionFixedObst = 50;

bool MovingObstacle::is_inside(VEC3 p)
{
	VEC3 vec, norm;
	Obstacle * obst;

	for (unsigned int i = 0; i < nbVertices; i++)
	{
		obst = this->obstacles_[i];

		vec = VEC3(obst->p2 - obst->p1);
		norm[0] = vec[1];
		norm[1] = -vec[0];

		vec = VEC3(obst->p2 -p);
		if (vec*norm < 0)
			return false;
	}

	return true;
}

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

pitiot's avatar
pitiot committed
63
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
64
65
66
67
68
69
70
71
72
{
	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
73
MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, std::vector<VEC3> goals, bool spin) :
pitiot's avatar
maj    
pitiot committed
74
75
76
77
78
79
		index(ind),
		newVelocity_(0),
		sim_(sim)
{
	assert(pos.size() > 2);

pitiot's avatar
pitiot committed
80
//	movingObstNeighbors_.reserve(maxNeighbors_);
pitiot's avatar
pitiot committed
81
	spinning=spin;
pitiot's avatar
pitiot committed
82
83
	goals_=goals;
	curGoal_ = 1;
pitiot's avatar
maj    
pitiot committed
84
85
86
87
88
89
90
	VEC3 sum = 0;
	nb_agents_voisins = 0;
	nb_register_cells = 0;
	float pos_max = 0;

	velocity_factor = 0.8f;
	nbVertices = pos.size();
91
92
	Dart d = sim_->envMap_.getBelongingCell(pos[0]);
	registering_part = new CGoGN::Algo::MovingObjects::ParticleCell2DMemo<PFP>(sim_->envMap_.map, d, pos[0], sim_->envMap_.position);
pitiot's avatar
maj    
pitiot committed
93

94
	vertices.reserve(nbVertices);
pitiot's avatar
maj    
pitiot committed
95
	obstacles_ = new Obstacle*[nbVertices];
96
97
	belonging_cells = new std::vector<Dart>[nbVertices];
	neighbor_cells = new std::vector<Dart>[nbVertices];
pitiot's avatar
maj    
pitiot committed
98
99
	for (unsigned int i = 0; i < nbVertices; ++i)
	{
100
		vertices[i]=pos [i];
pitiot's avatar
pitiot committed
101
102
103
104
105
106
107
108
109
110
111
112
113
114
		sum += vertices[i];
	}
	center = sum / nbVertices;
	front=(vertices[1] + vertices[2]) / 2;
	if (spinning) //départ face à la cible en cas d'obstacles pouvant effectuer des rotations
	{
		angle = get_angle(goals_[curGoal_] - center,front  - center);
		for (unsigned int i = 0; i < nbVertices; ++i)
		{
			vertices[i]+=rotate(vertices[i], center, angle);

		}
		angle=0;
		front=(vertices[1] + vertices[2]) / 2;
pitiot's avatar
maj    
pitiot committed
115
116
117
	}
	for (unsigned int i = 0; i < nbVertices; ++i)
	{
pitiot's avatar
pitiot committed
118

119
120
121
122
		Obstacle* o = new Obstacle(vertices[i],
								   vertices[(i + 1) % nbVertices],
								   vertices[(i - 1 + nbVertices) % nbVertices],
								   vertices[(i + 2) % nbVertices], this, i);
pitiot's avatar
maj    
pitiot committed
123
124
		obstacles_[i] = o;

pitiot's avatar
pitiot committed
125
		sim_->envMap_.pushObstacleInCells(o);
pitiot's avatar
maj    
pitiot committed
126
127
128
	}


pitiot's avatar
pitiot committed
129

pitiot's avatar
maj    
pitiot committed
130
131
132
133
134
135
136
137
138
	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
pitiot committed
139

pitiot's avatar
pitiot committed
140

pitiot's avatar
pitiot committed
141
142
143



pitiot's avatar
maj    
pitiot committed
144
145
146
147
148
149
}

bool MovingObstacle::test_opposition(VEC3 o, VEC3 p1, VEC3 p2)
{
	o.normalize();
	o *= -1;
150
151
152
	VEC3 vector(p1 - p2);
	vector.normalize();
	return (o - vector).norm2() < 0.1;
pitiot's avatar
maj    
pitiot committed
153
154
155
}

// TODO Check position
pitiot's avatar
pitiot committed
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
//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
210

pitiot's avatar
pitiot committed
211
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
212
{
pitiot's avatar
pitiot committed
213
214
215
216
217
//	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
218
//	{
pitiot's avatar
pitiot committed
219
220
221
222
223
224
//		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
225
//
pitiot's avatar
pitiot committed
226
227
228
229
//		for (std::vector<Agent*>::const_iterator it = agents.begin(); it != agents.end(); ++it)
//		{
//			if ((*it)->alive)
//			{
pitiot's avatar
pitiot committed
230
//
pitiot's avatar
pitiot committed
231
232
233
234
//				float distSq = (center - (*it)->getPosition()).norm2() ;
//				if ((agentNeighbors_.size() < maxNeighbors_ || distSq < maxDist)
//					&& distSq < neighborDistSq_)
//				{
pitiot's avatar
pitiot committed
235
//
pitiot's avatar
pitiot committed
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
//					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
271
//	}
pitiot's avatar
pitiot committed
272
273
274
275
276
277
}

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
278
//
pitiot's avatar
pitiot committed
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
//	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
295
//
pitiot's avatar
pitiot committed
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
//		}
//	}
//	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
312
313
}

pitiot's avatar
pitiot committed
314

pitiot's avatar
maj    
pitiot committed
315
316
317
// TODO Check position
void MovingObstacle::update()
{
pitiot's avatar
pitiot committed
318
	assert(sim_->envMap_.map.getCurrentLevel() == sim_->envMap_.map.getMaxLevel()) ;
319
320
321
	if(sim_->detect_agent_collision)
		general_belonging.clear();

pitiot's avatar
maj    
pitiot committed
322
323
324
	PFP::VEC3 bary = 0;

	Dart d;
pitiot's avatar
pitiot committed
325
326
	velocity_[0] = newVelocity_[0] * velocity_factor;
	velocity_[1] = newVelocity_[1] * velocity_factor;
Arash HABIBI's avatar
Arash HABIBI committed
327
328
	// velocity_[0] = 0.0;
	// velocity_[1] = 0.0;
pitiot's avatar
maj    
pitiot committed
329
330

	// MAJ des particules
pitiot's avatar
pitiot committed
331
332
333
334
335
	float abs_angle= angle > 0 ? 1 : -1;
	float rotor = abs_angle*angle > 0.01f ? 0.01f : abs_angle*angle ;
//	CGoGNout << "Obstacle "<< index << CGoGNendl;
//	CGoGNout << "vitesse : "<< velocity_ << CGoGNendl;
	//	on fait tourner l'obstacle
pitiot's avatar
maj    
pitiot committed
336
337
338
	for (unsigned int i = 0; i < nbVertices; ++i)
	{

pitiot's avatar
pitiot committed
339
		VEC3 target =vertices[i];
pitiot's avatar
maj    
pitiot committed
340

pitiot's avatar
pitiot committed
341
		target += rotate(vertices[i], center, abs_angle*rotor);
pitiot's avatar
maj    
pitiot committed
342

pitiot's avatar
pitiot committed
343
		target += (velocity_ * sim_->timeStep_);
344
		vertices[i]=target;
pitiot's avatar
maj    
pitiot committed
345
346
		bary += target;
	}
pitiot's avatar
pitiot committed
347
348
349
350
351
352
353
354
355
	front=(vertices[1] + vertices[2]) / 2;
	if(angle >0)
		{
		angle -= rotor;
		}
	else
	{
		angle += rotor;
	}
pitiot's avatar
maj    
pitiot committed
356

pitiot's avatar
pitiot committed
357
	center = bary / nbVertices;
pitiot's avatar
maj    
pitiot committed
358
359
360
361
362
363
364
365
366
367

	// MAJ des obstacles
	for (unsigned int i = 0; i < nbVertices; ++i)
	{
		//		CGoGNout << "avant une etape : Obstacle "<< i << CGoGNendl;
		Obstacle* o = obstacles_[i];
//		o->p1 = parts_[i]->getPosition();
//		o->p2 = parts_[(i + 1) % nbVertices]->getPosition();
//		o->prevP = parts_[(i - 1 + nbVertices) % nbVertices]->getPosition();
//		o->nextP = parts_[(i + 2) % nbVertices]->getPosition();
pitiot's avatar
pitiot committed
368
369
370
		sim_->envMap_.popAndPushObstacleInCells(o,i);


371
372
373
374
375
376
377
		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
378
379
380
381
382
383
		//		CGoGNout << "Apres une etape : Obstacle "<< i << CGoGNendl;
	}

	//
	//		envMap_->addMovingObstAsNeighbor(this,general_belonging,&(general_neighbors));
	//		CGoGNout << "nouvel update : "<< CGoGNendl;
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
	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
400

401
402
		}
	}
pitiot's avatar
maj    
pitiot committed
403
404
405

}

Arash HABIBI's avatar
Arash HABIBI committed
406

407
std::vector<Dart> MovingObstacle::getMemoCross(const VEC3& pos, const VEC3& dest, Dart& d1)
pitiot's avatar
maj    
pitiot committed
408
{
pitiot's avatar
pitiot committed
409
410
	registering_part->move(pos);

411
	d1=registering_part->d;
412
//	CGoGNout<<"d1 : "<< *d1<<"|| d2 : "<< *d2<<"|| start : "<< pos<<"|| stop : "<< dest<<CGoGNendl;
pitiot's avatar
pitiot committed
413
	return (registering_part->move(dest));
pitiot's avatar
maj    
pitiot committed
414
415
}

416
void resetPartSubdiv(Obstacle* o)
pitiot's avatar
maj    
pitiot committed
417
418
{
	MovingObstacle * mo = o->mo;
419

pitiot's avatar
maj    
pitiot committed
420
421
	if (mo != NULL)
	{
422
		VEC3 pos =mo->registering_part->getPosition();
Arash HABIBI's avatar
Arash HABIBI committed
423
424
		mo->registering_part->CGoGN::Algo::MovingObjects::ParticleBase<PFP>::move(Algo::Geometry::faceCentroid<PFP>(mo->sim_->envMap_.map, mo->registering_part->d, mo->sim_->envMap_.position)) ;

425
		mo->registering_part->setState(FACE) ;
pitiot's avatar
pitiot committed
426
		mo->registering_part->move(pos) ;
pitiot's avatar
maj    
pitiot committed
427
428
429
430
431
432
	}
}

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

pitiot's avatar
maj    
pitiot committed
434
435
	if (mo != NULL) {

436
		VEC3 pos1 = mo->registering_part->getPosition();
pitiot's avatar
maj    
pitiot committed
437
438
		if (Algo::Geometry::isPointInConvexFace2D < PFP
				> (mo->sim_->envMap_.map, d1, mo->sim_->envMap_.position, pos1, true)) {
439
			mo->registering_part->d = d1;
pitiot's avatar
maj    
pitiot committed
440
441
442
443
444
		}

	}
}

445
void resetPart(MovingObstacle * mo, Dart d1)
pitiot's avatar
maj    
pitiot committed
446
{
447
448
	if (mo->registering_part->d == mo->sim_->envMap_.map.phi1(d1))
		mo->registering_part->d = d1;
pitiot's avatar
pitiot committed
449

pitiot's avatar
maj    
pitiot committed
450
451
452
453
454
455
}

void displayMO(Obstacle * o)
{
	CGoGNout << "obstacle problematique : " << (o->mo->index) << CGoGNendl;
}
pitiot's avatar
pitiot committed
456
457
458
459
460
461
462
463
464
465
466
467

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



// TODO Check position
void MovingObstacle::computePrefVelocity() //calcul du vecteur optimal pour atteindre l'objectif // changer pour 2.5 ?
{
	VEC3 goalVector = goals_[curGoal_] - center ;
	float goalDist2 = goalVector.norm2() ;
pitiot's avatar
pitiot committed
468

pitiot's avatar
pitiot committed
469
470
471
472
473
474
475
476
477
478
479
480
	if (goalDist2 < 2.0f)
	{
		curGoal_ = (curGoal_ + 1) % goals_.size() ;
		goalVector = goals_[curGoal_] - center ;
		goalDist2 = goalVector.norm2() ;
	}

	if (goalDist2 > maxSpeed_)
	{
		goalVector.normalize() ;
		goalVector *= maxSpeed_;
	}
pitiot's avatar
pitiot committed
481
	if (spinning) angle =get_angle(goalVector,front-center);
pitiot's avatar
pitiot committed
482
483
484
485
486
	prefVelocity_ = goalVector ;

}
void MovingObstacle::computeNewVelocity()  //comportement des obstacles en tenant compte de l'environnement.
{
pitiot's avatar
pitiot committed
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
	if (spinning)
	{
		VEC3 forward = front-center;
		float goalDist2 = forward.norm2() ;
		float objective = prefVelocity_.norm2();
		if (goalDist2 > objective)
			{
				forward.normalize() ;
				forward *= objective;
			}
		newVelocity_=forward;
	}
	else
	{
		newVelocity_=prefVelocity_;
	}
pitiot's avatar
pitiot committed
503
}