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
		verticesDeformation.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
VEC3 MovingObstacle::getDilatedPosition(unsigned int ind)
{
160
	return vertices[ind]+verticesDeformation[ind];
161
162
163
164
165
166
167
}

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
336
337
}

void MovingObstacle::update()
{
pitiot's avatar
pitiot committed
338
	assert(sim_->envMap_.map.getCurrentLevel() == sim_->envMap_.map.getMaxLevel()) ;
339
340
341
	if(sim_->detect_agent_collision)
		general_belonging.clear();

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

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

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

pitiot's avatar
merging    
pitiot committed
358
	float rotor=0;
359

pitiot's avatar
merging    
pitiot committed
360
361
362
363
	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
364

pitiot's avatar
pitiot committed
365
366
367
//	CGoGNout << "Obstacle "<< index << CGoGNendl;
//	CGoGNout << "vitesse : "<< velocity_ << CGoGNendl;
	//	on fait tourner l'obstacle
pitiot's avatar
maj    
pitiot committed
368
369
	for (unsigned int i = 0; i < nbVertices; ++i)
	{
370
371
		VEC3& target =vertices[i];
		target += rotate(target, center, abs_angle*rotor);
pitiot's avatar
pitiot committed
372
		target += (velocity_ * sim_->timeStep_);
373

pitiot's avatar
maj    
pitiot committed
374
375
		bary += target;
	}
376

pitiot's avatar
pitiot committed
377
378
379
380
381
	front=(vertices[1] + vertices[2]) / 2;
	if(angle >0)
		angle -= rotor;
	else
		angle += rotor;
pitiot's avatar
maj    
pitiot committed
382

pitiot's avatar
pitiot committed
383
	center = bary / nbVertices;
pitiot's avatar
maj    
pitiot committed
384

385
386
387
	//computing deformation vector on vertices
	VEC3 vel = velocity_.normalized();
	for(unsigned int i = 0; i < nbVertices; ++i)
388
	{
389
390
391
392
		VEC3 v(vertices[i] - center);
		VEC3 vN = v.normalized();
		float dot = vN * vel;
		verticesDeformation[i] = v * 2.0f * (dot+1.0f)/2.0f;
393
394
	}

pitiot's avatar
merging    
pitiot committed
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
	//-------- 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
414
415
416
417
418
	// MAJ des obstacles
	for (unsigned int i = 0; i < nbVertices; ++i)
	{
		//		CGoGNout << "avant une etape : Obstacle "<< i << CGoGNendl;
		Obstacle* o = obstacles_[i];
419
420
421
422
		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
423
424
425
		sim_->envMap_.popAndPushObstacleInCells(o,i);


426
427
428
429
430
431
432
		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
433
434
435
436
437
438
		//		CGoGNout << "Apres une etape : Obstacle "<< i << CGoGNendl;
	}

	//
	//		envMap_->addMovingObstAsNeighbor(this,general_belonging,&(general_neighbors));
	//		CGoGNout << "nouvel update : "<< CGoGNendl;
439
440
441
442
443
444
445
	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
446
				if (this->is_inside((*it)->part_.getPosition()))
447
448
449
450
451
452
453
454
				{
					(*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
455

456
457
		}
	}
pitiot's avatar
maj    
pitiot committed
458
459
460

}

pitiot's avatar
merging    
pitiot committed
461

462
std::vector<Dart> MovingObstacle::getMemoCross(const VEC3& pos, const VEC3& dest, Dart& d1)
pitiot's avatar
maj    
pitiot committed
463
{
pitiot's avatar
pitiot committed
464
465
	registering_part->move(pos);

466
	d1=registering_part->d;
467
//	CGoGNout<<"d1 : "<< *d1<<"|| d2 : "<< *d2<<"|| start : "<< pos<<"|| stop : "<< dest<<CGoGNendl;
pitiot's avatar
pitiot committed
468
	return (registering_part->move(dest));
pitiot's avatar
maj    
pitiot committed
469
470
}

471
void resetPartSubdiv(Obstacle* o)
pitiot's avatar
maj    
pitiot committed
472
473
{
	MovingObstacle * mo = o->mo;
474

pitiot's avatar
maj    
pitiot committed
475
476
	if (mo != NULL)
	{
477
		VEC3 pos =mo->registering_part->getPosition();
pitiot's avatar
merging    
pitiot committed
478
479
		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)) ;

480
		mo->registering_part->setState(FACE) ;
pitiot's avatar
pitiot committed
481
		mo->registering_part->move(pos) ;
pitiot's avatar
maj    
pitiot committed
482
483
484
485
486
487
	}
}

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

pitiot's avatar
maj    
pitiot committed
489
490
	if (mo != NULL) {

491
		VEC3 pos1 = mo->registering_part->getPosition();
pitiot's avatar
maj    
pitiot committed
492
493
		if (Algo::Geometry::isPointInConvexFace2D < PFP
				> (mo->sim_->envMap_.map, d1, mo->sim_->envMap_.position, pos1, true)) {
494
			mo->registering_part->d = d1;
pitiot's avatar
maj    
pitiot committed
495
496
497
498
499
		}

	}
}

500
void resetPart(MovingObstacle * mo, Dart d1)
pitiot's avatar
maj    
pitiot committed
501
{
502
503
	if (mo->registering_part->d == mo->sim_->envMap_.map.phi1(d1))
		mo->registering_part->d = d1;
pitiot's avatar
pitiot committed
504

pitiot's avatar
maj    
pitiot committed
505
506
507
508
509
510
}

void displayMO(Obstacle * o)
{
	CGoGNout << "obstacle problematique : " << (o->mo->index) << CGoGNendl;
}
pitiot's avatar
pitiot committed
511
512
513
514
515
516
517
518
519
520

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////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
521
522
	VEC3 goalVector;
	if (index_parent<1)
pitiot's avatar
pitiot committed
523
524
525
	{
		goalVector = goals_[curGoal_] - center ;

pitiot's avatar
merging    
pitiot committed
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
		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
542
	{
pitiot's avatar
merging    
pitiot committed
543
544
545
546
547
548
549
		goalVector = get_center(parent,index_parent-1) -center;
		float goalDist2 = goalVector.norm2() ;
		if (goalDist2 > maxSpeed_)
		{
			goalVector.normalize() ;
			goalVector *= maxSpeed_;
		}
pitiot's avatar
pitiot committed
550
	}
pitiot's avatar
pitiot committed
551
	if (spinning) angle =get_angle(goalVector,front-center);
pitiot's avatar
merging    
pitiot committed
552

pitiot's avatar
pitiot committed
553
554
555
556
557
	prefVelocity_ = goalVector ;

}
void MovingObstacle::computeNewVelocity()  //comportement des obstacles en tenant compte de l'environnement.
{
pitiot's avatar
pitiot committed
558
559
560
561
562
563
564
565
566
567
	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
568
		if(index_parent==0 && angle>0.01f) forward/=10;
pitiot's avatar
pitiot committed
569
570
571
572
573
574
		newVelocity_=forward;
	}
	else
	{
		newVelocity_=prefVelocity_;
	}
pitiot's avatar
pitiot committed
575
}