moving_obstacle.cpp 15.1 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
float MovingObstacle::timeHorizonObst_ = 10.0f;
unsigned int MovingObstacle::maxNeighbors_ = 20;
float MovingObstacle::detectionFixedObst = 50;

bool MovingObstacle::is_inside(VEC3 p)
{
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
	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
33
34
}

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

pitiot's avatar
pitiot committed
62
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
63
64
65
66
67
68
69
70
71
{
	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
merging    
pitiot committed
72
MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, std::vector<VEC3> goals, bool spin, ArticulatedObstacle * art, int ind2) :
pitiot's avatar
maj    
pitiot committed
73
74
		index(ind),
		newVelocity_(0),
pitiot's avatar
merging    
pitiot committed
75
76
77
		sim_(sim),
		parent(art),
		index_parent(ind2)
pitiot's avatar
maj    
pitiot committed
78
{
pitiot's avatar
merging    
pitiot committed
79
80
81
82
	seen = false;
	color1=1.0f;
	color2=1.0f;
	color3=1.0f;
pitiot's avatar
maj    
pitiot committed
83
84
	assert(pos.size() > 2);

pitiot's avatar
pitiot committed
85
//	movingObstNeighbors_.reserve(maxNeighbors_);
pitiot's avatar
pitiot committed
86
	spinning=spin;
pitiot's avatar
pitiot committed
87
88
	goals_=goals;
	curGoal_ = 1;
pitiot's avatar
maj    
pitiot committed
89
90
91
92
93
94
95
	VEC3 sum = 0;
	nb_agents_voisins = 0;
	nb_register_cells = 0;
	float pos_max = 0;

	velocity_factor = 0.8f;
	nbVertices = pos.size();
96
97
	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
98

99
	vertices.reserve(nbVertices);
pitiot's avatar
maj    
pitiot committed
100
	obstacles_ = new Obstacle*[nbVertices];
101
102
	belonging_cells = new std::vector<Dart>[nbVertices];
	neighbor_cells = new std::vector<Dart>[nbVertices];
pitiot's avatar
maj    
pitiot committed
103
104
	for (unsigned int i = 0; i < nbVertices; ++i)
	{
105
		vertices[i]=pos [i];
pitiot's avatar
pitiot committed
106
		sum += vertices[i];
107
		verticesDetails.push_back(VEC3(0));
pitiot's avatar
pitiot committed
108
109
110
	}
	center = sum / nbVertices;
	front=(vertices[1] + vertices[2]) / 2;
pitiot's avatar
merging    
pitiot committed
111
112
113
114
115
116
117
118
119
120
121
122

	//-------- code ajoute par Arash pour les obstacles rectangulaires --------------

	length = (vertices[0]-vertices[1]).norm();
	width  = (vertices[1]-vertices[2]).norm();
	sum_dist_foci_rest = 2*(length + width*(sqrt(2)-0.5));
	// M appartient à l'ellipse ssi MF1 + MF2 = sum_dist_foci est une constante
	// où F1 et F2 sont les deux foyers.

	//-------- fin du code ajoute par Arash pour les obstacles rectangulaires -------

	if (spinning && parent==NULL) //départ face à la cible en cas d'obstacles pouvant effectuer des rotations
pitiot's avatar
pitiot committed
123
124
125
126
127
128
129
130
131
	{
		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
132
133
134
	}
	for (unsigned int i = 0; i < nbVertices; ++i)
	{
pitiot's avatar
pitiot committed
135

136
137
138
139
		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
140
141
		obstacles_[i] = o;

pitiot's avatar
pitiot committed
142
		sim_->envMap_.pushObstacleInCells(o);
pitiot's avatar
maj    
pitiot committed
143
144
145
	}


pitiot's avatar
pitiot committed
146

pitiot's avatar
maj    
pitiot committed
147
148
149
150
151
152
153
154
155
156
157
	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;
}

