Update.

[flatland.git] / polygon.cc

diff --git a/polygon.cc b/polygon.cc
index bfa95f8..b899950 100644 (file)
--- a/polygon.cc
+++ b/polygon.cc
@@ -1,26 +1,26 @@
 
 /*
- *  dyncnn is a deep-learning algorithm for the prediction of
- *  interacting object dynamics
- *
- *  Copyright (c) 2016 Idiap Research Institute, http://www.idiap.ch/
- *  Written by Francois Fleuret <francois.fleuret@idiap.ch>
- *
- *  This file is part of dyncnn.
- *
- *  dyncnn is free software: you can redistribute it and/or modify it
- *  under the terms of the GNU General Public License version 3 as
- *  published by the Free Software Foundation.
- *
- *  dyncnn is distributed in the hope that it will be useful, but
- *  WITHOUT ANY WARRANTY; without even the implied warranty of
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- *  General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License
- *  along with dyncnn.  If not, see <http://www.gnu.org/licenses/>.
- *
- */
+
+   flatland is a simple 2d physical simulator
+
+   Copyright (c) 2016 Idiap Research Institute, http://www.idiap.ch/
+   Written by Francois Fleuret <francois.fleuret@idiap.ch>
+
+   This file is part of flatland
+
+   flatland is free software: you can redistribute it and/or modify it
+   under the terms of the GNU General Public License version 3 as
+   published by the Free Software Foundation.
+
+   flatland is distributed in the hope that it will be useful, but
+   WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+   General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with flatland.  If not, see <http://www.gnu.org/licenses/>.
+
+*/
 
 #include <iostream>
 
@@ -31,7 +31,8 @@ using namespace std;
 
 static const scalar_t dl = 20.0;
 static const scalar_t repulsion_constant = 0.2;
-static const scalar_t dissipation = 0.5;
+static const scalar_t speed_max = 1e2;
+static const scalar_t angular_speed_max = M_PI / 10;
 
 Polygon::Polygon(scalar_t mass,
                  scalar_t red, scalar_t green, scalar_t blue,
@@ -306,17 +307,27 @@ void Polygon::initialize(int nb_polygons) {
 }
 
 bool Polygon::update(scalar_t dt) {
+  scalar_t speed = sqrt(_dcenter_x * _dcenter_x + _dcenter_y * _dcenter_y);
+
+  if(speed > 0) {
+    scalar_t speed_target = speed_max - exp(-speed / speed_max) * speed_max;
+    _dcenter_x = speed_target * _dcenter_x / speed;
+    _dcenter_y = speed_target * _dcenter_y / speed;
+  }
+
+  scalar_t angular_speed = abs(_dtheta);
+
+  if(angular_speed > 0) {
+    scalar_t angular_speed_target = angular_speed_max - exp(-angular_speed / angular_speed_max) * angular_speed_max;
+    _dtheta = angular_speed_target * _dtheta / angular_speed;
+  }
+
   if(!_nailed) {
     _center_x += _dcenter_x * dt;
     _center_y += _dcenter_y * dt;
     _theta += _dtheta * dt;
   }
 
-  scalar_t d = exp(log(dissipation) * dt);
-  _dcenter_x *= d;
-  _dcenter_y *= d;
-  _dtheta *= d;
-
   scalar_t vx = cos(_theta), vy = sin(_theta);
 
   for(int n = 0; n < _nb_vertices; n++) {
@@ -366,11 +377,15 @@ void Polygon::apply_border_forces(scalar_t dt,
       scalar_t x = _x[v] * (1 - s) + _x[vp] * s;
       scalar_t y = _y[v] * (1 - s) + _y[vp] * s;
       scalar_t vx = 0, vy = 0;
-      if(x < xmin) vx = xmin - x;
+      if(x < xmin) vx = x - xmin;
       else if(x > xmax) vx = x - xmax;
-      if(y < ymin) vy = ymin - y;
+      if(y < ymin) vy = y - ymin;
       else if(y > ymax) vy = y - ymax;
-      apply_force(dt, x, y, - dl * vx * repulsion_constant, - dl * vy * repulsion_constant);
+      if(vx != 0 || vy != 0) {
+        // cerr << "apply_border_forces vx = " << vx << " vy = " << vy << endl;
+        apply_force(dt, x, y,
+                    - dl * vx * repulsion_constant, - dl * vy * repulsion_constant);
+      }
     }
   }
 }