Commit a4eaaeba authored by Pierre Kraemer's avatar Pierre Kraemer

SocialAgents -> proprification

parent c36d4029
#include "agent.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_);
}
}
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));
part = new Algo::MovingObjects::ParticleCell2D<PFP>(sim->envMap.map,nearestDart,position,sim->envMap.position,sim->envMap.closeMark);
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;
part = new Algo::MovingObjects::ParticleCell2D<PFP>(sim->envMap.map,nearestDart,position,sim->envMap.position,sim->envMap.closeMark);
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));
part = new Algo::MovingObjects::ParticleCell2D<PFP>(sim->envMap.map,nearestDart,position,sim->envMap.position,sim->envMap.closeMark);
rangeSq = sqr(timeHorizonObst_ * maxSpeed_ + radius_);
}
}
Agent::~Agent()
{
}
Agent::~Agent()
{
}
void Agent::setGoalNo(size_t goal)
{
......@@ -354,10 +354,8 @@ void Agent::update()
velocity_[1] = newVelocity_[1];
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));
nearestDart = part->getCell();
}
part->move(VEC3(position_[0],position_[1],0));
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)
......
......@@ -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]);
}
// #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();
}
// #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);
agents_[i]->update();
envMap.pushAgentInCell(agents_[i],agents_[i]->part->d);
agents_[i]->treated=false;
}
envMap.updateMap();
// envMap.updateMap();
globalTime_ += timeStep_;
}
......
......@@ -40,9 +40,7 @@ class ParticleCell2D : public ParticleBase
DartMarker& obstacle;
bool chgCell;
ParticleCell2D() : chgCell(true) {};
ParticleCell2D() {}
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
void move(const VEC3& newCurrent)
{
// std::cout << "move to : " << newCurrent << std::endl;
prevPos = m_position;
chgCell=false;
switch(state) {
case VERTEX_ORBIT : vertexState(newCurrent); break;
case EDGE_ORBIT : edgeState(newCurrent); break;
case FACE_ORBIT : faceState(newCurrent); break;
if(!Geom::arePointsEquals(newCurrent, m_position)) {
// std::cout << "move to : " << newCurrent << std::endl;
prevPos = m_position;
switch(state) {
case VERTEX_ORBIT : vertexState(newCurrent); break;
case EDGE_ORBIT : edgeState(newCurrent); break;
case FACE_ORBIT : faceState(newCurrent); break;
}
}
this->display();
}
......
......@@ -69,8 +69,6 @@ void ParticleCell2D<PFP>::vertexState(const VEC3& current)
#endif
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)) {
state = VERTEX_ORBIT;
return;
......@@ -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(Algo::Geometry::isPointOnEdge<PFP>(m,d,m_positions,m_position));
chgCell=true;
if(sideOfEdge==Geom::ALIGNED)
sideOfEdge = getOrientationEdge(current,d);
......@@ -178,8 +174,6 @@ void ParticleCell2D<PFP>::faceState(const VEC3& current)
std::cout << "faceState" << d << std::endl;
#endif
// chgCell=true;
// 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(Algo::Geometry::isPointInConvexFace2D<PFP>(m,d,m_positions,m_position,true));
......@@ -200,9 +194,9 @@ void ParticleCell2D<PFP>::faceState(const VEC3& current)
if(dd==d) {
do {
switch (getOrientationEdge(current,d)) {
case Geom::LEFT : d=m.phi1(d);
case Geom::LEFT : d=m.phi1(d);
break;
case Geom::ALIGNED : m_position = current;
case Geom::ALIGNED :m_position = current;
edgeState(current);
return;
case Geom::RIGHT :
......
......@@ -159,13 +159,13 @@ bool isEdgeInOrIntersectingTetrahedron(VEC3 points[4], VEC3& point1, VEC3& point
}
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);
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
float b = (n * Dir) ;
if(fabs(b) < precision) //ray parallel to triangle
return NO_INTERSECTION ;
return NO_INTERSECTION ;
//compute intersection
T r = a / b ;
......
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