Commit 895e18f6 authored by Arash HABIBI's avatar Arash HABIBI
Browse files

03_2013

parent 52c29bed
......@@ -79,6 +79,11 @@ public:
std::set<Dart> general_belonging;
VEC3 front;
// voisinage
std::vector<std::pair<float, Obstacle*> > obstacleNeighbors_ ;
std::vector<std::pair<float, Obstacle*> > movingObstacleNeighbors_;
VEC3 finalGoal;
float angle;
// Dart finalDart;
......
......@@ -398,42 +398,44 @@ void MovingObstacle::updateAgentNeighbors() // agents voisins avec distance au b
void MovingObstacle::updateObstacleNeighbors() // obstacles voisins , distance par rapport aux centres des segments// a mettre en place si besoin
{
// obstacleNeighbors_.clear() ;
// movingObstacleNeighbors_.clear() ;
//
// std::vector<Obstacle*>& obst = sim_->envMap_.obstvect[part_.d] ;
// std::vector<Obstacle*>& neighborObst = sim_->envMap_.neighborObstvect[part_.d] ;
// for(std::vector<Obstacle*>::const_iterator it = obst.begin() ; it != obst.end() ; ++it)
// {
// float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, part_.getPosition()) ;
// if (distSq < rangeSq_)
// {
// if (Geom::testOrientation2D(part_.getPosition(), (*it)->p1, (*it)->p2) == Geom::RIGHT)
// {
// if ((*it)->mo==NULL)
// obstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
// else
// {
// movingObstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
// }
// }
//
// }
// }
// for(std::vector<Obstacle*>::const_iterator it = neighborObst.begin() ; it != neighborObst.end() ; ++it)
// {
// float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, part_.getPosition()) ;
// if(distSq < rangeSq_)
// {
// if(Geom::testOrientation2D(part_.getPosition(), (*it)->p1, (*it)->p2) == Geom::RIGHT)
// {
// if ((*it)->mo==NULL)
// obstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
// else
// movingObstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
// }
// }
// }
// ARASH en decommentant ce qui suit, on rend les obstacles perceptibles par la tete de la limace (ou du chariot)
obstacleNeighbors_.clear() ;
movingObstacleNeighbors_.clear() ;
std::vector<Obstacle*>& obst = sim_->envMap_.obstvect[part_.d] ;
std::vector<Obstacle*>& neighborObst = sim_->envMap_.neighborObstvect[part_.d] ;
for(std::vector<Obstacle*>::const_iterator it = obst.begin() ; it != obst.end() ; ++it)
{
float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, part_.getPosition()) ;
if (distSq < rangeSq_)
{
if (Geom::testOrientation2D(part_.getPosition(), (*it)->p1, (*it)->p2) == Geom::RIGHT)
{
if ((*it)->mo==NULL)
obstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
else
{
movingObstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
}
}
}
}
for(std::vector<Obstacle*>::const_iterator it = neighborObst.begin() ; it != neighborObst.end() ; ++it)
{
float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, part_.getPosition()) ;
if(distSq < rangeSq_)
{
if(Geom::testOrientation2D(part_.getPosition(), (*it)->p1, (*it)->p2) == Geom::RIGHT)
{
if ((*it)->mo==NULL)
obstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
else
movingObstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
}
}
}
}
void MovingObstacle::update()
......@@ -467,6 +469,8 @@ void MovingObstacle::update()
else
rotor = abs_angle*angle ;
// masse ressort pour la limace
if(!rigid_)
{
Dart d = groundFace;
......@@ -482,15 +486,19 @@ void MovingObstacle::update()
CellMarkerStore<EDGE> cm(map);
DartMarkerStore dm(map);
// ARASH : On parcourt les sommets de la grande face
for (unsigned int i = 1; i < nbVertices; ++i)
{
Dart dd = d;
// ARASH : On parcourt les sous-faces triangulaire de la grande face
do {
if(!cm.isMarked(dd))
{
cm.mark(dd);
VEC3 p1Next = position[map.phi1(dd)]+(velocity[map.phi1(dd)] * sim_->timeStep_);
VEC3 p2Next = position[dd]+(velocity[dd] * sim_->timeStep_);
// p1Next et p2Next sont la position des extremites de l'arete.
VEC3 v1 = (p1Next-p2Next);
//stretch spring : /!\ max rigidity relative to the timestep used (unstable otherwise)
......@@ -508,6 +516,7 @@ void MovingObstacle::update()
dd = map.phi1(dd);
} while(dd!=d);
// ARASH : les ressorts angulaires
do {
if(!dm.isMarked(dd))
{
......@@ -537,6 +546,48 @@ void MovingObstacle::update()
dd = map.phi1(dd);
} while(dd!=d);
//-------------------------------------------------------------------------
// ARASH : A présent on calcule les interactions avec les autres obstacles.
do {
if(!cm.isMarked(dd))
{
cm.mark(dd);
VEC3 p = position[dd]+(velocity[dd] * sim_->timeStep_);
for(std::vector<std::pair<float, Obstacle*> >::iterator it = movingObstacleNeighbors_.begin() ;
it != movingObstacleNeighbors_.end() ; ++it)
{
Obstacle * obst = it->second;
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 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-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);
// 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[dd] -= force_value * normal;
// This force is not symmetrically applied
// We assume that a homologous vertex in the other obstacle
// is also intersecting the current obstacle
}
}
}
dd = map.phi1(dd);
} while(dd!=d);
map.next(d);
}
......
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