Commit 762c7fdb authored by Jund Thomas's avatar Jund Thomas

masse ressort pour limace pour tester : ./bin/socialAgents 5 41.5 0 1 : pas de...

masse ressort pour limace pour tester : ./bin/socialAgents 5 41.5 0 1 : pas de temps modifié pour augmenter rigidité obstacle
parent 622c61be
......@@ -328,7 +328,7 @@ inline void EnvMap::addObstAsNeighbor (Obstacle * o, const std::vector<Dart>& b
} while(first==NIL && d!=beg);
++it;
}while(first==NIL && it!=belonging_cells.end());
assert(!buildingMark.isMarked(d)) ;
// assert(!buildingMark.isMarked(d)) ;
assert(first!=NIL) ;
// CGoGNout<<"first : "<<first<<CGoGNendl;
d=first;
......
......@@ -5,6 +5,8 @@
#include "agent.h"
#include "Utils/colorMaps.h"
#include "Algo/Render/GL1/map_glRender.h"
inline void renderDart(EnvMap& m, Dart d)
{
PFP::VEC3 p1 = m.position[d] ;
......@@ -55,6 +57,12 @@ inline void renderObstacle(EnvMap& m, MovingObstacle * obst, bool showBelonging=
}
glEnd() ;
glPolygonMode(GL_FRONT_AND_BACK, GL_LINE) ;
glColor3f(0,1,0);
Algo::Render::GL1::renderTriQuadPoly<PFP>(obst->map, Algo::Render::GL1::LINE,
1.0, obst->position,
obst->normal) ;
if(showVertices)
{
glPointSize(3.0f) ;
......@@ -78,16 +86,16 @@ inline void renderObstacle(EnvMap& m, MovingObstacle * obst, bool showBelonging=
}
glEnd() ;
glColor3f(0.0,1.0,1.0);
glBegin(GL_LINES) ;
for (unsigned int i = 0 ; i < (obst->nbVertices) ; ++i)
{
const VEC3& p = obst->getPosition(i) ;
glVertex3fv(p.data()) ;
const VEC3& p2 = obst->getDilatedPosition(i) ;
glVertex3fv(p2.data()) ;
}
glEnd() ;
// glColor3f(0.0,1.0,1.0);
// glBegin(GL_LINES) ;
// for (unsigned int i = 0 ; i < (obst->nbVertices) ; ++i)
// {
// const VEC3& p = obst->getPosition(i) ;
// glVertex3fv(p.data()) ;
// const VEC3& p2 = obst->getDilatedPosition(i) ;
// glVertex3fv(p2.data()) ;
// }
// glEnd() ;
glLineWidth(5.0f) ;
glColor3f(1.0,1.0,0.0);
......
......@@ -16,7 +16,7 @@ class Simulator ;
class MovingObstacle
{
public:
MovingObstacle(Simulator* sim, int index, std::vector<PFP::VEC3> pos, std::vector<VEC3> goals,bool spin, ArticulatedObstacle * art=NULL, int ind2=-1);
MovingObstacle(Simulator* sim, int index, std::vector<PFP::VEC3> pos, std::vector<VEC3> goals, bool rigid, bool spin, ArticulatedObstacle * art=NULL, int indParent=-1);
bool test_opposition(VEC3 o, VEC3 p1, VEC3 p2);
// void contournerBatiment();
void updateAgentNeighbors() ;
......@@ -33,8 +33,28 @@ public:
unsigned int nbVertices;
CGoGN::Algo::MovingObjects::ParticleCell2DMemo<PFP> *registering_part;
std::vector<PFP::VEC3> vertices;
std::vector<PFP::VEC3> verticesDeformation;
// std::vector<PFP::VEC3> vertices;
PFP::MAP map;
VertexAttribute<VEC3> position;
VertexAttribute<VEC3> normal;
VertexAttribute<VEC3> deformation;
VertexAttribute<VEC3> velocity;
EdgeAttribute<float> edgeLength;
Dart groundFace;
//vector de détails pour la deformation
// std::vector<PFP::VEC3> verticesDeformation;
//gestion pour du mass-ressort
// std::vector<PFP::VEC3> verticesVelocity;
// std::vector<float> edgeLength;
// std::vector<float> verticesAngle;
// float rigidity;
float gravity_dist;
VEC3 center;
int nb_agents_voisins;
......@@ -75,6 +95,7 @@ public:
VEC3 newVelocity_;
VEC3 prefVelocity_;
Simulator* sim_;
bool rigid_;
bool spinning;
ArticulatedObstacle * parent;
int index_parent;
......
......@@ -10,7 +10,7 @@ ArticulatedObstacle::ArticulatedObstacle(Simulator* sim, int index, int currentI
{
std::vector<VEC3> goal;
goal.push_back(members[i-1]->center);
MovingObstacle * mo4= new MovingObstacle(sim,currentIndex+1+i ,pos[i],goal,true, this,i);
MovingObstacle * mo4= new MovingObstacle(sim,currentIndex+1+i ,pos[i],goal, true, true, this,i);
members.push_back(mo4);
}
......
......@@ -98,13 +98,13 @@ void EnvMap::init(unsigned int config, REAL width, REAL height, REAL minSize, RE
Algo::Import::importSVG<PFP>(map, filename, position, obstacleMark, buildingMark) ;
std::string filename2 = "./svg/mapBuild.svg" ;
Algo::Import::importSVG<PFP>(mapScenary, filename2, positionScenary, obstacleMarkS, buildingMarkS) ;
// Algo::Import::importSVG<PFP>(mapScenary, filename2, positionScenary, obstacleMarkS, buildingMarkS) ;
Geom::BoundingBox<typename PFP::VEC3> bb1, bb2 ;
bb1 = Algo::Geometry::computeBoundingBox<PFP>(map, position) ;
bb2 = Algo::Geometry::computeBoundingBox<PFP>(mapScenary, positionScenary) ;
bb1.addPoint(bb2.min());
bb1.addPoint(bb2.max());
// bb2 = Algo::Geometry::computeBoundingBox<PFP>(mapScenary, positionScenary) ;
// bb1.addPoint(bb2.min());
// bb1.addPoint(bb2.max());
// CityGenerator::planetify<PFP>(map, position, 100.0f, bb1);
// CityGenerator::planetify<PFP>(mapScenary, positionScenary, 100.0f, bb1);
......
......@@ -146,6 +146,7 @@ std::vector<VEC3> MovingMesh::computeProjectedPointSet(float maxHeight)
}
res = jarvisConvexHull(res);
res.pop_back();
Geom::Plane3D<float> pl = Algo::Geometry::facePlane<PFP>(motherMap, motherMap.begin(), motherPosition);
for(unsigned int i = 0; i < res.size() ; ++i)
......@@ -166,7 +167,7 @@ std::vector<VEC3> MovingMesh::jarvisConvexHull(std::vector<VEC3> pointSet)
unsigned int curr;
for(unsigned int i=1; i<pointSet.size(); ++i) //FOR ALL POINTS IN THE SET, FIND MIN POINT
if(pointSet[i][1]<pointSet[minPoint][1])
if(pointSet[i][1]>pointSet[minPoint][1])
minPoint=i;
convHull.push_back(minPoint);
......
......@@ -3,6 +3,8 @@
#include "agent.h"
#include "simulator.h"
#include "Algo/Modelisation/triangulation.h"
//float MovingObstacle::neighborDistSq_ = 5.0f * 5.0f;
float MovingObstacle::maxSpeed_ = 2.0f;
......@@ -69,12 +71,16 @@ VEC3 rotate(VEC3 pos1, VEC3 center, float angle) // renvoie le déplacement nece
return pos2;
}
MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, std::vector<VEC3> goals, bool spin, ArticulatedObstacle * art, int ind2) :
MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, std::vector<VEC3> goals, bool rigid, bool spin, ArticulatedObstacle * art, int indParent) :
center(0),
index(ind),
goals_(goals),
newVelocity_(0),
sim_(sim),
rigid_(rigid),
spinning(spin),
parent(art),
index_parent(ind2)
index_parent(indParent)
{
seen = false;
color1=1.0f;
......@@ -83,10 +89,7 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
assert(pos.size() > 2);
// movingObstNeighbors_.reserve(maxNeighbors_);
spinning=spin;
goals_=goals;
curGoal_ = 1;
VEC3 sum = 0;
nb_agents_voisins = 0;
nb_register_cells = 0;
float pos_max = 0;
......@@ -96,23 +99,55 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
Dart d = sim_->envMap_.getBelongingCell(pos[0]);
registering_part = new CGoGN::Algo::MovingObjects::ParticleCell2DMemo<PFP>(sim_->envMap_.map, d, pos[0], sim_->envMap_.position);
vertices.reserve(nbVertices);
obstacles_ = new Obstacle*[nbVertices];
belonging_cells = new std::vector<Dart>[nbVertices];
neighbor_cells = new std::vector<Dart>[nbVertices];
// for (unsigned int i = 0; i < nbVertices; ++i)
position = map.addAttribute<VEC3, VERTEX>("position") ;
normal = map.addAttribute<VEC3, VERTEX>("normal") ;
deformation = map.addAttribute<VEC3, VERTEX>("deformation") ;
velocity = map.addAttribute<VEC3, VERTEX>("velocity") ;
edgeLength = map.addAttribute<float, EDGE>("edgeLength") ;
groundFace = map.newFace(nbVertices);
d = groundFace;
for (unsigned int i = 0; i < nbVertices; ++i)
{
vertices[i]=pos [i];
sum += vertices[i];
verticesDeformation.push_back(VEC3(0));
position[d] = pos[i];
deformation[d] = VEC3(0);
velocity[d] = VEC3(0);
center += pos[i];
d = map.phi1(d);
}
center = sum / nbVertices;
front=(vertices[1] + vertices[2]) / 2;
if(!rigid_)
{
//extrude face to build a cage
// compute edgeLength for mass-spring
Algo::Modelisation::extrudeFace<PFP>(map, position, d, 5.0f) ;
map.fillHole(groundFace);
groundFace = map.phi2(groundFace);
Algo::Modelisation::EarTriangulation<PFP> et(map);
et.triangule();
TraversorE<PFP::MAP> tE(map);
for(Dart d = tE.begin() ; d != tE.end() ; d = tE.next())
{
edgeLength[d] = VEC3(position[map.phi1(d)]-position[d]).norm();
}
}
center /= nbVertices;
front=(pos[1] + pos[2]) / 2;
//-------- code ajoute par Arash pour les obstacles rectangulaires --------------
length = (vertices[0]-vertices[1]).norm();
width = (vertices[1]-vertices[2]).norm();
length = (pos[0]-pos[1]).norm();
width = (pos[1]-pos[2]).norm();
sum_dist_foci_rest = 2*(length + width*(sqrt(2)-0.5));
// M appartient à l'ellipse ssi MF1 + MF2 = sum_dist_foci est une constante
// où F1 et F2 sont les deux foyers.
......@@ -124,26 +159,23 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
angle = get_angle(goals_[curGoal_] - center,front - center);
for (unsigned int i = 0; i < nbVertices; ++i)
{
vertices[i]+=rotate(vertices[i], center, angle);
position[i]+=rotate(position[i], center, angle);
}
angle=0;
front=(vertices[1] + vertices[2]) / 2;
}
for (unsigned int i = 0; i < nbVertices; ++i)
{
Obstacle* o = new Obstacle(vertices[i],
vertices[(i + 1) % nbVertices],
vertices[(i - 1 + nbVertices) % nbVertices],
vertices[(i + 2) % nbVertices], this, i);
Obstacle* o = new Obstacle(position[i],
position[(i + 1) % nbVertices],
position[(i - 1 + nbVertices) % nbVertices],
position[(i + 2) % nbVertices], this, i);
obstacles_[i] = o;
sim_->envMap_.pushObstacleInCells(o);
}
for (unsigned int i = 0; i < nbVertices; ++i)
{
float it = (center - pos[i]).norm();
......@@ -157,12 +189,14 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
VEC3 MovingObstacle::getDilatedPosition(unsigned int ind)
{
return vertices[ind]+verticesDeformation[ind];
Dart d(ind); //WARNING : works only for one face created at start !
return position[d]+deformation[d];
}
VEC3 MovingObstacle::getPosition(unsigned int ind)
{
return vertices[ind];
Dart d(ind);
return position[d];
}
bool MovingObstacle::test_opposition(VEC3 o, VEC3 p1, VEC3 p2)
......@@ -346,7 +380,7 @@ void MovingObstacle::update()
color3=1.0f;
}
seen=false;
PFP::VEC3 bary = 0;
PFP::VEC3 bary(0);
Dart d;
velocity_[0] = newVelocity_[0] * velocity_factor;
......@@ -362,39 +396,101 @@ void MovingObstacle::update()
else
rotor = abs_angle*angle ;
// CGoGNout << "Obstacle "<< index << CGoGNendl;
// CGoGNout << "vitesse : "<< velocity_ << CGoGNendl;
// on fait tourner l'obstacle
for (unsigned int i = 0; i < nbVertices; ++i)
if(!rigid_)
{
VEC3& target =vertices[i];
target += rotate(target, center, abs_angle*rotor);
target += (velocity_ * sim_->timeStep_);
Dart d = groundFace;
map.next(d);
bary += target;
}
CellMarkerStore<EDGE> cm(map);
for (unsigned int i = 1; i < nbVertices; ++i)
{
Dart dd = d;
do {
if(!cm.isMarked(dd))
{
cm.mark(dd);
VEC3 v1 = (position[map.phi1(dd)]-position[dd]);
//inertie
velocity[dd] = velocity[dd]*0.9f;
//stretch spring : /!\ max rigidity relative to the timestep used (unstable otherwise)
float norm = v1.norm();
float rigidity = 500.0f;
float stretch = rigidity*(edgeLength[dd]-v1.norm());
VEC3 f = stretch*(v1/norm)*sim_->timeStep_;
//apply force symmetrically
velocity[dd] -= f;
velocity[map.phi1(dd)] += f;
}
front=(vertices[1] + vertices[2]) / 2;
if(angle >0)
angle -= rotor;
dd = map.phi1(dd);
} while(dd!=d);
map.next(d);
}
//guiding vertex = first vertex (set the displacement)
velocity[groundFace] = velocity_;
//apply force to each vertex
d = groundFace;
for (unsigned int i = 0; i < nbVertices; ++i)
{
position[d] += (velocity[d] * sim_->timeStep_);
position[map.phi<211>(d)] += (velocity[d] * sim_->timeStep_);
bary += position[d];
map.next(d);
}
}
else
angle += rotor;
{
// CGoGNout << "Obstacle "<< index << CGoGNendl;
// CGoGNout << "vitesse : "<< velocity_ << CGoGNendl;
// on fait tourner l'obstacle
Dart d = groundFace;
for (unsigned int i = 0; i < nbVertices; ++i)
{
position[d] += rotate(position[d], center, abs_angle*rotor);
position[d] += (velocity_ * sim_->timeStep_);
// position[map.phi<211>(d)] += (velocity_ * sim_->timeStep_);
bary += position[d];
map.next(d);
}
front=(position[1] + position[2]) / 2;
if(angle >0)
angle -= rotor;
else
angle += rotor;
}
center = bary / nbVertices;
//computing deformation vector on vertices
//computing deformation vector on vertices :
//depending on the velocity, extend the perceived shape
VEC3 vel = velocity_.normalized();
for(unsigned int i = 0; i < nbVertices; ++i)
d = groundFace;
for (unsigned int i = 0; i < nbVertices; ++i)
{
VEC3 v(vertices[i] - center);
VEC3 v(position[d] - center);
VEC3 vN = v.normalized();
float dot = vN * vel;
verticesDeformation[i] = v * 2.0f * (dot+1.0f)/2.0f;
deformation[d] = v * 2.0f * (dot+1.0f)/2.0f;
map.next(d);
}
if(!rigid_)
center = position[groundFace];
//-------- code ajoute par Arash pour les obstacles rectangulaires --------------
VEC3 P0_P1 = vertices[1] - vertices[0];
VEC3 P0_P1 = position[1] - position[0];
float velocity_coef = 10.0;
if(P0_P1 * velocity_ > 0) // P0_P1 est dans le sens de la vitesse
......@@ -422,7 +518,6 @@ void MovingObstacle::update()
o->nextP = getDilatedPosition((i + 2 + nbVertices) % nbVertices);
sim_->envMap_.popAndPushObstacleInCells(o,i);
if(sim_->detect_agent_collision)
{
for (std::vector<Dart>::iterator it = belonging_cells[i].begin(); it != belonging_cells[i].end(); ++it)
......@@ -458,7 +553,6 @@ void MovingObstacle::update()
}
std::vector<Dart> MovingObstacle::getMemoCross(const VEC3& pos, const VEC3& dest, Dart& d1)
{
registering_part->move(pos);
......@@ -486,14 +580,11 @@ void resetObstPartInFace(Obstacle* o, Dart d1)
{
MovingObstacle * mo = o->mo;
if (mo != NULL) {
if (mo != NULL)
{
VEC3 pos1 = mo->registering_part->getPosition();
if (Algo::Geometry::isPointInConvexFace2D < PFP
> (mo->sim_->envMap_.map, d1, mo->sim_->envMap_.position, pos1, true)) {
if (Algo::Geometry::isPointInConvexFace2D <PFP> (mo->sim_->envMap_.map, d1, mo->sim_->envMap_.position, pos1, true))
mo->registering_part->d = d1;
}
}
}
......@@ -501,7 +592,6 @@ void resetPart(MovingObstacle * mo, Dart d1)
{
if (mo->registering_part->d == mo->sim_->envMap_.map.phi1(d1))
mo->registering_part->d = d1;
}
void displayMO(Obstacle * o)
......@@ -522,6 +612,7 @@ void MovingObstacle::computePrefVelocity() //calcul du vecteur optimal pour atte
if (index_parent<1)
{
goalVector = goals_[curGoal_] - center ;
// goalVector = goals_[curGoal_] - position[groundFace] ;
float goalDist2 = goalVector.norm2() ;
......@@ -529,6 +620,7 @@ void MovingObstacle::computePrefVelocity() //calcul du vecteur optimal pour atte
{
curGoal_ = (curGoal_ + 1) % goals_.size() ;
goalVector = goals_[curGoal_] - center ;
// goalVector = goals_[curGoal_] - position[groundFace] ;
goalDist2 = goalVector.norm2() ;
}
......@@ -548,7 +640,9 @@ void MovingObstacle::computePrefVelocity() //calcul du vecteur optimal pour atte
goalVector *= maxSpeed_;
}
}
if (spinning) angle =get_angle(goalVector,front-center);
if (spinning)
angle =get_angle(goalVector,front-center);
prefVelocity_ = goalVector ;
......@@ -560,12 +654,16 @@ void MovingObstacle::computeNewVelocity() //comportement des obstacles en tenan
VEC3 forward = front-center;
float goalDist2 = forward.norm2() ;
float objective = prefVelocity_.norm2();
if (goalDist2 > objective)
{
forward.normalize() ;
forward *= objective;
}
if(index_parent==0 && angle>0.01f) forward/=10;
{
forward.normalize() ;
forward *= objective;
}
if(index_parent==0 && angle>0.01f)
forward/=10;
newVelocity_=forward;
}
else
......
#include "simulator.h"
Simulator::Simulator(unsigned int config, unsigned int minS, unsigned int nbAgent, unsigned int nbObst) :
timeStep_(0.2f),
timeStep_(0.02f),
globalTime_(0.0f),
nbSteps_(0),
nbUpdates(0),
......@@ -110,6 +110,8 @@ void Simulator::doStep()
envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
#endif
movingObstacles_[i]->update() ;
//PIERRE
}
for (unsigned int i = 0 ; i < agents_.size() ; ++i)
......@@ -274,7 +276,7 @@ void Simulator::setupCircleScenario(unsigned int nbAgents , unsigned int nbObsta
vPos.push_back(start-xSide+ySide);
vPos.push_back(start-xSide-ySide);
mo4= new MovingObstacle(this, i,vPos,goals,true);
mo4= new MovingObstacle(this, i,vPos,goals, true, true);
movingObstacles_.push_back(mo4);
}
......@@ -295,7 +297,7 @@ void Simulator::setupCircleScenario(unsigned int nbAgents , unsigned int nbObsta
vPos.push_back(start-xSide+ySide);
vPos.push_back(start-xSide-ySide);
mo4= new MovingObstacle(this, i,vPos,goals,true);
mo4= new MovingObstacle(this, i,vPos,goals, true, true);
movingObstacles_.push_back(mo4);
}
......@@ -397,7 +399,7 @@ void Simulator::setupCorridorScenario(unsigned int nbAgents, unsigned int nbObst
std::vector<VEC3> goals;
goals.push_back(start);
goals.push_back(goal);
mo4= new MovingObstacle(this, i,vPos,goals,false);
mo4= new MovingObstacle(this, i,vPos,goals, true, false);
// //for generating a random path
......@@ -674,9 +676,14 @@ void Simulator::addMovingObstacle(Dart d, unsigned int obstType)
std::vector<VEC3> goals;
goals.push_back(start);
MovingObstacle * mo = new MovingObstacle(this, movingObstacles_.size(),vPos,goals,false);
MovingObstacle * mo = new MovingObstacle(this, movingObstacles_.size(),vPos,goals, (obstType==0), false);
movingObstacles_.push_back(mo);
if(obstType==1)
{
//PIERRE : calcul coord bary d som du movMesh en fction de la cage movingObstacle
}
}
void Simulator::addPathToObstacles()
......
......@@ -1139,7 +1139,7 @@ bool SocialAgents::exportScenePov(PFP::MAP& map, PFP::TVEC3& position, const std
for(unsigned int i=0; i <= nbPoints ; ++i)
{
const VEC3& pos = (*it)->vertices[i%nbPoints];
const VEC3& pos = (*it)->getPosition(i%nbPoints);
out << "<" << pos[0] << "," << pos[1] << ">" << std::endl;
}
out << "pigment { Red }" << std::endl;
......
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