158
159
160
161
162
163
164
165
166
167
VEC3 MovingObstacle::getDilatedPosition(unsigned int ind)
{
	return vertices[ind]+verticesDetails[ind];
}

VEC3 MovingObstacle::getPosition(unsigned int ind)
{
	return vertices[ind];
}

pitiot's avatar
maj    
pitiot committed
168
169
170
171
bool MovingObstacle::test_opposition(VEC3 o, VEC3 p1, VEC3 p2)
{
	o.normalize();
	o *= -1;
172
173
174
	VEC3 vector(p1 - p2);
	vector.normalize();
	return (o - vector).norm2() < 0.1;
pitiot's avatar
maj    
pitiot committed
175
176
177
}

// TODO Check position
pitiot's avatar
pitiot committed
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
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
//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
232

pitiot's avatar
pitiot committed
233
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
234
{
pitiot's avatar
pitiot committed
235
236
237
238
239
//	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
240
//	{
pitiot's avatar
pitiot committed
241
242
243
244
245
246
//		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
247
//
pitiot's avatar
pitiot committed
248
249
250
251
//		for (std::vector<Agent*>::const_iterator it = agents.begin(); it != agents.end(); ++it)
//		{
//			if ((*it)->alive)
//			{
pitiot's avatar
pitiot committed
252
//
pitiot's avatar
pitiot committed
253
254
255
256
//				float distSq = (center - (*it)->getPosition()).norm2() ;
//				if ((agentNeighbors_.size() < maxNeighbors_ || distSq < maxDist)
//					&& distSq < neighborDistSq_)
//				{
pitiot's avatar
pitiot committed
257
//
pitiot's avatar
pitiot committed
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
//					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
293
//	}
pitiot's avatar
pitiot committed
294
295
296
297
298
299
}

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
300
//
pitiot's avatar
pitiot committed
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
//	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
317
//
pitiot's avatar
pitiot committed
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
//		}
//	}
//	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
334
335
}

pitiot's avatar
pitiot committed
336

