VoronoiPositionGenerator.cpp 22.1 KB
Newer Older
1 2 3 4 5 6 7
/*
 * VoronoiPositionGenerator.cpp
 *
 *  Created on: Sep 2, 2015
 *      Author: gsp1502
 */

8
#define PLOT_VORONOI_DIAGRAM 0
Qiancheng Xu's avatar
Qiancheng Xu committed
9
#define NOMINMAX
Mohcine Chraibi's avatar
Mohcine Chraibi committed
10
static int global_count = 0;
Mohcine Chraibi's avatar
Mohcine Chraibi committed
11

12 13


14 15 16 17
#include "VoronoiPositionGenerator.h"
//check if all includes are necessary
#include "../pedestrian/AgentsSourcesManager.h"
#include "../pedestrian/Pedestrian.h"
Mohcine Chraibi's avatar
Mohcine Chraibi committed
18 19 20 21 22
//#include "../pedestrian/StartDistribution.h"
//#include "../pedestrian/PedDistributor.h"
//#include "../pedestrian/AgentsSource.h"
//#include "../geometry/Building.h"
//#include "../geometry/Point.h"
23 24

//#include "../mpi/LCGrid.h"
Mohcine Chraibi's avatar
Mohcine Chraibi committed
25
//#include <iostream>
26
#include <thread>
Mohcine Chraibi's avatar
Mohcine Chraibi committed
27
//#include <chrono>
28 29


Mohcine Chraibi's avatar
Mohcine Chraibi committed
30 31 32 33 34
//#include "../geometry/SubRoom.h"
//#include <stdlib.h>
//#include <time.h>
//#include <string>
//#include <random>
35 36 37 38 39 40 41 42 43

using boost::polygon::voronoi_builder;
using boost::polygon::voronoi_diagram;
using boost::polygon::x;
using boost::polygon::y;
using boost::polygon::low;
using boost::polygon::high;


44
//wrapping the boost objects (just the point object)
Mohcine Chraibi's avatar
Mohcine Chraibi committed
45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62
namespace boost {
namespace polygon {
template <>
struct geometry_concept<Point> {
     typedef point_concept type;
};

template <>
struct point_traits<Point> {
     typedef int coordinate_type;

     static inline coordinate_type get(
               const Point& point, orientation_2d orient) {
          return (orient == HORIZONTAL) ? point._x : point._y;
     }
};
}  // polygon
}  // boost
63 64 65 66 67 68


using namespace std;


//functions
69
//TODO: refactor the function
70
bool IsEnoughInSubroom( SubRoom* subroom, Point& pt, double radius )
71
{
72 73 74 75
     for (const auto& wall: subroom->GetAllWalls())
          if(wall.DistTo(pt)<radius)
               return false;

76
     for(const auto& trans: subroom->GetAllTransitions() )
Mohcine Chraibi's avatar
Mohcine Chraibi committed
77 78
         if ( trans->DistTo(pt) < radius + 0.1 )
                 return false;
79 80

     for( const auto& cross: subroom->GetAllCrossings() )
Mohcine Chraibi's avatar
Mohcine Chraibi committed
81 82
         if( cross->DistTo(pt) < radius + 0.1 )
                 return false;
83 84

     return true;
85 86
}

87 88
bool ComputeBestPositionVoronoiBoost(AgentsSource* src, std::vector<Pedestrian*>& peds,
         Building* building)
