Commit 31b1668a authored by pitiot's avatar pitiot

fini ? corrigé limaces 3D

parent bb6a8e0e
2 282.553 -549.754 -431.112 243.297 -467.981 -389.015 0.278767 0.541963 -0.792821
2 380.527 224.803 -813.318 330.469 214.097 -727.414 -0.172437 0.984769 0.0222493
0 2892
0 13.2112 -343.6 594.385 10.9265 -307.115 501.306 0.0322209 0.930812 0.364074
1637 13.2112 -343.6 594.385 10.9265 -307.115 501.306 0.0322209 0.930812 0.364074
2263 -225.649 -65.4916 186.836 -153.811 -25.9154 129.625 0.511978 0.255997 0.819966
2892 154.528 244.386 80.6884 119.103 153.875 57.1753 0.0753229 -0.278238 0.957554
......@@ -7,7 +7,7 @@ using namespace std;
class ArticulatedObstacle
{
public:
ArticulatedObstacle(Simulator* sim, int index, int currentIndex, std::vector<PFP::VEC3> * pos, int nbParts, std::vector<VEC3> goals);
ArticulatedObstacle(Simulator* sim, int index, int currentIndex, std::vector<PFP::VEC3> * pos, int nbParts, std::vector<VEC3> goals, int curGoal =0);
std::vector<MovingObstacle *> members;
int index;
......
......@@ -34,7 +34,7 @@ class ArticulatedObstacle;
//#define EXPORTING3
//#define TWO_AND_HALF_DIM
#define TWO_AND_HALF_DIM
#ifdef EXPORTING3
......
......@@ -6,8 +6,12 @@
#include "utils.h"
#include "env_map.h"
#include <set>
#define LINEAR
// #define SECURED
#ifdef LINEAR
#include "ShapeMatching/shapeMatchingLinear.h"
#else
......@@ -179,7 +183,7 @@ public:
#endif
PFP::REAL beta;
float alpha;
float speed_detection_factor;
};
......
......@@ -7,9 +7,9 @@ class Obstacle
{
public:
Obstacle(const VEC3 point1, const VEC3 point2,
MovingObstacle * moving1=NULL, unsigned int ind=0) :
MovingObstacle * moving1=NULL, unsigned int ind=0,double stiff_agent=20,double stiff_obst=5,double radius_agent=500,double radius_obst=10) :
p1(point1), p2(point2),
mo(moving1), index(ind),p3(0,0,0),obst_stiffness_agent(20),obst_stiffness_obst(20)
mo(moving1), index(ind),p3(0,0,0),obst_stiffness_agent(stiff_agent),obst_stiffness_obst(stiff_obst),obst_radius_infl_agent(radius_agent),obst_radius_infl_obst(radius_obst)
{
// p1[2] = 0 ;
// p2[2] = 0 ;
......@@ -29,6 +29,8 @@ public:
double obst_stiffness_agent;
double obst_stiffness_obst;
double obst_radius_infl_agent;
double obst_radius_infl_obst;
} ;
#endif
......@@ -93,6 +93,7 @@ public:
bool reachedGoal() ;
void setupCircleScenario(unsigned int nbAgents, unsigned int nbObstacles) ;
void setupCrowdedScenario(unsigned int nbAgents , unsigned int nbObstacles, int snake);
void setupCorridorScenario(unsigned int nbAgents, unsigned int nbObstacles) ;
void setupSnakeCorridorScenario(unsigned int nbAgents, unsigned int nbSnakes, int snakeSize) ;
void setupCityScenario(int nbLines, int nbRank) ;
......
......@@ -347,14 +347,14 @@ void Agent::updateObstacleNeighbors()
std::vector<Obstacle*>& neighborObst = sim_->envMap_.neighborObstvect[part_.d] ;
float maxDistObst = 0.0f ;
float maxDistMovingObst = 0.0f ;
float range =1000;
for(std::vector<Obstacle*>::const_iterator it = obst.begin() ; it != obst.end() ; ++it)
{
if ((*it)->mo==NULL)
{
float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, part_.getPosition()) ;
if ((obstacleNeighbors_.size() < maxMovingObstacles_|| distSq < maxDistObst)
&& distSq < rangeSq_)
&& distSq < range)
{
// if (sim_->envMap_.testOrientation(part_.getPosition(), (*it)->p1, (*it)->p2, part_.d) == 1)
{
......@@ -374,7 +374,7 @@ void Agent::updateObstacleNeighbors()
{
float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, part_.getPosition()) ;
if ((movingObstacleNeighbors_.size() < maxMovingObstacles_ || distSq < maxDistMovingObst)
&& distSq < rangeSq_)
&& distSq < range)
{
// if (sim_->envMap_.testOrientation(part_.getPosition(), (*it)->p1, (*it)->p2, part_.d) == 1)
{
......@@ -396,7 +396,7 @@ void Agent::updateObstacleNeighbors()
{
float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, part_.getPosition()) ;
if ((obstacleNeighbors_.size() < maxNeighbors_ || distSq < maxDistObst)
&&distSq < rangeSq_)
&&distSq < range)
{
// if (sim_->envMap_.testOrientation(part_.getPosition(), (*it)->p1, (*it)->p2, part_.d) == 1)
{
......@@ -412,7 +412,7 @@ void Agent::updateObstacleNeighbors()
{
float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, part_.getPosition()) ;
if ((movingObstacleNeighbors_.size() < maxNeighbors_ || distSq < maxDistMovingObst)
&&distSq < rangeSq_)
&&distSq < range)
{
// if (sim_->envMap_.testOrientation(part_.getPosition(), (*it)->p1, (*it)->p2, part_.d) == 1)
{
......@@ -812,11 +812,7 @@ void Agent::computeNewVelocity()
int obst_power = 2 ; // the power to which elevate the agent-obstacle distance
double obst_radius_infl;
if(sim_->config==0)
obst_radius_infl = 1000.; // scenario 0
else
obst_radius_infl = 40.; // scenario 1 et 3
float force_value;
int nobst=0;
......@@ -833,7 +829,7 @@ void Agent::computeNewVelocity()
VEC3 p1=obst->p1 ;
VEC3 p2=obst->p2 ;
double longueur2 = (p1-p2).norm2();
double rest_sum_of_dists = 2 * sqrt(obst_radius_infl*obst_radius_infl + longueur2/4);
double rest_sum_of_dists = 2 * sqrt(obst->obst_radius_infl_agent*obst->obst_radius_infl_agent + longueur2/4); // constante de l'ellipse
VEC3 p = getPosition();
double d1 = (p-p1).norm();
......@@ -844,10 +840,17 @@ void Agent::computeNewVelocity()
collision_softening_factor = pow(1-sum_of_dists/rest_sum_of_dists,obst_power);
force_value = obst->obst_stiffness_agent*collision_softening_factor*(rest_sum_of_dists - sum_of_dists);
VEC3 v_obst = p2 - p1;
VEC3 normal = normFace ^v_obst;
// Ajouter une composante tangentielle
normal += v_obst * ((d1-d2)/(5*sum_of_dists));
// Le facteur 5 est là seulement pour diminuer l'influence de la composante tangentielle
VEC3 normal;
///new vector tangente à l'ellipse
VEC3 bc = p-p2;
normal =((bc.norm())/sum_of_dists)*v_obst+bc;
// previous vector
// normal = normFace ^v_obst;
// // Ajouter une composante tangentielle
// normal += v_obst * ((d1-d2)/(5*sum_of_dists));
// // Le facteur 5 est là seulement pour diminuer l'influence de la composante tangentielle
//
normal.normalize();
forces += force_value * normal;
}
......
#include "articulated_obstacle.h"
ArticulatedObstacle::ArticulatedObstacle(Simulator* sim, int index, int currentIndex, std::vector<PFP::VEC3> * pos, int size , std::vector<VEC3> goals)
ArticulatedObstacle::ArticulatedObstacle(Simulator* sim, int index, int currentIndex, std::vector<PFP::VEC3> * pos, int size , std::vector<VEC3> goals,int curGoal)
{
this->index=index;
nbBodyPart = size;
MovingObstacle * mo4= new MovingObstacle(sim,currentIndex+1 ,pos[0],goals,true,true,1, NIL,this,0);
MovingObstacle * mo4= new MovingObstacle(sim,currentIndex+1 ,pos[0],goals,true,true,curGoal, NIL,this,0);
members.push_back(mo4);
for(int i =1; i<nbBodyPart; i++)
{
......
......@@ -96,8 +96,7 @@ void EnvMap::init(unsigned int config, REAL width, REAL height, REAL minSize, RE
// CityGenerator::generateCity<PFP>(*this,10,500.0f) ;
break ;
case 4 :
CityGenerator::generatePlanet<PFP>(*this) ;
CityGenerator::generateCity<PFP>(*this,10,500.0f) ;
CityGenerator::generateGrid<PFP>(*this) ;
break ;
case 5 :
{
......
......@@ -24,8 +24,8 @@ constrainedV(map)
#endif
position = map.getAttribute<VEC3, VERTEX>(attrNames[0]) ;
float area = Algo::Surface::Geometry::convexFaceArea<PFP>(envMap.map, d, envMap.position);
scaleValue = std::max(area/1400.0f,2.0f);
// float area = Algo::Surface::Geometry::convexFaceArea<PFP>(envMap.map, d, envMap.position);
scaleValue = 2.0f;
// std::cout << "scaleVal " << scaleValue << std::endl;
// scale(scaleValue/1.8f);
......@@ -232,7 +232,7 @@ std::vector<VEC3> MovingMesh::computeProjectedPointSet(float maxHeight, Dart d)
std::vector<bool> active;
active.assign(points.size(), true);
simplifyCurve(points, active, 0, points.size()-1, 0.5f);
simplifyCurve(points, active, 0, points.size()-1, 0.1f);
std::vector<VEC3> res;
VEC3 center =Algo::Surface::Geometry::faceCentroid<PFP>(motherMap, d, motherPosition);
......
This diff is collapsed.
......@@ -17,7 +17,7 @@ timespec timespec_delta(const timespec& t1, const timespec& t2) {
Simulator::Simulator(unsigned int config, unsigned int minS, unsigned int nbAgent, unsigned int nbObst, bool resolution) :
#ifdef TWO_AND_HALF_DIM
timeStep_(0.5f),
timeStep_(0.4f),
#else
// timeStep_(config > 2 ? 0.01f : 0.25f),
timeStep_(0.25f),
......@@ -70,16 +70,17 @@ void Simulator::init( float dimension, unsigned int nbAgent, unsigned int nbObst
setupSnakeCorridorScenario(nbAgent,nbObst,10) ;
break ;
case 3 :
envMap_.init(config, 1000.0f, 1000.0f, minSize, 100.0f) ; //grosses cases
setupScenario(nbAgent, false) ;
envMap_.init(config, 500.0f, 500.0f, minSize, 50.0f) ; //grosses cases
addMovingObstacles(nbObst, 1);
setupScenario(nbAgent, false) ;
addPathToObstacles(envMap_.buildingMark, true);
//// setupCityScenario(-1.0f * (12 * (70.0f / 2.0f) - 10),
//// -1.0f * (12 * (70.0f / 2.0f) - 10), 20, 20);
break ;
// case 4 :
// importAgents("myAgents.pos") ;
// break ;
case 4 :
setupCrowdedScenario(nbAgent,nbObst,1) ;
break ;
case 5:
envMap_.init(config, 1600.0f, 1200.0f, minSize, 400.0f) ; //svg import
setupScenario(nbAgent, true) ;
......@@ -436,6 +437,132 @@ void Simulator::setupCircleScenario(unsigned int nbAgents , unsigned int nbObsta
std::cout << "nb agents : " << agents_.size() << std::endl ;
}
void Simulator::setupCrowdedScenario(unsigned int nbAgents , unsigned int nbObstacles, int snake)
{
if (multires)
envMap_.init(config, 1500.0f, 1500.0f, minSize, 400.0f) ; //grosses cases
else
{
// envMap_.init(config, 2000.0f, 2000.0f, minSize, 50.0f) ; //cases fines
envMap_.init(config, 1000.0f,1000.0f, minSize, 400.0f) ; //grosses cases
}
std::cout << " - Setup Crowded Scenario : " << nbAgents << " agents et 1 obstacle"<< std::endl ;
// Les coordonnées sont comprises entre xMin et xMin+xDelta
VEC3 center = (envMap_.geometry.min() + envMap_.geometry.max()) / 2 ;
Dart d = envMap_.getBelongingCell(center);
unsigned int nbx = 25;
unsigned int nby = 25;
VEC3 pos=center;
Dart dCell =d;
float ecart = 5.0f*Agent::radius_;
VEC3 posinit = VEC3(pos[0] - (float(nbx) / 2.0f * ecart),
pos[1] - (float(nby) / 2.0f * ecart), pos[2]) ;
for (unsigned int curx = 0 ; curx < nbx ; ++curx)
{
for (unsigned int cury = 0 ; cury < nby ; ++cury)
{
VEC3 posagent = posinit + VEC3(ecart * curx, ecart * cury, 0.0f) ;
addAgent(posagent, posagent, dCell) ;
}
}
VEC3 xSide (5.0f,0.0f,0.0f);
VEC3 ySide (0.0f,10.0f,0.0f);
std::vector<VEC3> vPos;
MovingObstacle* mo4;
int curGoal=0;
VEC3 start;
start = center;
float mapSize=(envMap_.geometry.max()[1]-envMap_.geometry.min()[1]);
start[1]-= mapSize/4;
std::vector<VEC3> goals;
VEC3 goal ;
vPos.push_back(start+xSide+ySide);
vPos.push_back(start+xSide-ySide);
vPos.push_back(start-xSide-ySide);
vPos.push_back(start-xSide+ySide);
if(snake==1)
{
int snakeSize=10;
std::vector<PFP::VEC3> positions [snakeSize] ;
positions[0]=vPos;
goal=center;
goals.push_back(goal);
goal=center+VEC3(mapSize/4,mapSize/4,0);
goals.push_back(goal);
goal=center;
goals.push_back(goal);
goal=center+VEC3(-mapSize/4,mapSize/4,0);
goals.push_back(goal);
goal=center;
goals.push_back(goal);
goal=start;
goals.push_back(goal);
for (int i = 1 ; i < snakeSize ; i++)
{
start=start-ySide-ySide;
vPos.clear();
vPos.push_back(start+xSide+ySide);
vPos.push_back(start+xSide-ySide);
vPos.push_back(start-xSide-ySide);
vPos.push_back(start-xSide+ySide);
positions[i]=vPos;
}
// CGoGNout<<"positions : "<< positions[0][0]<<CGoGNendl;
ArticulatedObstacle * art=new ArticulatedObstacle (this,0,0,positions,snakeSize,goals,curGoal);
for (int i = 0 ; i < snakeSize ; i++)
movingObstacles_.push_back(art->members[i]);
}
else
{
goal=start;
goal[1]+=(envMap_.geometry.max()[1]-envMap_.geometry.min()[1])/2;
goals.push_back(goal);
goals.push_back(start);
curGoal = 0;
mo4= new MovingObstacle(this, 0,vPos,goals, true, true,curGoal);
movingObstacles_.push_back(mo4);
}
#ifndef SPATIAL_HASHING
envMap_.clearUpdateCandidates() ;
envMap_.map.setCurrentLevel(0) ;
#endif
std::cout << "nb agents : " << agents_.size() << std::endl ;
}
void Simulator::setupCorridorScenario(unsigned int nbAgents, unsigned int nbObstacles)
{
VEC3 xSide (5.0f,0.0f,0.0f);
......
......@@ -2173,7 +2173,8 @@ int main(int argc, char** argv)
// sa.cam_output_file.open("cams/cam.out",std::ofstream::out);
// char camfile[100];
// sprintf(camfile,"cams/cam.scn%d.spline",config);
// sa.readCameraInputFile(camfile);
//
// sa.readCameraInputFile("cams/toto");
// if(sa.cif_exists)
// {
// unsigned int i;
......
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