Interaction between user input and floor visualization

base.py

@@ -15,18 +15,22 @@ class Vehicle:
         self.mass = mass
         self.wheels_weight = np.array([2,2,1,1]) / 6  # weight
         self.wheels_position_delta = np.array([[1,2],[-1,2],[-1,-2],[1,-2]]) / 5
-        self.wheels_position = self.get_wheel_positions()
 
         self.floor = floor
 
-    def get_wheel_positions(self):
+    def get_wheel_positions(self, cyclic=False):
         # build rotation matrix R
         c = np.cos(self.rotation)
         s = np.sin(self.rotation)
         R = np.array([[c, -s], [s, c]])
 
         # return absolute wheel positions
-        return self.position + np.matmul(R, self.wheels_position_delta.T).T
+        result = self.position + np.matmul(R, self.wheels_position_delta.T).T
+
+        if cyclic:
+            return np.vstack((result, result[0]))
+        else:
+            return result
 
     def update_floor(self):
         # could be optimized by binary search tree and fixed length of sensor time-series
@@ -41,7 +45,7 @@ class Vehicle:
             sensor.uncertainties = np.append(sensor.uncertainties, 0.0)
 
         # check in which tile a wheel is at the moment
-        for iw, wheel in enumerate(self.wheels_position):
+        for iw, wheel in enumerate(self.get_wheel_positions()):
             for it, tile in f.tiles.items():
                 sensor = tile["sensor"]
                 if self.is_inside(wheel, tile):
@@ -55,14 +59,15 @@ class Vehicle:
         # decompose tile into two triangles
         # use Barycentric coordinates for check (https://en.wikipedia.org/wiki/Barycentric_coordinate_system)
         # check first triangle
-        A = np.vstack((c[:-1].T, np.ones(3)))
+        A = np.vstack((c[:3].T, np.ones(3)))
         x = np.linalg.solve(A, b)
 
         if np.all(0 < x) and np.all(x < 1):
             return True
         
-         # check first triangle
-        A = np.vstack((c[1:].T, np.ones(3)))
+        # check second triangle
+        cr = np.roll(c, 2, axis=0)
+        A = np.vstack((cr[:3].T, np.ones(3)))
         x = np.linalg.solve(A, b)
         
         if np.all(0 < x) and np.all(x < 1):
@@ -73,13 +78,15 @@ class Vehicle:
 
 class VehicleInteractor:
 
-    def __init__(self, vehicle, ax):
+    def __init__(self, vehicle, pc):
         self.vehicle = vehicle
-        self.ax = ax
+        self.pc = pc
+
+        self.shape_press_active = False
 
     def get_shape_and_arrow(self):
         # represent vehicle by a rectangle from wheel positions
-        shape = mpl.patches.Polygon(self.vehicle.wheels_position, fill=False)
+        shape = mpl.patches.Polygon(self.vehicle.get_wheel_positions(), fill=False)
 
         # arrow direction
         x = self.vehicle.position[0]
@@ -94,18 +101,66 @@ class VehicleInteractor:
     def update_representation(self, shape, arrow):
         pass
 
-    def connect(self):
-        pass
+    def connect(self, shape, arrow):
+
+        self.shape = shape
+        self.arrow = arrow
+
+        # translation
+        self.shape_press = self.shape.figure.canvas.mpl_connect('button_press_event', self.on_shape_press)
+        self.shape_release = self.shape.figure.canvas.mpl_connect('button_release_event', self.on_shape_release)
+        self.shape_motion = self.shape.figure.canvas.mpl_connect('motion_notify_event', self.on_shape_motion)
+
+        # rotation
+        #self.arrow_press = self.shape.figure.canvas.mpl_connect('button_press_event', self.on_arrow_press)
+        #self.arrow_release = self.shape.figure.canvas.mpl_connect('button_release_event', self.on_arrow_release)
+        #self.arrow_motion = self.shape.figure.canvas.mpl_connect('motion_notify_event', self.on_arrow_motion)
+
+    def on_shape_press(self, event):
+        if event.inaxes != self.shape.axes: return
+        contains, attrd = self.shape.contains(event)
+        if not contains: return
 
-    def on_press(self, event):
+        self.shape_press_active = True
+
+        # save position of object and press
+        xy0 = self.shape.xy
+        self.press = [xy0, event.xdata, event.ydata]
+
+    def on_shape_motion(self, event):
+        if event.inaxes != self.shape.axes: return
+        if not self.shape_press_active: return
+
+        # set vehicle
+        self.vehicle.position = np.array([event.xdata, event.ydata])
+
+        # set representation of vehicle
+        self.shape.set_xy(self.vehicle.get_wheel_positions(cyclic=True))
+        self.shape.figure.canvas.draw()
+
+    def on_shape_release(self, event):
+        self.shape_press_active = False
+        self.vehicle.update_floor()
+        self.pc.set_array(self.vehicle.floor.get_tile_values().ravel())
+        self.shape.figure.canvas.draw()
+
+    def on_arrow_press(self, event):
         pass
 
-    def on_motion(self, event):
+    def on_arrow_motion(self, event):
         pass
 
-    def on_release(self, event):
+    def on_arrow_release(self, event):
         pass
 
+    def disconnect(self):
+        self.shape.figure.canvas.mpl_disconnect(self.shape_press)
+        self.shape.figure.canvas.mpl_disconnect(self.shape_motion)
+        self.shape.figure.canvas.mpl_disconnect(self.shape_release)
+
+        #self.shape.figure.canvas.mpl_disconnect(self.arrow_press)
+        #self.shape.figure.canvas.mpl_disconnect(self.arrow_motion)
+        #self.shape.figure.canvas.mpl_disconnect(self.arrow_release)
 
 
 class Floor():

main.py

@@ -2,28 +2,31 @@ from base import Vehicle, Floor, VehicleInteractor
 import matplotlib.pyplot as plt
 
 
-
+# define floor and vehicle on floor
 f = Floor()
 v = Vehicle(floor=f)
+
+# update sensors on floor
 v.update_floor()
 
+
+# visualize
 fig = plt.figure()
 ax = fig.add_subplot(111)
 
-
-# vehicle representation
-vi = VehicleInteractor(v, ax)
-v_shape, v_arrow = vi.get_shape_and_arrow()
-
-ax.add_patch(v_shape)
-ax.add_patch(v_arrow)
-
-
 # draw floor
 fx, fy = f.get_grid()
 fc = f.get_tile_values()
-ax.pcolormesh(fx, fy, fc, cmap="Greens")
+pc = ax.pcolormesh(fx, fy, fc, cmap="Greens")
+
+# vehicle representation
+vi = VehicleInteractor(v, pc)
+shape, arrow = vi.get_shape_and_arrow()
+
+shape_patch = ax.add_patch(shape)
+arrow_patch = ax.add_patch(arrow)
 
+vi.connect(shape_patch, arrow_patch)
 
 # plot the scene
 ax.set_xlim(*f.draw_limits[0])