Commit a4eaaeba authored by Pierre Kraemer's avatar Pierre Kraemer
Browse files

SocialAgents -> proprification

parent c36d4029
#include "agent.h" #include "agent.h"
#include "simulator.h" #include "simulator.h"
Agent::Agent(Simulator* sim, size_t agentNo) : agentNeighbors_(), maxNeighbors_(0), maxSpeed_(0.0f), neighborDist_(0.0f), newVelocity_(), obstacleNeighbors_(), orcaLines_(), position_(), prefVelocity_(), radius_(0.0f), sim_(sim), timeHorizon_(0.0f), timeHorizonObst_(0.0f), velocity_(), agentNo_(agentNo), goalNo_(agentNo), treated(false) Agent::Agent(Simulator* sim, size_t agentNo) : agentNeighbors_(), maxNeighbors_(0), maxSpeed_(0.0f), neighborDist_(0.0f), newVelocity_(), obstacleNeighbors_(), orcaLines_(), position_(), prefVelocity_(), radius_(0.0f), sim_(sim), timeHorizon_(0.0f), timeHorizonObst_(0.0f), velocity_(), agentNo_(agentNo), goalNo_(agentNo), treated(false)
{ {
rangeSq = sqr(timeHorizonObst_ * maxSpeed_ + radius_); rangeSq = sqr(timeHorizonObst_ * maxSpeed_ + radius_);
} }
Agent::Agent(Simulator* sim, const VEC_A& position, size_t agentNo) : agentNeighbors_(), maxNeighbors_(sim->defaultAgent_->maxNeighbors_), maxSpeed_(sim->defaultAgent_->maxSpeed_), neighborDist_(sim->defaultAgent_->neighborDist_), newVelocity_(sim->defaultAgent_->velocity_), obstacleNeighbors_(), orcaLines_(), position_(position), prefVelocity_(), radius_(sim->defaultAgent_->radius_), sim_(sim), timeHorizon_(sim->defaultAgent_->timeHorizon_), timeHorizonObst_(sim->defaultAgent_->timeHorizonObst_), velocity_(sim->defaultAgent_->velocity_), agentNo_(agentNo), goalNo_(agentNo), treated(false) Agent::Agent(Simulator* sim, const VEC_A& position, size_t agentNo) : agentNeighbors_(), maxNeighbors_(sim->defaultAgent_->maxNeighbors_), maxSpeed_(sim->defaultAgent_->maxSpeed_), neighborDist_(sim->defaultAgent_->neighborDist_), newVelocity_(sim->defaultAgent_->velocity_), obstacleNeighbors_(), orcaLines_(), position_(position), prefVelocity_(), radius_(sim->defaultAgent_->radius_), sim_(sim), timeHorizon_(sim->defaultAgent_->timeHorizon_), timeHorizonObst_(sim->defaultAgent_->timeHorizonObst_), velocity_(sim->defaultAgent_->velocity_), agentNo_(agentNo), goalNo_(agentNo), treated(false)
{ {
nearestDart = sim->envMap.getBelongingCell(VEC3(position[0],position[1],0)); nearestDart = sim->envMap.getBelongingCell(VEC3(position[0],position[1],0));
part = new Algo::MovingObjects::ParticleCell2D<PFP>(sim->envMap.map,nearestDart,position,sim->envMap.position,sim->envMap.closeMark); part = new Algo::MovingObjects::ParticleCell2D<PFP>(sim->envMap.map,nearestDart,position,sim->envMap.position,sim->envMap.closeMark);
rangeSq = sqr(timeHorizonObst_ * maxSpeed_ + radius_); rangeSq = sqr(timeHorizonObst_ * maxSpeed_ + radius_);
} }
Agent::Agent(Simulator* sim, const VEC_A& position, size_t agentNo, Dart d) : agentNeighbors_(), maxNeighbors_(sim->defaultAgent_->maxNeighbors_), maxSpeed_(sim->defaultAgent_->maxSpeed_), neighborDist_(sim->defaultAgent_->neighborDist_), newVelocity_(sim->defaultAgent_->velocity_), obstacleNeighbors_(), orcaLines_(), position_(position), prefVelocity_(), radius_(sim->defaultAgent_->radius_), sim_(sim), timeHorizon_(sim->defaultAgent_->timeHorizon_), timeHorizonObst_(sim->defaultAgent_->timeHorizonObst_), velocity_(sim->defaultAgent_->velocity_), agentNo_(agentNo), goalNo_(agentNo), treated(false) Agent::Agent(Simulator* sim, const VEC_A& position, size_t agentNo, Dart d) : agentNeighbors_(), maxNeighbors_(sim->defaultAgent_->maxNeighbors_), maxSpeed_(sim->defaultAgent_->maxSpeed_), neighborDist_(sim->defaultAgent_->neighborDist_), newVelocity_(sim->defaultAgent_->velocity_), obstacleNeighbors_(), orcaLines_(), position_(position), prefVelocity_(), radius_(sim->defaultAgent_->radius_), sim_(sim), timeHorizon_(sim->defaultAgent_->timeHorizon_), timeHorizonObst_(sim->defaultAgent_->timeHorizonObst_), velocity_(sim->defaultAgent_->velocity_), agentNo_(agentNo), goalNo_(agentNo), treated(false)
{ {
nearestDart = d; nearestDart = d;
part = new Algo::MovingObjects::ParticleCell2D<PFP>(sim->envMap.map,nearestDart,position,sim->envMap.position,sim->envMap.closeMark); part = new Algo::MovingObjects::ParticleCell2D<PFP>(sim->envMap.map,nearestDart,position,sim->envMap.position,sim->envMap.closeMark);
rangeSq = sqr(timeHorizonObst_ * maxSpeed_ + radius_); rangeSq = sqr(timeHorizonObst_ * maxSpeed_ + radius_);
} }
Agent::Agent(Simulator* sim, const VEC_A& position, float neighborDist, size_t maxNeighbors, float timeHorizon, float timeHorizonObst, float radius, const VEC_A& velocity, float maxSpeed, size_t agentNo) : agentNeighbors_(), maxNeighbors_(maxNeighbors), maxSpeed_(maxSpeed), neighborDist_(neighborDist), newVelocity_(velocity), obstacleNeighbors_(), orcaLines_(), position_(position), prefVelocity_(), radius_(radius), sim_(sim), timeHorizon_(timeHorizon), timeHorizonObst_(timeHorizonObst), velocity_(velocity), agentNo_(agentNo), goalNo_(agentNo), treated(false) Agent::Agent(Simulator* sim, const VEC_A& position, float neighborDist, size_t maxNeighbors, float timeHorizon, float timeHorizonObst, float radius, const VEC_A& velocity, float maxSpeed, size_t agentNo) : agentNeighbors_(), maxNeighbors_(maxNeighbors), maxSpeed_(maxSpeed), neighborDist_(neighborDist), newVelocity_(velocity), obstacleNeighbors_(), orcaLines_(), position_(position), prefVelocity_(), radius_(radius), sim_(sim), timeHorizon_(timeHorizon), timeHorizonObst_(timeHorizonObst), velocity_(velocity), agentNo_(agentNo), goalNo_(agentNo), treated(false)
{ {
nearestDart = sim->envMap.getBelongingCell(VEC3(position[0],position[1],0)); nearestDart = sim->envMap.getBelongingCell(VEC3(position[0],position[1],0));
part = new Algo::MovingObjects::ParticleCell2D<PFP>(sim->envMap.map,nearestDart,position,sim->envMap.position,sim->envMap.closeMark); part = new Algo::MovingObjects::ParticleCell2D<PFP>(sim->envMap.map,nearestDart,position,sim->envMap.position,sim->envMap.closeMark);
rangeSq = sqr(timeHorizonObst_ * maxSpeed_ + radius_); rangeSq = sqr(timeHorizonObst_ * maxSpeed_ + radius_);
} }
Agent::~Agent() Agent::~Agent()
{ {
} }
void Agent::setGoalNo(size_t goal) void Agent::setGoalNo(size_t goal)
{ {
...@@ -354,10 +354,8 @@ void Agent::update() ...@@ -354,10 +354,8 @@ void Agent::update()
velocity_[1] = newVelocity_[1]; velocity_[1] = newVelocity_[1];
position_ += velocity_ * sim_->timeStep_; position_ += velocity_ * sim_->timeStep_;
if(position_[0]!=part->m_position[0] || position_[1]!=part->m_position[1]) { part->move(VEC3(position_[0],position_[1],0));
part->move(VEC3(position_[0],position_[1],0)); nearestDart = part->getCell();
nearestDart = part->getCell();
}
} }
bool linearProgram1(const std::vector<Line>& lines, size_t lineNo, float radius, const Agent::Agent::VEC_A& optVelocity, bool directionOpt, Agent::VEC_A& result) bool linearProgram1(const std::vector<Line>& lines, size_t lineNo, float radius, const Agent::Agent::VEC_A& optVelocity, bool directionOpt, Agent::VEC_A& result)
......
...@@ -96,25 +96,25 @@ void Simulator::doStep() ...@@ -96,25 +96,25 @@ void Simulator::doStep()
} }
for (int i = 0; i < static_cast<int>(agents_.size()); ++i) { for (unsigned int i = 0; i < agents_.size(); ++i) {
envMap.linkAgentWithCells(agents_[i]); envMap.linkAgentWithCells(agents_[i]);
} }
// #pragma omp parallel for // #pragma omp parallel for
for (int i = 0; i < static_cast<int>(agents_.size()); ++i) { for (unsigned int i = 0; i < agents_.size(); ++i) {
agents_[i]->computeNewVelocity(); agents_[i]->computeNewVelocity();
} }
// #pragma omp parallel for // #pragma omp parallel for
for (int i = 0; i < static_cast<int>(agents_.size()); ++i) { for (unsigned int i = 0; i < agents_.size(); ++i) {
envMap.popAgentInCell(agents_[i],agents_[i]->part->d); envMap.popAgentInCell(agents_[i],agents_[i]->part->d);
agents_[i]->update(); agents_[i]->update();
envMap.pushAgentInCell(agents_[i],agents_[i]->part->d); envMap.pushAgentInCell(agents_[i],agents_[i]->part->d);
agents_[i]->treated=false; agents_[i]->treated=false;
} }
envMap.updateMap(); // envMap.updateMap();
globalTime_ += timeStep_; globalTime_ += timeStep_;
} }
......
...@@ -40,9 +40,7 @@ class ParticleCell2D : public ParticleBase ...@@ -40,9 +40,7 @@ class ParticleCell2D : public ParticleBase
DartMarker& obstacle; DartMarker& obstacle;
bool chgCell; ParticleCell2D() {}
ParticleCell2D() : chgCell(true) {};
ParticleCell2D(Map& map, Dart belonging_cell, VEC3 pos, TAB_POS tabPos, DartMarker& obst) : ParticleBase(pos), m(map), d(belonging_cell), m_positions(tabPos), obstacle(obst) ParticleCell2D(Map& map, Dart belonging_cell, VEC3 pos, TAB_POS tabPos, DartMarker& obst) : ParticleBase(pos), m(map), d(belonging_cell), m_positions(tabPos), obstacle(obst)
{ {
...@@ -73,18 +71,16 @@ class ParticleCell2D : public ParticleBase ...@@ -73,18 +71,16 @@ class ParticleCell2D : public ParticleBase
void move(const VEC3& newCurrent) void move(const VEC3& newCurrent)
{ {
// std::cout << "move to : " << newCurrent << std::endl; if(!Geom::arePointsEquals(newCurrent, m_position)) {
prevPos = m_position; // std::cout << "move to : " << newCurrent << std::endl;
prevPos = m_position;
chgCell=false;
switch(state) {
switch(state) { case VERTEX_ORBIT : vertexState(newCurrent); break;
case VERTEX_ORBIT : vertexState(newCurrent); break; case EDGE_ORBIT : edgeState(newCurrent); break;
case EDGE_ORBIT : edgeState(newCurrent); break; case FACE_ORBIT : faceState(newCurrent); break;
case FACE_ORBIT : faceState(newCurrent); break; }
} }
this->display();
} }
......
...@@ -69,8 +69,6 @@ void ParticleCell2D<PFP>::vertexState(const VEC3& current) ...@@ -69,8 +69,6 @@ void ParticleCell2D<PFP>::vertexState(const VEC3& current)
#endif #endif
assert(std::isfinite(current[0]) && std::isfinite(current[1]) && std::isfinite(current[2])); assert(std::isfinite(current[0]) && std::isfinite(current[1]) && std::isfinite(current[2]));
chgCell=true;
if(Algo::Geometry::isPointOnVertex<PFP>(m,d,m_positions,current)) { if(Algo::Geometry::isPointOnVertex<PFP>(m,d,m_positions,current)) {
state = VERTEX_ORBIT; state = VERTEX_ORBIT;
return; return;
...@@ -135,8 +133,6 @@ void ParticleCell2D<PFP>::edgeState(const VEC3& current, Geom::Orientation2D sid ...@@ -135,8 +133,6 @@ void ParticleCell2D<PFP>::edgeState(const VEC3& current, Geom::Orientation2D sid
assert(std::isfinite(current[0]) && std::isfinite(current[1]) && std::isfinite(current[2])); assert(std::isfinite(current[0]) && std::isfinite(current[1]) && std::isfinite(current[2]));
// assert(Algo::Geometry::isPointOnEdge<PFP>(m,d,m_positions,m_position)); // assert(Algo::Geometry::isPointOnEdge<PFP>(m,d,m_positions,m_position));
chgCell=true;
if(sideOfEdge==Geom::ALIGNED) if(sideOfEdge==Geom::ALIGNED)
sideOfEdge = getOrientationEdge(current,d); sideOfEdge = getOrientationEdge(current,d);
...@@ -178,8 +174,6 @@ void ParticleCell2D<PFP>::faceState(const VEC3& current) ...@@ -178,8 +174,6 @@ void ParticleCell2D<PFP>::faceState(const VEC3& current)
std::cout << "faceState" << d << std::endl; std::cout << "faceState" << d << std::endl;
#endif #endif
// chgCell=true;
// assert(std::isfinite(m_position[0]) && std::isfinite(m_position[1]) && std::isfinite(m_position[2])); // assert(std::isfinite(m_position[0]) && std::isfinite(m_position[1]) && std::isfinite(m_position[2]));
// assert(std::isfinite(current[0]) && std::isfinite(current[1]) && std::isfinite(current[2])); // assert(std::isfinite(current[0]) && std::isfinite(current[1]) && std::isfinite(current[2]));
// assert(Algo::Geometry::isPointInConvexFace2D<PFP>(m,d,m_positions,m_position,true)); // assert(Algo::Geometry::isPointInConvexFace2D<PFP>(m,d,m_positions,m_position,true));
...@@ -200,9 +194,9 @@ void ParticleCell2D<PFP>::faceState(const VEC3& current) ...@@ -200,9 +194,9 @@ void ParticleCell2D<PFP>::faceState(const VEC3& current)
if(dd==d) { if(dd==d) {
do { do {
switch (getOrientationEdge(current,d)) { switch (getOrientationEdge(current,d)) {
case Geom::LEFT : d=m.phi1(d); case Geom::LEFT : d=m.phi1(d);
break; break;
case Geom::ALIGNED : m_position = current; case Geom::ALIGNED :m_position = current;
edgeState(current); edgeState(current);
return; return;
case Geom::RIGHT : case Geom::RIGHT :
......
...@@ -159,13 +159,13 @@ bool isEdgeInOrIntersectingTetrahedron(VEC3 points[4], VEC3& point1, VEC3& point ...@@ -159,13 +159,13 @@ bool isEdgeInOrIntersectingTetrahedron(VEC3 points[4], VEC3& point1, VEC3& point
} }
template <typename VEC3> template <typename VEC3>
bool arePointsEquals(const VEC3& point1,const VEC3& point2) bool arePointsEquals(const VEC3& point1, const VEC3& point2)
{ {
typedef typename VEC3::DATA_TYPE T ;
VEC3 v(point1 - point2); VEC3 v(point1 - point2);
return v.norm2() <= T(3)*std::numeric_limits<T>::min(); #define PRECISION 1e-20
return v.norm2() <= PRECISION ;
#undef PRECISION
} }
} }
......
...@@ -236,7 +236,7 @@ Intersection intersectionSegmentTriangle(const VEC3& PA, const VEC3& PB, const V ...@@ -236,7 +236,7 @@ Intersection intersectionSegmentTriangle(const VEC3& PA, const VEC3& PB, const V
float b = (n * Dir) ; float b = (n * Dir) ;
if(fabs(b) < precision) //ray parallel to triangle if(fabs(b) < precision) //ray parallel to triangle
return NO_INTERSECTION ; return NO_INTERSECTION ;
//compute intersection //compute intersection
T r = a / b ; T r = a / b ;
......
Supports Markdown
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