Newtonian Simulator Fixes and Cleanup (#309)

* Fixed scaling-warp issues.

* Fixed wobble, cleaned up, and added debug render mode.

* Added per car width/height scaling.

* Moved scenicToScreenVal int cast.

* Added clarification comment.

src/scenic/simulators/newtonian/driving_model.scenic

@@ -14,7 +14,9 @@ from scenic.domains.driving.model import *  # includes basic actions and behavio
 
 from scenic.simulators.utils.colors import Color
 
-simulator NewtonianSimulator(network, render=render)
+param debugRender = False
+
+simulator NewtonianSimulator(network, render=render, debug_render=globalParameters.debugRender)
 
 class NewtonianActor(DrivingObject):
     throttle: 0

src/scenic/simulators/newtonian/simulator.py

@@ -5,6 +5,7 @@
 from math import copysign, degrees, radians, sin
 import os
 import pathlib
+import statistics
 import time
 
 from PIL import Image
@@ -58,15 +59,16 @@ class NewtonianSimulator(DrivingSimulator):
         when not otherwise specified is still 0.1 seconds.
     """
 
-    def __init__(self, network=None, render=False, export_gif=False):
+    def __init__(self, network=None, render=False, debug_render=False, export_gif=False):
         super().__init__()
         self.export_gif = export_gif
         self.render = render
+        self.debug_render = debug_render
         self.network = network
 
     def createSimulation(self, scene, **kwargs):
         simulation = NewtonianSimulation(
-            scene, self.network, self.render, self.export_gif, **kwargs
+            scene, self.network, self.render, self.export_gif, self.debug_render, **kwargs
         )
         if self.export_gif and self.render:
             simulation.generate_gif("simulation.gif")
@@ -76,11 +78,14 @@ def createSimulation(self, scene, **kwargs):
 class NewtonianSimulation(DrivingSimulation):
     """Implementation of `Simulation` for the Newtonian simulator."""
 
-    def __init__(self, scene, network, render, export_gif, timestep, **kwargs):
+    def __init__(
+        self, scene, network, render, export_gif, debug_render, timestep, **kwargs
+    ):
         self.export_gif = export_gif
         self.render = render
         self.network = network
         self.frames = []
+        self.debug_render = debug_render
 
         if timestep is None:
             timestep = 0.1
@@ -102,10 +107,31 @@ def setup(self):
             )
             self.screen.fill((255, 255, 255))
             x, y, _ = self.objects[0].position
-            self.min_x, self.max_x = min_x - 50, max_x + 50
-            self.min_y, self.max_y = min_y - 50, max_y + 50
+            self.min_x, self.max_x = min_x - 40, max_x + 40
+            self.min_y, self.max_y = min_y - 40, max_y + 40
             self.size_x = self.max_x - self.min_x
             self.size_y = self.max_y - self.min_y
+
+            # Generate a uniform screen scaling (applied to width and height)
+            # that includes all of both dimensions.
+            self.screenScaling = min(WIDTH / self.size_x, HEIGHT / self.size_y)
+
+            # Calculate a screen translation that brings the mean vehicle
+            # position to the center of the screen.
+
+            # N.B. screenTranslation is initialized to (0, 0) here intentionally.
+            # so that the actual screenTranslation can be set later based off what
+            # was computed with this null value.
+            self.screenTranslation = (0, 0)
+
+            scaled_positions = map(
+                lambda x: self.scenicToScreenVal(x.position), self.objects
+            )
+            mean_x, mean_y = map(statistics.mean, zip(*scaled_positions))
+
+            self.screenTranslation = (WIDTH / 2 - mean_x, HEIGHT / 2 - mean_y)
+
+            # Create screen polygon to avoid rendering entirely invisible images
             self.screen_poly = shapely.geometry.Polygon(
                 (
                     (self.min_x, self.min_y),
@@ -117,9 +143,7 @@ def setup(self):
 
             img_path = os.path.join(current_dir, "car.png")
             self.car = pygame.image.load(img_path)
-            self.car_width = int(3.5 * WIDTH / self.size_x)
-            self.car_height = self.car_width
-            self.car = pygame.transform.scale(self.car, (self.car_width, self.car_height))
+
             self.parse_network()
             self.draw_objects()
 
@@ -149,9 +173,14 @@ def addRegion(region, color, width=1):
 
     def scenicToScreenVal(self, pos):
         x, y = pos[:2]
-        x_prop = (x - self.min_x) / self.size_x
-        y_prop = (y - self.min_y) / self.size_y
-        return int(x_prop * WIDTH), HEIGHT - 1 - int(y_prop * HEIGHT)
+
+        screen_x = (x - self.min_x) * self.screenScaling
+        screen_y = HEIGHT - 1 - (y - self.min_y) * self.screenScaling
+
+        screen_x = screen_x + self.screenTranslation[0]
+        screen_y = screen_y + self.screenTranslation[1]
+
+        return int(screen_x), int(screen_y)
 
     def createObjectInSimulator(self, obj):
         # Set actor's initial speed
@@ -207,21 +236,14 @@ def draw_objects(self):
 
         for i, obj in enumerate(self.objects):
             color = (255, 0, 0) if i == 0 else (0, 0, 255)
-            h, w = obj.length, obj.width
-            pos_vec = Vector(-1.75, 1.75)
-            neg_vec = Vector(w / 2, h / 2)
-            heading_vec = Vector(0, 10).rotatedBy(obj.heading)
-            dx, dy = int(heading_vec.x), -int(heading_vec.y)
-            x, y = self.scenicToScreenVal(obj.position)
-            rect_x, rect_y = self.scenicToScreenVal(obj.position + pos_vec)
+
+            if self.debug_render:
+                self.draw_rect(obj, color)
+
             if hasattr(obj, "isCar") and obj.isCar:
-                self.rotated_car = pygame.transform.rotate(
-                    self.car, math.degrees(obj.heading)
-                )
-                self.screen.blit(self.rotated_car, (rect_x, rect_y))
+                self.draw_car(obj)
             else:
-                corners = [self.scenicToScreenVal(corner) for corner in obj._corners2D]
-                pygame.draw.polygon(self.screen, color, corners)
+                self.draw_rect(obj, color)
 
         pygame.display.update()
 
@@ -232,6 +254,19 @@ def draw_objects(self):
 
         time.sleep(self.timestep)
 
+    def draw_rect(self, obj, color):
+        corners = [self.scenicToScreenVal(corner) for corner in obj._corners2D]
+        pygame.draw.polygon(self.screen, color, corners)
+
+    def draw_car(self, obj):
+        car_width = int(obj.width * self.screenScaling)
+        car_height = int(obj.height * self.screenScaling)
+        scaled_car = pygame.transform.scale(self.car, (car_width, car_height))
+        rotated_car = pygame.transform.rotate(scaled_car, math.degrees(obj.heading))
+        car_rect = rotated_car.get_rect()
+        car_rect.center = self.scenicToScreenVal(obj.position)
+        self.screen.blit(rotated_car, car_rect)
+
     def generate_gif(self, filename="simulation.gif"):
         imgs = [Image.fromarray(frame) for frame in self.frames]
         imgs[0].save(filename, save_all=True, append_images=imgs[1:], duration=50, loop=0)