pitiot's avatar
maj    
pitiot committed
337
338
339
// TODO Check position
void MovingObstacle::update()
{
pitiot's avatar
pitiot committed
340
	assert(sim_->envMap_.map.getCurrentLevel() == sim_->envMap_.map.getMaxLevel()) ;
341
342
343
	if(sim_->detect_agent_collision)
		general_belonging.clear();

pitiot's avatar
merging    
pitiot committed
344
345
346
347
348
349
350
	if(!seen)
	{
		color1=1.0f;
		color2=1.0f;
		color3=1.0f;
	}
	seen=false;
pitiot's avatar
maj    
pitiot committed
351
352
353
	PFP::VEC3 bary = 0;

	Dart d;
pitiot's avatar
pitiot committed
354
355
	velocity_[0] = newVelocity_[0] * velocity_factor;
	velocity_[1] = newVelocity_[1] * velocity_factor;
pitiot's avatar
maj    
pitiot committed
356
357

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

pitiot's avatar
merging    
pitiot committed
360
361
362
363
364
365
366
367
368
	float rotor=0;
	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
369

pitiot's avatar
pitiot committed
370
371
372
//	CGoGNout << "Obstacle "<< index << CGoGNendl;
//	CGoGNout << "vitesse : "<< velocity_ << CGoGNendl;
	//	on fait tourner l'obstacle
pitiot's avatar
maj    
pitiot committed
373
374
375
	for (unsigned int i = 0; i < nbVertices; ++i)
	{

pitiot's avatar
pitiot committed
376
		VEC3 target =vertices[i];
pitiot's avatar
maj    
pitiot committed
377

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

pitiot's avatar
pitiot committed
380
		target += (velocity_ * sim_->timeStep_);
381
		vertices[i] = target;
pitiot's avatar
maj    
pitiot committed
382
383
		bary += target;
	}
384

pitiot's avatar
pitiot committed
385
386
387
388
389
390
391
392
393
	front=(vertices[1] + vertices[2]) / 2;
	if(angle >0)
		{
		angle -= rotor;
		}
	else
	{
		angle += rotor;
	}
pitiot's avatar
maj    
pitiot committed
394

pitiot's avatar
pitiot committed
395
	center = bary / nbVertices;
pitiot's avatar
maj    
pitiot committed
396

397
398
399
400
401
402
403
404
405
406
407
408
	//computing details vector on vertices
	for (unsigned int i = 0; i < nbVertices; ++i)
	{
		VEC3 v(vertices[i]-center);
		if(v*velocity_>=0)
		{
			verticesDetails[i] = v*2.0f;
		}
		else
			verticesDetails[i] = VEC3(0);
	}

pitiot's avatar
merging    
pitiot committed
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
	//-------- code ajoute par Arash pour les obstacles rectangulaires --------------

	VEC3 P0_P1 = vertices[1] - vertices[0];
	float velocity_coef = 10.0;

	if(P0_P1 * velocity_ > 0) 	// P0_P1 est dans le sens de la vitesse
	{
		focus1  = center - P0_P1*(1-(width/length)*(sqrt(2)-0.5));
		focus2 = center + P0_P1*(1-(width/length)*(sqrt(2)-0.5)) + (velocity_coef*velocity_);
	}
	else
	{
		focus1  = center - P0_P1*(1-(width/length)*(sqrt(2)-0.5)) + (velocity_coef*velocity_);
		focus2 = center + P0_P1*(1-(width/length)*(sqrt(2)-0.5));
	}
	sum_dist_foci = sum_dist_foci_rest + velocity_coef*velocity_.norm();

	//-------- fin du code ajoute par Arash pour les obstacles rectangulaires -------

pitiot's avatar
maj    
pitiot committed
428
429
430
431
432
	// MAJ des obstacles
	for (unsigned int i = 0; i < nbVertices; ++i)
	{
		//		CGoGNout << "avant une etape : Obstacle "<< i << CGoGNendl;
		Obstacle* o = obstacles_[i];
433
434
435
436
		o->p1 = getDilatedPosition(i);
		o->p2 = getDilatedPosition((i+1) % nbVertices);
		o->prevP = getDilatedPosition((i - 1 + nbVertices) % nbVertices);
		o->nextP = getDilatedPosition((i + 2 + nbVertices) % nbVertices);
pitiot's avatar
pitiot committed
437
438
439
		sim_->envMap_.popAndPushObstacleInCells(o,i);


440
441
442
443
444
445
446
		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
447
448
449
450
451
452
		//		CGoGNout << "Apres une etape : Obstacle "<< i << CGoGNendl;
	}

	//
	//		envMap_->addMovingObstAsNeighbor(this,general_belonging,&(general_neighbors));
	//		CGoGNout << "nouvel update : "<< CGoGNendl;
453
454
455
456
457
458
459
	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)
			{
pitiot's avatar
merging    
pitiot committed
460
				if (this->is_inside((*it)->part_.getPosition()))
461
462
463
464
465
466
467
468
				{
					(*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
469

470
471
		}
	}
pitiot's avatar
maj    
pitiot committed
472
473
474

}

pitiot's avatar
merging    
pitiot committed
475

476
std::vector<Dart> MovingObstacle::getMemoCross(const VEC3& pos, const VEC3& dest, Dart& d1)
pitiot's avatar
maj    
pitiot committed
477
{
pitiot's avatar
pitiot committed
478
479
	registering_part->move(pos);

480
	d1=registering_part->d;
481
//	CGoGNout<<"d1 : "<< *d1<<"|| d2 : "<< *d2<<"|| start : "<< pos<<"|| stop : "<< dest<<CGoGNendl;
pitiot's avatar
pitiot committed
482
	return (registering_part->move(dest));
pitiot's avatar
maj    
pitiot committed
483
484
}

