Skip to content

GitLab

Commit 14a82956 authored by pitiot's avatar pitiot
Browse files

version avec pb Dart==NIL

parent cb708f28
Hide whitespace changes
Inline Side-by-side
Showing with 339 additions and 228 deletions
include/env_map.h
View file @ 14a82956
......@@ -188,6 +188,7 @@ void displayMO(Obstacle * o);
* INLINE FUNCTIONS *
**************************************/
template <typename T>
inline void addElementToVector(std::vector<T>& a, T ag)
{
......@@ -245,6 +246,7 @@ inline void EnvMap::pushAgentInCells(Agent* agent, Dart d)
{
assert(map.getCurrentLevel() == map.getMaxLevel()) ;
// assert(std::find(agentvect[d].begin(), agentvect[d].end(), agent) == agentvect[d].end());
// map.check();
addElementToVector<Agent*>(agentvect[d],agent);
// agentvect[d].push_back(agent) ;
......@@ -265,6 +267,7 @@ inline void EnvMap::pushAgentInCells(Agent* agent, Dart d)
} while (dd != d) ;
}
inline void EnvMap::popAgentInCells(Agent* agent, Dart d)
{
assert(map.getCurrentLevel() == map.getMaxLevel()) ;
......@@ -284,6 +287,16 @@ inline void EnvMap::popAgentInCells(Agent* agent, Dart d)
}
dd = map.phi1(dd) ;
} while (dd != d) ;
TraversorF<PFP::MAP> tF(map);
for(Dart ddd = tF.begin() ; ddd != tF.end() ; ddd = tF.next())
{
if(std::find(agentvect[ddd].begin(), agentvect[ddd].end(), agent) != agentvect[ddd].end())
std::cout <<agent << " SO WRONG" <<ddd.index<< std::endl;
if(std::find(neighborAgentvect[ddd].begin(), neighborAgentvect[ddd].end(), agent) != neighborAgentvect[ddd].end())
std::cout <<agent<< " SO SO WRONG " <<ddd.index<< std::endl;
}
}
......@@ -297,7 +310,7 @@ inline void EnvMap::addObstAsNeighbor (Obstacle * o, const std::vector<Dart>& b
neighbor_cells->clear();
CellMarkerMemo<FACE> memo_mark(map);
CellMarkerMemo<FACE> OneRingMark(map);
memo_mark.unmarkAll();
for (std::vector<Dart>::const_iterator it =belonging_cells.begin();it<belonging_cells.end();++it)
memo_mark.mark(*it);
......@@ -306,16 +319,25 @@ inline void EnvMap::addObstAsNeighbor (Obstacle * o, const std::vector<Dart>& b
Dart first =NIL;
Dart d=NIL;
Dart dd=NIL;
// CGoGNout<<"beg : "<<(*it)<<CGoGNendl;
//boucle pour trouver une face du voisinage de l'obstacle ne contenant pas l'obstacle
/////////////////////////////////////////////boucle pour trouver une face du voisinage de l'obstacle ne contenant pas l'obstacle
// CGoGNout<<"debut neighbors cellules : ";
// for (std::vector<Dart>::const_iterator it =belonging_cells.begin();it<belonging_cells.end();++it)
// CGoGNout<<(*it).index<<" ; ";
// CGoGNout<<CGoGNendl;
do
{
beg = *it;
first = NIL;
d=beg;
do {
// CGoGNout<<"beg : " << beg;
do
{
dd=map.alpha1(map.alpha1(d));
do {
// CGoGNout<<"dd : " << dd;
do
{
if (!memo_mark.isMarked(dd))
{
first = dd;
......@@ -326,9 +348,11 @@ inline void EnvMap::addObstAsNeighbor (Obstacle * o, const std::vector<Dart>& b
d=map.phi1(d);
} while(first==NIL && d!=beg);
// CGoGNout<<CGoGNendl;
++it;
}while(first==NIL && it!=belonging_cells.end());
// assert(!buildingMark.isMarked(d)) ;
assert(first!=NIL) ;
// CGoGNout<<"first : "<<first<<CGoGNendl;
d=first;
......@@ -341,16 +365,7 @@ inline void EnvMap::addObstAsNeighbor (Obstacle * o, const std::vector<Dart>& b
}
find_next(o,&d, memo_mark);
if(d==NIL)
{
CGoGNout<<"cellule de début : "<<first<<CGoGNendl;
CGoGNout<<"cellules markées OneRing : "<<CGoGNendl;
std::vector<Dart> v=OneRingMark.get_markedCells();
for(std::vector<Dart>::iterator it=v.begin();it<v.end();++it)
{
CGoGNout<<(*it).index<<CGoGNendl;
}
}
// CGoGNout<<"d : "<<d<<CGoGNendl;
}while(!map.sameFace(d,first));
}
......
include/env_render.h
View file @ 14a82956
......@@ -71,6 +71,16 @@ inline void renderObstacle(EnvMap& m, MovingObstacle * obst, bool showBelonging=
for (unsigned int i = 0 ; i < (obst->nbVertices) ; ++i)
{
const VEC3& p = obst->getPosition(i) ;
if (obst->obstacles_[i]==(Obstacle*)0xf09f80 || obst->obstacles_[(i-1)%(obst->nbVertices)]==(Obstacle *)0xf09f80 )
{
glPointSize(10.0f) ;
glColor3f(1.0,0.0,0.0);
}
else
{
glPointSize(3.0f) ;
glColor3f(0.0,0.0,1.0);
}
glVertex3fv(p.data()) ;
}
glEnd() ;
......@@ -97,14 +107,14 @@ inline void renderObstacle(EnvMap& m, MovingObstacle * obst, bool showBelonging=
// }
// glEnd() ;
glLineWidth(5.0f) ;
glColor3f(1.0,1.0,0.0);
glBegin(GL_LINES) ;
VEC3 p = obst->getPosition(0) ;
glVertex3fv(p.data()) ;
p += 20.0f*obst->velocity_;
glVertex3fv(p.data()) ;
glEnd() ;
// glLineWidth(5.0f) ;
// glColor3f(1.0,1.0,0.0);
// glBegin(GL_LINES) ;
// VEC3 p = obst->getPosition(0) ;
// glVertex3fv(p.data()) ;
// p += 20.0f*obst->velocity_;
// glVertex3fv(p.data()) ;
// glEnd() ;
if (renderPath)
{
......@@ -148,9 +158,9 @@ inline void renderAgent(EnvMap& m, Agent* agent, bool showNeighborDist = false,
glLineWidth(1.0f) ;
VEC3 col = Utils::color_map_BCGYR(float(agent->agentNo)/float(agent->sim_->agents_.size()));
glColor3fv(col.data());
// glColor3f(c1,c2,c3) ;
// VEC3 col = Utils::color_map_BCGYR(float(agent->agentNo)/float(agent->sim_->agents_.size()));
// glColor3fv(col.data());
glColor3f(c1,c2,c3) ;
glBegin(GL_POLYGON) ;
for(unsigned int i = 0 ; i < 5 ; ++i)
glVertex3f(pos[0] + (cosT[i] * radius), pos[1] + (sinT[i] * radius), pos[2]+0.01f) ;
......
include/moving_obstacle.h
View file @ 14a82956
......@@ -26,7 +26,7 @@ public:
bool is_inside (VEC3 p);
void computePrefVelocity();
void computeNewVelocity();
std::vector<Dart> getMemoCross(const VEC3& pos, const VEC3& dest, Dart& d1, CGoGN::Algo::MovingObjects::ParticleCell2DMemo<PFP> * registering_part);
std::vector<Dart> getMemoCross(const VEC3& pos, const VEC3& dest, Dart& d1, Dart& d2);
VEC3 getDilatedPosition(unsigned int ind); //vertex position with velocity dilatation
VEC3 getPosition(unsigned int ind); // vertex position
void update();
......@@ -37,10 +37,7 @@ public:
void updateMesh();
unsigned int nbVertices;
std::vector<PFP::VEC3> vertices;
CGoGN::Algo::MovingObjects::ParticleCell2D<PFP>* * parts_;
PFP::MAP map;
VertexAttribute<VEC3> position;
VertexAttribute<VEC3> normal;
......@@ -59,8 +56,6 @@ public:
// std::vector<float> verticesAngle;
// float rigidity;
// float gravity_dist;
VEC3 center;
int nb_agents_voisins;
int nb_register_cells;
......@@ -90,7 +85,6 @@ public:
static float neighborDist_;
static float neighborDistSq_;
static float maxSpeed_;
// float obstacle_range;
static float timeHorizonObst_;
float velocity_factor;
float color1;
......
include/obstacle.h
View file @ 14a82956
......@@ -6,7 +6,7 @@
class Obstacle
{
public:
Obstacle(const VEC3 point1, const VEC3 point2, const VEC3 prevPoint, const VEC3 nextPoint,
Obstacle(const VEC3& point1, const VEC3& point2, const VEC3& prevPoint, const VEC3& nextPoint,
MovingObstacle * moving1, unsigned int ind) :
p1(point1), p2(point2), prevP(prevPoint), nextP(nextPoint),
mo(moving1), index(ind)
......@@ -17,10 +17,10 @@ public:
// nextP[2] = 0 ;
}
VEC3 p1 ;
VEC3 p2 ;
VEC3 prevP ;
VEC3 nextP ;
const VEC3& p1 ;
const VEC3& p2 ;
const VEC3& prevP ;
const VEC3& nextP ;
MovingObstacle * mo ;
unsigned int index ;
} ;
......
include/simulator.h
View file @ 14a82956
......@@ -80,7 +80,7 @@ public:
class Simulator
{
public:
Simulator(unsigned int config, unsigned int minSize, unsigned int nbAgent=1000, unsigned int nbObst=20) ;
Simulator(unsigned int config, unsigned int minSize, unsigned int nbAgent=1000, unsigned int nbObst=20, bool resolution =true) ;
~Simulator() ;
......
include/socialAgents.ui
View file @ 14a82956
......@@ -7,7 +7,7 @@
<x>0</x>
<y>0</y>
<width>219</width>
<height>457</height>
<height>684</height>
</rect>
</property>
<property name="windowTitle">
......@@ -112,7 +112,7 @@
</property>
</widget>
</item>
<item>
<item>
<widget class="QCheckBox" name="check_drawObstPath">
<property name="text">
<string>draw obst path</string>
......@@ -149,6 +149,16 @@
</property>
</widget>
</item>
<item>
<widget class="QCheckBox" name="checkAgent">
<property name="text">
<string>Agents n°</string>
</property>
</widget>
</item>
<item>
<widget class="QSpinBox" name="AgentSelect"/>
</item>
<item>
<spacer name="verticalSpacer">
<property name="orientation">
......
include/viewer.h
View file @ 14a82956
......@@ -51,7 +51,7 @@ class SocialAgents : public Utils::QT::SimpleQT
Q_OBJECT
public:
SocialAgents(unsigned int config, unsigned int minSize, unsigned int nbAgent , unsigned int nbObst , unsigned int iterations) ;
SocialAgents(unsigned int config, unsigned int minSize, unsigned int nbAgent , unsigned int nbObst ,bool resolution, unsigned int iterations) ;
void initGUI() ;
......@@ -103,9 +103,11 @@ public:
bool drawObstPredictionTri ;
bool drawObstPath ;
bool draw_dart;
bool target_Agent;
bool draw_posX;
bool draw_elipse;
unsigned int dartSlider;
unsigned int agentSlider;
int posXSlider;
int posYSlider;
......@@ -206,4 +208,14 @@ public slots:
draw_elipse = b;
updateGL();
}
void slot_Agent(bool b)
{
target_Agent = b ;
updateGL() ;
}
void slot_AgentSlider(int i)
{
agentSlider = i ;
updateGL() ;
}
} ;
src/agent.cpp
View file @ 14a82956
......@@ -40,6 +40,7 @@ Agent::Agent(Simulator* sim, const VEC3& start, const VEC3& goal, Dart d) :
sim_(sim),
alive(true)
{
init(start,goal);
}
......@@ -70,9 +71,17 @@ void Agent::init(const VEC3& start, const VEC3& goal)
goals_.push_back(start) ;
float ratio = (80.0f + rand() % 20) / 100.0f ;
maxSpeed_ = averageMaxSpeed_ * ratio ; // from 80% to 100% of averageMaxSpeed_
color1 = 1.0f * ratio ; // red = high speed agents
color2 = 1.0f * 5 * (1 - ratio) ; // green = low speed agents
color3 = 0 ;
// color1 = 1.0f * ratio ; // red = high speed agents
// color2 = 1.0f * 5 * (1 - ratio) ; // green = low speed agents
// color3 = 0 ;
color1 = 1.0f ; // red = high speed agents
color2 = 0.0f; // green = low speed agents
color3 = 0.0f ;
// if (agentNo==0)
// {
// color1=0;
// color2 = 1.0f;
// }
for (unsigned int i=0 ; i<4 ; ++i) meanVelocity_[i].set(0) ;
agentNeighbors_.reserve(maxNeighbors_ * 2) ;
obstacleNeighbors_.reserve(maxNeighbors_ * 2) ;
......@@ -545,7 +554,7 @@ void Agent::computeNewVelocity()
{
// The objective is to compute the sum of forces exerted on the agent.
double collision_softening_factor;
float ag_ambient_damping = 1.0;
// float ag_ambient_damping = 1.0;
double mass_var = 0.95;
double average_mass = 1.0;
// double average_radius = 20.f;
......@@ -697,6 +706,7 @@ void Agent::computeNewVelocity()
float previous_distSq = previous_relativePosition.norm2();
float previous_dist = sqrt(previous_distSq);
// const VEC3 u_other(relativePosition);
VEC3 u_other(relativePosition);
u_other = relativePosition;
......@@ -704,6 +714,7 @@ void Agent::computeNewVelocity()
// cerr << "dist=" << dist << " combinedRadius=" << combinedRadius << endl;
if(dist < combinedRadius)
{
collision_softening_factor = pow(1 - dist/combinedRadius,ag_power);
......@@ -734,29 +745,29 @@ void Agent::computeNewVelocity()
//------- color depending on velocity -------------------------------
double vmax = 0.5*maxSpeed_;
VEC3 min_vx_color(1.0,0.0,0.0); // agents going towards the positive xs are red
VEC3 max_vx_color(0.0,0.1,0.1); // agents going towards the negative xs are cyan
VEC3 min_vy_color(0.0,1.0,0.0);
VEC3 max_vy_color(1.0,0.0,1.1);
double alpha_x = (newVelocity_[0]+vmax) / (2*vmax);
double alpha_y = (newVelocity_[1]+vmax) / (2*vmax);
double tmp_color1 =
abs(alpha_x-0.5)*(alpha_x * max_vx_color[0] + (1-alpha_x)*min_vx_color[0]) +
abs(alpha_y-0.5)*(alpha_y * max_vy_color[0] + (1-alpha_y)*min_vy_color[0]);
double tmp_color2 =
abs(alpha_x-0.5)*(alpha_x * max_vx_color[1] + (1-alpha_x)*min_vx_color[1]) +
abs(alpha_y-0.5)*(alpha_y * max_vy_color[1] + (1-alpha_y)*min_vy_color[1]);
double tmp_color3 =
abs(alpha_x-0.5)*(alpha_x * max_vx_color[2] + (1-alpha_x)*min_vx_color[2]) +
abs(alpha_y-0.5)*(alpha_y * max_vy_color[2] + (1-alpha_y)*min_vy_color[2]);
float blend = 0.1;
color1 = blend*tmp_color1 + (1-blend)*color1;
color2 = blend*tmp_color2 + (1-blend)*color2;
color3 = blend*tmp_color3 + (1-blend)*color3;
// double vmax = 0.5*maxSpeed_;
// VEC3 min_vx_color(1.0,0.0,0.0); // agents going towards the positive xs are red
// VEC3 max_vx_color(0.0,0.1,0.1); // agents going towards the negative xs are cyan
// VEC3 min_vy_color(0.0,1.0,0.0);
// VEC3 max_vy_color(1.0,0.0,1.1);
//
// double alpha_x = (newVelocity_[0]+vmax) / (2*vmax);
// double alpha_y = (newVelocity_[1]+vmax) / (2*vmax);
//
// double tmp_color1 =
// abs(alpha_x-0.5)*(alpha_x * max_vx_color[0] + (1-alpha_x)*min_vx_color[0]) +
// abs(alpha_y-0.5)*(alpha_y * max_vy_color[0] + (1-alpha_y)*min_vy_color[0]);
// double tmp_color2 =
// abs(alpha_x-0.5)*(alpha_x * max_vx_color[1] + (1-alpha_x)*min_vx_color[1]) +
// abs(alpha_y-0.5)*(alpha_y * max_vy_color[1] + (1-alpha_y)*min_vy_color[1]);
// double tmp_color3 =
// abs(alpha_x-0.5)*(alpha_x * max_vx_color[2] + (1-alpha_x)*min_vx_color[2]) +
// abs(alpha_y-0.5)*(alpha_y * max_vy_color[2] + (1-alpha_y)*min_vy_color[2]);
//
// float blend = 0.1;
// color1 = blend*tmp_color1 + (1-blend)*color1;
// color2 = blend*tmp_color2 + (1-blend)*color2;
// color3 = blend*tmp_color3 + (1-blend)*color3;
}
///* Search for the best new velocity. */
......
src/env_map.cpp
View file @ 14a82956
......@@ -536,23 +536,24 @@ void EnvMap::pushObstacleInCells(Obstacle * o)// réenregistre l'obstacle en que
if (mo != NULL)
{
int n = o->index;
VEC3 p1 = mo->parts_[n]->getPosition();
VEC3 p2 = mo->parts_[(n+1)%mo->nbVertices]->getPosition();
VEC3 p1 = o->p1;
VEC3 p2 = o->p2;
Dart d1=NIL;
Dart d2=NIL;
std::vector<Dart> memo;
CGoGN::Algo::MovingObjects::ParticleCell2DMemo<PFP> * registering_part = new CGoGN::Algo::MovingObjects::ParticleCell2DMemo<PFP>(map, mo->parts_[n]->d,mo->parts_[n]->getPosition(),position);
d1=mo->parts_[n]->d;
memo = mo->getMemoCross(p1,p2,d1,registering_part);
d2=registering_part->d;
memo = mo->getMemoCross(p1,p2,d1,d2);
if(map.sameFace(d1,d2))
{
pushObstacleInOneRingCells(o , d1 , n);
}
else
// if(map.sameFace(d1,d2))
// {
// pushObstacleInOneRingCells(o , d1 , n);
// }
// else
{
pushObstacleInCells(o, n, memo);
}
}
......@@ -561,20 +562,20 @@ void EnvMap::pushObstacleInCells(Obstacle * o)// réenregistre l'obstacle en que
Dart EnvMap::popAndPushObstacleInCells(Obstacle* o, int n)// maj de l'enregistrement
{
MovingObstacle * mo = o->mo;
VEC3 p1 = mo->parts_[n]->getPosition();
VEC3 p2 = mo->parts_[(n+1)%mo->nbVertices]->getPosition();
VEC3 p1 = o->p1;
VEC3 p2 = o->p2;
Dart d1=NIL;
Dart d2=NIL;
std::vector<Dart> memo;
CGoGN::Algo::MovingObjects::ParticleCell2DMemo<PFP> * registering_part = new CGoGN::Algo::MovingObjects::ParticleCell2DMemo<PFP>(map, mo->parts_[n]->d,mo->parts_[n]->getPosition(),position);
d1=mo->parts_[n]->d;
// bool modif=false;
// if(p1->crossCell != CGoGN::Algo::MovingObjects::NO_CROSS
// || p2->crossCell != CGoGN::Algo::MovingObjects::NO_CROSS)
// {
memo = mo->getMemoCross(p1,p2,d1,registering_part);
d2=registering_part->d;
memo = mo->getMemoCross(p1,p2,d1,d2);
// memo.sort();
// modif=true;
// }
......@@ -590,11 +591,11 @@ Dart EnvMap::popAndPushObstacleInCells(Obstacle* o, int n)// maj de l'enregistre
{
popObstacleInCells(o, n);
if(map.sameFace(d1,d2))
{
pushObstacleInOneRingCells(o , d1 , n);
}
else
// if(map.sameFace(d1,d2))
// {
// pushObstacleInOneRingCells(o , d1 , n);
// }
// else
{
pushObstacleInCells(o, n, memo);
}
......@@ -611,9 +612,9 @@ void EnvMap::pushObstacleInOneRingCells(Obstacle * o, Dart d, int n)
addElementToVector<Obstacle*>(obstvect[d],o);
mo->belonging_cells[n].clear();
// mo->belonging_cells[n].clear();
mo->belonging_cells[n].push_back(d);
mo->neighbor_cells[n].clear();
// mo->neighbor_cells[n].clear();
Dart dd = d;
do
......@@ -639,7 +640,8 @@ void EnvMap::pushObstacleInCells(Obstacle* o, int n, const std::vector<Dart>& me
assert(!memo_cross.empty());
MovingObstacle * mo = o->mo;
mo->belonging_cells[n].clear();
// mo->belonging_cells[n].clear();
// mo->neighbor_cells[n].clear();
mo->belonging_cells[n] = memo_cross;
for (std::vector<Dart>::iterator it = mo->belonging_cells[n].begin(); it != mo->belonging_cells[n].end(); ++it)
......@@ -675,6 +677,8 @@ void EnvMap::popObstacleInCells(Obstacle* o, int n)
{
removeElementFromVector<Obstacle*>(neighborObstvect[*it], o) ;
}
mo->belonging_cells[n].clear();
mo->neighbor_cells[n].clear();
}
}
......@@ -751,6 +755,7 @@ void EnvMap::refine()
if (subdivisable)
{
// CGoGNout<<"debut refine "<<d.index <<CGoGNendl;
if (fLevel == -1)
fLevel = map.faceLevel(old) ;
......@@ -786,28 +791,6 @@ void EnvMap::refine()
}
map.setCurrentLevel(map.getMaxLevel()) ;
//agents contained in the subdivided cell are pushed correctly
for (PFP::AGENTS::iterator ait = oldAgents.begin(); ait != oldAgents.end(); ++ait)
{
resetAgentInFace(*ait) ;
pushAgentInCells(*ait, (*ait)->part_.d) ;
}
//same for obstacles contained
for (PFP::OBSTACLEVECT::iterator ait = oldObst.begin(); ait != oldObst.end(); ++ait)
{
resetPartSubdiv(*ait);
pushObstacleInCells(*ait);
}
//same for adjacent obstacles
for (PFP::OBSTACLEVECT::iterator ait = oldNeighborObst.begin();
ait != oldNeighborObst.end(); ++ait)
{
pushObstacleInCells(*ait) ;
}
//retrieve neighbors agents from onering cells
dd = old ;
do
......@@ -854,6 +837,32 @@ void EnvMap::refine()
d3 = map.phi1(d3) ;
} while (d3 != centerFace) ;
}
//agents contained in the subdivided cell are pushed correctly
for (PFP::AGENTS::iterator ait = oldAgents.begin(); ait != oldAgents.end(); ++ait)
{
resetAgentInFace(*ait) ;
pushAgentInCells(*ait, (*ait)->part_.d) ;
}
//same for obstacles contained
for (PFP::OBSTACLEVECT::iterator ait = oldObst.begin(); ait != oldObst.end(); ++ait)
{
resetPartSubdiv(*ait);
pushObstacleInCells(*ait);
}
//same for adjacent obstacles
for (PFP::OBSTACLEVECT::iterator ait = oldNeighborObst.begin();
ait != oldNeighborObst.end(); ++ait)
{
pushObstacleInCells(*ait) ;
}
// CGoGNout<<"fin refine"<<CGoGNendl;
}
}
}
......@@ -1123,15 +1132,18 @@ void EnvMap::updateMap()
assert(map.getCurrentLevel() == map.getMaxLevel()) ;
refine();
coarse();
// coarse();
}
void EnvMap::resetAgentInFace(Agent* agent)
{
Dart old = agent->part_.d;
VEC3 pos = agent->part_.getPosition() ;
agent->part_.move(Algo::Geometry::faceCentroid<PFP>(map, agent->part_.d, position)) ;
agent->part_.ParticleBase<PFP>::move(Algo::Geometry::faceCentroid<PFP>(map, old, position)) ;
agent->part_.setState(FACE) ;
agent->part_.move(pos) ;
}
#endif
......
src/moving_obstacle.cpp
View file @ 14a82956
......@@ -156,9 +156,12 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
Dart d = sim_->envMap_.getBelongingCell(pos[i]);
CGoGN::Algo::MovingObjects::ParticleCell2D<PFP>* part = new CGoGN::Algo::MovingObjects::ParticleCell2D<PFP>(sim_->envMap_.map, d, pos[i], sim_->envMap_.position);
parts_[i] = part;
if(i==0)
dDir = d;
}
center /= nbVertices;
front=(parts_[1]->getPosition() + parts_[2]->getPosition()) / 2;
front=(position[1] + position[2]) / 2;
//-------- code ajoute par Arash pour les obstacles rectangulaires --------------
......@@ -172,14 +175,15 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
if (spinning && parent==NULL) //départ face à la cible en cas d'obstacles pouvant effectuer des rotations
{
CGoGNout<<"orienté "<<index<<CGoGNendl;
angle = get_angle(goals_[curGoal_] - center,front - center);
Dart d = groundFace;
for (unsigned int i = 0; i < nbVertices; ++i)
{
position[d]+=rotate(position[d], center, angle);
d = map.phi1(d);
parts_[i]->move(position[d]);
d = map.phi1(d);
}
angle=0;
front=(position[1] + position[2]) / 2;
......@@ -188,12 +192,12 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
for (unsigned int i = 0; i < nbVertices; ++i)
{