Création d'un compte pour un collaborateur extérieur au laboratoire depuis l'intranet ICube : https://intranet.icube.unistra.fr/fr/labs/member/profile

Commit b93f517a authored by Thomas's avatar Thomas
Browse files

hop

parent c4365042
#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;
}
}
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment