moving_obstacle.cpp 3.05 KB
Newer Older
Thomas's avatar
hop    
Thomas committed
1
2
3
4
5
6
7
8
9
10
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
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
#include "moving_obstacle.h"
#include "obstacle.h"
#include "agent.h"

float MovingObstacle::neighborDistSq_ = 5.0f * 5.0f;

float MovingObstacle::timeStep = 0.25f;

MovingObstacle::MovingObstacle(EnvMap* envmap, std::vector<PFP::VEC3> pos) :
envMap_(envmap)
{
	assert(pos.size()>2);

	nbVertices = pos.size();

	parts_ = new CGoGN::Algo::MovingObjects::ParticleCell2D<PFP>*[nbVertices];
	obstacles_ = new Obstacle*[nbVertices];

	PFP::VEC3 prev = pos[pos.size()-1];
	for(unsigned int i = 0; i < nbVertices ; ++i)
	{
		Obstacle* o = new Obstacle(pos[i], pos[(i+1)%nbVertices], pos[(i-1+nbVertices)%nbVertices], pos[(i+2)%nbVertices]);
		obstacles_[i] = o;

		Dart d = envMap_->getBelongingCell(pos[i]);
		CGoGN::Algo::MovingObjects::ParticleCell2D<PFP>* part = new CGoGN::Algo::MovingObjects::ParticleCell2D<PFP>(envMap_->map, d, pos[i], envMap_->position);
		parts_[i] = part;

		envMap_->pushObstacleInCells(o,d);
	}
}

void MovingObstacle::updateAgentNeighbors()
{
	agentNeighbors_.clear();

	for(unsigned int i = 0 ; i< nbVertices ; ++i)
	{
		std::vector<Agent*>& agents = envMap_->agentvect[parts_[i]->d];
		std::vector<Agent*>& neighborAgents = envMap_->neighborAgentvect[parts_[i]->d];

		for(std::vector<Agent*>::iterator it = agents.begin(); it != agents.end(); ++it)
		{
			float distSq = (parts_[i]->m_position - (*it)->part_.m_position).norm2();
			if(distSq < neighborDistSq_)
			{
				agentNeighbors_.push_back(std::make_pair(distSq, *it));
			}
		}

		for(std::vector<Agent*>::iterator it = neighborAgents.begin(); it != neighborAgents.end(); ++it)
		{
			float distSq = (parts_[i]->m_position - (*it)->part_.m_position).norm2();
			if(distSq < neighborDistSq_)
			{
				agentNeighbors_.push_back(std::make_pair(distSq, *it));
			}
		}
	}
}

void MovingObstacle::computePrefVelocity()
{

}

void MovingObstacle::computeNewVelocity()
{
	if(parts_[0]->m_position[1]<400 && newVelocity_[1]>0.0f)
		newVelocity_[1] = 0.2f;
	else if(parts_[0]->m_position[1]>-400)
		newVelocity_[1] = -0.2f;
	else
		newVelocity_[1] = 0.2f;

	if(agentNeighbors_.size()>0)
		newVelocity_ /= agentNeighbors_.size();

	for(std::vector<std::pair<float, Agent*> >::iterator it = agentNeighbors_.begin();
			it != agentNeighbors_.end();
			++it)
	{
		Agent* other = it->second;
		float relativeVelocity = newVelocity_ * other->velocity_;
		if(it->first<2.0f*2.0f || relativeVelocity<-0.0001f)
		{
			newVelocity_[1] *= 0.00001f;
		}
	}
}

void MovingObstacle::update()
{
	velocity_[0] = newVelocity_[0];
	velocity_[1] = newVelocity_[1];

	for(unsigned int i=0; i < nbVertices ; ++i)
	{
		Dart oldFace =  parts_[i]->d;

		VEC3 target = parts_[i]->m_position + (velocity_ * timeStep);
		parts_[i]->move(target);

		if(parts_[i]->crossCell != CGoGN::Algo::MovingObjects::NO_CROSS)
		{
			envMap_->obstacleChangeFace(obstacles_[i], parts_[i]->d, oldFace);
		}
	}

	for(unsigned int i=0; i < nbVertices ; ++i)
	{
		Obstacle* o = obstacles_[i];
		o->p1 = parts_[i]->m_position;
		o->p2 = parts_[(i+1)%nbVertices]->m_position;
		o->prevP = parts_[(i-1+nbVertices)%nbVertices]->m_position;
		o->nextP = parts_[(i+2)%nbVertices]->m_position;
	}
}