Mohcine Chraibi's avatar
Mohcine Chraibi committed
89
{
90 91 92 93
    bool return_value = true;
    auto dist = src->GetStartDistribution();
    int roomID = dist->GetRoomId();
    int subroomID = dist->GetSubroomID();
94
    // std::string caption = (building->GetRoom( roomID ))->GetCaption();
95 96 97 98

    std::vector<Pedestrian*> existing_peds;
    std::vector<Pedestrian*> peds_without_place;
    building->GetPedestrians(roomID, subroomID, existing_peds);
Mohcine Chraibi's avatar
Mohcine Chraibi committed
99 100
    for (auto  pp : existing_peds)
         std::cout << "existing peds: " << pp->GetID() << " in " << pp->GetPos()._x << ", " << pp->GetPos()._y << std::endl;
101 102 103 104 105 106 107 108 109 110 111 112

    double radius = 0.3; //radius of a person, 0.3 is just some number(needed for the fake_peds bellow), will be changed afterwards

    SubRoom* subroom = building->GetRoom( roomID )->GetSubRoom(subroomID);

    double factor = 100;  //factor for conversion to integer for the boost voronoi

    std::vector<Point> fake_peds;
    Point temp(0,0);
    //fake_peds will be the positions of "fake" pedestrians, multiplied by factor and converted to int
    for (auto vert: subroom->GetPolygon() ) //room vertices
    {
113 114 115
          const Point& center_pos = subroom->GetCentroid();
          temp._x = ( center_pos._x-vert._x );
          temp._y = ( center_pos._y-vert._y );
Mohcine Chraibi's avatar
Mohcine Chraibi committed
116
          temp = temp/temp.Norm();
117 118 119 120 121
          temp = temp*(radius*1.4);  //now the norm of the vector is ~r*sqrt(2), pointing to the center
          temp = temp + vert;
          temp._x = (int)(temp._x*factor);
          temp._y = (int)(temp._y*factor);
          fake_peds.push_back( temp );
122 123 124 125 126
    }

    std::vector<Pedestrian*>::iterator iter_ped;
    for (iter_ped = peds.begin(); iter_ped != peds.end(); )
    {
127
         Pedestrian* ped = *iter_ped;
128 129 130 131
         radius = ped->GetEllipse().GetBmax(); //max radius of the current pedestrian

         if(existing_peds.size() == 0 )
         {
Mohcine Chraibi's avatar
Mohcine Chraibi committed
132
              const Point& center_pos = subroom->GetCentroid();
133 134 135 136 137 138 139 140 141 142 143 144 145 146 147

              double x_coor = 3 * ( (double)rand() / (double)RAND_MAX ) - 1.5;
              double y_coor = 3 * ( (double)rand() / (double)RAND_MAX ) - 1.5;
              Point random_pos(x_coor, y_coor);
              Point new_pos = center_pos + random_pos;
              //this could be better, but needs to work with any polygon - random point inside a polygon?
              if ( subroom->IsInSubRoom( new_pos ) )
              {
                   if( IsEnoughInSubroom(subroom, new_pos, radius ) )
                   {
                        ped->SetPos(center_pos + random_pos, true);
                   }
              }
              else
              {
148
                    ped->SetPos(center_pos, true);
149
              }
Mohcine Chraibi's avatar
Mohcine Chraibi committed
150

151 152 153 154 155 156 157 158 159 160
              Point v;
              if (ped->GetExitLine()) {
                    v = (ped->GetExitLine()->ShortestPoint(ped->GetPos())- ped->GetPos()).Normalized();
              } else {
                    v = Point(0., 0.);
              }
              //double speed=ped->GetV0Norm();
              double speed = ped->GetEllipse().GetV0(); //@todo: some peds do not have a navline. This should not be accepted.
              v=v*speed;
              ped->SetV(v);
161 162 163 164 165 166
              existing_peds.push_back(ped);

         }//0

         else //more than one pedestrian
         {
Mohcine Chraibi's avatar
Mohcine Chraibi committed
167 168
           //it would be better to maybe have a mapping between discrete_positions and pointers to the pedestrians
           //then there would be no need to remember the velocities_vector and goal_vector
Mohcine Chraibi's avatar
Mohcine Chraibi committed
169
              std::vector<Point> discrete_positions;
170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186
              std::vector<Point> velocities_vector;
              std::vector<int> goal_vector;
              Point tmp(0,0);
              Point v(0,0);
              double no = 0;

              //points from double to integer
              for (const auto& eped: existing_peds)
              {
                   const Point& pos = eped->GetPos();
                   tmp._x = (int)( pos._x*factor );
                   tmp._y = (int)( pos._y*factor );
                   discrete_positions.push_back( tmp );
                   velocities_vector.push_back( eped->GetV() );
                   goal_vector.push_back( eped->GetFinalDestination() );

                   //calculating the mean, using it for the fake pedestrians
Mohcine Chraibi's avatar
Mohcine Chraibi committed
187
                   v += eped->GetV();
188 189 190 191 192 193
                   no++;
              }
              // sum up the weighted velocity in the loop
              v = v/no; //this is the mean of all velocities

              //adding fake people to the vector for constructing voronoi diagram
Mohcine Chraibi's avatar
Mohcine Chraibi committed
194 195
              //for (unsigned int i=0; i<subroom->GetPolygon().size(); i++ )
              for(auto fake_ped: fake_peds)
196
              {
Mohcine Chraibi's avatar
Mohcine Chraibi committed
197
                   discrete_positions.push_back( fake_ped );
198 199 200 201 202 203 204
                   velocities_vector.push_back( v );
                   goal_vector.push_back( -10 );
              }

              //constructing the diagram
              voronoi_diagram<double> vd;
              construct_voronoi(discrete_positions.begin(), discrete_positions.end(), &vd);
Mohcine Chraibi's avatar
Mohcine Chraibi committed
205 206 207
#if PLOT_VORONOI_DIAGRAM
              plotVoronoi(discrete_positions, vd, subroom, factor);
#endif
208 209
              voronoi_diagram<double>::const_vertex_iterator chosen_it = vd.vertices().begin();
              double dis = 0;
Mohcine Chraibi's avatar
Mohcine Chraibi committed
210 211 212 213 214
              //std::default_random_engine gen = dist->GetGenerator();
              if(!src->Greedy())
                    VoronoiBestVertexRandMax(discrete_positions, vd, subroom, factor, chosen_it, dis, radius);
              else
                    VoronoiBestVertexGreedy(discrete_positions, vd, subroom, factor, chosen_it, dis, radius);
Mohcine Chraibi's avatar
Mohcine Chraibi committed
215

Mohcine Chraibi's avatar
Mohcine Chraibi committed
216
              if( dis > 4*radius*radius)
217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245
              {
                   Point pos( chosen_it->x()/factor, chosen_it->y()/factor ); //check!
                   ped->SetPos(pos , true);
                   VoronoiAdjustVelocityNeighbour(chosen_it, ped, velocities_vector, goal_vector);

                   // proceed to the next pedestrian
                   existing_peds.push_back(ped);
                   ++iter_ped;

              }
              else
              {
                   //reject the pedestrian:
                   return_value = false;
                   peds_without_place.push_back(*iter_ped); //Put in a different queue, they will be put back in the source.
                   iter_ped=peds.erase(iter_ped); // remove from the initial vector since it should only contain the pedestrians that could find a place
              }

         }// >0


    }//for loop


    //maybe not all pedestrians could find a place, requeue them in the source
    if(peds_without_place.size()>0)
         src->AddAgentsToPool(peds_without_place);
    return return_value;
}
246 247

