[MISC] Add stub code for mouse interaction (#1321)

Author: gasnica

genesis/ext/pyrender/interaction/ray.py

@@ -0,0 +1,54 @@
+from dataclasses import dataclass
+
+from .vec3 import Vec3
+
+
+EPSILON = 1e-6
+
+
+class Ray:
+    origin: Vec3
+    direction: Vec3
+
+    def __init__(self, origin: Vec3, direction: Vec3):
+        self.origin = origin
+        self.direction = direction.normalized()
+
+    def __repr__(self) -> str:
+        return f"Ray(origin={self.origin}, direction={self.direction})"
+
+
+@dataclass
+class RayHit:
+    is_hit: bool
+    distance: float
+    normal: Vec3
+    position: Vec3
+    object_idx: int
+
+
+class Plane:
+    normal: Vec3
+    distance: float  # distance from plane to origin along normal
+
+    def __init__(self, normal: Vec3, point: Vec3):
+        self.normal = normal
+        self.distance = -normal.dot(point)
+
+    def raycast(self, ray: Ray) -> RayHit:
+        dot = ray.direction.dot(self.normal)
+        dist = ray.origin.dot(self.normal) + self.distance
+
+        if -EPSILON < dot or dist < EPSILON:
+            return RayHit(is_hit=False, distance=0, normal=Vec3.zero(), position=Vec3.zero(), object_idx=-1)
+
+        dist_along_ray = dist / -dot
+
+        return RayHit(
+            is_hit=True,
+            distance=dist_along_ray,
+            normal=self.normal,
+            position=ray.origin + ray.direction * dist_along_ray,
+            object_idx=0
+        )
+

genesis/ext/pyrender/interaction/vec3.py

@@ -0,0 +1,76 @@
+import numpy as np
+from numpy.typing import NDArray
+
+
+class Vec3:
+    """
+    Use this wrapper around np.array if you want to ensure adherence to float32 arithmethic
+    with runtime checks, and avoid hidden and costly conversions between float32 and float64.
+
+    This also makes vector dimensionality explicit for linting and static analysis.
+    """
+    v: NDArray[np.float32]
+
+    def __init__(self, v: NDArray[np.float32]):
+        assert v.shape == (3,), f"Vec3 must be initialized with a 3-element array, got {v.shape}"
+        assert v.dtype == np.float32, f"Vec3 must be initialized with a float32 array, got {v.dtype}"
+        self.v = v
+
+    def __add__(self, other: 'Vec3') -> 'Vec3':
+        return Vec3(self.v + other.v)
+
+    def __sub__(self, other: 'Vec3') -> 'Vec3':
+        return Vec3(self.v - other.v)
+
+    def __mul__(self, other: float) -> 'Vec3':
+        return Vec3(self.v * np.float32(other))
+
+    def __rmul__(self, other: float) -> 'Vec3':
+        return Vec3(self.v * np.float32(other))
+
+    def dot(self, other: 'Vec3') -> float:
+        return np.dot(self.v, other.v).item()
+
+    def cross(self, other: 'Vec3') -> 'Vec3':
+        return Vec3(np.cross(self.v, other.v))
+
+    def normalized(self) -> 'Vec3':
+        return Vec3(self.v / (np.linalg.norm(self.v) + 1e-24))
+
+    def copy(self) -> 'Vec3':
+        return Vec3(self.v.copy())
+
+    def __repr__(self) -> str:
+        return f"Vec3({self.v[0]}, {self.v[1]}, {self.v[2]})"
+
+
+    @classmethod
+    def from_xyz(cls, x: float, y: float, z: float) -> 'Vec3':
+        return cls(np.array([x, y, z], dtype=np.float32))
+
+    @classmethod
+    def from_int32(cls, v: NDArray[np.int32]) -> 'Vec3':
+        assert v.shape == (3,), f"Vec3 must be initialized with a 3-element array, got {v.shape}"
+        assert v.dtype == np.int32, f"from_int32 must be initialized with a int32 array, got {v.dtype}"
+        return cls.from_xyz(*v)
+
+    @classmethod
+    def from_int64(cls, v: NDArray[np.int64]) -> 'Vec3':
+        assert v.shape == (3,), f"Vec3 must be initialized with a 3-element array, got {v.shape}"
+        assert v.dtype == np.int64, f"from_int64 must be initialized with a int64 array, got {v.dtype}"
+        return cls.from_xyz(*v)
+
+    @classmethod
+    def from_float64(cls, v: NDArray[np.float64]) -> 'Vec3':
+        assert v.shape == (3,), f"Vec3 must be initialized with a 3-element array, got {v.shape}"
+        assert v.dtype == np.float64, f"from_float64 must be initialized with a float64 array, got {v.dtype}"
+        return cls.from_xyz(*v)
+
+
+    @classmethod
+    def zero(cls):
+        return cls(np.array([0, 0, 0], dtype=np.float32))
+
+    @classmethod
+    def one(cls):
+        return cls(np.array([1, 1, 1], dtype=np.float32))

genesis/ext/pyrender/interaction/viewer_interaction.py

@@ -0,0 +1,115 @@
+from typing import TYPE_CHECKING
+from typing_extensions import override
+
+import numpy as np
+
+from pyglet.event import EVENT_HANDLE_STATE
+
+from .ray import Plane, Ray, RayHit
+from .vec3 import Vec3
+from .viewer_interaction_base import ViewerInteractionBase
+
+if TYPE_CHECKING:
+    from genesis.engine.scene import Scene
+    from genesis.ext.pyrender.node import Node
+
+
+class ViewerInteraction(ViewerInteractionBase):
+    """Functionalities to be implemented:
+    - mouse picking
+    - mouse dragging
+    """
+
+    camera: 'Node'
+    scene: 'Scene'
+    viewport_size: tuple[int, int]
+    camera_yfov: float
+
+    tan_half_fov: float
+    prev_mouse_pos: tuple[int, int]
+
+    def __init__(self, 
+        camera: 'Node', 
+        scene: 'Scene', 
+        viewport_size: tuple[int, int], 
+        camera_yfov: float,
+        log_events: bool = False,
+        camera_fov: float = 60.0,
+    ):
+        super().__init__(log_events)
+        self.camera = camera
+        self.scene = scene
+        self.viewport_size = viewport_size
+        self.camera_yfov = camera_yfov
+
+        self.tan_half_fov = np.tan(0.5 * self.camera_yfov)
+        self.prev_mouse_pos = tuple(np.array(viewport_size) / 2)
+
+    @override
+    def on_mouse_motion(self, x: int, y: int, dx: int, dy: int) -> EVENT_HANDLE_STATE:
+        super().on_mouse_motion(x, y, dx, dy)
+        self.prev_mouse_pos = (x, y)
+
+    @override
+    def on_mouse_drag(self, x: int, y: int, dx: int, dy: int, buttons: int, modifiers: int) -> EVENT_HANDLE_STATE:
+        super().on_mouse_drag(x, y, dx, dy, buttons, modifiers)
+        self.prev_mouse_pos = (x, y)
+
+    @override
+    def on_mouse_press(self, x: int, y: int, button: int, modifiers: int) -> EVENT_HANDLE_STATE:
+        super().on_mouse_press(x, y, button, modifiers)
+        mouse_ray = self.screen_position_to_ray(x, y)
+        # print(f"mouse_ray: {mouse_ray}")
+
+    @override
+    def on_draw(self) -> None:
+        super().on_draw()
+        if self.scene._visualizer is not None and self.scene._visualizer.viewer_lock is not None:
+            self.scene.clear_debug_objects()
+        
+            ray_hit = self._raycast_against_ground(self.screen_position_to_ray(*self.prev_mouse_pos))
+            if ray_hit.is_hit:
+                self.scene.draw_debug_sphere(ray_hit.position.v, 0.01, (0, 1, 0, 1))
+                self._draw_arrow(ray_hit.position, ray_hit.normal, (0, 1, 0, 1))
+
+    def screen_position_to_ray(self, x: float, y: float) -> Ray:
+        # convert screen position to ray
+        if True:
+            x = x - 0.5 * self.viewport_size[0]
+            y = y - 0.5 * self.viewport_size[1]
+            x = 2.0 * x / self.viewport_size[1] * self.tan_half_fov
+            y = 2.0 * y / self.viewport_size[1] * self.tan_half_fov
+        else:
+            # alternative way
+            projection_matrix = self.camera.camera.get_projection_matrix(*self.viewport_size)
+            x = x - 0.5 * self.viewport_size[0]
+            y = y - 0.5 * self.viewport_size[1]
+            x = 2.0 * x / self.viewport_size[0] / projection_matrix[0, 0]
+            y = 2.0 * y / self.viewport_size[1] / projection_matrix[1, 1]
+
+        # Note: ignoring pixel aspect ratio
+
+        mtx = self.camera.matrix
+        position = Vec3.from_float64(mtx[:3, 3])
+        forward = Vec3.from_float64(-mtx[:3, 2])
+        right = Vec3.from_float64(mtx[:3, 0])
+        up = Vec3.from_float64(mtx[:3, 1])
+
+        direction = forward + right * x + up * y
+        return Ray(position, direction)
+
+    def get_camera_ray(self) -> Ray:
+        mtx = self.camera.matrix
+        position = Vec3.from_float64(mtx[:3, 3])
+        forward = Vec3.from_float64(-mtx[:3, 2])
+        return Ray(position, forward)
+
+    def _raycast_against_ground(self, ray: Ray) -> RayHit:
+        ground_plane = Plane(Vec3.from_xyz(0, 0, 1), Vec3.zero())
+        return ground_plane.raycast(ray)
+
+    def _draw_arrow(
+        self, pos: Vec3, dir: Vec3, color: tuple[float, float, float, float] = (1.0, 1.0, 1.0, 1.0),
+    ) -> None:
+        self.scene.draw_debug_arrow(pos.v, dir.v, color=color)  # Only draws arrowhead -- bug?
+        self.scene.draw_debug_line(pos.v, pos.v + dir.v, color=color)

genesis/ext/pyrender/interaction/viewer_interaction_base.py

@@ -0,0 +1,41 @@
+from pyglet.event import EVENT_HANDLE_STATE
+
+import genesis as gs
+
+# Note: Viewer window is based on pyglet.window.Window, mouse events are defined in pyglet.window.BaseWindow
+
+class ViewerInteractionBase():
+    """Base class for handling pyglet.window.Window events.
+    """
+
+    log_events: bool
+
+    def __init__(self, log_events: bool = False):
+        self.log_events = log_events
+
+    def on_mouse_motion(self, x: int, y: int, dx: int, dy: int) -> EVENT_HANDLE_STATE:
+        if self.log_events:
+            gs.logger.info(f"Mouse moved to {x}, {y}")
+
+    def on_mouse_drag(self, x: int, y: int, dx: int, dy: int, buttons: int, modifiers: int) -> EVENT_HANDLE_STATE:
+        if self.log_events:
+            gs.logger.info(f"Mouse dragged to {x}, {y}")
+
+    def on_mouse_press(self, x: int, y: int, button: int, modifiers: int) -> EVENT_HANDLE_STATE:
+        if self.log_events:
+            gs.logger.info(f"Mouse buttons {button} pressed at {x}, {y}")
+
+    def on_mouse_release(self, x: int, y: int, button: int, modifiers: int) -> EVENT_HANDLE_STATE:
+        if self.log_events:
+            gs.logger.info(f"Mouse buttons {button} released at {x}, {y}")
+
+    def on_key_press(self, symbol: int, modifiers: int) -> EVENT_HANDLE_STATE:
+        if self.log_events:
+            gs.logger.info(f"Key pressed: {chr(symbol)}")
+
+    def on_key_release(self, symbol: int, modifiers: int) -> EVENT_HANDLE_STATE:
+        if self.log_events:
+            gs.logger.info(f"Key released: {chr(symbol)}")
+
+    def on_draw(self) -> None:
+        pass