Added bounding box utility methods

armarx_core/math/bounding_box.py

+from __future__ import annotations
+from abc import ABC, abstractmethod
+import numpy as np
+
+
+class BoundingBox(ABC):
+    @abstractmethod
+    def get_center(self):
+        pass
+
+    @abstractmethod
+    def get_corners(self):
+        pass
+
+    @abstractmethod
+    def is_within(self, point):
+        pass
+
+    def is_within_bbx(self, bbx: BoundingBox):
+        for corner in self.get_corners():
+            if not bbx.is_within(corner):
+                return False
+        return True
+
+
+class AxisAlignedBoundingBox(BoundingBox):
+    def __init__(self, min_x, max_x, min_y, max_y, min_z, max_z):
+        self.min_x = min_x
+        self.max_x = max_x
+        self.min_y = min_y
+        self.max_y = max_y
+        self.min_z = min_z
+        self.max_z = max_z
+
+    def get_center(self):
+        center = np.array([
+            (self.min_x + self.max_x) / 2,
+            (self.min_y + self.max_y) / 2,
+            (self.min_z + self.max_z) / 2
+        ])
+        assert center.shape == (3,), "Center must be a 3D vector"
+        return center
+
+    def get_corners(self):
+        corners = np.array([
+            [self.min_x, self.min_y, self.min_z],
+            [self.min_x, self.min_y, self.max_z],
+            [self.min_x, self.max_y, self.min_z],
+            [self.min_x, self.max_y, self.max_z],
+            [self.max_x, self.min_y, self.min_z],
+            [self.max_x, self.min_y, self.max_z],
+            [self.max_x, self.max_y, self.min_z],
+            [self.max_x, self.max_y, self.max_z]
+        ])
+        assert corners.shape == (8, 3), "Corners must be an 8x3 array"
+        return corners
+
+    def is_within(self, point):
+        point = np.array(point)
+        assert point.shape == (3,), "Point must be a 3D vector"
+        x, y, z = point
+        return (self.min_x <= x <= self.max_x and
+                self.min_y <= y <= self.max_y and
+                self.min_z <= z <= self.max_z)
+
+
+class ObjectOrientedBoundingBox(BoundingBox):
+    def __init__(self, center, orientation, extents):
+        self.center = np.array(center)
+        self.orientation = np.array(orientation)  # 3x3 rotation matrix
+        self.extents = np.array(extents)  # Half-lengths along each principal axis
+
+        assert self.center.shape == (3,), "Center must be a 3D vector"
+        assert self.orientation.shape == (3, 3), "Orientation must be a 3x3 matrix"
+        assert self.extents.shape == (3,), "Extents must be a 3D vector"
+
+    def get_center(self):
+        assert self.center.shape == (3,), "Center must be a 3D vector"
+        return self.center
+
+    def get_corners(self):
+        offsets = np.array([
+            [-1, -1, -1], [-1, -1, 1], [-1, 1, -1], [-1, 1, 1],
+            [1, -1, -1], [1, -1, 1], [1, 1, -1], [1, 1, 1]
+        ]) * self.extents
+        corners = self.center + (offsets @ self.orientation.T)
+        assert corners.shape == (8, 3), "Corners must be an 8x3 array"
+        return corners
+
+    def is_within(self, point):
+        point = np.array(point)
+        assert point.shape == (3,), "Point must be a 3D vector"
+        local_point = np.linalg.inv(self.orientation) @ (point - self.center)
+        return np.all(np.abs(local_point) <= self.extents)