485
void resetPartSubdiv(Obstacle* o)
pitiot's avatar
maj    
pitiot committed
486
487
{
	MovingObstacle * mo = o->mo;
488

pitiot's avatar
maj    
pitiot committed
489
490
	if (mo != NULL)
	{
491
		VEC3 pos =mo->registering_part->getPosition();
pitiot's avatar
merging    
pitiot committed
492
493
		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)) ;

494
		mo->registering_part->setState(FACE) ;
pitiot's avatar
pitiot committed
495
		mo->registering_part->move(pos) ;
pitiot's avatar
maj    
pitiot committed
496
497
498
499
500
501
	}
}

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

pitiot's avatar
maj    
pitiot committed
503
504
	if (mo != NULL) {

505
		VEC3 pos1 = mo->registering_part->getPosition();
pitiot's avatar
maj    
pitiot committed
506
507
		if (Algo::Geometry::isPointInConvexFace2D < PFP
				> (mo->sim_->envMap_.map, d1, mo->sim_->envMap_.position, pos1, true)) {
508
			mo->registering_part->d = d1;
pitiot's avatar
maj    
pitiot committed
509
510
511
512
513
		}

	}
}

514
void resetPart(MovingObstacle * mo, Dart d1)
pitiot's avatar
maj    
pitiot committed
515
{
516
517
	if (mo->registering_part->d == mo->sim_->envMap_.map.phi1(d1))
		mo->registering_part->d = d1;
pitiot's avatar
pitiot committed
518

pitiot's avatar
maj    
pitiot committed
519
520
521
522
523
524
}

void displayMO(Obstacle * o)
{
	CGoGNout << "obstacle problematique : " << (o->mo->index) << CGoGNendl;
}
pitiot's avatar
pitiot committed
525
526
527
528
529
530
531
532
533
534

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////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
535
536
	VEC3 goalVector;
	if (index_parent<1)
pitiot's avatar
pitiot committed
537
538
539
	{
		goalVector = goals_[curGoal_] - center ;

pitiot's avatar
merging    
pitiot committed
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
		float goalDist2 = goalVector.norm2() ;

		if (goalDist2 < 5.0f)
		{
			curGoal_ = (curGoal_ + 1) % goals_.size() ;
			goalVector = goals_[curGoal_] - center ;
			goalDist2 = goalVector.norm2() ;
		}

		if (goalDist2 > maxSpeed_)
		{
			goalVector.normalize() ;
			goalVector *= maxSpeed_;
		}
	}
	else
pitiot's avatar
pitiot committed
556
	{
pitiot's avatar
merging    
pitiot committed
557
558
559
560
561
562
563
		goalVector = get_center(parent,index_parent-1) -center;
		float goalDist2 = goalVector.norm2() ;
		if (goalDist2 > maxSpeed_)
		{
			goalVector.normalize() ;
			goalVector *= maxSpeed_;
		}
pitiot's avatar
pitiot committed
564
	}
pitiot's avatar
pitiot committed
565
	if (spinning) angle =get_angle(goalVector,front-center);
pitiot's avatar
merging    
pitiot committed
566

pitiot's avatar
pitiot committed
567
568
569
570
571
	prefVelocity_ = goalVector ;

}
void MovingObstacle::computeNewVelocity()  //comportement des obstacles en tenant compte de l'environnement.
{
pitiot's avatar
pitiot committed
572
573
574
575
576
577
578
579
580
581
	if (spinning)
	{
		VEC3 forward = front-center;
		float goalDist2 = forward.norm2() ;
		float objective = prefVelocity_.norm2();
		if (goalDist2 > objective)
			{
				forward.normalize() ;
				forward *= objective;
			}
pitiot's avatar
merging    
pitiot committed
582
		if(index_parent==0 && angle>0.01f) forward/=10;
pitiot's avatar
pitiot committed
583
584
585
586
587
588
		newVelocity_=forward;
	}
	else
	{
		newVelocity_=prefVelocity_;
	}
pitiot's avatar
pitiot committed
589
}