//gives an agent the mean velocity of his voronoi-neighbors
Mohcine Chraibi's avatar
Mohcine Chraibi committed
248 249
void VoronoiAdjustVelocityNeighbour(voronoi_diagram<double>::const_vertex_iterator& chosen_it, Pedestrian* ped,
        const std::vector<Point>& velocities_vector, const std::vector<int>& goal_vector)
250
{
251 252 253
     //finding the neighbors (nearest pedestrians) of the chosen vertex
     const voronoi_diagram<double>::vertex_type &vertex = *chosen_it;
     const voronoi_diagram<double>::edge_type *edge = vertex.incident_edge();
254 255
     double no1=0,no2=0;
     double backup_speed = 0;
256
     //std::size_t index;
Mohcine Chraibi's avatar
Mohcine Chraibi committed
257 258 259 260 261 262 263 264
     Point v(0,0);
     if(ped->GetExitLine() != nullptr)
          v = (ped->GetExitLine()->ShortestPoint(ped->GetPos())- ped->GetPos()).Normalized(); //the direction
     else
     {
          int gotRoute = ped->FindRoute();
          if(gotRoute < 0) printf("\nWARNING: source agent %d can not get exit\n", ped->GetID());
     }
265
     double speed = 0;
266 267
     do
     {
268
          std::size_t index = ( edge->cell() )->source_index();
269 270
          if( ped->GetFinalDestination() == goal_vector[index]  )
          {
Mohcine Chraibi's avatar
Mohcine Chraibi committed
271 272
                  no1++;
                  speed += velocities_vector[index].Norm();
273 274 275
          }
          else
          {
Mohcine Chraibi's avatar
Mohcine Chraibi committed
276 277
                  no2++;
                  backup_speed += velocities_vector[index].Norm();
278
          }
279 280 281
          edge = edge->rot_next();
     } while (edge != vertex.incident_edge());

282
     if(no1)
Mohcine Chraibi's avatar
Mohcine Chraibi committed
283
         speed = speed/no1;
284
     else
Mohcine Chraibi's avatar
Mohcine Chraibi committed
285
         speed = backup_speed/(no2*3.0); //just some small speed
286

287
     v = v*speed;
288
     ped->SetV(v);
289 290 291

}

