Commit e8e3ed59 authored by Arash HABIBI's avatar Arash HABIBI
Browse files

collisionOK

parents 75277889 63f28931
......@@ -11,10 +11,10 @@ ENDIF (WIN32)
include_directories(
${CMAKE_CURRENT_BINARY_DIR}
${CMAKE_CURRENT_SOURCE_DIR}/Debug
# ${CMAKE_CURRENT_SOURCE_DIR}
../include
${CGoGN_ROOT_DIR}/include
${COMMON_INCLUDES}
${CMAKE_CURRENT_BINARY_DIR}
)
file(
......
......@@ -11,10 +11,10 @@ ENDIF (WIN32)
include_directories(
${CMAKE_CURRENT_BINARY_DIR}
${CMAKE_CURRENT_SOURCE_DIR}/Release
# ${CMAKE_CURRENT_SOURCE_DIR}
../include
${CGoGN_ROOT_DIR}/include
${COMMON_INCLUDES}
${CMAKE_CURRENT_SOURCE_DIR}
)
file(
......
......@@ -77,10 +77,7 @@ public:
std::vector<Dart> * belonging_cells;
std::vector<Dart> * neighbor_cells;
std::set<Dart> general_belonging;
int use_ellipse;
VEC3 front;
VEC3 focus1, focus2;
double length, width, sum_dist_foci, sum_dist_foci_rest;
VEC3 finalGoal;
float angle;
......
# Blender MTL File: 'Limace.002.blend'
# Material Count: 1
newmtl _rect2987.png
Ns 0
Ka 0.000000 0.000000 0.000000
Kd 0.8 0.8 0.8
Ks 0.8 0.8 0.8
d 1
illum 2
map_Kd C:\Users\larue\Downloads\Limace.002.blend\limace.png
This diff is collapsed.
......@@ -246,37 +246,37 @@ void Agent::updateObstacleNeighbors()
for(std::vector<Obstacle*>::const_iterator it = neighborObst.begin() ; it != neighborObst.end() ; ++it)
{
if ((*it)->mo==NULL)
{
float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, part_.getPosition()) ;
if ((obstacleNeighbors_.size() < maxNeighbors_ || distSq < maxDistObst)
&&distSq < rangeSq_)
{
if (Geom::testOrientation2D(part_.getPosition(), (*it)->p1, (*it)->p2) == Geom::RIGHT)
{
float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, part_.getPosition()) ;
if ((obstacleNeighbors_.size() < maxNeighbors_ || distSq < maxDistObst)
&&distSq < rangeSq_)
{
if (Geom::testOrientation2D(part_.getPosition(), (*it)->p1, (*it)->p2) == Geom::RIGHT)
{
if (distSq > maxDistObst) maxDistObst = distSq ;
obstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
}
if (distSq > maxDistObst) maxDistObst = distSq ;
obstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
}
}
else
{
float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, part_.getPosition()) ;
if ((movingObstacleNeighbors_.size() < maxNeighbors_ || distSq < maxDistMovingObst)
&&distSq < rangeSq_)
{
if (Geom::testOrientation2D(part_.getPosition(), (*it)->p1, (*it)->p2) == Geom::RIGHT)
{
if (distSq > maxDistMovingObst) maxDistMovingObst = distSq ;
movingObstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
}
}
else
{
float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, part_.getPosition()) ;
if ((movingObstacleNeighbors_.size() < maxNeighbors_ || distSq < maxDistMovingObst)
&&distSq < rangeSq_)
{
if (Geom::testOrientation2D(part_.getPosition(), (*it)->p1, (*it)->p2) == Geom::RIGHT)
{
}
if (distSq > maxDistMovingObst) maxDistMovingObst = distSq ;
movingObstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
}
}
}
}
}
// if (obstacleNeighbors_.size() > maxNeighbors_)
......@@ -513,7 +513,7 @@ void Agent::computePrefVelocity()
}
//-----------------------------------------------------------------
/*
static int moAppend(MovingObstacle **moving_obstacles, int nb_mos, int max, MovingObstacle *mo)
{
unsigned char already_processed=false;
......@@ -541,7 +541,7 @@ static int moAppend(MovingObstacle **moving_obstacles, int nb_mos, int max, Movi
}
return(nb_mos);
}
*/
//-----------------------------------------------------------------
//-----------------------------------------------------------------
//-----------------------------------------------------------------
......@@ -551,9 +551,11 @@ 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;
double mass_var = 0.95;
double average_mass = 1.0;
float ag_ambient_damping = 10.0;
// double mass_var = 0.95;
// double average_mass = 1.0;
// double average_radius = 20.f;
// double radius_var = 0.9;
// range_ = 2*timeHorizonObst_ * averageMaxSpeed_ + radius_ ;
......@@ -596,64 +598,48 @@ void Agent::computeNewVelocity()
//----- forces dues à la répulsion des obstacles en mouvement ----------
VEC3 norm;
// double obst_stiffness = 10000.0; // agent-obstacle interaction stiffness
double obst_stiffness = 1000.0; // agent-obstacle interaction stiffness
// double obst_damping = 1.0; // agent-obstacle interaction damping
int obst_power = 2 ; // the power to which elevate the agent-obstacle distance
Obstacle* obst ;
//#define ARASH
double obst_stiffness = 100000.0; // agent-obstacle interaction stiffness
int obst_power = 1 ; // the power to which elevate the agent-obstacle distance
double obst_radius_infl = 10.;
float force_value;
int nobst=0;
#define ARASH
#ifdef ARASH
nb_mos = 0;
for(std::vector<std::pair<float, Obstacle*> >::iterator it = movingObstacleNeighbors_.begin() ;
it != movingObstacleNeighbors_.end() ;
++it)
{
obst=it->second;
nb_mos=moAppend(movingObstacles_,nb_mos,maxMovingObstacles_,obst->mo);
}
MovingObstacle *mo;
float force_value;
int mo_count;
for(mo_count=0;
mo_count<nb_mos;
mo_count++)
it != movingObstacleNeighbors_.end() ; ++it)
{
mo = movingObstacles_[mo_count];
// cerr << "mo_use_ellipse = " << mo->use_ellipse << endl;
if(mo->use_ellipse)
Obstacle * obst = it->second;
if(obst->mo!=NULL)
{
float dist = (getPosition()-mo->focus2).norm() + (getPosition()-mo->focus1).norm();
double effective_range = mo->sum_dist_foci;
if(dist <= effective_range)
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);
VEC3 p = getPosition();
double d1 = (p-p1).norm();
double d2 = (p-p2).norm();
double sum_of_dists = d1+d2;
if(sum_of_dists < rest_sum_of_dists)
{
collision_softening_factor = pow(1 - dist/effective_range,obst_power);
force_value = obst_stiffness*collision_softening_factor*(effective_range-dist);
norm.zero();
// norm = (getPosition()-mo->focus2) + (getPosition()-mo->focus1);
// norm.normalize();
VEC3 vec1 = getPosition()-mo->focus1;
vec1.normalize();
VEC3 vec2 = getPosition()-mo->focus2;
vec2.normalize();
norm = vec1 + vec2;
norm.normalize();
forces += force_value * norm;
collision_softening_factor = pow(1-sum_of_dists/rest_sum_of_dists,obst_power);
force_value = obst_stiffness*collision_softening_factor*(rest_sum_of_dists - sum_of_dists);
VEC3 v_obst = p2 - p1;
VEC3 normal = VEC3(v_obst[1],-v_obst[0],0);
// cerr << "p1=" << p1 << " p2=" << p2 << " normal = " << normal << endl;
// Ajouter une composante tangentielle
normal += v_obst * ((d1-d2)/(5*sum_of_dists));
// Le facteur 3 est là seulement pour diminuer l'influence de la composante tangentielle
normal.normalize();
forces += force_value * normal;
// cerr << "--------------- limace -------------" << agentNo << "------" << force_value << endl;
}
}
else
{
}
}
#else
float force_value;
for(std::vector<std::pair<float, Obstacle*> >::iterator it = movingObstacleNeighbors_.begin() ;
it != movingObstacleNeighbors_.end() ; ++it)
{
......@@ -680,7 +666,7 @@ void Agent::computeNewVelocity()
force_value = obst_stiffness*pow(1/dist,obst_power);
std::cout << "ag " << agentNo << "angle " << a << " force val " << force_value << " " << norm2 << " dist " << dist << std::endl;
// std::cout << "ag " << agentNo << "angle " << a << " force val " << force_value << " " << norm2 << " dist " << dist << std::endl;
forces += force_value * norm2;
}
......@@ -688,14 +674,13 @@ void Agent::computeNewVelocity()
//----- forces dues à la répulsion des obstacles fixes ----------
nobst=0;
// double fixed_obst_stiffness = 50000.0; // agent-obstacle interaction stiffness
double fixed_obst_stiffness = 50000.0; // agent-obstacle interaction stiffness
// double fixed_obst_damping = 1.0; // agent-obstacle interaction damping
int fixed_obst_power = 1; // the power to which elevate the agent-obstacle distance
Obstacle* fixed_obst ;
int nobst=0;
for(std::vector<std::pair<float, Obstacle*> >::iterator it = obstacleNeighbors_.begin() ;
it != obstacleNeighbors_.end() ;
++it)
......@@ -707,7 +692,7 @@ void Agent::computeNewVelocity()
float force_value=0.0;
if(dist < effective_range)
{
collision_softening_factor = pow(1 - dist/effective_range,fixed_obst_power);
collision_softening_factor = pow(1-dist/effective_range,fixed_obst_power);
force_value = fixed_obst_stiffness*collision_softening_factor*(effective_range-dist);
}
......@@ -765,7 +750,7 @@ void Agent::computeNewVelocity()
if(dist < combinedRadius)
{
collision_softening_factor = pow(1 - dist/combinedRadius,ag_power);
collision_softening_factor = pow(1-dist/combinedRadius,ag_power);
float force_value = - ag_stiffness*collision_softening_factor*(combinedRadius-dist)
- ag_damping * (dist - previous_dist) / sim_->timeStep_;
......@@ -780,7 +765,7 @@ void Agent::computeNewVelocity()
//------- calcul de la trainee --------------------------------------
// forces -= ag_ambient_damping * velocity_;
forces -= ag_ambient_damping * velocity_;
//------- calcul de la nouvelle valeur de la vitesse ----------------
......
......@@ -23,8 +23,15 @@ constrainedV(map)
// scale(2.5f);
// scale(0.5f);
// Geom::Matrix44f m;
// m.identity();
// VEC3 axis(1.0f,0.0f,0.0f);
// Geom::rotate(axis, float(M_PI/2.0f),m);
// transform(m);
VEC3 v = Algo::Surface::Geometry::faceCentroid<PFP>(motherMap, d, motherPosition) ;
translate(v);
// translate(VEC3(0,0,5));
// TraversorV<PFP::MAP> tv(map);
// std::vector<Dart> obstDarts;
......@@ -146,8 +153,11 @@ void MovingMesh::draw()
{
m_positionVBO->updateData(position) ;
m_render->initPrimitives<PFP>(map, allDarts, Algo::Render::GL2::TRIANGLES) ;
m_render->initPrimitives<PFP>(map, allDarts, Algo::Render::GL2::LINES) ;
m_simpleColorShader->setColor(Geom::Vec4f(1.0,0.627,0.0,0.));
m_render->draw(m_simpleColorShader, Algo::Render::GL2::TRIANGLES);
m_simpleColorShader->setColor(Geom::Vec4f(0.0,0.0,0.0,0.));
m_render->draw(m_simpleColorShader, Algo::Render::GL2::LINES);
}
std::vector<VEC3> MovingMesh::computeProjectedPointSet(float maxHeight)
......
......@@ -73,7 +73,6 @@ VEC3 rotate(VEC3 pos1, VEC3 center, float angle) // renvoie le déplacement nece
}
MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, std::vector<VEC3> goals, bool rigid, bool spin, Dart dInside, ArticulatedObstacle * art, int indParent) :
nbVertices(pos.size()),
center(0),
......@@ -137,7 +136,7 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
{
//extrude face to build a cage
// compute edgeLength for mass-spring
Algo::Surface::Modelisation::extrudeFace<PFP>(map, position, groundFace, 10.0f) ;
Algo::Surface::Modelisation::extrudeFace<PFP>(map, position, groundFace, 15.0f) ;
map.fillHole(groundFace);
groundFace = map.phi2(groundFace);
......@@ -168,23 +167,6 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
center /= nbVertices;
front=(pos[1] + pos[2]) / 2;
//-------- code ajoute par Arash pour les obstacles rectangulaires --------------
use_ellipse = 0;
if(nbVertices==4)
use_ellipse = 1;
if(use_ellipse)
{
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.
}
//-------- fin du code ajoute par Arash pour les obstacles rectangulaires -------
if (spinning && parent==NULL) //départ face à la cible en cas d'obstacles pouvant effectuer des rotations
{
angle = get_angle(goals_[curGoal_] - center,front - center);
......@@ -513,7 +495,7 @@ void MovingObstacle::update()
//stretch spring : /!\ max rigidity relative to the timestep used (unstable otherwise)
float norm = v1.norm();
float rigidity = 50.0f;
float rigidity = 70.0f;
float stretch = rigidity*(edgeLength[dd]-v1.norm());
VEC3 f = stretch*(v1/norm);
......@@ -541,7 +523,7 @@ void MovingObstacle::update()
float restAngle = vertexAngle[dd];
if(restAngle!=0.0f)
{
float angularRig = 50.0f;
float angularRig = 70.0f;
float curAngle = Algo::Surface::Geometry::angle<PFP>(map, map.phi_1(dd),map.phi1(dd),position);
float angularStretch = angularRig*(restAngle-curAngle);
......@@ -617,29 +599,6 @@ void MovingObstacle::update()
// if(!rigid_)
// center = position[groundFace];
//-------- code ajoute par Arash pour les obstacles rectangulaires --------------
if(use_ellipse)
{
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
{
focus1 = center - P0_P1*(1-(width/length)*(sqrt(2)-0.5));
focus2 = center + P0_P1*(1-(width/length)*(sqrt(2)-0.5)) + (velocity_coef*velocity_);
}
else
{
focus1 = center - P0_P1*(1-(width/length)*(sqrt(2)-0.5)) + (velocity_coef*velocity_);
focus2 = center + P0_P1*(1-(width/length)*(sqrt(2)-0.5));
}
sum_dist_foci = sum_dist_foci_rest + velocity_coef*velocity_.norm();
}
//-------- fin du code ajoute par Arash pour les obstacles rectangulaires -------
// MAJ des obstacles
for (unsigned int i = 0; i < nbVertices; ++i)
{
......
......@@ -665,7 +665,7 @@ void Simulator::addMovingObstacle(Dart d, unsigned int obstType)
{
std::vector<VEC3> vPos;
VEC3 start;
float maxHeight=5.0f;
float maxHeight=2.0f;
MovingMesh* mm = NULL;
......@@ -685,7 +685,7 @@ void Simulator::addMovingObstacle(Dart d, unsigned int obstType)
break;
case 1 :
{
mm = new MovingMesh(envMap_, d, "meshRessources/Limace.ply");
mm = new MovingMesh(envMap_, d, "meshRessources/Limace_2.obj");
movingMeshes_.push_back(mm);
vPos = mm->computeProjectedPointSet(maxHeight);
}
......
......@@ -26,6 +26,7 @@
#include "env_generator.h"
#include <string>
#include "glm/gtc/type_ptr.hpp"
#include "Algo/Export/export.h"
SocialAgents::SocialAgents(unsigned int config, unsigned int minSize, unsigned int nbAgent, unsigned int nbObst, unsigned int iterations) :
m_renderStyle(0),
......@@ -287,11 +288,6 @@ void SocialAgents::initRendering()
m_Ground_Shader->setAttributePosition(m_Ground_VBO);
registerShader(m_Ground_Shader);
for(unsigned int i = 0 ; i < simulator.movingMeshes_.size() ; ++i)
registerShader(simulator.movingMeshes_[i]->m_simpleColorShader);
if( !m_DeferedShader.loadShaders("./shaders/Defered.vert", "./shaders/Defered.frag") )
std::cout << __FUNCTION__ << ": pas glop!" << std::endl;
......@@ -299,6 +295,9 @@ void SocialAgents::initRendering()
std::cout << __FUNCTION__ << ": pas glop!" << std::endl;
#endif
for(unsigned int i = 0 ; i < simulator.movingMeshes_.size() ; ++i)
registerShader(simulator.movingMeshes_[i]->m_simpleColorShader);
}
void SocialAgents::updateObstacleVBO()
......@@ -467,8 +466,7 @@ void SocialAgents::cb_redraw()
glClearColor( 0.8f, 0.8f, 1.0f, 1.0f );
glClear( GL_COLOR_BUFFER_BIT );
// glCullFace(GL_BACK);
glEnable(GL_CULL_FACE);
// glEnable(GL_CULL_FACE);
glEnable(GL_MULTISAMPLE);
glEnable(GL_LINE_SMOOTH);
glHint(GL_LINE_SMOOTH_HINT, GL_NICEST);
......@@ -1477,6 +1475,12 @@ void SocialAgents::cb_keyPress(int keycode)
simulator.envMap_.buildingMark, filename) ;
std::string filename2("myAgents.pos") ;
simulator.exportAgents(filename2) ;
std::cout << "exporting meshes" << std::endl ;
std::string filename3("roadMap.ply") ;
std::string filename4("buildingMap.ply") ;
Algo::Surface::Export::exportPLY<PFP>(simulator.envMap_.map, simulator.envMap_.position,filename3.c_str(), false);
if(simulator.config==5)
Algo::Surface::Export::exportPLY<PFP>(simulator.envMap_.mapScenary, simulator.envMap_.positionScenary,filename4.c_str(), false);
break ;
}
case 'p' :
......
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