def accessibility_cost_cuboid_penetration(
    scene: trimesh.Scene,
    a: Union[str, list[str]],
    b: Union[str, list[str]],
    normal_dir: np.ndarray,
    dist: float,
    bvh_cache: dict = None,
    vis=False,
):
    """
    Extrude the bbox of a by dist in the direction of normal_dir, and check for collisions with b
    Return the maximum distance that any part of b penetrates this extrusion
    """

    if isinstance(a, str):
        a = [a]
    if isinstance(b, str):
        b = [b]

    if len(a) == 0 or len(b) == 0:
        return 0

    a_free_col = trimesh.collision.CollisionManager()

    # find which of +X, -X +Y, -Y, +Z, -Z is the normal_dir. Only these values are supported
    if (
        not np.isclose(np.linalg.norm(normal_dir), 1)
        or np.isclose(normal_dir, 0).sum() != 2
    ):
        raise ValueError(
            f"Invalid normal_dir {normal_dir=}, expected +X, -X, +Y, -Y, +Z, -Z"
        )
    normal_axis = np.argmax(np.abs(normal_dir))
    normal_sign = np.sign(normal_dir[normal_axis])

    visobjs = []
    for name in a:
        T, g = scene.graph[name]
        geom = scene.geometry[g]

        # create an extrusion of the bbox by dist in the direction of normal_dir
        bpy_obj = bpy.data.objects[name]

        freespace_exts = np.copy(np.array(bpy_obj.dimensions))
        freespace_exts[normal_axis] = dist
        freespace_box = trimesh.creation.box(freespace_exts)

        bbox = np.array(bpy_obj.bound_box)
        origin_to_bbox_center = bbox.mean(axis=0)
        extent_from_real_origin = bbox[0 if normal_sign < 0 else -1][normal_axis]

        offset_vec = normal_dir * (
            dist / 2 + extent_from_real_origin - origin_to_bbox_center[normal_axis]
        )
        total_offset_vec = origin_to_bbox_center + offset_vec

        freespace_box_transform = np.array(
            bpy_obj.matrix_world
        ) @ trimesh.transformations.translation_matrix(total_offset_vec)

        a_free_col.add_object(name, freespace_box, freespace_box_transform)

        visobjs.append(geom.apply_transform(T))

        visobjs.append(freespace_box.apply_transform(freespace_box_transform))

    b_col = iu.col_from_subset(scene, b, bvh_cache=bvh_cache)
    hit, contacts = b_col.in_collision_other(a_free_col, return_data=True)

    if vis:
        bobjs = iu.meshes_from_names(scene, b)
        print(
            f"{np.round(origin_to_bbox_center, 3)=} {extent_from_real_origin} {bpy_obj.dimensions}"
        )
        if not all(name in _accessibility_vis_seen_objs for name in a + b):
            trimesh.Scene(visobjs + bobjs).show()
        _accessibility_vis_seen_objs.update(a + b)

    if hit:
        return max(c.depth for c in contacts)
    else:
        return 0

    Extrude the bbox of a by dist in the direction of normal_dir, and check for collisions with b
    Return the maximum distance that any part of b penetrates this extrusion
    
infinigen/core/constraints/evaluator/node_impl/trimesh_geometry.py