292

293
//gives the voronoi vertex with max distance
Mohcine Chraibi's avatar
Mohcine Chraibi committed
294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328
//void VoronoiBestVertexMax(const std::vector<Point>& discrete_positions, const voronoi_diagram<double>& vd,
//        SubRoom* subroom, double factor, voronoi_diagram<double>::const_vertex_iterator& max_it, double& max_dis,
//        double radius)
//{
//     double dis = 0;
//     double score;
//     double max_score = -100; //calculated using distance and considering the goal
//
//
//
//     for (auto it = vd.vertices().begin(); it != vd.vertices().end(); ++it)
//     {
//          Point vert_pos( it->x()/factor, it->y()/factor );
//          if( subroom->IsInSubRoom(vert_pos) )
//               if( IsEnoughInSubroom( subroom, vert_pos, radius ) )
//               {
//                    const voronoi_diagram<double>::vertex_type &vertex = *it;
//                    const voronoi_diagram<double>::edge_type *edge = vertex.incident_edge();
//
//                    std::size_t index = ( edge->cell() )->source_index();
//                    Point p = discrete_positions[index];
//
//                    dis = ( p._x - it->x() )*( p._x - it->x() )  + ( p._y - it->y() )*( p._y - it->y() )  ;
//
//                    score = dis;
//
//
//                    //constructing the checking line
///*
//                    Point p2 = (ped->GetExitLine()->ShortestPoint(vert_pos)-vert_pos).Normalized(); //problem: ped does not have a position
//                    p2 = p2 + p2; //looking 2m in front
//                    Line check_line(vert_pos, vert_pos + p2);  //this is the first 2m of exit line
//
//                    do
//                    {
Mohcine Chraibi's avatar
Mohcine Chraibi committed
329 330 331 332 333 334 335
//                      //do something
//                      if( goal_vector[index]!=-3 &&  goal_vector[index]!=ped->GetFinalDestination() ) //
//                              if( check_line.IntersectionWithCircle(p,1.0) )    //0.7 because the radius is around 0.3
//                              {
//                                      score -= 100;
//                                      break;
//                              }
Mohcine Chraibi's avatar
Mohcine Chraibi committed
336 337
//
//
Mohcine Chraibi's avatar
Mohcine Chraibi committed
338 339 340 341
//                      //change edge
//                      edge = edge->rot_next();
//                      index = ( edge->cell() )->source_index();
//                      p = discrete_positions[index]/factor;
Mohcine Chraibi's avatar
Mohcine Chraibi committed
342 343 344 345 346 347
//
//                    } while( edge != vertex.incident_edge() );
//*/
//                    if(score > max_score)
//                    {
//                         max_score =score;
Mohcine Chraibi's avatar
Mohcine Chraibi committed
348
//                       max_dis = dis;
Mohcine Chraibi's avatar
Mohcine Chraibi committed
349 350 351 352 353 354
//                         max_it = it;
//                    }
//               }
//     }
//     //at the end, max_it is the choosen vertex, or the first vertex - max_dis=0 assures that this position will not be taken
//}
355

Mohcine Chraibi's avatar
Mohcine Chraibi committed
356 357 358 359 360 361 362 363 364 365 366 367 368 369 370 371
/**
 * Returns a Voronoi vertex randomly with respect to weights proportional to distances^2
 * For vertexes \f$v_i\f$  and distances \f$d_i\f$ to their surrounding seeds
 * calculate the probabilities \f$p_i\f$ as
 * \f[
 *     p_i= \frac{d_i^2}{\sum_j^n d_j^2}
 * \f]
 *
 * @param discrete_positions: agents and imaginary agents
 * @param vd: Voronoi diagram
 * @param subroom
 * @param factor: used to convert the coordinates back from int (was necessary for Boost Voronoi calculations)
 * @param chosen_it: return best_vertex
 * @param dis: distance squared of  the best_vertex to its surrouding seeds.
 * @param radius: radius of pedestrian
 */
