From d16564037784f502131798e4f718b5f24b5948e3 Mon Sep 17 00:00:00 2001 From: James Harton Date: Mon, 29 Dec 2025 20:08:10 +1300 Subject: [PATCH 1/2] feat: add collision detection module Add a comprehensive collision detection system for BB robots: - Add capsule geometry type to DSL (cylinder with hemispherical caps) - Implement collision primitives: sphere-sphere, capsule-capsule, sphere-capsule, sphere-box, capsule-box, box-box (OBB with SAT) - Add broad phase AABB culling for fast collision pair elimination - Add STL mesh parsing with bounding geometry computation and caching - Implement self-collision detection with adjacent link exclusion - Implement environment collision detection with obstacle API Public API: - `BB.Collision.self_collision?/3` - check for self-collision - `BB.Collision.detect_self_collisions/3` - get collision details - `BB.Collision.collides_with?/4` - check against obstacles - `BB.Collision.detect_collisions/4` - get obstacle collision details - `BB.Collision.obstacle/3,4` - create sphere/capsule/box obstacles All functions support an optional margin parameter for safety buffers. --- lib/bb/collision.ex | 485 ++++++++++++++++++++++ lib/bb/collision/broad_phase.ex | 246 ++++++++++++ lib/bb/collision/mesh.ex | 270 +++++++++++++ lib/bb/collision/primitives.ex | 479 ++++++++++++++++++++++ lib/bb/dsl.ex | 30 +- lib/bb/dsl/capsule.ex | 31 ++ lib/bb/robot/builder.ex | 8 + lib/bb/robot/link.ex | 1 + lib/bb/urdf/exporter.ex | 10 + test/bb/collision/broad_phase_test.exs | 377 ++++++++++++++++++ test/bb/collision/mesh_test.exs | 210 ++++++++++ test/bb/collision/primitives_test.exs | 531 +++++++++++++++++++++++++ test/bb/collision_test.exs | 242 +++++++++++ test/fixtures/cube.stl | 86 ++++ test/support/example_robots.ex | 95 +++++ 15 files changed, 3099 insertions(+), 2 deletions(-) create mode 100644 lib/bb/collision.ex create mode 100644 lib/bb/collision/broad_phase.ex create mode 100644 lib/bb/collision/mesh.ex create mode 100644 lib/bb/collision/primitives.ex create mode 100644 lib/bb/dsl/capsule.ex create mode 100644 test/bb/collision/broad_phase_test.exs create mode 100644 test/bb/collision/mesh_test.exs create mode 100644 test/bb/collision/primitives_test.exs create mode 100644 test/bb/collision_test.exs create mode 100644 test/fixtures/cube.stl diff --git a/lib/bb/collision.ex b/lib/bb/collision.ex new file mode 100644 index 0000000..d953a25 --- /dev/null +++ b/lib/bb/collision.ex @@ -0,0 +1,485 @@ +# SPDX-FileCopyrightText: 2025 James Harton +# +# SPDX-License-Identifier: Apache-2.0 + +defmodule BB.Collision do + @moduledoc """ + Collision detection for BB robots. + + This module provides self-collision detection and environment collision detection, + following the same architectural pattern as `BB.Robot.Kinematics`. + + ## Self-Collision Detection + + Self-collision checking determines if any parts of the robot are colliding with + each other. Adjacent links (connected by a joint) are automatically excluded from + collision checks since they are expected to be in contact. + + # Quick boolean check + BB.Collision.self_collision?(robot, positions) + + # Detailed collision information + BB.Collision.detect_self_collisions(robot, positions) + + ## Environment Collision Detection + + Environment collision checks if the robot collides with obstacles in the workspace. + + obstacles = [ + BB.Collision.obstacle(:box, centre, half_extents), + BB.Collision.obstacle(:sphere, centre, radius) + ] + + BB.Collision.collides_with?(robot, positions, obstacles) + + ## Performance + + Collision detection uses a two-phase approach: + 1. **Broad phase**: Fast AABB overlap tests to eliminate non-colliding pairs + 2. **Narrow phase**: Precise primitive intersection tests for potential collisions + + For a typical 6-DOF robot, self-collision checks complete in under 1ms. + """ + + alias BB.Robot + alias BB.Robot.Kinematics + alias BB.Collision.{BroadPhase, Primitives} + alias BB.Math.{Transform, Vec3, Quaternion} + + @type positions :: %{atom() => float()} + + @type collision_info :: %{ + link_a: atom(), + link_b: atom() | :environment, + collision_a: atom() | nil, + collision_b: atom() | nil, + penetration_depth: float() + } + + @type obstacle :: %{ + type: :sphere | :capsule | :box, + geometry: Primitives.geometry(), + aabb: BroadPhase.aabb() + } + + # ============================================================================ + # Self-Collision Detection + # ============================================================================ + + @doc """ + Check if the robot is in self-collision at the given joint positions. + + Returns `true` if any non-adjacent links are colliding, `false` otherwise. + + ## Options + + - `:margin` - Safety margin in metres added to all geometries (default: 0.0) + + ## Examples + + positions = %{shoulder: 0.0, elbow: 1.57, wrist: 0.0} + BB.Collision.self_collision?(robot, positions) + # => false + + BB.Collision.self_collision?(robot, positions, margin: 0.01) + # => true (with 1cm safety margin) + """ + @spec self_collision?(Robot.t(), positions(), keyword()) :: boolean() + def self_collision?(%Robot{} = robot, positions, opts \\ []) when is_map(positions) do + case detect_self_collisions(robot, positions, opts) do + [] -> false + [_ | _] -> true + end + end + + @doc """ + Detect all self-collisions at the given joint positions. + + Returns a list of collision info maps describing each collision. Adjacent links + (connected by a joint) are excluded from checks. + + ## Options + + - `:margin` - Safety margin in metres added to all geometries (default: 0.0) + + ## Examples + + collisions = BB.Collision.detect_self_collisions(robot, positions) + # => [%{link_a: :forearm, link_b: :base, collision_a: nil, collision_b: nil, penetration_depth: 0.02}] + """ + @spec detect_self_collisions(Robot.t(), positions(), keyword()) :: [collision_info()] + def detect_self_collisions(%Robot{} = robot, positions, opts \\ []) when is_map(positions) do + margin = Keyword.get(opts, :margin, 0.0) + + # Get all link transforms in world space + transforms = Kinematics.all_link_transforms(robot, positions) + + # Build adjacency set (pairs of links that should not be checked) + adjacent = build_adjacency_set(robot) + + # Get all links with collision geometry + links_with_collisions = + robot.links + |> Map.values() + |> Enum.filter(fn link -> link.collisions != [] end) + + # Compute AABBs for all collision geometries + link_aabbs = compute_link_aabbs(links_with_collisions, transforms, margin) + + # Generate all non-adjacent pairs + pairs = generate_collision_pairs(links_with_collisions, adjacent) + + # Two-phase collision detection + pairs + |> Enum.flat_map(fn {link_a, link_b} -> + check_link_pair(link_a, link_b, transforms, link_aabbs, margin) + end) + end + + # ============================================================================ + # Environment Collision Detection + # ============================================================================ + + @doc """ + Check if the robot collides with any obstacles at the given joint positions. + + ## Options + + - `:margin` - Safety margin in metres (default: 0.0) + + ## Examples + + obstacles = [BB.Collision.obstacle(:sphere, Vec3.new(0.5, 0, 0.3), 0.1)] + BB.Collision.collides_with?(robot, positions, obstacles) + """ + @spec collides_with?(Robot.t(), positions(), [obstacle()], keyword()) :: boolean() + def collides_with?(%Robot{} = robot, positions, obstacles, opts \\ []) do + case detect_collisions(robot, positions, obstacles, opts) do + [] -> false + [_ | _] -> true + end + end + + @doc """ + Detect all collisions between the robot and environment obstacles. + + ## Options + + - `:margin` - Safety margin in metres (default: 0.0) + """ + @spec detect_collisions(Robot.t(), positions(), [obstacle()], keyword()) :: [collision_info()] + def detect_collisions(%Robot{} = robot, positions, obstacles, opts \\ []) do + margin = Keyword.get(opts, :margin, 0.0) + + transforms = Kinematics.all_link_transforms(robot, positions) + + links_with_collisions = + robot.links + |> Map.values() + |> Enum.filter(fn link -> link.collisions != [] end) + + link_aabbs = compute_link_aabbs(links_with_collisions, transforms, margin) + + links_with_collisions + |> Enum.flat_map(fn link -> + check_link_obstacles(link, obstacles, transforms, link_aabbs, margin) + end) + end + + # ============================================================================ + # Obstacle Creation + # ============================================================================ + + @doc """ + Create an obstacle for environment collision detection. + + ## Sphere + + obstacle = BB.Collision.obstacle(:sphere, centre, radius) + + ## Capsule + + obstacle = BB.Collision.obstacle(:capsule, point_a, point_b, radius) + + ## Axis-Aligned Box + + obstacle = BB.Collision.obstacle(:box, centre, half_extents) + + ## Oriented Box + + rotation = Quaternion.from_axis_angle(Vec3.unit_z(), :math.pi() / 4) + obstacle = BB.Collision.obstacle(:box, centre, half_extents, rotation) + """ + @spec obstacle(:sphere, Vec3.t(), float()) :: obstacle() + @spec obstacle(:capsule, Vec3.t(), Vec3.t(), float()) :: obstacle() + @spec obstacle(:box, Vec3.t(), Vec3.t()) :: obstacle() + @spec obstacle(:box, Vec3.t(), Vec3.t(), Quaternion.t()) :: obstacle() + def obstacle(:sphere, centre, radius) do + geometry = {:sphere, centre, radius} + aabb = sphere_aabb(centre, radius) + %{type: :sphere, geometry: geometry, aabb: aabb} + end + + def obstacle(:box, centre, half_extents) do + axes = {Vec3.unit_x(), Vec3.unit_y(), Vec3.unit_z()} + geometry = {:box, centre, half_extents, axes} + aabb = box_aabb(centre, half_extents) + %{type: :box, geometry: geometry, aabb: aabb} + end + + def obstacle(:capsule, point_a, point_b, radius) do + geometry = {:capsule, point_a, point_b, radius} + aabb = capsule_aabb(point_a, point_b, radius) + %{type: :capsule, geometry: geometry, aabb: aabb} + end + + def obstacle(:box, centre, half_extents, orientation) do + ax = Quaternion.rotate_vector(orientation, Vec3.unit_x()) + ay = Quaternion.rotate_vector(orientation, Vec3.unit_y()) + az = Quaternion.rotate_vector(orientation, Vec3.unit_z()) + + geometry = {:box, centre, half_extents, {ax, ay, az}} + aabb = oriented_box_aabb(centre, half_extents, {ax, ay, az}) + %{type: :box, geometry: geometry, aabb: aabb} + end + + # ============================================================================ + # Adjacency + # ============================================================================ + + @doc """ + Build a set of adjacent link pairs from the robot topology. + + Adjacent links are connected by a joint and should not be checked for collision. + """ + @spec build_adjacency_set(Robot.t()) :: MapSet.t({atom(), atom()}) + def build_adjacency_set(%Robot{} = robot) do + robot.joints + |> Map.values() + |> Enum.reduce(MapSet.new(), fn joint, set -> + # Add both orderings for easy lookup + set + |> MapSet.put({joint.parent_link, joint.child_link}) + |> MapSet.put({joint.child_link, joint.parent_link}) + end) + end + + # ============================================================================ + # Private - Link Pair Checking + # ============================================================================ + + defp generate_collision_pairs(links, adjacent) do + for link_a <- links, + link_b <- links, + link_a.name < link_b.name, + not MapSet.member?(adjacent, {link_a.name, link_b.name}) do + {link_a, link_b} + end + end + + defp check_link_pair(link_a, link_b, transforms, link_aabbs, margin) do + aabbs_a = Map.get(link_aabbs, link_a.name, []) + aabbs_b = Map.get(link_aabbs, link_b.name, []) + + transform_a = Map.fetch!(transforms, link_a.name) + transform_b = Map.fetch!(transforms, link_b.name) + + for {coll_a, aabb_a} <- aabbs_a, + {coll_b, aabb_b} <- aabbs_b, + BroadPhase.overlap?(aabb_a, aabb_b), + collision = narrow_phase_check(coll_a, coll_b, transform_a, transform_b, margin), + collision != nil do + %{ + link_a: link_a.name, + link_b: link_b.name, + collision_a: coll_a.name, + collision_b: coll_b.name, + penetration_depth: collision.penetration_depth + } + end + end + + defp check_link_obstacles(link, obstacles, transforms, link_aabbs, margin) do + link_collision_aabbs = Map.get(link_aabbs, link.name, []) + transform = Map.fetch!(transforms, link.name) + + for {coll, link_aabb} <- link_collision_aabbs, + obstacle <- obstacles, + expanded_obstacle_aabb = BroadPhase.expand(obstacle.aabb, margin), + BroadPhase.overlap?(link_aabb, expanded_obstacle_aabb), + collision = narrow_phase_obstacle(coll, obstacle, transform, margin), + collision != nil do + %{ + link_a: link.name, + link_b: :environment, + collision_a: coll.name, + collision_b: nil, + penetration_depth: collision.penetration_depth + } + end + end + + # ============================================================================ + # Private - AABB Computation + # ============================================================================ + + defp compute_link_aabbs(links, transforms, margin) do + links + |> Enum.map(fn link -> + transform = Map.fetch!(transforms, link.name) + + aabbs = + link.collisions + |> Enum.filter(fn coll -> coll.geometry != nil end) + |> Enum.map(fn coll -> + coll_transform = compose_collision_transform(transform, coll.origin) + aabb = BroadPhase.compute_aabb(coll.geometry, coll_transform) + expanded_aabb = BroadPhase.expand(aabb, margin) + {coll, expanded_aabb} + end) + + {link.name, aabbs} + end) + |> Map.new() + end + + defp compose_collision_transform(link_transform, nil), do: link_transform + + defp compose_collision_transform(link_transform, {pos, orient}) do + local_transform = Transform.from_origin(%{position: pos, orientation: orient}) + Transform.compose(link_transform, local_transform) + end + + # ============================================================================ + # Private - Narrow Phase + # ============================================================================ + + defp narrow_phase_check(coll_a, coll_b, transform_a, transform_b, margin) do + geom_a = to_world_geometry(coll_a, transform_a) + geom_b = to_world_geometry(coll_b, transform_b) + + case Primitives.test_with_margin(geom_a, geom_b, margin) do + {:collision, depth} -> %{penetration_depth: depth} + :no_collision -> nil + end + end + + defp narrow_phase_obstacle(coll, obstacle, transform, margin) do + geom = to_world_geometry(coll, transform) + + case Primitives.test_with_margin(geom, obstacle.geometry, margin) do + {:collision, depth} -> %{penetration_depth: depth} + :no_collision -> nil + end + end + + defp to_world_geometry(collision, link_transform) do + coll_transform = compose_collision_transform(link_transform, collision.origin) + geometry_to_primitive(collision.geometry, coll_transform) + end + + defp geometry_to_primitive({:sphere, %{radius: r}}, transform) do + centre = Transform.get_translation(transform) + {:sphere, centre, r} + end + + defp geometry_to_primitive({:capsule, %{radius: r, length: l}}, transform) do + centre = Transform.get_translation(transform) + orientation = Transform.get_quaternion(transform) + + half_length = l / 2 + local_a = Vec3.new(0.0, 0.0, -half_length) + local_b = Vec3.new(0.0, 0.0, half_length) + + point_a = Vec3.add(centre, Quaternion.rotate_vector(orientation, local_a)) + point_b = Vec3.add(centre, Quaternion.rotate_vector(orientation, local_b)) + + {:capsule, point_a, point_b, r} + end + + defp geometry_to_primitive({:cylinder, %{radius: r, height: h}}, transform) do + # Treat cylinder as capsule + geometry_to_primitive({:capsule, %{radius: r, length: h}}, transform) + end + + defp geometry_to_primitive({:box, %{x: hx, y: hy, z: hz}}, transform) do + centre = Transform.get_translation(transform) + orientation = Transform.get_quaternion(transform) + + ax = Quaternion.rotate_vector(orientation, Vec3.unit_x()) + ay = Quaternion.rotate_vector(orientation, Vec3.unit_y()) + az = Quaternion.rotate_vector(orientation, Vec3.unit_z()) + + {:box, centre, Vec3.new(hx, hy, hz), {ax, ay, az}} + end + + defp geometry_to_primitive({:mesh, %{filename: filename, scale: scale}}, transform) do + # Use bounding sphere for mesh collision + case BB.Collision.Mesh.load_bounds(filename) do + {:ok, bounds} -> + {local_centre, radius} = bounds.bounding_sphere + centre = Transform.get_translation(transform) + orientation = Transform.get_quaternion(transform) + + scaled_local = Vec3.scale(local_centre, scale) + world_centre = Vec3.add(centre, Quaternion.rotate_vector(orientation, scaled_local)) + + {:sphere, world_centre, radius * scale} + + {:error, _} -> + # Fallback: unit sphere at transform origin + centre = Transform.get_translation(transform) + {:sphere, centre, 1.0} + end + end + + defp geometry_to_primitive({:mesh, _}, transform) do + centre = Transform.get_translation(transform) + {:sphere, centre, 1.0} + end + + # ============================================================================ + # Private - Obstacle AABBs + # ============================================================================ + + defp sphere_aabb(centre, radius) do + offset = Vec3.new(radius, radius, radius) + {Vec3.subtract(centre, offset), Vec3.add(centre, offset)} + end + + defp capsule_aabb(point_a, point_b, radius) do + min_pt = + Vec3.new( + min(Vec3.x(point_a), Vec3.x(point_b)) - radius, + min(Vec3.y(point_a), Vec3.y(point_b)) - radius, + min(Vec3.z(point_a), Vec3.z(point_b)) - radius + ) + + max_pt = + Vec3.new( + max(Vec3.x(point_a), Vec3.x(point_b)) + radius, + max(Vec3.y(point_a), Vec3.y(point_b)) + radius, + max(Vec3.z(point_a), Vec3.z(point_b)) + radius + ) + + {min_pt, max_pt} + end + + defp box_aabb(centre, half_extents) do + {Vec3.subtract(centre, half_extents), Vec3.add(centre, half_extents)} + end + + defp oriented_box_aabb(centre, half_extents, {ax, ay, az}) do + # Project half-extents onto world axes + hx = Vec3.x(half_extents) + hy = Vec3.y(half_extents) + hz = Vec3.z(half_extents) + + extent_x = hx * abs(Vec3.x(ax)) + hy * abs(Vec3.x(ay)) + hz * abs(Vec3.x(az)) + extent_y = hx * abs(Vec3.y(ax)) + hy * abs(Vec3.y(ay)) + hz * abs(Vec3.y(az)) + extent_z = hx * abs(Vec3.z(ax)) + hy * abs(Vec3.z(ay)) + hz * abs(Vec3.z(az)) + + world_extent = Vec3.new(extent_x, extent_y, extent_z) + {Vec3.subtract(centre, world_extent), Vec3.add(centre, world_extent)} + end +end diff --git a/lib/bb/collision/broad_phase.ex b/lib/bb/collision/broad_phase.ex new file mode 100644 index 0000000..d146a90 --- /dev/null +++ b/lib/bb/collision/broad_phase.ex @@ -0,0 +1,246 @@ +# SPDX-FileCopyrightText: 2025 James Harton +# +# SPDX-License-Identifier: Apache-2.0 + +defmodule BB.Collision.BroadPhase do + @moduledoc """ + Broad phase collision detection using Axis-Aligned Bounding Boxes (AABBs). + + The broad phase is a fast culling step that eliminates pairs of objects that + cannot possibly be colliding. This reduces the number of expensive narrow-phase + collision tests required. + + AABBs are simple boxes aligned to the world axes, making overlap tests very fast. + They may be larger than the actual geometry (especially for rotated objects), + so a positive broad phase result only indicates *potential* collision. + """ + + alias BB.Math.{Transform, Vec3, Quaternion} + + @type aabb :: {min :: Vec3.t(), max :: Vec3.t()} + + @doc """ + Check if two AABBs overlap. + + This is a very fast O(1) test - two AABBs overlap if and only if they overlap + on all three axes. + """ + @spec overlap?(aabb(), aabb()) :: boolean() + def overlap?({min1, max1}, {min2, max2}) do + Vec3.x(min1) <= Vec3.x(max2) and + Vec3.x(max1) >= Vec3.x(min2) and + Vec3.y(min1) <= Vec3.y(max2) and + Vec3.y(max1) >= Vec3.y(min2) and + Vec3.z(min1) <= Vec3.z(max2) and + Vec3.z(max1) >= Vec3.z(min2) + end + + @doc """ + Compute the AABB for a collision geometry in world space. + + Takes a geometry specification (as stored in Robot.Link) and a transform + representing the geometry's position and orientation in world space. + + ## Supported Geometry Types + + - `{:sphere, %{radius: float()}}` - Sphere + - `{:capsule, %{radius: float(), length: float()}}` - Capsule (cylinder with spherical caps) + - `{:cylinder, %{radius: float(), height: float()}}` - Cylinder (treated as capsule) + - `{:box, %{x: float(), y: float(), z: float()}}` - Box + + ## Examples + + iex> geometry = {:sphere, %{radius: 1.0}} + iex> transform = Transform.identity() + iex> {min, max} = BB.Collision.BroadPhase.compute_aabb(geometry, transform) + iex> {Vec3.x(min), Vec3.x(max)} + {-1.0, 1.0} + """ + @spec compute_aabb(BB.Robot.Link.geometry(), Transform.t()) :: aabb() + def compute_aabb({:sphere, %{radius: r}}, transform) do + centre = Transform.get_translation(transform) + offset = Vec3.new(r, r, r) + {Vec3.subtract(centre, offset), Vec3.add(centre, offset)} + end + + def compute_aabb({:capsule, %{radius: r, length: l}}, transform) do + # Capsule extends along local Z axis + # Total length is l (cylinder) + 2*r (hemispherical caps) + half_length = l / 2 + + centre = Transform.get_translation(transform) + orientation = Transform.get_quaternion(transform) + + # Local endpoints (along Z axis) + local_a = Vec3.new(0.0, 0.0, -half_length) + local_b = Vec3.new(0.0, 0.0, half_length) + + # Transform to world space + world_a = Vec3.add(centre, Quaternion.rotate_vector(orientation, local_a)) + world_b = Vec3.add(centre, Quaternion.rotate_vector(orientation, local_b)) + + # AABB is the bounds of both endpoints expanded by radius + min_pt = + Vec3.new( + min(Vec3.x(world_a), Vec3.x(world_b)) - r, + min(Vec3.y(world_a), Vec3.y(world_b)) - r, + min(Vec3.z(world_a), Vec3.z(world_b)) - r + ) + + max_pt = + Vec3.new( + max(Vec3.x(world_a), Vec3.x(world_b)) + r, + max(Vec3.y(world_a), Vec3.y(world_b)) + r, + max(Vec3.z(world_a), Vec3.z(world_b)) + r + ) + + {min_pt, max_pt} + end + + def compute_aabb({:cylinder, %{radius: r, height: h}}, transform) do + # Treat cylinder as capsule for conservative bounding + compute_aabb({:capsule, %{radius: r, length: h}}, transform) + end + + def compute_aabb({:box, %{x: hx, y: hy, z: hz}}, transform) do + # For an oriented box, we need to transform all 8 corners and find bounds + centre = Transform.get_translation(transform) + orientation = Transform.get_quaternion(transform) + + # Half extents in each direction + corners = [ + Vec3.new(hx, hy, hz), + Vec3.new(hx, hy, -hz), + Vec3.new(hx, -hy, hz), + Vec3.new(hx, -hy, -hz), + Vec3.new(-hx, hy, hz), + Vec3.new(-hx, hy, -hz), + Vec3.new(-hx, -hy, hz), + Vec3.new(-hx, -hy, -hz) + ] + + # Transform corners to world space + world_corners = + Enum.map(corners, fn local -> + Vec3.add(centre, Quaternion.rotate_vector(orientation, local)) + end) + + # Find min/max + xs = Enum.map(world_corners, &Vec3.x/1) + ys = Enum.map(world_corners, &Vec3.y/1) + zs = Enum.map(world_corners, &Vec3.z/1) + + {Vec3.new(Enum.min(xs), Enum.min(ys), Enum.min(zs)), + Vec3.new(Enum.max(xs), Enum.max(ys), Enum.max(zs))} + end + + def compute_aabb({:mesh, %{filename: filename, scale: scale}}, transform) do + case BB.Collision.Mesh.load_bounds(filename) do + {:ok, bounds} -> + # Transform the mesh AABB to world space + transform_mesh_aabb(bounds.aabb, scale, transform) + + {:error, _} -> + # Fall back to placeholder (unit sphere AABB) + centre = Transform.get_translation(transform) + offset = Vec3.new(1.0, 1.0, 1.0) + {Vec3.subtract(centre, offset), Vec3.add(centre, offset)} + end + end + + def compute_aabb({:mesh, _mesh_data}, transform) do + # Mesh without filename - use placeholder + centre = Transform.get_translation(transform) + offset = Vec3.new(1.0, 1.0, 1.0) + {Vec3.subtract(centre, offset), Vec3.add(centre, offset)} + end + + defp transform_mesh_aabb({local_min, local_max}, scale, transform) do + # Scale the local AABB + scaled_min = Vec3.scale(local_min, scale) + scaled_max = Vec3.scale(local_max, scale) + + centre = Transform.get_translation(transform) + orientation = Transform.get_quaternion(transform) + + # Get all 8 corners of the scaled local AABB + corners = [ + Vec3.new(Vec3.x(scaled_min), Vec3.y(scaled_min), Vec3.z(scaled_min)), + Vec3.new(Vec3.x(scaled_min), Vec3.y(scaled_min), Vec3.z(scaled_max)), + Vec3.new(Vec3.x(scaled_min), Vec3.y(scaled_max), Vec3.z(scaled_min)), + Vec3.new(Vec3.x(scaled_min), Vec3.y(scaled_max), Vec3.z(scaled_max)), + Vec3.new(Vec3.x(scaled_max), Vec3.y(scaled_min), Vec3.z(scaled_min)), + Vec3.new(Vec3.x(scaled_max), Vec3.y(scaled_min), Vec3.z(scaled_max)), + Vec3.new(Vec3.x(scaled_max), Vec3.y(scaled_max), Vec3.z(scaled_min)), + Vec3.new(Vec3.x(scaled_max), Vec3.y(scaled_max), Vec3.z(scaled_max)) + ] + + # Transform corners to world space + world_corners = + Enum.map(corners, fn local -> + Vec3.add(centre, Quaternion.rotate_vector(orientation, local)) + end) + + # Find world-space AABB + xs = Enum.map(world_corners, &Vec3.x/1) + ys = Enum.map(world_corners, &Vec3.y/1) + zs = Enum.map(world_corners, &Vec3.z/1) + + {Vec3.new(Enum.min(xs), Enum.min(ys), Enum.min(zs)), + Vec3.new(Enum.max(xs), Enum.max(ys), Enum.max(zs))} + end + + @doc """ + Expand an AABB by a margin in all directions. + + Useful for adding safety buffers to collision checks. + """ + @spec expand(aabb(), float()) :: aabb() + def expand({min_pt, max_pt}, margin) do + offset = Vec3.new(margin, margin, margin) + {Vec3.subtract(min_pt, offset), Vec3.add(max_pt, offset)} + end + + @doc """ + Merge two AABBs into a single AABB that contains both. + """ + @spec merge(aabb(), aabb()) :: aabb() + def merge({min1, max1}, {min2, max2}) do + {Vec3.new( + min(Vec3.x(min1), Vec3.x(min2)), + min(Vec3.y(min1), Vec3.y(min2)), + min(Vec3.z(min1), Vec3.z(min2)) + ), + Vec3.new( + max(Vec3.x(max1), Vec3.x(max2)), + max(Vec3.y(max1), Vec3.y(max2)), + max(Vec3.z(max1), Vec3.z(max2)) + )} + end + + @doc """ + Compute the centre point of an AABB. + """ + @spec centre(aabb()) :: Vec3.t() + def centre({min_pt, max_pt}) do + Vec3.lerp(min_pt, max_pt, 0.5) + end + + @doc """ + Compute the size (extents) of an AABB in each dimension. + """ + @spec size(aabb()) :: Vec3.t() + def size({min_pt, max_pt}) do + Vec3.subtract(max_pt, min_pt) + end + + @doc """ + Check if a point is inside an AABB. + """ + @spec contains_point?(aabb(), Vec3.t()) :: boolean() + def contains_point?({min_pt, max_pt}, point) do + Vec3.x(point) >= Vec3.x(min_pt) and Vec3.x(point) <= Vec3.x(max_pt) and + Vec3.y(point) >= Vec3.y(min_pt) and Vec3.y(point) <= Vec3.y(max_pt) and + Vec3.z(point) >= Vec3.z(min_pt) and Vec3.z(point) <= Vec3.z(max_pt) + end +end diff --git a/lib/bb/collision/mesh.ex b/lib/bb/collision/mesh.ex new file mode 100644 index 0000000..27cf3af --- /dev/null +++ b/lib/bb/collision/mesh.ex @@ -0,0 +1,270 @@ +# SPDX-FileCopyrightText: 2025 James Harton +# +# SPDX-License-Identifier: Apache-2.0 + +defmodule BB.Collision.Mesh do + @moduledoc """ + Mesh loading and bounding geometry computation for collision detection. + + This module provides basic mesh support for collision detection by computing + bounding primitives (spheres or AABBs) from mesh vertices. Triangle-level + collision detection is not supported - meshes are approximated by their + bounding geometry. + + Currently supports: + - Binary STL files + - ASCII STL files + + ## Usage + + # Load mesh and compute bounds + {:ok, bounds} = BB.Collision.Mesh.load_bounds("/path/to/model.stl") + + # bounds contains: + %{ + aabb: {min_vec3, max_vec3}, + bounding_sphere: {centre_vec3, radius} + } + + ## Caching + + Mesh bounds are cached in an ETS table to avoid repeated file parsing. + The cache key includes the file path and modification time. + """ + + alias BB.Math.Vec3 + + @type bounds :: %{ + aabb: {Vec3.t(), Vec3.t()}, + bounding_sphere: {Vec3.t(), float()} + } + + @cache_table :bb_collision_mesh_cache + + @doc """ + Load mesh bounds from a file. + + Parses the mesh file, computes bounding geometry, and caches the result. + Subsequent calls with the same file (unchanged) return cached bounds. + + Returns `{:ok, bounds}` or `{:error, reason}`. + """ + @spec load_bounds(String.t()) :: {:ok, bounds()} | {:error, term()} + def load_bounds(path) do + with {:ok, cache_key} <- get_cache_key(path) do + case get_cached(cache_key) do + {:ok, bounds} -> + {:ok, bounds} + + :miss -> + with {:ok, vertices} <- parse_mesh(path), + {:ok, bounds} <- compute_bounds(vertices) do + put_cached(cache_key, bounds) + {:ok, bounds} + end + end + end + end + + @doc """ + Load mesh bounds, raising on error. + """ + @spec load_bounds!(String.t()) :: bounds() + def load_bounds!(path) do + case load_bounds(path) do + {:ok, bounds} -> bounds + {:error, reason} -> raise "Failed to load mesh bounds: #{inspect(reason)}" + end + end + + @doc """ + Clear the mesh bounds cache. + """ + @spec clear_cache() :: :ok + def clear_cache do + ensure_cache_table() + :ets.delete_all_objects(@cache_table) + :ok + end + + @doc """ + Compute bounding geometry from a list of vertices. + + Each vertex should be a `{x, y, z}` tuple of floats. + """ + @spec compute_bounds([{float(), float(), float()}]) :: {:ok, bounds()} | {:error, :empty_mesh} + def compute_bounds([]), do: {:error, :empty_mesh} + + def compute_bounds(vertices) when is_list(vertices) do + aabb = compute_aabb(vertices) + bounding_sphere = compute_bounding_sphere(vertices, aabb) + {:ok, %{aabb: aabb, bounding_sphere: bounding_sphere}} + end + + # ============================================================================ + # STL Parsing + # ============================================================================ + + defp parse_mesh(path) do + case File.read(path) do + {:ok, data} -> + parse_stl(data) + + {:error, reason} -> + {:error, {:file_read_error, reason}} + end + end + + defp parse_stl(<<"solid ", rest::binary>>) do + # Could be ASCII STL, but binary STL can also start with "solid" + # Check if it looks like ASCII by finding "facet" keyword + if String.contains?(rest, "facet") do + parse_ascii_stl(<<"solid ", rest::binary>>) + else + parse_binary_stl(<<"solid ", rest::binary>>) + end + end + + defp parse_stl(data), do: parse_binary_stl(data) + + defp parse_binary_stl(data) when byte_size(data) < 84 do + {:error, :invalid_stl} + end + + defp parse_binary_stl(<<_header::binary-size(80), num_triangles::little-unsigned-32, rest::binary>>) do + expected_size = num_triangles * 50 + + if byte_size(rest) >= expected_size do + vertices = parse_binary_triangles(rest, num_triangles, []) + {:ok, vertices} + else + {:error, :truncated_stl} + end + end + + defp parse_binary_triangles(_data, 0, acc), do: Enum.uniq(acc) + + defp parse_binary_triangles( + <<_nx::little-float-32, _ny::little-float-32, _nz::little-float-32, + v1x::little-float-32, v1y::little-float-32, v1z::little-float-32, + v2x::little-float-32, v2y::little-float-32, v2z::little-float-32, + v3x::little-float-32, v3y::little-float-32, v3z::little-float-32, + _attr::binary-size(2), rest::binary>>, + remaining, + acc + ) do + vertices = [ + {v1x, v1y, v1z}, + {v2x, v2y, v2z}, + {v3x, v3y, v3z} + | acc + ] + + parse_binary_triangles(rest, remaining - 1, vertices) + end + + defp parse_ascii_stl(data) do + # Extract all vertex lines + vertices = + data + |> String.split("\n") + |> Enum.filter(&String.contains?(&1, "vertex")) + |> Enum.map(&parse_vertex_line/1) + |> Enum.reject(&is_nil/1) + |> Enum.uniq() + + if Enum.empty?(vertices) do + {:error, :no_vertices_found} + else + {:ok, vertices} + end + end + + defp parse_vertex_line(line) do + case Regex.run(~r/vertex\s+([\d.eE+-]+)\s+([\d.eE+-]+)\s+([\d.eE+-]+)/, line) do + [_, x, y, z] -> + {parse_float(x), parse_float(y), parse_float(z)} + + _ -> + nil + end + end + + defp parse_float(str) do + case Float.parse(str) do + {f, _} -> f + :error -> 0.0 + end + end + + # ============================================================================ + # Bounding Geometry Computation + # ============================================================================ + + defp compute_aabb(vertices) do + {xs, ys, zs} = + Enum.reduce(vertices, {[], [], []}, fn {x, y, z}, {xs, ys, zs} -> + {[x | xs], [y | ys], [z | zs]} + end) + + min_pt = Vec3.new(Enum.min(xs), Enum.min(ys), Enum.min(zs)) + max_pt = Vec3.new(Enum.max(xs), Enum.max(ys), Enum.max(zs)) + + {min_pt, max_pt} + end + + defp compute_bounding_sphere(vertices, {min_pt, max_pt}) do + # Use Ritter's algorithm for a reasonable bounding sphere + # Start with AABB centre, then expand to contain all points + + centre = Vec3.lerp(min_pt, max_pt, 0.5) + + # Find the maximum distance from centre to any vertex + radius = + vertices + |> Enum.map(fn {x, y, z} -> + vertex = Vec3.new(x, y, z) + Vec3.distance(centre, vertex) + end) + |> Enum.max() + + {centre, radius} + end + + # ============================================================================ + # Caching + # ============================================================================ + + defp get_cache_key(path) do + case File.stat(path) do + {:ok, %{mtime: mtime}} -> + {:ok, {path, mtime}} + + {:error, reason} -> + {:error, {:file_stat_error, reason}} + end + end + + defp ensure_cache_table do + if :ets.whereis(@cache_table) == :undefined do + :ets.new(@cache_table, [:named_table, :set, :public, read_concurrency: true]) + end + + :ok + end + + defp get_cached(key) do + ensure_cache_table() + + case :ets.lookup(@cache_table, key) do + [{^key, bounds}] -> {:ok, bounds} + [] -> :miss + end + end + + defp put_cached(key, bounds) do + ensure_cache_table() + :ets.insert(@cache_table, {key, bounds}) + :ok + end +end diff --git a/lib/bb/collision/primitives.ex b/lib/bb/collision/primitives.ex new file mode 100644 index 0000000..8cffbd8 --- /dev/null +++ b/lib/bb/collision/primitives.ex @@ -0,0 +1,479 @@ +# SPDX-FileCopyrightText: 2025 James Harton +# +# SPDX-License-Identifier: Apache-2.0 + +defmodule BB.Collision.Primitives do + @moduledoc """ + Collision detection algorithms for primitive geometry pairs. + + All functions take world-space geometry (position + orientation applied) + and return either `{:collision, penetration_depth}` or `:no_collision`. + + Penetration depth is the estimated overlap distance - how far the geometries + would need to be separated to no longer collide. + + ## Supported Geometry Types + + - Sphere: `{:sphere, centre :: Vec3.t(), radius :: float()}` + - Capsule: `{:capsule, point_a :: Vec3.t(), point_b :: Vec3.t(), radius :: float()}` + - Box (OBB): `{:box, centre :: Vec3.t(), half_extents :: Vec3.t(), axes :: {Vec3.t(), Vec3.t(), Vec3.t()}}` + + Cylinders are converted to capsules internally for simpler, more conservative collision detection. + """ + + alias BB.Math.Vec3 + + @type sphere :: {:sphere, centre :: Vec3.t(), radius :: float()} + @type capsule :: {:capsule, point_a :: Vec3.t(), point_b :: Vec3.t(), radius :: float()} + @type box :: {:box, centre :: Vec3.t(), half_extents :: Vec3.t(), axes :: {Vec3.t(), Vec3.t(), Vec3.t()}} + @type geometry :: sphere() | capsule() | box() + + @type collision_result :: {:collision, penetration_depth :: float()} | :no_collision + + # ============================================================================ + # Public API + # ============================================================================ + + @doc """ + Test two geometries for collision. + + Dispatches to the appropriate collision test based on geometry types. + Order of arguments doesn't matter - the function handles symmetry internally. + + ## Examples + + iex> sphere1 = {:sphere, Vec3.new(0, 0, 0), 1.0} + iex> sphere2 = {:sphere, Vec3.new(1.5, 0, 0), 1.0} + iex> BB.Collision.Primitives.test(sphere1, sphere2) + {:collision, 0.5} + + iex> sphere1 = {:sphere, Vec3.new(0, 0, 0), 1.0} + iex> sphere2 = {:sphere, Vec3.new(3.0, 0, 0), 1.0} + iex> BB.Collision.Primitives.test(sphere1, sphere2) + :no_collision + """ + @spec test(geometry(), geometry()) :: collision_result() + def test({:sphere, _, _} = a, {:sphere, _, _} = b), do: sphere_sphere(a, b) + def test({:capsule, _, _, _} = a, {:capsule, _, _, _} = b), do: capsule_capsule(a, b) + def test({:box, _, _, _} = a, {:box, _, _, _} = b), do: box_box(a, b) + + def test({:sphere, _, _} = a, {:capsule, _, _, _} = b), do: sphere_capsule(a, b) + def test({:capsule, _, _, _} = a, {:sphere, _, _} = b), do: sphere_capsule(b, a) + + def test({:sphere, _, _} = a, {:box, _, _, _} = b), do: sphere_box(a, b) + def test({:box, _, _, _} = a, {:sphere, _, _} = b), do: sphere_box(b, a) + + def test({:capsule, _, _, _} = a, {:box, _, _, _} = b), do: capsule_box(a, b) + def test({:box, _, _, _} = a, {:capsule, _, _, _} = b), do: capsule_box(b, a) + + @doc """ + Test two geometries with an additional margin/padding. + + The margin is added to both geometries, effectively expanding them. + Useful for detecting "near misses" or adding safety buffers. + """ + @spec test_with_margin(geometry(), geometry(), margin :: float()) :: collision_result() + def test_with_margin(a, b, margin) when margin > 0 do + a_expanded = expand_geometry(a, margin) + b_expanded = expand_geometry(b, margin) + test(a_expanded, b_expanded) + end + + def test_with_margin(a, b, _margin), do: test(a, b) + + # ============================================================================ + # Sphere-Sphere Collision + # ============================================================================ + + @doc """ + Test collision between two spheres. + + Two spheres collide if the distance between their centres is less than + the sum of their radii. + """ + @spec sphere_sphere(sphere(), sphere()) :: collision_result() + def sphere_sphere({:sphere, c1, r1}, {:sphere, c2, r2}) do + distance = Vec3.distance(c1, c2) + sum_radii = r1 + r2 + + if distance < sum_radii do + {:collision, sum_radii - distance} + else + :no_collision + end + end + + # ============================================================================ + # Capsule-Capsule Collision + # ============================================================================ + + @doc """ + Test collision between two capsules. + + Two capsules collide if the closest distance between their line segments + is less than the sum of their radii. + """ + @spec capsule_capsule(capsule(), capsule()) :: collision_result() + def capsule_capsule({:capsule, a1, b1, r1}, {:capsule, a2, b2, r2}) do + {_closest1, _closest2, distance} = closest_points_segments(a1, b1, a2, b2) + sum_radii = r1 + r2 + + if distance < sum_radii do + {:collision, sum_radii - distance} + else + :no_collision + end + end + + # ============================================================================ + # Sphere-Capsule Collision + # ============================================================================ + + @doc """ + Test collision between a sphere and a capsule. + + A sphere and capsule collide if the closest distance from the sphere's + centre to the capsule's line segment is less than the sum of their radii. + """ + @spec sphere_capsule(sphere(), capsule()) :: collision_result() + def sphere_capsule({:sphere, centre, r_sphere}, {:capsule, cap_a, cap_b, r_capsule}) do + {_closest, distance} = closest_point_on_segment(centre, cap_a, cap_b) + sum_radii = r_sphere + r_capsule + + if distance < sum_radii do + {:collision, sum_radii - distance} + else + :no_collision + end + end + + # ============================================================================ + # Sphere-Box (OBB) Collision + # ============================================================================ + + @doc """ + Test collision between a sphere and an oriented bounding box. + + The sphere collides with the box if the closest point on the box + to the sphere's centre is within the sphere's radius. + """ + @spec sphere_box(sphere(), box()) :: collision_result() + def sphere_box({:sphere, centre, radius}, {:box, box_centre, half_extents, axes}) do + {_closest, distance} = closest_point_on_box(centre, box_centre, half_extents, axes) + + if distance < radius do + {:collision, radius - distance} + else + :no_collision + end + end + + # ============================================================================ + # Capsule-Box Collision + # ============================================================================ + + @doc """ + Test collision between a capsule and an oriented bounding box. + + Finds the closest distance between the capsule's line segment and the box, + then checks if it's less than the capsule's radius. + """ + @spec capsule_box(capsule(), box()) :: collision_result() + def capsule_box({:capsule, cap_a, cap_b, radius}, {:box, box_centre, half_extents, axes}) do + distance = closest_distance_segment_box(cap_a, cap_b, box_centre, half_extents, axes) + + if distance < radius do + {:collision, radius - distance} + else + :no_collision + end + end + + # ============================================================================ + # Box-Box (OBB) Collision using Separating Axis Theorem + # ============================================================================ + + @doc """ + Test collision between two oriented bounding boxes using the Separating Axis Theorem. + + Two convex shapes are separated if there exists an axis along which their + projections don't overlap. For two OBBs, we need to test 15 potential + separating axes: + - 3 face normals from box A + - 3 face normals from box B + - 9 cross products of edges from A and B + """ + @spec box_box(box(), box()) :: collision_result() + def box_box({:box, c1, h1, {a1x, a1y, a1z}}, {:box, c2, h2, {a2x, a2y, a2z}}) do + # Vector from centre of box1 to centre of box2 + t = Vec3.subtract(c2, c1) + + # Half extents as floats + {h1x, h1y, h1z} = {Vec3.x(h1), Vec3.y(h1), Vec3.z(h1)} + {h2x, h2y, h2z} = {Vec3.x(h2), Vec3.y(h2), Vec3.z(h2)} + + # All axes to test + axes = [ + # Face normals of box 1 + a1x, + a1y, + a1z, + # Face normals of box 2 + a2x, + a2y, + a2z, + # Cross products of edges + Vec3.cross(a1x, a2x), + Vec3.cross(a1x, a2y), + Vec3.cross(a1x, a2z), + Vec3.cross(a1y, a2x), + Vec3.cross(a1y, a2y), + Vec3.cross(a1y, a2z), + Vec3.cross(a1z, a2x), + Vec3.cross(a1z, a2y), + Vec3.cross(a1z, a2z) + ] + + box1_axes = {a1x, a1y, a1z} + box2_axes = {a2x, a2y, a2z} + box1_half = {h1x, h1y, h1z} + box2_half = {h2x, h2y, h2z} + + # Find minimum penetration across all axes + min_penetration = + axes + |> Enum.reduce_while(:infinity, fn axis, min_pen -> + # Skip degenerate axes (from parallel edges) + if Vec3.magnitude_squared(axis) < 1.0e-10 do + {:cont, min_pen} + else + axis_normalized = Vec3.normalise(axis) + + case test_axis(axis_normalized, t, box1_axes, box1_half, box2_axes, box2_half) do + :separated -> + {:halt, :separated} + + {:overlap, pen} -> + {:cont, min(min_pen, pen)} + end + end + end) + + case min_penetration do + :separated -> :no_collision + :infinity -> :no_collision + pen when is_float(pen) -> {:collision, pen} + end + end + + # ============================================================================ + # Helper Functions - Line Segment Operations + # ============================================================================ + + @doc """ + Find the closest point on a line segment to a given point. + + Returns `{closest_point, distance}`. + """ + @spec closest_point_on_segment(Vec3.t(), Vec3.t(), Vec3.t()) :: {Vec3.t(), float()} + def closest_point_on_segment(point, seg_a, seg_b) do + ab = Vec3.subtract(seg_b, seg_a) + ap = Vec3.subtract(point, seg_a) + + ab_len_sq = Vec3.magnitude_squared(ab) + + # Degenerate segment (point) + if ab_len_sq < 1.0e-10 do + {seg_a, Vec3.distance(point, seg_a)} + else + t = Vec3.dot(ap, ab) / ab_len_sq + t_clamped = max(0.0, min(1.0, t)) + + closest = Vec3.add(seg_a, Vec3.scale(ab, t_clamped)) + {closest, Vec3.distance(point, closest)} + end + end + + @doc """ + Find the closest points between two line segments. + + Returns `{closest_on_seg1, closest_on_seg2, distance}`. + + Uses the algorithm from "Real-Time Collision Detection" by Christer Ericson. + """ + @spec closest_points_segments(Vec3.t(), Vec3.t(), Vec3.t(), Vec3.t()) :: + {Vec3.t(), Vec3.t(), float()} + def closest_points_segments(a1, b1, a2, b2) do + d1 = Vec3.subtract(b1, a1) + d2 = Vec3.subtract(b2, a2) + r = Vec3.subtract(a1, a2) + + a = Vec3.dot(d1, d1) + e = Vec3.dot(d2, d2) + f = Vec3.dot(d2, r) + + # Check for degenerate segments + cond do + a < 1.0e-10 and e < 1.0e-10 -> + # Both segments are points + {a1, a2, Vec3.distance(a1, a2)} + + a < 1.0e-10 -> + # First segment is a point + {closest, dist} = closest_point_on_segment(a1, a2, b2) + {a1, closest, dist} + + e < 1.0e-10 -> + # Second segment is a point + {closest, dist} = closest_point_on_segment(a2, a1, b1) + {closest, a2, dist} + + true -> + # General case + c = Vec3.dot(d1, r) + b = Vec3.dot(d1, d2) + denom = a * e - b * b + + # Compute s (parameter on first segment) + s = + if abs(denom) < 1.0e-10 do + 0.0 + else + clamp((b * f - c * e) / denom, 0.0, 1.0) + end + + # Compute t (parameter on second segment) + t = (b * s + f) / e + + # Clamp t and recompute s if needed + {s, t} = + cond do + t < 0.0 -> + {clamp(-c / a, 0.0, 1.0), 0.0} + + t > 1.0 -> + {clamp((b - c) / a, 0.0, 1.0), 1.0} + + true -> + {s, t} + end + + closest1 = Vec3.add(a1, Vec3.scale(d1, s)) + closest2 = Vec3.add(a2, Vec3.scale(d2, t)) + {closest1, closest2, Vec3.distance(closest1, closest2)} + end + end + + # ============================================================================ + # Helper Functions - Box Operations + # ============================================================================ + + @doc """ + Find the closest point on an OBB to a given point. + + Returns `{closest_point, distance}`. + """ + @spec closest_point_on_box(Vec3.t(), Vec3.t(), Vec3.t(), {Vec3.t(), Vec3.t(), Vec3.t()}) :: + {Vec3.t(), float()} + def closest_point_on_box(point, box_centre, half_extents, {ax, ay, az}) do + # Vector from box centre to point + d = Vec3.subtract(point, box_centre) + + # Project onto each axis and clamp + {hx, hy, hz} = {Vec3.x(half_extents), Vec3.y(half_extents), Vec3.z(half_extents)} + + dx = clamp(Vec3.dot(d, ax), -hx, hx) + dy = clamp(Vec3.dot(d, ay), -hy, hy) + dz = clamp(Vec3.dot(d, az), -hz, hz) + + # Reconstruct closest point + closest = + box_centre + |> Vec3.add(Vec3.scale(ax, dx)) + |> Vec3.add(Vec3.scale(ay, dy)) + |> Vec3.add(Vec3.scale(az, dz)) + + {closest, Vec3.distance(point, closest)} + end + + # ============================================================================ + # Helper Functions - Capsule-Box + # ============================================================================ + + # Find the closest distance between a line segment and an OBB + defp closest_distance_segment_box(seg_a, seg_b, box_centre, half_extents, axes) do + # Sample points along the segment and find minimum distance to box + # This is an approximation - exact solution is more complex + num_samples = 8 + + 0..num_samples + |> Enum.map(fn i -> + t = i / num_samples + point = Vec3.lerp(seg_a, seg_b, t) + {_closest, distance} = closest_point_on_box(point, box_centre, half_extents, axes) + distance + end) + |> Enum.min() + end + + # ============================================================================ + # Helper Functions - SAT (Separating Axis Theorem) + # ============================================================================ + + # Test a single axis for the SAT algorithm + defp test_axis(axis, t, {a1x, a1y, a1z}, {h1x, h1y, h1z}, {a2x, a2y, a2z}, {h2x, h2y, h2z}) do + # Project the translation vector onto the axis + t_proj = abs(Vec3.dot(t, axis)) + + # Project box 1's half-extents onto the axis + r1 = + h1x * abs(Vec3.dot(a1x, axis)) + + h1y * abs(Vec3.dot(a1y, axis)) + + h1z * abs(Vec3.dot(a1z, axis)) + + # Project box 2's half-extents onto the axis + r2 = + h2x * abs(Vec3.dot(a2x, axis)) + + h2y * abs(Vec3.dot(a2y, axis)) + + h2z * abs(Vec3.dot(a2z, axis)) + + # Check for separation + if t_proj > r1 + r2 do + :separated + else + {:overlap, r1 + r2 - t_proj} + end + end + + # ============================================================================ + # Helper Functions - Geometry Expansion + # ============================================================================ + + defp expand_geometry({:sphere, centre, radius}, margin) do + {:sphere, centre, radius + margin} + end + + defp expand_geometry({:capsule, a, b, radius}, margin) do + {:capsule, a, b, radius + margin} + end + + defp expand_geometry({:box, centre, half_extents, axes}, margin) do + # Expand each half-extent by the margin + expanded = + Vec3.new( + Vec3.x(half_extents) + margin, + Vec3.y(half_extents) + margin, + Vec3.z(half_extents) + margin + ) + + {:box, centre, expanded, axes} + end + + # ============================================================================ + # Utility Functions + # ============================================================================ + + defp clamp(value, min_val, max_val) do + value |> max(min_val) |> min(max_val) + end +end diff --git a/lib/bb/dsl.ex b/lib/bb/dsl.ex index c29124e..1c0c899 100644 --- a/lib/bb/dsl.ex +++ b/lib/bb/dsl.ex @@ -355,6 +355,32 @@ defmodule BB.Dsl do ] } + @capsule %Entity{ + name: :capsule, + describe: """ + A capsule geometry (cylinder with hemispherical caps). + + The origin of the capsule is the centre of the cylindrical portion. + The length is the distance between the centres of the hemispherical caps. + Total height is length + 2 * radius. + """, + target: BB.Dsl.Capsule, + identifier: {:auto, :unique_integer}, + imports: [BB.Unit], + schema: [ + radius: [ + type: unit_type(compatible: :meter), + doc: "The radius of the capsule (cylinder and hemispherical caps)", + required: true + ], + length: [ + type: unit_type(compatible: :meter), + doc: "The length of the cylindrical portion (between cap centres)", + required: true + ] + ] + } + @mesh %Entity{ name: :mesh, describe: """ @@ -453,7 +479,7 @@ defmodule BB.Dsl do identifier: {:auto, :unique_integer}, imports: [BB.Unit], entities: [ - geometry: [@box, @cylinder, @sphere, @mesh], + geometry: [@box, @cylinder, @sphere, @capsule, @mesh], material: [@material], origin: [ %{ @@ -551,7 +577,7 @@ defmodule BB.Dsl do "The refrence frame of the collision element, relative to the reference frame of the link" } ], - geometry: [@box, @cylinder, @sphere, @mesh] + geometry: [@box, @cylinder, @sphere, @capsule, @mesh] ], singleton_entity_keys: [:origin, :geometry], schema: [ diff --git a/lib/bb/dsl/capsule.ex b/lib/bb/dsl/capsule.ex new file mode 100644 index 0000000..cd24dac --- /dev/null +++ b/lib/bb/dsl/capsule.ex @@ -0,0 +1,31 @@ +# SPDX-FileCopyrightText: 2025 James Harton +# +# SPDX-License-Identifier: Apache-2.0 + +defmodule BB.Dsl.Capsule do + @moduledoc """ + A capsule geometry (cylinder with hemispherical caps). + + Capsules are defined by a radius and length. The length is the distance + between the centres of the two hemispherical caps (i.e., the length of + the cylindrical portion). The total height is `length + 2 * radius`. + + Capsules are commonly used for collision detection because they have + simpler intersection algorithms than cylinders and better approximate + robot limbs. + """ + defstruct __identifier__: nil, + __spark_metadata__: nil, + radius: nil, + length: nil + + alias Cldr.Unit + alias Spark.Dsl.Entity + + @type t :: %__MODULE__{ + __identifier__: any, + __spark_metadata__: Entity.spark_meta(), + radius: Unit.t(), + length: Unit.t() + } +end diff --git a/lib/bb/robot/builder.ex b/lib/bb/robot/builder.ex index 4e0e5fc..01ffee5 100644 --- a/lib/bb/robot/builder.ex +++ b/lib/bb/robot/builder.ex @@ -462,6 +462,14 @@ defmodule BB.Robot.Builder do {:sphere, %{radius: Units.to_meters(sphere.radius)}} end + defp convert_geometry(%Dsl.Capsule{} = capsule) do + {:capsule, + %{ + radius: Units.to_meters(capsule.radius), + length: Units.to_meters(capsule.length) + }} + end + defp convert_geometry(%Dsl.Mesh{} = mesh) do {:mesh, %{filename: mesh.filename, scale: mesh.scale}} end diff --git a/lib/bb/robot/link.ex b/lib/bb/robot/link.ex index fdd6654..da45652 100644 --- a/lib/bb/robot/link.ex +++ b/lib/bb/robot/link.ex @@ -69,6 +69,7 @@ defmodule BB.Robot.Link do {:box, %{x: float(), y: float(), z: float()}} | {:cylinder, %{radius: float(), height: float()}} | {:sphere, %{radius: float()}} + | {:capsule, %{radius: float(), length: float()}} | {:mesh, %{filename: String.t(), scale: float()}} @typedoc "Material specification" diff --git a/lib/bb/urdf/exporter.ex b/lib/bb/urdf/exporter.ex index 1f0d3c2..965a735 100644 --- a/lib/bb/urdf/exporter.ex +++ b/lib/bb/urdf/exporter.ex @@ -138,6 +138,16 @@ defmodule BB.Urdf.Exporter do ]) end + # Capsules are exported as cylinders (URDF doesn't have native capsule support) + # Total height = length + 2 * radius + defp build_geometry_element({:capsule, %{radius: r, length: l}}) do + total_height = l + 2 * r + + Xml.element(:geometry, [], [ + Xml.element(:cylinder, radius: Xml.format_float(r), length: Xml.format_float(total_height)) + ]) + end + defp build_geometry_element({:mesh, %{filename: filename, scale: scale}}) do scale_str = Xml.format_xyz({scale, scale, scale}) diff --git a/test/bb/collision/broad_phase_test.exs b/test/bb/collision/broad_phase_test.exs new file mode 100644 index 0000000..0cc59bf --- /dev/null +++ b/test/bb/collision/broad_phase_test.exs @@ -0,0 +1,377 @@ +# SPDX-FileCopyrightText: 2025 James Harton +# +# SPDX-License-Identifier: Apache-2.0 + +defmodule BB.Collision.BroadPhaseTest do + use ExUnit.Case, async: true + + alias BB.Collision.BroadPhase + alias BB.Math.{Transform, Vec3, Quaternion} + + describe "overlap?/2" do + test "detects overlapping AABBs" do + aabb1 = {Vec3.new(0.0, 0.0, 0.0), Vec3.new(2.0, 2.0, 2.0)} + aabb2 = {Vec3.new(1.0, 1.0, 1.0), Vec3.new(3.0, 3.0, 3.0)} + + assert BroadPhase.overlap?(aabb1, aabb2) + end + + test "detects touching AABBs as overlapping" do + aabb1 = {Vec3.new(0.0, 0.0, 0.0), Vec3.new(1.0, 1.0, 1.0)} + aabb2 = {Vec3.new(1.0, 0.0, 0.0), Vec3.new(2.0, 1.0, 1.0)} + + assert BroadPhase.overlap?(aabb1, aabb2) + end + + test "returns false for separated AABBs on X axis" do + aabb1 = {Vec3.new(0.0, 0.0, 0.0), Vec3.new(1.0, 1.0, 1.0)} + aabb2 = {Vec3.new(2.0, 0.0, 0.0), Vec3.new(3.0, 1.0, 1.0)} + + refute BroadPhase.overlap?(aabb1, aabb2) + end + + test "returns false for separated AABBs on Y axis" do + aabb1 = {Vec3.new(0.0, 0.0, 0.0), Vec3.new(1.0, 1.0, 1.0)} + aabb2 = {Vec3.new(0.0, 2.0, 0.0), Vec3.new(1.0, 3.0, 1.0)} + + refute BroadPhase.overlap?(aabb1, aabb2) + end + + test "returns false for separated AABBs on Z axis" do + aabb1 = {Vec3.new(0.0, 0.0, 0.0), Vec3.new(1.0, 1.0, 1.0)} + aabb2 = {Vec3.new(0.0, 0.0, 2.0), Vec3.new(1.0, 1.0, 3.0)} + + refute BroadPhase.overlap?(aabb1, aabb2) + end + + test "detects one AABB containing another" do + outer = {Vec3.new(-1.0, -1.0, -1.0), Vec3.new(3.0, 3.0, 3.0)} + inner = {Vec3.new(0.0, 0.0, 0.0), Vec3.new(1.0, 1.0, 1.0)} + + assert BroadPhase.overlap?(outer, inner) + assert BroadPhase.overlap?(inner, outer) + end + end + + describe "compute_aabb/2 for spheres" do + test "sphere at origin" do + geometry = {:sphere, %{radius: 1.0}} + transform = Transform.identity() + + {min_pt, max_pt} = BroadPhase.compute_aabb(geometry, transform) + + assert_in_delta Vec3.x(min_pt), -1.0, 0.001 + assert_in_delta Vec3.y(min_pt), -1.0, 0.001 + assert_in_delta Vec3.z(min_pt), -1.0, 0.001 + assert_in_delta Vec3.x(max_pt), 1.0, 0.001 + assert_in_delta Vec3.y(max_pt), 1.0, 0.001 + assert_in_delta Vec3.z(max_pt), 1.0, 0.001 + end + + test "sphere with translation" do + geometry = {:sphere, %{radius: 0.5}} + transform = Transform.from_position_quaternion(Vec3.new(1.0, 2.0, 3.0), Quaternion.identity()) + + {min_pt, max_pt} = BroadPhase.compute_aabb(geometry, transform) + + assert_in_delta Vec3.x(min_pt), 0.5, 0.001 + assert_in_delta Vec3.y(min_pt), 1.5, 0.001 + assert_in_delta Vec3.z(min_pt), 2.5, 0.001 + assert_in_delta Vec3.x(max_pt), 1.5, 0.001 + assert_in_delta Vec3.y(max_pt), 2.5, 0.001 + assert_in_delta Vec3.z(max_pt), 3.5, 0.001 + end + + test "sphere rotation has no effect" do + geometry = {:sphere, %{radius: 1.0}} + rotation = Quaternion.from_axis_angle(Vec3.unit_z(), :math.pi() / 4) + transform = Transform.from_quaternion(rotation) + + {min_pt, max_pt} = BroadPhase.compute_aabb(geometry, transform) + + assert_in_delta Vec3.x(min_pt), -1.0, 0.001 + assert_in_delta Vec3.y(min_pt), -1.0, 0.001 + assert_in_delta Vec3.z(min_pt), -1.0, 0.001 + assert_in_delta Vec3.x(max_pt), 1.0, 0.001 + assert_in_delta Vec3.y(max_pt), 1.0, 0.001 + assert_in_delta Vec3.z(max_pt), 1.0, 0.001 + end + end + + describe "compute_aabb/2 for capsules" do + test "capsule aligned with Z axis at origin" do + geometry = {:capsule, %{radius: 0.5, length: 2.0}} + transform = Transform.identity() + + {min_pt, max_pt} = BroadPhase.compute_aabb(geometry, transform) + + # Capsule extends from z=-1 to z=1 (half length), plus radius + assert_in_delta Vec3.x(min_pt), -0.5, 0.001 + assert_in_delta Vec3.y(min_pt), -0.5, 0.001 + assert_in_delta Vec3.z(min_pt), -1.5, 0.001 + assert_in_delta Vec3.x(max_pt), 0.5, 0.001 + assert_in_delta Vec3.y(max_pt), 0.5, 0.001 + assert_in_delta Vec3.z(max_pt), 1.5, 0.001 + end + + test "capsule rotated 90 degrees around Y" do + geometry = {:capsule, %{radius: 0.5, length: 2.0}} + rotation = Quaternion.from_axis_angle(Vec3.unit_y(), :math.pi() / 2) + transform = Transform.from_quaternion(rotation) + + {min_pt, max_pt} = BroadPhase.compute_aabb(geometry, transform) + + # After rotation, capsule lies along X axis + assert_in_delta Vec3.x(min_pt), -1.5, 0.001 + assert_in_delta Vec3.y(min_pt), -0.5, 0.001 + assert_in_delta Vec3.z(min_pt), -0.5, 0.001 + assert_in_delta Vec3.x(max_pt), 1.5, 0.001 + assert_in_delta Vec3.y(max_pt), 0.5, 0.001 + assert_in_delta Vec3.z(max_pt), 0.5, 0.001 + end + + test "capsule with translation" do + geometry = {:capsule, %{radius: 0.5, length: 2.0}} + transform = Transform.from_position_quaternion(Vec3.new(5.0, 0.0, 0.0), Quaternion.identity()) + + {min_pt, max_pt} = BroadPhase.compute_aabb(geometry, transform) + + assert_in_delta Vec3.x(min_pt), 4.5, 0.001 + assert_in_delta Vec3.x(max_pt), 5.5, 0.001 + end + end + + describe "compute_aabb/2 for cylinders" do + test "cylinder treated as capsule" do + capsule_geometry = {:capsule, %{radius: 0.5, length: 2.0}} + cylinder_geometry = {:cylinder, %{radius: 0.5, height: 2.0}} + transform = Transform.identity() + + capsule_aabb = BroadPhase.compute_aabb(capsule_geometry, transform) + cylinder_aabb = BroadPhase.compute_aabb(cylinder_geometry, transform) + + {cap_min, cap_max} = capsule_aabb + {cyl_min, cyl_max} = cylinder_aabb + + assert_in_delta Vec3.x(cap_min), Vec3.x(cyl_min), 0.001 + assert_in_delta Vec3.y(cap_min), Vec3.y(cyl_min), 0.001 + assert_in_delta Vec3.z(cap_min), Vec3.z(cyl_min), 0.001 + assert_in_delta Vec3.x(cap_max), Vec3.x(cyl_max), 0.001 + assert_in_delta Vec3.y(cap_max), Vec3.y(cyl_max), 0.001 + assert_in_delta Vec3.z(cap_max), Vec3.z(cyl_max), 0.001 + end + end + + describe "compute_aabb/2 for boxes" do + test "axis-aligned box at origin" do + geometry = {:box, %{x: 1.0, y: 2.0, z: 0.5}} + transform = Transform.identity() + + {min_pt, max_pt} = BroadPhase.compute_aabb(geometry, transform) + + assert_in_delta Vec3.x(min_pt), -1.0, 0.001 + assert_in_delta Vec3.y(min_pt), -2.0, 0.001 + assert_in_delta Vec3.z(min_pt), -0.5, 0.001 + assert_in_delta Vec3.x(max_pt), 1.0, 0.001 + assert_in_delta Vec3.y(max_pt), 2.0, 0.001 + assert_in_delta Vec3.z(max_pt), 0.5, 0.001 + end + + test "box rotated 45 degrees around Z" do + geometry = {:box, %{x: 1.0, y: 1.0, z: 1.0}} + rotation = Quaternion.from_axis_angle(Vec3.unit_z(), :math.pi() / 4) + transform = Transform.from_quaternion(rotation) + + {min_pt, max_pt} = BroadPhase.compute_aabb(geometry, transform) + + # When rotated 45 degrees, the AABB expands + # sqrt(2) ≈ 1.414 + expected_extent = :math.sqrt(2) + assert_in_delta Vec3.x(min_pt), -expected_extent, 0.001 + assert_in_delta Vec3.y(min_pt), -expected_extent, 0.001 + assert_in_delta Vec3.z(min_pt), -1.0, 0.001 + assert_in_delta Vec3.x(max_pt), expected_extent, 0.001 + assert_in_delta Vec3.y(max_pt), expected_extent, 0.001 + assert_in_delta Vec3.z(max_pt), 1.0, 0.001 + end + + test "box with translation" do + geometry = {:box, %{x: 0.5, y: 0.5, z: 0.5}} + transform = Transform.from_position_quaternion(Vec3.new(10.0, 20.0, 30.0), Quaternion.identity()) + + {min_pt, max_pt} = BroadPhase.compute_aabb(geometry, transform) + + assert_in_delta Vec3.x(min_pt), 9.5, 0.001 + assert_in_delta Vec3.y(min_pt), 19.5, 0.001 + assert_in_delta Vec3.z(min_pt), 29.5, 0.001 + assert_in_delta Vec3.x(max_pt), 10.5, 0.001 + assert_in_delta Vec3.y(max_pt), 20.5, 0.001 + assert_in_delta Vec3.z(max_pt), 30.5, 0.001 + end + end + + describe "compute_aabb/2 for meshes" do + @fixtures_dir Path.join([__DIR__, "..", "..", "fixtures"]) + + test "mesh with valid file computes accurate AABB" do + path = Path.join(@fixtures_dir, "cube.stl") + geometry = {:mesh, %{filename: path, scale: 1.0}} + transform = Transform.identity() + + {min_pt, max_pt} = BroadPhase.compute_aabb(geometry, transform) + + # Cube is 0-1 in all dimensions + assert_in_delta Vec3.x(min_pt), 0.0, 0.001 + assert_in_delta Vec3.y(min_pt), 0.0, 0.001 + assert_in_delta Vec3.z(min_pt), 0.0, 0.001 + assert_in_delta Vec3.x(max_pt), 1.0, 0.001 + assert_in_delta Vec3.y(max_pt), 1.0, 0.001 + assert_in_delta Vec3.z(max_pt), 1.0, 0.001 + end + + test "mesh with scale applies scaling" do + path = Path.join(@fixtures_dir, "cube.stl") + geometry = {:mesh, %{filename: path, scale: 2.0}} + transform = Transform.identity() + + {min_pt, max_pt} = BroadPhase.compute_aabb(geometry, transform) + + # Scaled cube is 0-2 in all dimensions + assert_in_delta Vec3.x(min_pt), 0.0, 0.001 + assert_in_delta Vec3.y(min_pt), 0.0, 0.001 + assert_in_delta Vec3.z(min_pt), 0.0, 0.001 + assert_in_delta Vec3.x(max_pt), 2.0, 0.001 + assert_in_delta Vec3.y(max_pt), 2.0, 0.001 + assert_in_delta Vec3.z(max_pt), 2.0, 0.001 + end + + test "mesh with translation" do + path = Path.join(@fixtures_dir, "cube.stl") + geometry = {:mesh, %{filename: path, scale: 1.0}} + transform = Transform.from_position_quaternion(Vec3.new(5.0, 0.0, 0.0), Quaternion.identity()) + + {min_pt, max_pt} = BroadPhase.compute_aabb(geometry, transform) + + assert_in_delta Vec3.x(min_pt), 5.0, 0.001 + assert_in_delta Vec3.x(max_pt), 6.0, 0.001 + end + + test "mesh with non-existent file returns placeholder AABB" do + geometry = {:mesh, %{filename: "/nonexistent/model.stl", scale: 1.0}} + transform = Transform.from_position_quaternion(Vec3.new(5.0, 0.0, 0.0), Quaternion.identity()) + + {min_pt, max_pt} = BroadPhase.compute_aabb(geometry, transform) + + # Placeholder is a unit sphere around the translation + assert_in_delta Vec3.x(min_pt), 4.0, 0.001 + assert_in_delta Vec3.x(max_pt), 6.0, 0.001 + end + + test "mesh without filename returns placeholder AABB" do + geometry = {:mesh, %{other_data: "something"}} + transform = Transform.from_position_quaternion(Vec3.new(5.0, 0.0, 0.0), Quaternion.identity()) + + {min_pt, max_pt} = BroadPhase.compute_aabb(geometry, transform) + + # Placeholder is a unit sphere around the translation + assert_in_delta Vec3.x(min_pt), 4.0, 0.001 + assert_in_delta Vec3.x(max_pt), 6.0, 0.001 + end + end + + describe "expand/2" do + test "expands AABB by margin" do + aabb = {Vec3.new(0.0, 0.0, 0.0), Vec3.new(1.0, 1.0, 1.0)} + + {min_pt, max_pt} = BroadPhase.expand(aabb, 0.5) + + assert_in_delta Vec3.x(min_pt), -0.5, 0.001 + assert_in_delta Vec3.y(min_pt), -0.5, 0.001 + assert_in_delta Vec3.z(min_pt), -0.5, 0.001 + assert_in_delta Vec3.x(max_pt), 1.5, 0.001 + assert_in_delta Vec3.y(max_pt), 1.5, 0.001 + assert_in_delta Vec3.z(max_pt), 1.5, 0.001 + end + end + + describe "merge/2" do + test "merges two AABBs" do + aabb1 = {Vec3.new(0.0, 0.0, 0.0), Vec3.new(1.0, 1.0, 1.0)} + aabb2 = {Vec3.new(2.0, 2.0, 2.0), Vec3.new(3.0, 3.0, 3.0)} + + {min_pt, max_pt} = BroadPhase.merge(aabb1, aabb2) + + assert_in_delta Vec3.x(min_pt), 0.0, 0.001 + assert_in_delta Vec3.y(min_pt), 0.0, 0.001 + assert_in_delta Vec3.z(min_pt), 0.0, 0.001 + assert_in_delta Vec3.x(max_pt), 3.0, 0.001 + assert_in_delta Vec3.y(max_pt), 3.0, 0.001 + assert_in_delta Vec3.z(max_pt), 3.0, 0.001 + end + + test "merge is commutative" do + aabb1 = {Vec3.new(0.0, 0.0, 0.0), Vec3.new(1.0, 1.0, 1.0)} + aabb2 = {Vec3.new(-1.0, 2.0, 0.5), Vec3.new(0.5, 3.0, 2.0)} + + result1 = BroadPhase.merge(aabb1, aabb2) + result2 = BroadPhase.merge(aabb2, aabb1) + + {min1, max1} = result1 + {min2, max2} = result2 + + assert_in_delta Vec3.x(min1), Vec3.x(min2), 0.001 + assert_in_delta Vec3.y(min1), Vec3.y(min2), 0.001 + assert_in_delta Vec3.z(min1), Vec3.z(min2), 0.001 + assert_in_delta Vec3.x(max1), Vec3.x(max2), 0.001 + assert_in_delta Vec3.y(max1), Vec3.y(max2), 0.001 + assert_in_delta Vec3.z(max1), Vec3.z(max2), 0.001 + end + end + + describe "centre/1" do + test "computes centre of AABB" do + aabb = {Vec3.new(0.0, 0.0, 0.0), Vec3.new(2.0, 4.0, 6.0)} + + centre = BroadPhase.centre(aabb) + + assert_in_delta Vec3.x(centre), 1.0, 0.001 + assert_in_delta Vec3.y(centre), 2.0, 0.001 + assert_in_delta Vec3.z(centre), 3.0, 0.001 + end + end + + describe "size/1" do + test "computes size of AABB" do + aabb = {Vec3.new(1.0, 2.0, 3.0), Vec3.new(4.0, 6.0, 9.0)} + + size = BroadPhase.size(aabb) + + assert_in_delta Vec3.x(size), 3.0, 0.001 + assert_in_delta Vec3.y(size), 4.0, 0.001 + assert_in_delta Vec3.z(size), 6.0, 0.001 + end + end + + describe "contains_point?/2" do + test "point inside AABB" do + aabb = {Vec3.new(0.0, 0.0, 0.0), Vec3.new(2.0, 2.0, 2.0)} + point = Vec3.new(1.0, 1.0, 1.0) + + assert BroadPhase.contains_point?(aabb, point) + end + + test "point on AABB boundary" do + aabb = {Vec3.new(0.0, 0.0, 0.0), Vec3.new(2.0, 2.0, 2.0)} + point = Vec3.new(0.0, 1.0, 1.0) + + assert BroadPhase.contains_point?(aabb, point) + end + + test "point outside AABB" do + aabb = {Vec3.new(0.0, 0.0, 0.0), Vec3.new(2.0, 2.0, 2.0)} + point = Vec3.new(3.0, 1.0, 1.0) + + refute BroadPhase.contains_point?(aabb, point) + end + end +end diff --git a/test/bb/collision/mesh_test.exs b/test/bb/collision/mesh_test.exs new file mode 100644 index 0000000..9defee6 --- /dev/null +++ b/test/bb/collision/mesh_test.exs @@ -0,0 +1,210 @@ +# SPDX-FileCopyrightText: 2025 James Harton +# +# SPDX-License-Identifier: Apache-2.0 + +defmodule BB.Collision.MeshTest do + use ExUnit.Case, async: true + + alias BB.Collision.Mesh + alias BB.Math.Vec3 + + @fixtures_dir Path.join([__DIR__, "..", "..", "fixtures"]) + + setup do + Mesh.clear_cache() + :ok + end + + describe "load_bounds/1" do + test "loads ASCII STL file" do + path = Path.join(@fixtures_dir, "cube.stl") + assert {:ok, bounds} = Mesh.load_bounds(path) + + {min_pt, max_pt} = bounds.aabb + assert_in_delta Vec3.x(min_pt), 0.0, 0.001 + assert_in_delta Vec3.y(min_pt), 0.0, 0.001 + assert_in_delta Vec3.z(min_pt), 0.0, 0.001 + assert_in_delta Vec3.x(max_pt), 1.0, 0.001 + assert_in_delta Vec3.y(max_pt), 1.0, 0.001 + assert_in_delta Vec3.z(max_pt), 1.0, 0.001 + end + + test "computes bounding sphere" do + path = Path.join(@fixtures_dir, "cube.stl") + assert {:ok, bounds} = Mesh.load_bounds(path) + + {centre, radius} = bounds.bounding_sphere + + # Centre should be at (0.5, 0.5, 0.5) + assert_in_delta Vec3.x(centre), 0.5, 0.001 + assert_in_delta Vec3.y(centre), 0.5, 0.001 + assert_in_delta Vec3.z(centre), 0.5, 0.001 + + # Radius should be distance from centre to corner: sqrt(0.5^2 * 3) ≈ 0.866 + expected_radius = :math.sqrt(0.75) + assert_in_delta radius, expected_radius, 0.001 + end + + test "caches results" do + path = Path.join(@fixtures_dir, "cube.stl") + + # First load + {:ok, bounds1} = Mesh.load_bounds(path) + + # Second load should return same result (from cache) + {:ok, bounds2} = Mesh.load_bounds(path) + + assert bounds1 == bounds2 + end + + test "returns error for non-existent file" do + assert {:error, {:file_stat_error, :enoent}} = Mesh.load_bounds("/nonexistent/file.stl") + end + end + + describe "load_bounds!/1" do + test "returns bounds for valid file" do + path = Path.join(@fixtures_dir, "cube.stl") + bounds = Mesh.load_bounds!(path) + + assert is_map(bounds) + assert Map.has_key?(bounds, :aabb) + assert Map.has_key?(bounds, :bounding_sphere) + end + + test "raises for invalid file" do + assert_raise RuntimeError, ~r/Failed to load mesh bounds/, fn -> + Mesh.load_bounds!("/nonexistent/file.stl") + end + end + end + + describe "compute_bounds/1" do + test "computes AABB from vertices" do + vertices = [ + {0.0, 0.0, 0.0}, + {2.0, 0.0, 0.0}, + {0.0, 3.0, 0.0}, + {0.0, 0.0, 4.0} + ] + + {:ok, bounds} = Mesh.compute_bounds(vertices) + {min_pt, max_pt} = bounds.aabb + + assert_in_delta Vec3.x(min_pt), 0.0, 0.001 + assert_in_delta Vec3.y(min_pt), 0.0, 0.001 + assert_in_delta Vec3.z(min_pt), 0.0, 0.001 + assert_in_delta Vec3.x(max_pt), 2.0, 0.001 + assert_in_delta Vec3.y(max_pt), 3.0, 0.001 + assert_in_delta Vec3.z(max_pt), 4.0, 0.001 + end + + test "computes bounding sphere from vertices" do + vertices = [ + {-1.0, 0.0, 0.0}, + {1.0, 0.0, 0.0}, + {0.0, -1.0, 0.0}, + {0.0, 1.0, 0.0} + ] + + {:ok, bounds} = Mesh.compute_bounds(vertices) + {centre, radius} = bounds.bounding_sphere + + # Centre should be at origin + assert_in_delta Vec3.x(centre), 0.0, 0.001 + assert_in_delta Vec3.y(centre), 0.0, 0.001 + assert_in_delta Vec3.z(centre), 0.0, 0.001 + + # Radius should be 1.0 + assert_in_delta radius, 1.0, 0.001 + end + + test "handles single vertex" do + vertices = [{5.0, 5.0, 5.0}] + + {:ok, bounds} = Mesh.compute_bounds(vertices) + {min_pt, max_pt} = bounds.aabb + {centre, radius} = bounds.bounding_sphere + + assert_in_delta Vec3.x(min_pt), 5.0, 0.001 + assert_in_delta Vec3.x(max_pt), 5.0, 0.001 + assert_in_delta Vec3.x(centre), 5.0, 0.001 + assert_in_delta radius, 0.0, 0.001 + end + + test "returns error for empty vertex list" do + assert {:error, :empty_mesh} = Mesh.compute_bounds([]) + end + + test "handles negative coordinates" do + vertices = [ + {-2.0, -3.0, -4.0}, + {1.0, 2.0, 3.0} + ] + + {:ok, bounds} = Mesh.compute_bounds(vertices) + {min_pt, max_pt} = bounds.aabb + + assert_in_delta Vec3.x(min_pt), -2.0, 0.001 + assert_in_delta Vec3.y(min_pt), -3.0, 0.001 + assert_in_delta Vec3.z(min_pt), -4.0, 0.001 + assert_in_delta Vec3.x(max_pt), 1.0, 0.001 + assert_in_delta Vec3.y(max_pt), 2.0, 0.001 + assert_in_delta Vec3.z(max_pt), 3.0, 0.001 + end + end + + describe "clear_cache/0" do + test "clears cached bounds" do + path = Path.join(@fixtures_dir, "cube.stl") + + # Load to populate cache + {:ok, _} = Mesh.load_bounds(path) + + # Clear cache + :ok = Mesh.clear_cache() + + # Should still work (reloads from file) + {:ok, _} = Mesh.load_bounds(path) + end + end + + describe "binary STL parsing" do + test "parses binary STL" do + # Create a minimal binary STL in memory with one triangle + header = String.duplicate(<<0>>, 80) + num_triangles = <<1::little-unsigned-32>> + + # Normal (0, 0, 1) + normal = <<0.0::little-float-32, 0.0::little-float-32, 1.0::little-float-32>> + + # Vertices forming a triangle + v1 = <<0.0::little-float-32, 0.0::little-float-32, 0.0::little-float-32>> + v2 = <<1.0::little-float-32, 0.0::little-float-32, 0.0::little-float-32>> + v3 = <<0.0::little-float-32, 1.0::little-float-32, 0.0::little-float-32>> + + # Attribute byte count + attr = <<0::16>> + + binary_stl = header <> num_triangles <> normal <> v1 <> v2 <> v3 <> attr + + # Write to temp file + path = Path.join(System.tmp_dir!(), "test_binary_#{:erlang.unique_integer([:positive])}.stl") + File.write!(path, binary_stl) + + try do + {:ok, bounds} = Mesh.load_bounds(path) + {min_pt, max_pt} = bounds.aabb + + assert_in_delta Vec3.x(min_pt), 0.0, 0.001 + assert_in_delta Vec3.y(min_pt), 0.0, 0.001 + assert_in_delta Vec3.z(min_pt), 0.0, 0.001 + assert_in_delta Vec3.x(max_pt), 1.0, 0.001 + assert_in_delta Vec3.y(max_pt), 1.0, 0.001 + assert_in_delta Vec3.z(max_pt), 0.0, 0.001 + after + File.rm(path) + end + end + end +end diff --git a/test/bb/collision/primitives_test.exs b/test/bb/collision/primitives_test.exs new file mode 100644 index 0000000..27cd49f --- /dev/null +++ b/test/bb/collision/primitives_test.exs @@ -0,0 +1,531 @@ +# SPDX-FileCopyrightText: 2025 James Harton +# +# SPDX-License-Identifier: Apache-2.0 + +defmodule BB.Collision.PrimitivesTest do + use ExUnit.Case, async: true + + alias BB.Collision.Primitives + alias BB.Math.Vec3 + + describe "sphere_sphere/2" do + test "detects collision between overlapping spheres" do + sphere1 = {:sphere, Vec3.new(0.0, 0.0, 0.0), 1.0} + sphere2 = {:sphere, Vec3.new(1.5, 0.0, 0.0), 1.0} + + assert {:collision, penetration} = Primitives.sphere_sphere(sphere1, sphere2) + assert_in_delta penetration, 0.5, 0.001 + end + + test "detects collision between touching spheres" do + sphere1 = {:sphere, Vec3.new(0.0, 0.0, 0.0), 1.0} + sphere2 = {:sphere, Vec3.new(2.0, 0.0, 0.0), 1.0} + + # Exactly touching - not technically colliding + assert :no_collision = Primitives.sphere_sphere(sphere1, sphere2) + end + + test "returns no collision for separated spheres" do + sphere1 = {:sphere, Vec3.new(0.0, 0.0, 0.0), 1.0} + sphere2 = {:sphere, Vec3.new(3.0, 0.0, 0.0), 1.0} + + assert :no_collision = Primitives.sphere_sphere(sphere1, sphere2) + end + + test "detects collision between concentric spheres" do + sphere1 = {:sphere, Vec3.new(0.0, 0.0, 0.0), 2.0} + sphere2 = {:sphere, Vec3.new(0.0, 0.0, 0.0), 1.0} + + assert {:collision, penetration} = Primitives.sphere_sphere(sphere1, sphere2) + assert_in_delta penetration, 3.0, 0.001 + end + + test "works with different radii" do + sphere1 = {:sphere, Vec3.new(0.0, 0.0, 0.0), 0.5} + sphere2 = {:sphere, Vec3.new(1.0, 0.0, 0.0), 1.0} + + assert {:collision, penetration} = Primitives.sphere_sphere(sphere1, sphere2) + assert_in_delta penetration, 0.5, 0.001 + end + end + + describe "capsule_capsule/2" do + test "detects collision between parallel capsules" do + # Two horizontal capsules side by side + cap1 = {:capsule, Vec3.new(0.0, 0.0, 0.0), Vec3.new(2.0, 0.0, 0.0), 0.5} + cap2 = {:capsule, Vec3.new(0.0, 0.8, 0.0), Vec3.new(2.0, 0.8, 0.0), 0.5} + + assert {:collision, penetration} = Primitives.capsule_capsule(cap1, cap2) + assert_in_delta penetration, 0.2, 0.001 + end + + test "detects collision between crossing capsules" do + # One horizontal, one vertical, crossing in the middle + cap1 = {:capsule, Vec3.new(-1.0, 0.0, 0.0), Vec3.new(1.0, 0.0, 0.0), 0.3} + cap2 = {:capsule, Vec3.new(0.0, -1.0, 0.0), Vec3.new(0.0, 1.0, 0.0), 0.3} + + assert {:collision, penetration} = Primitives.capsule_capsule(cap1, cap2) + assert_in_delta penetration, 0.6, 0.001 + end + + test "returns no collision for separated capsules" do + cap1 = {:capsule, Vec3.new(0.0, 0.0, 0.0), Vec3.new(2.0, 0.0, 0.0), 0.3} + cap2 = {:capsule, Vec3.new(0.0, 2.0, 0.0), Vec3.new(2.0, 2.0, 0.0), 0.3} + + assert :no_collision = Primitives.capsule_capsule(cap1, cap2) + end + + test "detects end-to-end collision" do + # Capsules touching at their ends + cap1 = {:capsule, Vec3.new(0.0, 0.0, 0.0), Vec3.new(1.0, 0.0, 0.0), 0.5} + cap2 = {:capsule, Vec3.new(1.5, 0.0, 0.0), Vec3.new(2.5, 0.0, 0.0), 0.5} + + assert {:collision, penetration} = Primitives.capsule_capsule(cap1, cap2) + assert_in_delta penetration, 0.5, 0.001 + end + + test "handles degenerate capsules (points)" do + # Capsule with zero length is essentially a sphere + cap1 = {:capsule, Vec3.new(0.0, 0.0, 0.0), Vec3.new(0.0, 0.0, 0.0), 0.5} + cap2 = {:capsule, Vec3.new(0.8, 0.0, 0.0), Vec3.new(0.8, 0.0, 0.0), 0.5} + + assert {:collision, penetration} = Primitives.capsule_capsule(cap1, cap2) + assert_in_delta penetration, 0.2, 0.001 + end + end + + describe "sphere_capsule/2" do + test "detects collision when sphere hits middle of capsule" do + sphere = {:sphere, Vec3.new(1.0, 0.6, 0.0), 0.5} + capsule = {:capsule, Vec3.new(0.0, 0.0, 0.0), Vec3.new(2.0, 0.0, 0.0), 0.3} + + assert {:collision, penetration} = Primitives.sphere_capsule(sphere, capsule) + assert_in_delta penetration, 0.2, 0.001 + end + + test "detects collision at capsule end" do + sphere = {:sphere, Vec3.new(2.3, 0.0, 0.0), 0.5} + capsule = {:capsule, Vec3.new(0.0, 0.0, 0.0), Vec3.new(2.0, 0.0, 0.0), 0.3} + + assert {:collision, penetration} = Primitives.sphere_capsule(sphere, capsule) + assert_in_delta penetration, 0.5, 0.001 + end + + test "returns no collision for separated sphere and capsule" do + sphere = {:sphere, Vec3.new(1.0, 2.0, 0.0), 0.3} + capsule = {:capsule, Vec3.new(0.0, 0.0, 0.0), Vec3.new(2.0, 0.0, 0.0), 0.3} + + assert :no_collision = Primitives.sphere_capsule(sphere, capsule) + end + end + + describe "sphere_box/2" do + test "detects collision when sphere inside box" do + sphere = {:sphere, Vec3.new(0.0, 0.0, 0.0), 0.3} + + box = + {:box, Vec3.new(0.0, 0.0, 0.0), Vec3.new(1.0, 1.0, 1.0), + {Vec3.new(1.0, 0.0, 0.0), Vec3.new(0.0, 1.0, 0.0), Vec3.new(0.0, 0.0, 1.0)}} + + assert {:collision, _penetration} = Primitives.sphere_box(sphere, box) + end + + test "detects collision when sphere touches box face" do + sphere = {:sphere, Vec3.new(1.3, 0.0, 0.0), 0.5} + + box = + {:box, Vec3.new(0.0, 0.0, 0.0), Vec3.new(1.0, 1.0, 1.0), + {Vec3.new(1.0, 0.0, 0.0), Vec3.new(0.0, 1.0, 0.0), Vec3.new(0.0, 0.0, 1.0)}} + + assert {:collision, penetration} = Primitives.sphere_box(sphere, box) + assert_in_delta penetration, 0.2, 0.001 + end + + test "detects collision when sphere touches box corner" do + # Sphere near corner of unit box centred at origin + # Distance from sphere centre (1.3, 1.3, 1.3) to box corner (1, 1, 1) is sqrt(0.3^2 * 3) ≈ 0.52 + # Sphere radius is 0.6, so should collide + sphere = {:sphere, Vec3.new(1.0 + 0.3, 1.0 + 0.3, 1.0 + 0.3), 0.6} + + box = + {:box, Vec3.new(0.0, 0.0, 0.0), Vec3.new(1.0, 1.0, 1.0), + {Vec3.new(1.0, 0.0, 0.0), Vec3.new(0.0, 1.0, 0.0), Vec3.new(0.0, 0.0, 1.0)}} + + assert {:collision, _penetration} = Primitives.sphere_box(sphere, box) + end + + test "returns no collision for separated sphere and box" do + sphere = {:sphere, Vec3.new(3.0, 0.0, 0.0), 0.5} + + box = + {:box, Vec3.new(0.0, 0.0, 0.0), Vec3.new(1.0, 1.0, 1.0), + {Vec3.new(1.0, 0.0, 0.0), Vec3.new(0.0, 1.0, 0.0), Vec3.new(0.0, 0.0, 1.0)}} + + assert :no_collision = Primitives.sphere_box(sphere, box) + end + + test "works with rotated box" do + # Box rotated 45 degrees around Z axis + # Half-extent of 0.5 along rotated axis extends to sqrt(2)/2 * 0.5 ≈ 0.354 in each direction + # So box corner reaches about 0.707 in X direction + angle = :math.pi() / 4 + cos_a = :math.cos(angle) + sin_a = :math.sin(angle) + + box = + {:box, Vec3.new(0.0, 0.0, 0.0), Vec3.new(0.5, 0.5, 0.5), + {Vec3.new(cos_a, sin_a, 0.0), Vec3.new(-sin_a, cos_a, 0.0), Vec3.new(0.0, 0.0, 1.0)}} + + # Sphere far enough away to not collide + sphere_far = {:sphere, Vec3.new(1.5, 0.0, 0.0), 0.3} + assert :no_collision = Primitives.sphere_box(sphere_far, box) + + # Sphere close enough to collide with rotated corner + sphere_near = {:sphere, Vec3.new(0.9, 0.0, 0.0), 0.3} + assert {:collision, _} = Primitives.sphere_box(sphere_near, box) + end + end + + describe "capsule_box/2" do + test "detects collision when capsule passes through box" do + capsule = {:capsule, Vec3.new(-2.0, 0.0, 0.0), Vec3.new(2.0, 0.0, 0.0), 0.2} + + box = + {:box, Vec3.new(0.0, 0.0, 0.0), Vec3.new(1.0, 1.0, 1.0), + {Vec3.new(1.0, 0.0, 0.0), Vec3.new(0.0, 1.0, 0.0), Vec3.new(0.0, 0.0, 1.0)}} + + assert {:collision, _penetration} = Primitives.capsule_box(capsule, box) + end + + test "returns no collision for separated capsule and box" do + capsule = {:capsule, Vec3.new(0.0, 3.0, 0.0), Vec3.new(2.0, 3.0, 0.0), 0.2} + + box = + {:box, Vec3.new(0.0, 0.0, 0.0), Vec3.new(1.0, 1.0, 1.0), + {Vec3.new(1.0, 0.0, 0.0), Vec3.new(0.0, 1.0, 0.0), Vec3.new(0.0, 0.0, 1.0)}} + + assert :no_collision = Primitives.capsule_box(capsule, box) + end + + test "detects collision when capsule grazes box edge" do + capsule = {:capsule, Vec3.new(1.2, -1.0, 0.0), Vec3.new(1.2, 1.0, 0.0), 0.3} + + box = + {:box, Vec3.new(0.0, 0.0, 0.0), Vec3.new(1.0, 1.0, 1.0), + {Vec3.new(1.0, 0.0, 0.0), Vec3.new(0.0, 1.0, 0.0), Vec3.new(0.0, 0.0, 1.0)}} + + # Capsule is 0.2 units from box edge, radius is 0.3 + assert {:collision, penetration} = Primitives.capsule_box(capsule, box) + assert_in_delta penetration, 0.1, 0.05 + end + end + + describe "box_box/2" do + test "detects collision between overlapping axis-aligned boxes" do + box1 = + {:box, Vec3.new(0.0, 0.0, 0.0), Vec3.new(1.0, 1.0, 1.0), + {Vec3.new(1.0, 0.0, 0.0), Vec3.new(0.0, 1.0, 0.0), Vec3.new(0.0, 0.0, 1.0)}} + + box2 = + {:box, Vec3.new(1.5, 0.0, 0.0), Vec3.new(1.0, 1.0, 1.0), + {Vec3.new(1.0, 0.0, 0.0), Vec3.new(0.0, 1.0, 0.0), Vec3.new(0.0, 0.0, 1.0)}} + + assert {:collision, penetration} = Primitives.box_box(box1, box2) + assert_in_delta penetration, 0.5, 0.001 + end + + test "returns no collision for separated axis-aligned boxes" do + box1 = + {:box, Vec3.new(0.0, 0.0, 0.0), Vec3.new(1.0, 1.0, 1.0), + {Vec3.new(1.0, 0.0, 0.0), Vec3.new(0.0, 1.0, 0.0), Vec3.new(0.0, 0.0, 1.0)}} + + box2 = + {:box, Vec3.new(3.0, 0.0, 0.0), Vec3.new(1.0, 1.0, 1.0), + {Vec3.new(1.0, 0.0, 0.0), Vec3.new(0.0, 1.0, 0.0), Vec3.new(0.0, 0.0, 1.0)}} + + assert :no_collision = Primitives.box_box(box1, box2) + end + + test "detects collision between rotated boxes" do + box1 = + {:box, Vec3.new(0.0, 0.0, 0.0), Vec3.new(1.0, 1.0, 1.0), + {Vec3.new(1.0, 0.0, 0.0), Vec3.new(0.0, 1.0, 0.0), Vec3.new(0.0, 0.0, 1.0)}} + + # Box rotated 45 degrees around Z + angle = :math.pi() / 4 + cos_a = :math.cos(angle) + sin_a = :math.sin(angle) + + box2 = + {:box, Vec3.new(1.5, 0.0, 0.0), Vec3.new(0.5, 0.5, 0.5), + {Vec3.new(cos_a, sin_a, 0.0), Vec3.new(-sin_a, cos_a, 0.0), Vec3.new(0.0, 0.0, 1.0)}} + + assert {:collision, _penetration} = Primitives.box_box(box1, box2) + end + + test "correctly handles edge-edge separation" do + # Two boxes that would collide on face normals but are separated on edge cross products + angle = :math.pi() / 4 + cos_a = :math.cos(angle) + sin_a = :math.sin(angle) + + box1 = + {:box, Vec3.new(0.0, 0.0, 0.0), Vec3.new(0.5, 0.5, 0.5), + {Vec3.new(1.0, 0.0, 0.0), Vec3.new(0.0, 1.0, 0.0), Vec3.new(0.0, 0.0, 1.0)}} + + box2 = + {:box, Vec3.new(1.5, 1.5, 0.0), Vec3.new(0.5, 0.5, 0.5), + {Vec3.new(cos_a, sin_a, 0.0), Vec3.new(-sin_a, cos_a, 0.0), Vec3.new(0.0, 0.0, 1.0)}} + + # These boxes should not collide + assert :no_collision = Primitives.box_box(box1, box2) + end + end + + describe "test/2 dispatch" do + test "correctly dispatches sphere-sphere" do + sphere1 = {:sphere, Vec3.new(0.0, 0.0, 0.0), 1.0} + sphere2 = {:sphere, Vec3.new(1.5, 0.0, 0.0), 1.0} + + assert {:collision, _} = Primitives.test(sphere1, sphere2) + end + + test "correctly dispatches capsule-capsule" do + cap1 = {:capsule, Vec3.new(0.0, 0.0, 0.0), Vec3.new(1.0, 0.0, 0.0), 0.5} + cap2 = {:capsule, Vec3.new(0.5, 0.8, 0.0), Vec3.new(1.5, 0.8, 0.0), 0.5} + + assert {:collision, _} = Primitives.test(cap1, cap2) + end + + test "is symmetric for mixed types" do + sphere = {:sphere, Vec3.new(0.0, 0.0, 0.0), 0.5} + capsule = {:capsule, Vec3.new(0.0, 0.6, 0.0), Vec3.new(1.0, 0.6, 0.0), 0.3} + + result1 = Primitives.test(sphere, capsule) + result2 = Primitives.test(capsule, sphere) + + assert result1 == result2 + end + + test "is symmetric for sphere-box" do + sphere = {:sphere, Vec3.new(1.3, 0.0, 0.0), 0.5} + + box = + {:box, Vec3.new(0.0, 0.0, 0.0), Vec3.new(1.0, 1.0, 1.0), + {Vec3.new(1.0, 0.0, 0.0), Vec3.new(0.0, 1.0, 0.0), Vec3.new(0.0, 0.0, 1.0)}} + + result1 = Primitives.test(sphere, box) + result2 = Primitives.test(box, sphere) + + assert result1 == result2 + end + + test "is symmetric for capsule-box" do + capsule = {:capsule, Vec3.new(-2.0, 0.0, 0.0), Vec3.new(2.0, 0.0, 0.0), 0.2} + + box = + {:box, Vec3.new(0.0, 0.0, 0.0), Vec3.new(1.0, 1.0, 1.0), + {Vec3.new(1.0, 0.0, 0.0), Vec3.new(0.0, 1.0, 0.0), Vec3.new(0.0, 0.0, 1.0)}} + + result1 = Primitives.test(capsule, box) + result2 = Primitives.test(box, capsule) + + assert result1 == result2 + end + end + + describe "test_with_margin/3" do + test "detects near-misses with margin" do + # Two spheres that are close but not touching + sphere1 = {:sphere, Vec3.new(0.0, 0.0, 0.0), 1.0} + sphere2 = {:sphere, Vec3.new(2.1, 0.0, 0.0), 1.0} + + # Without margin, no collision + assert :no_collision = Primitives.test(sphere1, sphere2) + + # With 0.1 margin, should detect collision + assert {:collision, _} = Primitives.test_with_margin(sphere1, sphere2, 0.1) + end + + test "margin of zero behaves like test/2" do + sphere1 = {:sphere, Vec3.new(0.0, 0.0, 0.0), 1.0} + sphere2 = {:sphere, Vec3.new(1.5, 0.0, 0.0), 1.0} + + assert Primitives.test(sphere1, sphere2) == Primitives.test_with_margin(sphere1, sphere2, 0) + end + + test "negative margin behaves like test/2" do + sphere1 = {:sphere, Vec3.new(0.0, 0.0, 0.0), 1.0} + sphere2 = {:sphere, Vec3.new(1.5, 0.0, 0.0), 1.0} + + assert Primitives.test(sphere1, sphere2) == + Primitives.test_with_margin(sphere1, sphere2, -0.1) + end + end + + describe "closest_point_on_segment/3" do + test "returns endpoint when point is before segment" do + {closest, _distance} = + Primitives.closest_point_on_segment( + Vec3.new(-1.0, 0.0, 0.0), + Vec3.new(0.0, 0.0, 0.0), + Vec3.new(2.0, 0.0, 0.0) + ) + + assert_in_delta Vec3.x(closest), 0.0, 0.001 + assert_in_delta Vec3.y(closest), 0.0, 0.001 + assert_in_delta Vec3.z(closest), 0.0, 0.001 + end + + test "returns endpoint when point is after segment" do + {closest, _distance} = + Primitives.closest_point_on_segment( + Vec3.new(3.0, 0.0, 0.0), + Vec3.new(0.0, 0.0, 0.0), + Vec3.new(2.0, 0.0, 0.0) + ) + + assert_in_delta Vec3.x(closest), 2.0, 0.001 + assert_in_delta Vec3.y(closest), 0.0, 0.001 + assert_in_delta Vec3.z(closest), 0.0, 0.001 + end + + test "returns point on segment when point is beside it" do + {closest, distance} = + Primitives.closest_point_on_segment( + Vec3.new(1.0, 1.0, 0.0), + Vec3.new(0.0, 0.0, 0.0), + Vec3.new(2.0, 0.0, 0.0) + ) + + assert_in_delta Vec3.x(closest), 1.0, 0.001 + assert_in_delta Vec3.y(closest), 0.0, 0.001 + assert_in_delta distance, 1.0, 0.001 + end + + test "handles degenerate segment" do + {closest, distance} = + Primitives.closest_point_on_segment( + Vec3.new(1.0, 1.0, 0.0), + Vec3.new(0.0, 0.0, 0.0), + Vec3.new(0.0, 0.0, 0.0) + ) + + assert_in_delta Vec3.x(closest), 0.0, 0.001 + assert_in_delta distance, :math.sqrt(2), 0.001 + end + end + + describe "closest_points_segments/4" do + test "finds closest points on parallel segments" do + {c1, c2, distance} = + Primitives.closest_points_segments( + Vec3.new(0.0, 0.0, 0.0), + Vec3.new(2.0, 0.0, 0.0), + Vec3.new(0.0, 1.0, 0.0), + Vec3.new(2.0, 1.0, 0.0) + ) + + # Any pair of points at same X should work, distance is 1 + assert_in_delta Vec3.y(c1), 0.0, 0.001 + assert_in_delta Vec3.y(c2), 1.0, 0.001 + assert_in_delta distance, 1.0, 0.001 + end + + test "finds closest points on crossing segments" do + {c1, c2, distance} = + Primitives.closest_points_segments( + Vec3.new(-1.0, 0.0, 0.0), + Vec3.new(1.0, 0.0, 0.0), + Vec3.new(0.0, -1.0, 1.0), + Vec3.new(0.0, 1.0, 1.0) + ) + + # Closest points should be at origin of each segment's projection + assert_in_delta Vec3.x(c1), 0.0, 0.001 + assert_in_delta Vec3.y(c1), 0.0, 0.001 + assert_in_delta Vec3.z(c1), 0.0, 0.001 + assert_in_delta Vec3.x(c2), 0.0, 0.001 + assert_in_delta Vec3.y(c2), 0.0, 0.001 + assert_in_delta Vec3.z(c2), 1.0, 0.001 + assert_in_delta distance, 1.0, 0.001 + end + + test "handles end-to-end segments" do + {_c1, _c2, distance} = + Primitives.closest_points_segments( + Vec3.new(0.0, 0.0, 0.0), + Vec3.new(1.0, 0.0, 0.0), + Vec3.new(1.5, 0.0, 0.0), + Vec3.new(2.5, 0.0, 0.0) + ) + + assert_in_delta distance, 0.5, 0.001 + end + end + + describe "closest_point_on_box/4" do + test "returns centre for point inside box" do + {closest, distance} = + Primitives.closest_point_on_box( + Vec3.new(0.0, 0.0, 0.0), + Vec3.new(0.0, 0.0, 0.0), + Vec3.new(1.0, 1.0, 1.0), + {Vec3.new(1.0, 0.0, 0.0), Vec3.new(0.0, 1.0, 0.0), Vec3.new(0.0, 0.0, 1.0)} + ) + + assert_in_delta Vec3.x(closest), 0.0, 0.001 + assert_in_delta Vec3.y(closest), 0.0, 0.001 + assert_in_delta Vec3.z(closest), 0.0, 0.001 + assert_in_delta distance, 0.0, 0.001 + end + + test "returns face point for point outside face" do + {closest, distance} = + Primitives.closest_point_on_box( + Vec3.new(2.0, 0.0, 0.0), + Vec3.new(0.0, 0.0, 0.0), + Vec3.new(1.0, 1.0, 1.0), + {Vec3.new(1.0, 0.0, 0.0), Vec3.new(0.0, 1.0, 0.0), Vec3.new(0.0, 0.0, 1.0)} + ) + + assert_in_delta Vec3.x(closest), 1.0, 0.001 + assert_in_delta Vec3.y(closest), 0.0, 0.001 + assert_in_delta Vec3.z(closest), 0.0, 0.001 + assert_in_delta distance, 1.0, 0.001 + end + + test "returns corner point for point outside corner" do + {closest, distance} = + Primitives.closest_point_on_box( + Vec3.new(2.0, 2.0, 2.0), + Vec3.new(0.0, 0.0, 0.0), + Vec3.new(1.0, 1.0, 1.0), + {Vec3.new(1.0, 0.0, 0.0), Vec3.new(0.0, 1.0, 0.0), Vec3.new(0.0, 0.0, 1.0)} + ) + + assert_in_delta Vec3.x(closest), 1.0, 0.001 + assert_in_delta Vec3.y(closest), 1.0, 0.001 + assert_in_delta Vec3.z(closest), 1.0, 0.001 + assert_in_delta distance, :math.sqrt(3), 0.001 + end + + test "works with rotated box" do + angle = :math.pi() / 4 + cos_a = :math.cos(angle) + sin_a = :math.sin(angle) + + {_closest, distance} = + Primitives.closest_point_on_box( + Vec3.new(2.0, 0.0, 0.0), + Vec3.new(0.0, 0.0, 0.0), + Vec3.new(1.0, 1.0, 1.0), + {Vec3.new(cos_a, sin_a, 0.0), Vec3.new(-sin_a, cos_a, 0.0), Vec3.new(0.0, 0.0, 1.0)} + ) + + # Rotated box extends further in X direction + # Distance should be less than 1.0 + assert distance < 1.0 + end + end +end diff --git a/test/bb/collision_test.exs b/test/bb/collision_test.exs new file mode 100644 index 0000000..a2e9d58 --- /dev/null +++ b/test/bb/collision_test.exs @@ -0,0 +1,242 @@ +# SPDX-FileCopyrightText: 2025 James Harton +# +# SPDX-License-Identifier: Apache-2.0 + +defmodule BB.CollisionTest do + use ExUnit.Case, async: true + + alias BB.Collision + alias BB.ExampleRobots.CollisionTestArm + alias BB.Math.{Vec3, Quaternion} + + describe "build_adjacency_set/1" do + test "builds adjacency set from robot topology" do + robot = CollisionTestArm.robot() + adjacent = Collision.build_adjacency_set(robot) + + # Base and upper_arm are connected via shoulder joint + assert MapSet.member?(adjacent, {:base, :upper_arm}) + assert MapSet.member?(adjacent, {:upper_arm, :base}) + + # Upper_arm and forearm are connected via elbow joint + assert MapSet.member?(adjacent, {:upper_arm, :forearm}) + assert MapSet.member?(adjacent, {:forearm, :upper_arm}) + + # Base and forearm are NOT adjacent + refute MapSet.member?(adjacent, {:base, :forearm}) + refute MapSet.member?(adjacent, {:forearm, :base}) + end + end + + describe "self_collision?/3" do + test "returns false when robot is in safe configuration" do + robot = CollisionTestArm.robot() + positions = %{shoulder: 0.0, elbow: 0.0} + + refute Collision.self_collision?(robot, positions) + end + + test "returns true when robot is in self-collision with margin" do + robot = CollisionTestArm.robot() + # With a very large margin, the collision volumes expand enough to overlap + positions = %{shoulder: 0.0, elbow: 0.0} + + # Without margin, no collision (links are well separated) + refute Collision.self_collision?(robot, positions) + + # With very large margin (0.5m), base and forearm volumes will overlap + assert Collision.self_collision?(robot, positions, margin: 0.5) + end + + test "respects margin parameter" do + robot = CollisionTestArm.robot() + # Configuration that's close but not quite colliding + positions = %{shoulder: 0.0, elbow: 2.5} + + # Without margin, no collision + refute Collision.self_collision?(robot, positions) + + # With large margin, should detect near-collision + assert Collision.self_collision?(robot, positions, margin: 0.1) + end + end + + describe "detect_self_collisions/3" do + test "returns empty list when no collisions" do + robot = CollisionTestArm.robot() + positions = %{shoulder: 0.0, elbow: 0.0} + + assert [] = Collision.detect_self_collisions(robot, positions) + end + + test "returns collision info when colliding" do + robot = CollisionTestArm.robot() + # Use large margin to force collision + positions = %{shoulder: 0.0, elbow: 0.0} + + collisions = Collision.detect_self_collisions(robot, positions, margin: 0.5) + + assert length(collisions) >= 1 + + collision = hd(collisions) + assert is_atom(collision.link_a) + assert is_atom(collision.link_b) + assert is_float(collision.penetration_depth) + assert collision.penetration_depth > 0 + end + + test "does not report collisions between adjacent links" do + robot = CollisionTestArm.robot() + # Even with extreme angles, adjacent links should not report collision + positions = %{shoulder: 0.0, elbow: 0.5} + + collisions = Collision.detect_self_collisions(robot, positions) + + for collision <- collisions do + # No collision should be between adjacent pairs + refute {collision.link_a, collision.link_b} in [ + {:base, :upper_arm}, + {:upper_arm, :base}, + {:upper_arm, :forearm}, + {:forearm, :upper_arm} + ] + end + end + end + + describe "obstacle/3 and obstacle/4" do + test "creates sphere obstacle" do + centre = Vec3.new(1.0, 2.0, 3.0) + obstacle = Collision.obstacle(:sphere, centre, 0.5) + + assert obstacle.type == :sphere + assert {:sphere, ^centre, 0.5} = obstacle.geometry + assert {min_pt, max_pt} = obstacle.aabb + assert_in_delta Vec3.x(min_pt), 0.5, 0.001 + assert_in_delta Vec3.x(max_pt), 1.5, 0.001 + end + + test "creates capsule obstacle" do + point_a = Vec3.new(0.0, 0.0, 0.0) + point_b = Vec3.new(1.0, 0.0, 0.0) + obstacle = Collision.obstacle(:capsule, point_a, point_b, 0.1) + + assert obstacle.type == :capsule + assert {:capsule, ^point_a, ^point_b, 0.1} = obstacle.geometry + end + + test "creates axis-aligned box obstacle" do + centre = Vec3.new(0.0, 0.0, 0.0) + half_extents = Vec3.new(1.0, 0.5, 0.25) + obstacle = Collision.obstacle(:box, centre, half_extents) + + assert obstacle.type == :box + {min_pt, max_pt} = obstacle.aabb + assert_in_delta Vec3.x(min_pt), -1.0, 0.001 + assert_in_delta Vec3.y(min_pt), -0.5, 0.001 + assert_in_delta Vec3.x(max_pt), 1.0, 0.001 + assert_in_delta Vec3.y(max_pt), 0.5, 0.001 + end + + test "creates oriented box obstacle" do + centre = Vec3.new(0.0, 0.0, 0.0) + half_extents = Vec3.new(1.0, 0.5, 0.25) + rotation = Quaternion.from_axis_angle(Vec3.unit_z(), :math.pi() / 4) + obstacle = Collision.obstacle(:box, centre, half_extents, rotation) + + assert obstacle.type == :box + # Rotated box AABB should be larger than axis-aligned version + {min_pt, max_pt} = obstacle.aabb + # When rotated 45 degrees, the 1.0 and 0.5 extents project to about 1.06 in X and Y + assert Vec3.x(min_pt) < -1.0 + assert Vec3.x(max_pt) > 1.0 + end + end + + describe "collides_with?/4" do + test "returns false when robot doesn't collide with obstacles" do + robot = CollisionTestArm.robot() + positions = %{shoulder: 0.0, elbow: 0.0} + + # Obstacle far from robot + obstacles = [Collision.obstacle(:sphere, Vec3.new(10.0, 0.0, 0.0), 0.1)] + + refute Collision.collides_with?(robot, positions, obstacles) + end + + test "returns true when robot collides with obstacle" do + robot = CollisionTestArm.robot() + positions = %{shoulder: 0.0, elbow: 0.0} + + # Obstacle at robot base position + obstacles = [Collision.obstacle(:sphere, Vec3.new(0.0, 0.0, 0.0), 0.2)] + + assert Collision.collides_with?(robot, positions, obstacles) + end + + test "returns false with empty obstacle list" do + robot = CollisionTestArm.robot() + positions = %{shoulder: 0.0, elbow: 0.0} + + refute Collision.collides_with?(robot, positions, []) + end + end + + describe "detect_collisions/4" do + test "returns empty list when no collisions" do + robot = CollisionTestArm.robot() + positions = %{shoulder: 0.0, elbow: 0.0} + + obstacles = [Collision.obstacle(:sphere, Vec3.new(10.0, 0.0, 0.0), 0.1)] + + assert [] = Collision.detect_collisions(robot, positions, obstacles) + end + + test "returns collision info when robot collides with obstacle" do + robot = CollisionTestArm.robot() + positions = %{shoulder: 0.0, elbow: 0.0} + + # Obstacle overlapping with base + obstacles = [Collision.obstacle(:sphere, Vec3.new(0.0, 0.0, 0.0), 0.2)] + + collisions = Collision.detect_collisions(robot, positions, obstacles) + + assert length(collisions) >= 1 + + collision = hd(collisions) + assert collision.link_b == :environment + assert is_float(collision.penetration_depth) + end + + test "detects collision with multiple obstacles" do + robot = CollisionTestArm.robot() + positions = %{shoulder: 0.0, elbow: 0.0} + + obstacles = [ + Collision.obstacle(:sphere, Vec3.new(0.0, 0.0, 0.0), 0.2), + Collision.obstacle(:sphere, Vec3.new(0.3, 0.0, 0.1), 0.1) + ] + + collisions = Collision.detect_collisions(robot, positions, obstacles) + + # Should detect collisions with both obstacles + assert length(collisions) >= 2 + end + + test "respects margin parameter" do + robot = CollisionTestArm.robot() + positions = %{shoulder: 0.0, elbow: 0.0} + + # Obstacle below the robot (base is at z=0, arm extends in +X/+Z direction) + # Place sphere at z=-0.1, just below the base (base extends to z=-0.05) + obstacles = [Collision.obstacle(:sphere, Vec3.new(0.0, 0.0, -0.08), 0.01)] + + # Without margin, no collision (0.02 gap from base) + assert [] = Collision.detect_collisions(robot, positions, obstacles) + + # With margin, should detect near-collision + collisions = Collision.detect_collisions(robot, positions, obstacles, margin: 0.03) + assert length(collisions) >= 1 + end + end +end diff --git a/test/fixtures/cube.stl b/test/fixtures/cube.stl new file mode 100644 index 0000000..8696c94 --- /dev/null +++ b/test/fixtures/cube.stl @@ -0,0 +1,86 @@ +solid cube + facet normal 0 0 -1 + outer loop + vertex 0 0 0 + vertex 1 0 0 + vertex 1 1 0 + endloop + endfacet + facet normal 0 0 -1 + outer loop + vertex 0 0 0 + vertex 1 1 0 + vertex 0 1 0 + endloop + endfacet + facet normal 0 0 1 + outer loop + vertex 0 0 1 + vertex 1 1 1 + vertex 1 0 1 + endloop + endfacet + facet normal 0 0 1 + outer loop + vertex 0 0 1 + vertex 0 1 1 + vertex 1 1 1 + endloop + endfacet + facet normal 0 -1 0 + outer loop + vertex 0 0 0 + vertex 1 0 1 + vertex 1 0 0 + endloop + endfacet + facet normal 0 -1 0 + outer loop + vertex 0 0 0 + vertex 0 0 1 + vertex 1 0 1 + endloop + endfacet + facet normal 0 1 0 + outer loop + vertex 0 1 0 + vertex 1 1 0 + vertex 1 1 1 + endloop + endfacet + facet normal 0 1 0 + outer loop + vertex 0 1 0 + vertex 1 1 1 + vertex 0 1 1 + endloop + endfacet + facet normal -1 0 0 + outer loop + vertex 0 0 0 + vertex 0 1 0 + vertex 0 1 1 + endloop + endfacet + facet normal -1 0 0 + outer loop + vertex 0 0 0 + vertex 0 1 1 + vertex 0 0 1 + endloop + endfacet + facet normal 1 0 0 + outer loop + vertex 1 0 0 + vertex 1 1 1 + vertex 1 1 0 + endloop + endfacet + facet normal 1 0 0 + outer loop + vertex 1 0 0 + vertex 1 0 1 + vertex 1 1 1 + endloop + endfacet +endsolid cube diff --git a/test/support/example_robots.ex b/test/support/example_robots.ex index 9be2044..5c874cd 100644 --- a/test/support/example_robots.ex +++ b/test/support/example_robots.ex @@ -7,6 +7,101 @@ defmodule BB.ExampleRobots do Example robot topologies for testing and documentation. """ + defmodule CollisionTestArm do + @moduledoc """ + A simple 2-DOF arm with collision geometry for testing self-collision detection. + + Structure: + - base (box) + - shoulder (revolute, Z-axis) + - upper_arm (capsule) + - elbow (revolute, Y-axis) + - forearm (capsule) + """ + use BB + import BB.Unit + import Kernel, except: [length: 1] + + settings do + name(:collision_test_arm) + end + + topology do + link :base do + collision do + box do + x(~u(0.1 meter)) + y(~u(0.1 meter)) + z(~u(0.05 meter)) + end + end + + joint :shoulder do + type(:revolute) + + origin do + z(~u(0.1 meter)) + end + + axis do + end + + limit do + lower(~u(-180 degree)) + upper(~u(180 degree)) + effort(~u(10 newton_meter)) + velocity(~u(180 degree_per_second)) + end + + link :upper_arm do + collision do + origin do + x(~u(0.15 meter)) + end + + capsule do + radius(~u(0.03 meter)) + length(~u(0.3 meter)) + end + end + + joint :elbow do + type(:revolute) + + origin do + x(~u(0.3 meter)) + end + + axis do + roll(~u(-90 degree)) + end + + limit do + lower(~u(-150 degree)) + upper(~u(150 degree)) + effort(~u(10 newton_meter)) + velocity(~u(180 degree_per_second)) + end + + link :forearm do + collision do + origin do + x(~u(0.1 meter)) + end + + capsule do + radius(~u(0.025 meter)) + length(~u(0.2 meter)) + end + end + end + end + end + end + end + end + end + defmodule DifferentialDriveRobot do @moduledoc """ A simple two-wheeled differential drive robot with a caster. From 5632703fd77533786c709f25ba9a1f028e8ea9f2 Mon Sep 17 00:00:00 2001 From: James Harton Date: Tue, 30 Dec 2025 12:12:33 +1300 Subject: [PATCH 2/2] improvement: address credo issues and refine collision detection - Fix alias ordering to be alphabetical in collision modules - Add explicit alias for `BB.Collision.Mesh` instead of inline references - Replace `length/1` assertions with pattern matching in tests - Extract helper functions to reduce cyclomatic complexity in `closest_points_segments` - Extract `check_box_axis/7` to reduce nesting depth in `box_box` - Update formatter config and documentation --- .formatter.exs | 2 + documentation/dsls/DSL-BB.md | 66 +++++++++++++++ lib/bb/collision.ex | 7 +- lib/bb/collision/broad_phase.ex | 5 +- lib/bb/collision/mesh.ex | 11 +-- lib/bb/collision/primitives.ex | 111 ++++++++++++++----------- lib/bb/dsl.ex | 8 +- lib/bb/dsl/capsule.ex | 8 +- lib/bb/robot/builder.ex | 2 +- test/bb/collision/broad_phase_test.exs | 26 ++++-- test/bb/collision/mesh_test.exs | 4 +- test/bb/collision_test.exs | 12 +-- test/fixtures/cube.stl.license | 3 + test/support/example_robots.ex | 5 +- 14 files changed, 181 insertions(+), 89 deletions(-) create mode 100644 test/fixtures/cube.stl.license diff --git a/.formatter.exs b/.formatter.exs index 2e9a853..1c02fd2 100644 --- a/.formatter.exs +++ b/.formatter.exs @@ -17,6 +17,8 @@ spark_locals_without_parens = [ box: 1, bridge: 2, bridge: 3, + capsule: 0, + capsule: 1, collision: 0, collision: 1, color: 0, diff --git a/documentation/dsls/DSL-BB.md b/documentation/dsls/DSL-BB.md index 5c343ff..8b37db1 100644 --- a/documentation/dsls/DSL-BB.md +++ b/documentation/dsls/DSL-BB.md @@ -18,6 +18,7 @@ Robot topology * box * cylinder * sphere + * capsule * mesh * material * color @@ -28,6 +29,7 @@ Robot topology * box * cylinder * sphere + * capsule * mesh * sensor * [joint](#topology-joint) @@ -59,6 +61,7 @@ A kinematic link (ie solid body). * box * cylinder * sphere + * capsule * mesh * material * color @@ -69,6 +72,7 @@ A kinematic link (ie solid body). * box * cylinder * sphere + * capsule * mesh * [sensor](#topology-link-sensor) @@ -180,6 +184,7 @@ Visual attributes for a link. * [box](#topology-link-visual-box) * [cylinder](#topology-link-visual-cylinder) * [sphere](#topology-link-visual-sphere) + * [capsule](#topology-link-visual-capsule) * [mesh](#topology-link-visual-mesh) * [material](#topology-link-visual-material) * color @@ -272,6 +277,36 @@ The origin of the sphere is its center. Target: `BB.Dsl.Sphere` +### topology.link.visual.capsule + + +A capsule geometry (cylinder with hemispherical caps). + +The origin of the capsule is the centre of the cylindrical portion. +The height is the distance between the centres of the hemispherical caps. +Total extent is height + 2 * radius. + + + + + + + +### Options + +| Name | Type | Default | Docs | +|------|------|---------|------| +| [`radius`](#topology-link-visual-capsule-radius){: #topology-link-visual-capsule-radius .spark-required} | `any` | | The radius of the capsule (cylinder and hemispherical caps) | +| [`height`](#topology-link-visual-capsule-height){: #topology-link-visual-capsule-height .spark-required} | `any` | | The height of the cylindrical portion (between cap centres) | + + + + + +### Introspection + +Target: `BB.Dsl.Capsule` + ### topology.link.visual.mesh @@ -426,6 +461,7 @@ The collision properties of a link. * [box](#topology-link-collision-box) * [cylinder](#topology-link-collision-cylinder) * [sphere](#topology-link-collision-sphere) + * [capsule](#topology-link-collision-capsule) * [mesh](#topology-link-collision-mesh) @@ -549,6 +585,36 @@ The origin of the sphere is its center. Target: `BB.Dsl.Sphere` +### topology.link.collision.capsule + + +A capsule geometry (cylinder with hemispherical caps). + +The origin of the capsule is the centre of the cylindrical portion. +The height is the distance between the centres of the hemispherical caps. +Total extent is height + 2 * radius. + + + + + + + +### Options + +| Name | Type | Default | Docs | +|------|------|---------|------| +| [`radius`](#topology-link-collision-capsule-radius){: #topology-link-collision-capsule-radius .spark-required} | `any` | | The radius of the capsule (cylinder and hemispherical caps) | +| [`height`](#topology-link-collision-capsule-height){: #topology-link-collision-capsule-height .spark-required} | `any` | | The height of the cylindrical portion (between cap centres) | + + + + + +### Introspection + +Target: `BB.Dsl.Capsule` + ### topology.link.collision.mesh diff --git a/lib/bb/collision.ex b/lib/bb/collision.ex index d953a25..577b563 100644 --- a/lib/bb/collision.ex +++ b/lib/bb/collision.ex @@ -41,10 +41,10 @@ defmodule BB.Collision do For a typical 6-DOF robot, self-collision checks complete in under 1ms. """ + alias BB.Collision.{BroadPhase, Mesh, Primitives} + alias BB.Math.{Quaternion, Transform, Vec3} alias BB.Robot alias BB.Robot.Kinematics - alias BB.Collision.{BroadPhase, Primitives} - alias BB.Math.{Transform, Vec3, Quaternion} @type positions :: %{atom() => float()} @@ -414,8 +414,7 @@ defmodule BB.Collision do end defp geometry_to_primitive({:mesh, %{filename: filename, scale: scale}}, transform) do - # Use bounding sphere for mesh collision - case BB.Collision.Mesh.load_bounds(filename) do + case Mesh.load_bounds(filename) do {:ok, bounds} -> {local_centre, radius} = bounds.bounding_sphere centre = Transform.get_translation(transform) diff --git a/lib/bb/collision/broad_phase.ex b/lib/bb/collision/broad_phase.ex index d146a90..b216d13 100644 --- a/lib/bb/collision/broad_phase.ex +++ b/lib/bb/collision/broad_phase.ex @@ -15,7 +15,8 @@ defmodule BB.Collision.BroadPhase do so a positive broad phase result only indicates *potential* collision. """ - alias BB.Math.{Transform, Vec3, Quaternion} + alias BB.Collision.Mesh + alias BB.Math.{Quaternion, Transform, Vec3} @type aabb :: {min :: Vec3.t(), max :: Vec3.t()} @@ -135,7 +136,7 @@ defmodule BB.Collision.BroadPhase do end def compute_aabb({:mesh, %{filename: filename, scale: scale}}, transform) do - case BB.Collision.Mesh.load_bounds(filename) do + case Mesh.load_bounds(filename) do {:ok, bounds} -> # Transform the mesh AABB to world space transform_mesh_aabb(bounds.aabb, scale, transform) diff --git a/lib/bb/collision/mesh.ex b/lib/bb/collision/mesh.ex index 27cf3af..d7b3eb2 100644 --- a/lib/bb/collision/mesh.ex +++ b/lib/bb/collision/mesh.ex @@ -131,7 +131,9 @@ defmodule BB.Collision.Mesh do {:error, :invalid_stl} end - defp parse_binary_stl(<<_header::binary-size(80), num_triangles::little-unsigned-32, rest::binary>>) do + defp parse_binary_stl( + <<_header::binary-size(80), num_triangles::little-unsigned-32, rest::binary>> + ) do expected_size = num_triangles * 50 if byte_size(rest) >= expected_size do @@ -145,10 +147,9 @@ defmodule BB.Collision.Mesh do defp parse_binary_triangles(_data, 0, acc), do: Enum.uniq(acc) defp parse_binary_triangles( - <<_nx::little-float-32, _ny::little-float-32, _nz::little-float-32, - v1x::little-float-32, v1y::little-float-32, v1z::little-float-32, - v2x::little-float-32, v2y::little-float-32, v2z::little-float-32, - v3x::little-float-32, v3y::little-float-32, v3z::little-float-32, + <<_nx::little-float-32, _ny::little-float-32, _nz::little-float-32, v1x::little-float-32, + v1y::little-float-32, v1z::little-float-32, v2x::little-float-32, v2y::little-float-32, + v2z::little-float-32, v3x::little-float-32, v3y::little-float-32, v3z::little-float-32, _attr::binary-size(2), rest::binary>>, remaining, acc diff --git a/lib/bb/collision/primitives.ex b/lib/bb/collision/primitives.ex index 8cffbd8..0c9653c 100644 --- a/lib/bb/collision/primitives.ex +++ b/lib/bb/collision/primitives.ex @@ -25,7 +25,9 @@ defmodule BB.Collision.Primitives do @type sphere :: {:sphere, centre :: Vec3.t(), radius :: float()} @type capsule :: {:capsule, point_a :: Vec3.t(), point_b :: Vec3.t(), radius :: float()} - @type box :: {:box, centre :: Vec3.t(), half_extents :: Vec3.t(), axes :: {Vec3.t(), Vec3.t(), Vec3.t()}} + @type box :: + {:box, centre :: Vec3.t(), half_extents :: Vec3.t(), + axes :: {Vec3.t(), Vec3.t(), Vec3.t()}} @type geometry :: sphere() | capsule() | box() @type collision_result :: {:collision, penetration_depth :: float()} | :no_collision @@ -241,22 +243,8 @@ defmodule BB.Collision.Primitives do # Find minimum penetration across all axes min_penetration = - axes - |> Enum.reduce_while(:infinity, fn axis, min_pen -> - # Skip degenerate axes (from parallel edges) - if Vec3.magnitude_squared(axis) < 1.0e-10 do - {:cont, min_pen} - else - axis_normalized = Vec3.normalise(axis) - - case test_axis(axis_normalized, t, box1_axes, box1_half, box2_axes, box2_half) do - :separated -> - {:halt, :separated} - - {:overlap, pen} -> - {:cont, min(min_pen, pen)} - end - end + Enum.reduce_while(axes, :infinity, fn axis, min_pen -> + check_box_axis(axis, t, box1_axes, box1_half, box2_axes, box2_half, min_pen) end) case min_penetration do @@ -329,38 +317,37 @@ defmodule BB.Collision.Primitives do {closest, a2, dist} true -> - # General case - c = Vec3.dot(d1, r) - b = Vec3.dot(d1, d2) - denom = a * e - b * b - - # Compute s (parameter on first segment) - s = - if abs(denom) < 1.0e-10 do - 0.0 - else - clamp((b * f - c * e) / denom, 0.0, 1.0) - end - - # Compute t (parameter on second segment) - t = (b * s + f) / e - - # Clamp t and recompute s if needed - {s, t} = - cond do - t < 0.0 -> - {clamp(-c / a, 0.0, 1.0), 0.0} - - t > 1.0 -> - {clamp((b - c) / a, 0.0, 1.0), 1.0} - - true -> - {s, t} - end - - closest1 = Vec3.add(a1, Vec3.scale(d1, s)) - closest2 = Vec3.add(a2, Vec3.scale(d2, t)) - {closest1, closest2, Vec3.distance(closest1, closest2)} + closest_points_general_case(a1, d1, a2, d2, r, a, e, f) + end + end + + defp closest_points_general_case(a1, d1, a2, d2, r, a, e, f) do + c = Vec3.dot(d1, r) + b = Vec3.dot(d1, d2) + denom = a * e - b * b + + s = compute_initial_s(denom, b, f, c, e) + t = (b * s + f) / e + {s, t} = clamp_segment_params(s, t, a, b, c) + + closest1 = Vec3.add(a1, Vec3.scale(d1, s)) + closest2 = Vec3.add(a2, Vec3.scale(d2, t)) + {closest1, closest2, Vec3.distance(closest1, closest2)} + end + + defp compute_initial_s(denom, b, f, c, e) do + if abs(denom) < 1.0e-10 do + 0.0 + else + clamp((b * f - c * e) / denom, 0.0, 1.0) + end + end + + defp clamp_segment_params(s, t, a, b, c) do + cond do + t < 0.0 -> {clamp(-c / a, 0.0, 1.0), 0.0} + t > 1.0 -> {clamp((b - c) / a, 0.0, 1.0), 1.0} + true -> {s, t} end end @@ -420,7 +407,31 @@ defmodule BB.Collision.Primitives do # Helper Functions - SAT (Separating Axis Theorem) # ============================================================================ - # Test a single axis for the SAT algorithm + defp check_box_axis(axis, t, box1_axes, box1_half, box2_axes, box2_half, current_min) do + if Vec3.magnitude_squared(axis) < 1.0e-10 do + {:cont, current_min} + else + axis_normalized = Vec3.normalise(axis) + + update_min_penetration( + axis_normalized, + t, + box1_axes, + box1_half, + box2_axes, + box2_half, + current_min + ) + end + end + + defp update_min_penetration(axis, t, box1_axes, box1_half, box2_axes, box2_half, current_min) do + case test_axis(axis, t, box1_axes, box1_half, box2_axes, box2_half) do + :separated -> {:halt, :separated} + {:overlap, pen} -> {:cont, min(current_min, pen)} + end + end + defp test_axis(axis, t, {a1x, a1y, a1z}, {h1x, h1y, h1z}, {a2x, a2y, a2z}, {h2x, h2y, h2z}) do # Project the translation vector onto the axis t_proj = abs(Vec3.dot(t, axis)) diff --git a/lib/bb/dsl.ex b/lib/bb/dsl.ex index 1c0c899..df9a158 100644 --- a/lib/bb/dsl.ex +++ b/lib/bb/dsl.ex @@ -361,8 +361,8 @@ defmodule BB.Dsl do A capsule geometry (cylinder with hemispherical caps). The origin of the capsule is the centre of the cylindrical portion. - The length is the distance between the centres of the hemispherical caps. - Total height is length + 2 * radius. + The height is the distance between the centres of the hemispherical caps. + Total extent is height + 2 * radius. """, target: BB.Dsl.Capsule, identifier: {:auto, :unique_integer}, @@ -373,9 +373,9 @@ defmodule BB.Dsl do doc: "The radius of the capsule (cylinder and hemispherical caps)", required: true ], - length: [ + height: [ type: unit_type(compatible: :meter), - doc: "The length of the cylindrical portion (between cap centres)", + doc: "The height of the cylindrical portion (between cap centres)", required: true ] ] diff --git a/lib/bb/dsl/capsule.ex b/lib/bb/dsl/capsule.ex index cd24dac..11344e7 100644 --- a/lib/bb/dsl/capsule.ex +++ b/lib/bb/dsl/capsule.ex @@ -6,9 +6,9 @@ defmodule BB.Dsl.Capsule do @moduledoc """ A capsule geometry (cylinder with hemispherical caps). - Capsules are defined by a radius and length. The length is the distance + Capsules are defined by a radius and height. The height is the distance between the centres of the two hemispherical caps (i.e., the length of - the cylindrical portion). The total height is `length + 2 * radius`. + the cylindrical portion). The total extent is `height + 2 * radius`. Capsules are commonly used for collision detection because they have simpler intersection algorithms than cylinders and better approximate @@ -17,7 +17,7 @@ defmodule BB.Dsl.Capsule do defstruct __identifier__: nil, __spark_metadata__: nil, radius: nil, - length: nil + height: nil alias Cldr.Unit alias Spark.Dsl.Entity @@ -26,6 +26,6 @@ defmodule BB.Dsl.Capsule do __identifier__: any, __spark_metadata__: Entity.spark_meta(), radius: Unit.t(), - length: Unit.t() + height: Unit.t() } end diff --git a/lib/bb/robot/builder.ex b/lib/bb/robot/builder.ex index 01ffee5..53d9526 100644 --- a/lib/bb/robot/builder.ex +++ b/lib/bb/robot/builder.ex @@ -466,7 +466,7 @@ defmodule BB.Robot.Builder do {:capsule, %{ radius: Units.to_meters(capsule.radius), - length: Units.to_meters(capsule.length) + length: Units.to_meters(capsule.height) }} end diff --git a/test/bb/collision/broad_phase_test.exs b/test/bb/collision/broad_phase_test.exs index 0cc59bf..07c83da 100644 --- a/test/bb/collision/broad_phase_test.exs +++ b/test/bb/collision/broad_phase_test.exs @@ -6,7 +6,7 @@ defmodule BB.Collision.BroadPhaseTest do use ExUnit.Case, async: true alias BB.Collision.BroadPhase - alias BB.Math.{Transform, Vec3, Quaternion} + alias BB.Math.{Quaternion, Transform, Vec3} describe "overlap?/2" do test "detects overlapping AABBs" do @@ -70,7 +70,9 @@ defmodule BB.Collision.BroadPhaseTest do test "sphere with translation" do geometry = {:sphere, %{radius: 0.5}} - transform = Transform.from_position_quaternion(Vec3.new(1.0, 2.0, 3.0), Quaternion.identity()) + + transform = + Transform.from_position_quaternion(Vec3.new(1.0, 2.0, 3.0), Quaternion.identity()) {min_pt, max_pt} = BroadPhase.compute_aabb(geometry, transform) @@ -132,7 +134,9 @@ defmodule BB.Collision.BroadPhaseTest do test "capsule with translation" do geometry = {:capsule, %{radius: 0.5, length: 2.0}} - transform = Transform.from_position_quaternion(Vec3.new(5.0, 0.0, 0.0), Quaternion.identity()) + + transform = + Transform.from_position_quaternion(Vec3.new(5.0, 0.0, 0.0), Quaternion.identity()) {min_pt, max_pt} = BroadPhase.compute_aabb(geometry, transform) @@ -197,7 +201,9 @@ defmodule BB.Collision.BroadPhaseTest do test "box with translation" do geometry = {:box, %{x: 0.5, y: 0.5, z: 0.5}} - transform = Transform.from_position_quaternion(Vec3.new(10.0, 20.0, 30.0), Quaternion.identity()) + + transform = + Transform.from_position_quaternion(Vec3.new(10.0, 20.0, 30.0), Quaternion.identity()) {min_pt, max_pt} = BroadPhase.compute_aabb(geometry, transform) @@ -248,7 +254,9 @@ defmodule BB.Collision.BroadPhaseTest do test "mesh with translation" do path = Path.join(@fixtures_dir, "cube.stl") geometry = {:mesh, %{filename: path, scale: 1.0}} - transform = Transform.from_position_quaternion(Vec3.new(5.0, 0.0, 0.0), Quaternion.identity()) + + transform = + Transform.from_position_quaternion(Vec3.new(5.0, 0.0, 0.0), Quaternion.identity()) {min_pt, max_pt} = BroadPhase.compute_aabb(geometry, transform) @@ -258,7 +266,9 @@ defmodule BB.Collision.BroadPhaseTest do test "mesh with non-existent file returns placeholder AABB" do geometry = {:mesh, %{filename: "/nonexistent/model.stl", scale: 1.0}} - transform = Transform.from_position_quaternion(Vec3.new(5.0, 0.0, 0.0), Quaternion.identity()) + + transform = + Transform.from_position_quaternion(Vec3.new(5.0, 0.0, 0.0), Quaternion.identity()) {min_pt, max_pt} = BroadPhase.compute_aabb(geometry, transform) @@ -269,7 +279,9 @@ defmodule BB.Collision.BroadPhaseTest do test "mesh without filename returns placeholder AABB" do geometry = {:mesh, %{other_data: "something"}} - transform = Transform.from_position_quaternion(Vec3.new(5.0, 0.0, 0.0), Quaternion.identity()) + + transform = + Transform.from_position_quaternion(Vec3.new(5.0, 0.0, 0.0), Quaternion.identity()) {min_pt, max_pt} = BroadPhase.compute_aabb(geometry, transform) diff --git a/test/bb/collision/mesh_test.exs b/test/bb/collision/mesh_test.exs index 9defee6..0b4c33f 100644 --- a/test/bb/collision/mesh_test.exs +++ b/test/bb/collision/mesh_test.exs @@ -189,7 +189,9 @@ defmodule BB.Collision.MeshTest do binary_stl = header <> num_triangles <> normal <> v1 <> v2 <> v3 <> attr # Write to temp file - path = Path.join(System.tmp_dir!(), "test_binary_#{:erlang.unique_integer([:positive])}.stl") + path = + Path.join(System.tmp_dir!(), "test_binary_#{:erlang.unique_integer([:positive])}.stl") + File.write!(path, binary_stl) try do diff --git a/test/bb/collision_test.exs b/test/bb/collision_test.exs index a2e9d58..ffbfe19 100644 --- a/test/bb/collision_test.exs +++ b/test/bb/collision_test.exs @@ -7,7 +7,7 @@ defmodule BB.CollisionTest do alias BB.Collision alias BB.ExampleRobots.CollisionTestArm - alias BB.Math.{Vec3, Quaternion} + alias BB.Math.{Quaternion, Vec3} describe "build_adjacency_set/1" do test "builds adjacency set from robot topology" do @@ -76,9 +76,7 @@ defmodule BB.CollisionTest do collisions = Collision.detect_self_collisions(robot, positions, margin: 0.5) - assert length(collisions) >= 1 - - collision = hd(collisions) + assert [collision | _] = collisions assert is_atom(collision.link_a) assert is_atom(collision.link_b) assert is_float(collision.penetration_depth) @@ -201,9 +199,7 @@ defmodule BB.CollisionTest do collisions = Collision.detect_collisions(robot, positions, obstacles) - assert length(collisions) >= 1 - - collision = hd(collisions) + assert [collision | _] = collisions assert collision.link_b == :environment assert is_float(collision.penetration_depth) end @@ -236,7 +232,7 @@ defmodule BB.CollisionTest do # With margin, should detect near-collision collisions = Collision.detect_collisions(robot, positions, obstacles, margin: 0.03) - assert length(collisions) >= 1 + assert [_ | _] = collisions end end end diff --git a/test/fixtures/cube.stl.license b/test/fixtures/cube.stl.license new file mode 100644 index 0000000..0eac483 --- /dev/null +++ b/test/fixtures/cube.stl.license @@ -0,0 +1,3 @@ +SPDX-FileCopyrightText: 2025 James Harton + +SPDX-License-Identifier: Apache-2.0 diff --git a/test/support/example_robots.ex b/test/support/example_robots.ex index 5c874cd..7f7f224 100644 --- a/test/support/example_robots.ex +++ b/test/support/example_robots.ex @@ -20,7 +20,6 @@ defmodule BB.ExampleRobots do """ use BB import BB.Unit - import Kernel, except: [length: 1] settings do name(:collision_test_arm) @@ -61,7 +60,7 @@ defmodule BB.ExampleRobots do capsule do radius(~u(0.03 meter)) - length(~u(0.3 meter)) + height(~u(0.3 meter)) end end @@ -91,7 +90,7 @@ defmodule BB.ExampleRobots do capsule do radius(~u(0.025 meter)) - length(~u(0.2 meter)) + height(~u(0.2 meter)) end end end