disable direction model and introduce basic noise

math/VelocityModel.cpp

@@ -249,7 +249,35 @@ void VelocityModel::ComputeNextTimeStep(double current, double deltaT, Building*
                 Point repWall = ForceRepRoom(allPeds[p], subroom);
 
                 // calculate new direction ei according to (6)
-                Point direction = e0(ped, room) + repPed + repWall;
+                //Point direction = e0(ped, room) + repPed + repWall;
+                Point direction = e0(ped, room);
+                //std::cout << "e0:" << direction._x << direction._y << std::endl;
+
+                // generate random angle
+                const double min_angle = 0;
+                const double max_angle = 30;
+                std::random_device rd;
+                std::mt19937 eng(rd());
+                std::uniform_int_distribution<> distr(min_angle, max_angle);
+
+                Point noise = Point(sin(2*3.14159*distr(eng)/360), sin(2*3.14159*distr(eng)/360));
+                direction = direction + noise;
+                
+
+                // Define random generator with Gaussian distribution
+                //const double mean = 0.0;
+                //const double stddev = 0.4;
+                //std::default_random_engine generator;
+                //std::normal_distribution<double> white_noise(mean, stddev);
+
+                // Add Gaussian noise
+
+                //std::cout << "old:" << direction._x << direction._y << std::endl;
+                //direction._x = direction._x + white_noise(generator);
+                //direction._y = direction._y + white_noise(generator);
+                //direction = direction.Normalized();
+                //std::cout << "new:" << direction._x << direction._y << std::endl;
+                
                 for (int i = 0; i < size; i++) {
                       Pedestrian* ped1 = neighbours[i];
                      // calculate spacing
@@ -380,6 +408,7 @@ Point VelocityModel::e0(Pedestrian* ped, Room* room) const
            (dynamic_cast<DirectionSubLocalFloorfield*>(_direction.get()))  ) {
           desired_direction = target-pos;
           if (desired_direction.NormSquare() < 0.25) {
+          //if (desired_direction.NormSquare() < 0.05) {
               desired_direction = lastE0;
               ped->SetLastE0(lastE0);
 //              Log->Write("desired_direction: %f    %f", desired_direction._x, desired_direction._y);
@@ -409,7 +438,8 @@ Point VelocityModel::e0(Pedestrian* ped, Room* room) const
 double VelocityModel::OptimalSpeed(Pedestrian* ped, double spacing) const
 {
       double v0 = ped->GetV0Norm();
-      double T = ped->GetT();
+      //double T = ped->GetT();
+      double T = 1.0;
       double l = 2*ped->GetEllipse().GetBmax(); //assume peds are circles with const radius
       double speed = (spacing-l)/T;
       speed = (speed>0)?speed:0;