372
void VoronoiBestVertexRandMax (const std::vector<Point>& discrete_positions, const voronoi_diagram<double>& vd, SubRoom* subroom,
Mohcine Chraibi's avatar
Mohcine Chraibi committed
373
                               double factor, voronoi_diagram<double>::const_vertex_iterator& chosen_it, double& dis	, double radius)
374
{
375 376
     std::vector< voronoi_diagram<double>::const_vertex_iterator > possible_vertices;
     vector<double> partial_sums;
Mohcine Chraibi's avatar
Mohcine Chraibi committed
377
     unsigned long size=0;
Mohcine Chraibi's avatar
Mohcine Chraibi committed
378
     for (auto it = vd.vertices().begin(); it != vd.vertices().end(); ++it){
379 380
          Point vert_pos = Point( it->x()/factor, it->y()/factor );
          if( subroom->IsInSubRoom( vert_pos ) )
381
               if( IsEnoughInSubroom(subroom, vert_pos,radius) )
382 383 384 385 386 387
               {
                    const voronoi_diagram<double>::vertex_type &vertex = *it;
                    const voronoi_diagram<double>::edge_type *edge = vertex.incident_edge();
                    std::size_t index = ( edge->cell() )->source_index();
                    Point p = discrete_positions[index];

Mohcine Chraibi's avatar
Mohcine Chraibi committed
388 389
                    dis = ( p._x - it->x() )*( p._x - it->x() )   + ( p._y - it->y() )*( p._y - it->y() ) ;
                    dis = dis / factor / factor;
390
                    possible_vertices.push_back( it );
Mohcine Chraibi's avatar
Mohcine Chraibi committed
391
                    partial_sums.push_back( dis );
392 393 394 395 396 397 398 399

                    size = partial_sums.size();
                    if( size > 1 )
                    {
                         partial_sums[ size - 1 ] += partial_sums[ size - 2 ];
                    }
               }
     }
Mohcine Chraibi's avatar
Mohcine Chraibi committed
400
     // partial_sums: [d_0^2,  d_0^2 + d_1^2,  d_0^2 + d_1^2 + d_2^2, ..., \sum_i^{n-1} d_i^3]
401 402 403 404
     //now we have the vector of possible vertices and weights and we can choose one randomly

     double lower_bound = 0;
     double upper_bound = partial_sums[size-1];
Mohcine Chraibi's avatar
Mohcine Chraibi committed
405 406 407 408 409 410 411 412 413
     std::random_device rd;
     std::mt19937 gen(rd()); //@todo use seed instead of rd(). Generator should not be here
     std::uniform_real_distribution<double> distribution(lower_bound, upper_bound); //std::nextafter(upper_bound, DBL_MAX));
     vector<double> random_numbers;
     for(unsigned int r=0; r<size;r++)
           random_numbers.push_back(distribution(gen));

     shuffle(random_numbers.begin(), random_numbers.end(), gen);
     double a_random_double = random_numbers[0];
Mohcine Chraibi's avatar
Mohcine Chraibi committed
414

Mohcine Chraibi's avatar
Mohcine Chraibi committed
415 416 417 418 419 420 421 422 423
     //the first element in the range [first, last) that is not less than a_random_double
     auto lower = std::lower_bound(partial_sums.begin(), partial_sums.end(), a_random_double);
     int iposition = lower - partial_sums.begin();
     // if iposition == size then no element is found. Should not happen..
     chosen_it = possible_vertices[iposition];
     dis = partial_sums[iposition];
     if (iposition>1)
           dis -= partial_sums[iposition-1];
}
424

Mohcine Chraibi's avatar
Mohcine Chraibi committed
425 426 427 428 429 430 431 432 433 434 435 436 437 438 439 440 441 442 443 444 445 446 447

/**
 * Returns a Voronoi vertex with the largest distances^2
 * For vertexes \f$v_i\f$  and distances \f$d_i\f$ to their surrounding seeds
 * Use the "greedy" approach by choosing the best_vertex as the
 * vertex with the biggest distance to the surrounding seeds
 *
 * @param discrete_positions: agents and imaginary agents
 * @param vd: Voronoi diagram
 * @param subroom
 * @param factor: used to convert the coordinates back from int (was necessary for Boost Voronoi calculations)
 * @param chosen_it: return best_vertex
 * @param dis: distance squared of  the best_vertex to its surrouding seeds.
 * @param radius: radius of pedestrian
 */
