Faster trajectory generation

fluids/assets/car.py

@@ -35,7 +35,7 @@ def __init__(self, vel=0, mass=400, max_vel=5,
                         PedCrossing]
         Shape.__init__(self,
                        collideables=collideables,
-                       color=(0x1d,0xb1,0xc1),#769BB0
+                       color=(0x1d,0xb1,0xb0),#769BB0
                        xdim=70,
                        ydim=35,
                        **kwargs)
@@ -121,9 +121,11 @@ def step(self, action):
         else:
             fluids_assert(False, "Car received an illegal action")
         while len(self.waypoints) < self.planning_depth and len(self.waypoints) and len(self.waypoints[-1].nxt):
-            next_waypoint = random.choice(self.waypoints[-1].nxt)
-            line = shapely.geometry.LineString([(self.waypoints[-1].x, self.waypoints[-1].y),
-                                                (next_waypoint.x, next_waypoint.y)]).buffer(self.ydim*0.5)
+            next_edge = random.choice(self.waypoints[-1].nxt)
+            next_waypoint = next_edge.out_p
+            line = next_edge.shapely_obj
+            # line = shapely.geometry.LineString([(self.waypoints[-1].x, self.waypoints[-1].y),
+            #                                     (next_waypoint.x, next_waypoint.y)]).buffer(self.ydim*0.5)
             self.trajectory.append(((self.waypoints[-1].x, self.waypoints[-1].y),
                                     (next_waypoint.x, next_waypoint.y), line))
             self.waypoints.append(next_waypoint)
@@ -182,12 +184,13 @@ def can_collide(self, other):
     def get_future_shape(self):
         if self.last_blob_time != self.running_time:
             if len(self.waypoints) and len(self.trajectory):
+
                 line = shapely.geometry.LineString([(self.waypoints[0].x, self.waypoints[0].y),
-                                                    (self.x, self.y)]).buffer(self.ydim * 0.5, resolution=2)
+                                                    (self.x, self.y)]).buffer(20, resolution=2)
                 buf = [t[2] for t in self.trajectory][:max(int(1+6*self.vel/self.max_vel), 0)]
-                self.cached_blob = shapely.geometry.MultiPolygon([line] + buf).buffer(self.ydim*0.2, resolution=2)
+                self.cached_blob = shapely.ops.cascaded_union([line] + buf)
             else:
-                self.cached_blob = self.shapely_obj.buffer(self.ydim*0.3, resolution=2)
+                self.cached_blob = self.shapely_obj
 
         self.last_blob_time = self.running_time
         return self.cached_blob

fluids/assets/pedestrian.py

@@ -33,7 +33,7 @@ def step(self, action):
             angle = angle
             self.update_points(x, y, angle)
         while len(self.waypoints) < self.planning_depth and len(self.waypoints) and len(self.waypoints[-1].nxt):
-            next_waypoint = random.choice(self.waypoints[-1].nxt)
+            next_waypoint = random.choice(self.waypoints[-1].nxt).out_p
             line = shapely.geometry.LineString([(self.waypoints[-1].x, self.waypoints[-1].y),
                                                 (next_waypoint.x, next_waypoint.y)]).buffer(self.ydim*0.5)
             self.trajectory.append(((self.waypoints[-1].x, self.waypoints[-1].y),

fluids/assets/waypoint.py

@@ -4,7 +4,7 @@
 
 import scipy.interpolate as si
 from fluids.assets.shape import Shape
-
+from fluids.assets.waypoint_edge import WaypointEdge
 def plan(x0,y0,a0,x1,y1,a1,smooth_level=3000):
     def interpolate(p0,p1,p2,p3,t):
         return [p0[0]*1.0*((1-t)**3) + p1[0]*3.0*t*(1-t)**2 + p2[0]*3.0*(t**2)*(1-t) + p3[0]*1.0*(t**3),
@@ -59,6 +59,12 @@ def smoothen(self, smooth_level=3000):
         self.nxt = new_nxt
         return all_news
 
+    def create_edges(self, **kwargs):
+        new_nxt = []
+        for n_p in self.nxt:
+            new_nxt.append(WaypointEdge(self, n_p, **kwargs))
+        self.nxt = new_nxt
+
     def render(self, surface, **kwargs):
         if 'color' in kwargs:
             color = kwargs['color']
@@ -73,5 +79,5 @@ def render(self, surface, **kwargs):
                 pygame.draw.line(surface,
                                  color,
                                  (int(self.x), int(self.y)),
-                                 (int(next_point.x), int(next_point.y)),
+                                 (int(next_point.out_p.x), int(next_point.out_p.y)),
                                  1)

fluids/assets/waypoint_edge.py

@@ -0,0 +1,16 @@
+import numpy as np
+import shapely
+from fluids.assets.shape import Shape
+
+
+class WaypointEdge(Shape):
+    def __init__(self, wp0, wp1, buff=10, **kwargs):
+
+        line = shapely.geometry.LineString([(wp0.x, wp0.y), (wp1.x, wp1.y)]).buffer(buff)
+        points = np.array(list(zip(*(line).exterior.coords.xy)))
+        #angle = np.arctan2([wp0.y - wp1.y], [wp0.x - wp1.x])[0]
+        angle = 0
+        super(WaypointEdge, self).__init__(angle=angle, points=points, color=(200, 200, 200), **kwargs)
+        self.in_p  = wp0
+        self.out_p = wp1
+

fluids/state.py

@@ -149,6 +149,12 @@ def __init__(self,
                           "nxt":[w.index for w in wp.nxt]}
                 layout['ped_waypoints'].append(wpdict)
 
+        for waypoint in self.waypoints:
+            waypoint.create_edges(buff=20)
+        for waypoint in self.ped_waypoints:
+            waypoint.create_edges(buff=5)
+
+
         fluids_print("Generating cars")
         for i in range(controlled_cars + background_cars):
             while True:
@@ -163,8 +169,8 @@ def __init__(self,
                     for waypoint in self.waypoints:
                         if car.intersects(waypoint):
                             while car.intersects(waypoint):
-                                waypoint = random.choice(waypoint.nxt)
-                            waypoint = random.choice(waypoint.nxt)
+                                waypoint = random.choice(waypoint.nxt).out_p
+                            waypoint = random.choice(waypoint.nxt).out_p
                             car.waypoints = [waypoint]
                             break
                     self.type_map[Car][key] = car
@@ -185,7 +191,7 @@ def __init__(self,
                 wp = random.choice(self.ped_waypoints)
                 ped = Pedestrian(state=self, x=wp.x, y=wp.y, angle=wp.angle, vis_level=vis_level)
                 while ped.intersects(wp):
-                    wp = random.choice(wp.nxt)
+                    wp = random.choice(wp.nxt).out_p
                 ped.waypoints = [wp]
                 if not self.is_in_collision(ped):
                     key = get_id()