void VoronoiBestVertexGreedy (const std::vector<Point>& discrete_positions, const voronoi_diagram<double>& vd, SubRoom* subroom,
                               double factor, voronoi_diagram<double>::const_vertex_iterator& chosen_it, double& dis	, double radius)
{
     std::vector< voronoi_diagram<double>::const_vertex_iterator > possible_vertices;
     vector<double> distances;
     for (auto it = vd.vertices().begin(); it != vd.vertices().end(); ++it){
          Point vert_pos = Point( it->x()/factor, it->y()/factor );
          if( subroom->IsInSubRoom( vert_pos ) )
Mohcine Chraibi's avatar
Mohcine Chraibi committed
448
               if( IsEnoughInSubroom(subroom, vert_pos, radius) )
Mohcine Chraibi's avatar
Mohcine Chraibi committed
449 450 451 452 453 454 455 456 457 458 459 460 461 462 463 464 465 466
               {
                    const voronoi_diagram<double>::vertex_type &vertex = *it;
                    const voronoi_diagram<double>::edge_type *edge = vertex.incident_edge();
                    std::size_t index = ( edge->cell() )->source_index();
                    Point p = discrete_positions[index];

                    dis = ( p._x - it->x() )*( p._x - it->x() )   + ( p._y - it->y() )*( p._y - it->y() ) ;
                    dis = dis / factor / factor;
                    possible_vertices.push_back( it );
                    distances.push_back( dis );

               }
     }

     auto biggest = std::max_element(distances.begin(), distances.end());
     int iposition = biggest - distances.begin(); // first biggest distance
     chosen_it = possible_vertices[iposition];
     dis = distances[iposition];
467 468
}

Mohcine Chraibi's avatar
Mohcine Chraibi committed
469

470
//gives a random voronoi vertex
Mohcine Chraibi's avatar
Mohcine Chraibi committed
471 472 473 474 475 476 477 478 479 480 481 482 483 484 485 486 487 488 489 490 491 492 493 494 495 496 497 498 499 500 501 502
//void VoronoiBestVertexRand (const std::vector<Point>& discrete_positions, const voronoi_diagram<double>& vd, SubRoom* subroom,
//          double factor, voronoi_diagram<double>::const_vertex_iterator& chosen_it, double& dis, double radius	)
//{
//     std::vector< voronoi_diagram<double>::const_vertex_iterator > possible_vertices;
//     std::vector<double> distances;
//
//     for (auto it = vd.vertices().begin(); it != vd.vertices().end(); ++it)
//     {
//          Point vert_pos = Point( it->x()/factor, it->y()/factor );
//          if( subroom->IsInSubRoom(vert_pos) )
//               if( IsEnoughInSubroom(subroom, vert_pos, radius) )
//               {
//                    const voronoi_diagram<double>::vertex_type &vertex = *it;
//                    const voronoi_diagram<double>::edge_type *edge = vertex.incident_edge();
//
//                    std::size_t index = ( edge->cell() )->source_index();
//                    Point p = discrete_positions[index];
//
//                    dis = ( p._x - it->x() )*( p._x - it->x() )   + ( p._y - it->y() )*( p._y - it->y() )  ;
//
//                    possible_vertices.push_back( it );
//                    distances.push_back( dis );
//               }
//     }
//     //now we have all the possible vertices and their distances and we can choose one randomly
//     //TODO: get the seed from the simulation/argumentparser
//     //srand (time(NULL));
//     unsigned int i = rand() % possible_vertices.size();
//     chosen_it = possible_vertices[i];
//     dis = distances[i];
//
//}
503 504


Mohcine Chraibi's avatar
Mohcine Chraibi committed
505 506 507 508 509

void plotVoronoi(const std::vector<Point>& discrete_positions, const voronoi_diagram<double>& vd, SubRoom* subroom, double factor)
{
     // =============== plot Voronoi Diagram =====================
     char name [50];
Mohcine Chraibi's avatar
Mohcine Chraibi committed
510
     sprintf(name,  "log_%.3d.py", global_count);
Mohcine Chraibi's avatar
Mohcine Chraibi committed
511 512
     FILE * f;
     f = fopen(name, "w");
Mohcine Chraibi's avatar
Mohcine Chraibi committed
513
    // plot cells
Mohcine Chraibi's avatar
Mohcine Chraibi committed
514 515
     fprintf(f, "# ------------------------------\n");
     fprintf(f, "import matplotlib.pyplot as plt\n");
Mohcine Chraibi's avatar
Mohcine Chraibi committed
516
     //  plot seeds
Mohcine Chraibi's avatar
Mohcine Chraibi committed
517 518 519 520 521 522 523 524 525 526 527
     for(auto pos : discrete_positions)
     {
           fprintf(f, "plt.plot([%f], [%f], \"or\")\n", pos._x/factor, pos._y/factor);
     }
// plot cells
     for (auto it = vd.cells().begin(); it != vd.cells().end(); ++it){
           const voronoi_diagram<double>::cell_type &cell = *it;
           const voronoi_diagram<double>::edge_type *edge = cell.incident_edge();
         do {
                 if(edge->vertex0() && edge->vertex1())
                 {
Mohcine Chraibi's avatar
Mohcine Chraibi committed
528
                       fprintf(f, "plt.plot([%f, %f], [%f, %f], \"bo-\", lw=2)\n",  edge->vertex0()->x()/factor, edge->vertex1()->x()/factor, edge->vertex0()->y()/factor, edge->vertex1()->y()/factor);
Mohcine Chraibi's avatar
Mohcine Chraibi committed
529
                 }
Mohcine Chraibi's avatar
Mohcine Chraibi committed
530

Mohcine Chraibi's avatar
Mohcine Chraibi committed
531
              edge = edge->next();
Mohcine Chraibi's avatar
Mohcine Chraibi committed
532
        } while (edge != cell.incident_edge());
Mohcine Chraibi's avatar
Mohcine Chraibi committed
533 534 535 536 537 538
     }
// plot geometry
     double max_x=std::numeric_limits<double>::min(), min_x=std::numeric_limits<double>::max();
     double max_y=std::numeric_limits<double>::min(), min_y=std::numeric_limits<double>::max();
     const vector<Point> polygon = subroom->GetPolygon();
     for(auto it = polygon.begin(); it != polygon.end(); ) {
Mohcine Chraibi's avatar
Mohcine Chraibi committed
539
           Point gpoint = *(it++);
Mohcine Chraibi's avatar
Mohcine Chraibi committed
540 541 542 543 544 545 546
           // Point gpointNext = *it;
           if(gpoint._x > max_x)
                 max_x = gpoint._x;
           if(gpoint._y > max_y)
                 max_y = gpoint._y;
           if(gpoint._x < min_x)
                 min_x = gpoint._x;
547
           if(gpoint._y < min_y)
Mohcine Chraibi's avatar
Mohcine Chraibi committed
548
                 min_y = gpoint._y;
Mohcine Chraibi's avatar
Mohcine Chraibi committed
549

Mohcine Chraibi's avatar
Mohcine Chraibi committed
550 551 552 553 554 555 556 557 558
           // fprintf(f, "plt.plot([%f, %f], [%f, %f], \"k-\", lw=2)\n",  gpoint._x, gpointNext._x, gpoint._y, gpointNext._y);
           if(it == polygon.end()){
                 // fprintf(f, "plt.plot([%f, %f], [%f, %f], \"k-\", lw=2)\n",  gpointNext._x, polygon.begin()->_x,  gpointNext._y, polygon.begin()->_y );
                 break;
           }
     }
     double eps=0.0;
     fprintf(f, "plt.xlim([%f, %f])\n", min_x-eps, max_x+eps);
     fprintf(f, "plt.ylim([%f, %f])\n", min_y-eps, max_y+eps);
Mohcine Chraibi's avatar
Mohcine Chraibi committed
559 560
     fprintf(f, "plt.title(\"agents = %3d\")\n", (int)discrete_positions.size());
     fprintf(f, "plt.savefig(\"%.4d.png\", dpi=600)\n", global_count++);
Mohcine Chraibi's avatar
Mohcine Chraibi committed
561 562
     fclose(f);
}