Skip to content

Move Aabb and Obb out of internal namespace #23120

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 5 commits into from
Jun 30, 2025
Merged

Move Aabb and Obb out of internal namespace #23120

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
4cde47a
Move Aabb and Obb out of internal namespace
xuchen-han Jun 23, 2025
c9b12e3
Add python bindings
xuchen-han Jun 24, 2025
b3ec14f
typo
xuchen-han Jun 25, 2025
01708bc
Move new bindings into its own cc file
xuchen-han Jun 27, 2025
0849705
Address review comments
xuchen-han Jun 30, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
10 changes: 4 additions & 6 deletions geometry/proximity/aabb.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,6 @@

namespace drake {
namespace geometry {
namespace internal {

using Eigen::Matrix3d;
using Eigen::Vector3d;
Expand All @@ -29,7 +28,7 @@ bool Aabb::HasOverlap(const Aabb& a_G, const Aabb& b_H,
= X_GH * b_H.center() - a_G.center() */
const RigidTransformd X_AB(X_GH.rotation(),
X_GH * b_H.center() - a_G.center());
return BoxesOverlap(a_G.half_width(), b_H.half_width(), X_AB);
return internal::BoxesOverlap(a_G.half_width(), b_H.half_width(), X_AB);
}

bool Aabb::HasOverlap(const Aabb& aabb_G, const Obb& obb_H,
Expand All @@ -47,17 +46,17 @@ bool Aabb::HasOverlap(const Aabb& aabb_G, const Obb& obb_H,
const RigidTransformd X_AO(
X_GH.rotation() * obb_H.pose().rotation(),
X_GH * obb_H.pose().translation() - aabb_G.center());
return BoxesOverlap(aabb_G.half_width(), obb_H.half_width(), X_AO);
return internal::BoxesOverlap(aabb_G.half_width(), obb_H.half_width(), X_AO);
}

template <typename MeshType>
Aabb AabbMaker<MeshType>::Compute() const {
auto itr = vertices_.begin();
Vector3d max_bounds = convert_to_double(mesh_M_.vertex(*itr));
Vector3d max_bounds = internal::convert_to_double(mesh_M_.vertex(*itr));
Vector3d min_bounds = max_bounds;
++itr;
for (; itr != vertices_.end(); ++itr) {
const Vector3d& vertex = convert_to_double(mesh_M_.vertex(*itr));
const Vector3d& vertex = internal::convert_to_double(mesh_M_.vertex(*itr));
// Compare its extent along each of the 3 axes.
min_bounds = min_bounds.cwiseMin(vertex);
max_bounds = max_bounds.cwiseMax(vertex);
Expand All @@ -72,6 +71,5 @@ template class AabbMaker<TriangleSurfaceMesh<AutoDiffXd>>;
template class AabbMaker<VolumeMesh<double>>;
template class AabbMaker<VolumeMesh<AutoDiffXd>>;

} // namespace internal
} // namespace geometry
} // namespace drake
43 changes: 23 additions & 20 deletions geometry/proximity/aabb.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -14,12 +14,16 @@ namespace internal {

// Forward declarations.
template <typename>
class AabbMaker;
template <typename>
class BvhUpdater;

} // namespace internal

// Forward declarations.
template <typename>
class AabbMaker;
class Obb;

/* Axis-aligned bounding box. The box is defined in a canonical frame B such
/** Axis-aligned bounding box. The box is defined in a canonical frame B such
that it is centered on Bo and its extents are aligned with B's axes. However,
the box is posed in a hierarchical frame H. Because this is an _axis-aligned_
bounding box, `R_HB = I`. Therefore the pose of the box is completely captured
Expand All @@ -39,11 +43,11 @@ class Aabb {
public:
DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(Aabb);

/* The class used for various creation operations on this bounding volume. */
/** The class used for various creation operations on this bounding volume. */
template <typename MeshType>
using Maker = AabbMaker<MeshType>;

/* Constructs an axis-aligned bounding box measured and expressed in frame H.
/** Constructs an axis-aligned bounding box measured and expressed in frame H.

@param p_HoBo_H The position vector from the hierarchy frame's origin to
the box's canonical origin, expressed in frame H. The
Expand All @@ -60,18 +64,18 @@ class Aabb {
DRAKE_DEMAND(half_width.z() >= 0.0);
}

/* Returns the center of the box -- equivalent to the position vector from
/** Returns the center of the box -- equivalent to the position vector from
the hierarchy frame's origin Ho to `this` box's origin Bo: `p_HoBo_H`. */
const Vector3<double>& center() const { return center_; }

/* Returns the half_width. */
/** Returns the half_width. */
const Vector3<double>& half_width() const { return half_width_; }

/* The point on the axis-aligned box with the smallest measures along the Bx-,
By-, and Bz-directions. */
/** The point on the axis-aligned box with the smallest measures along the
Bx-, By-, and Bz-directions. */
Vector3<double> lower() const { return center_ - half_width_; }

/* The point on the axis-aligned box with the largest measures along the Bx-,
/** The point on the axis-aligned box with the largest measures along the Bx-,
By-, and Bz-directions. */
Vector3<double> upper() const { return center_ + half_width_; }

Expand All @@ -86,17 +90,17 @@ class Aabb {
// defined inside the Bvh. I'll need to do some refactoring so that I don't
// get a circular dependency or any such thing (Or do things in a weird
// template-y fashion).
/* Returns the pose X_HB of the box frame B in the hierarchy frame H. */
/** Returns the pose X_HB of the box frame B in the hierarchy frame H. */
math::RigidTransformd pose() const { return math::RigidTransformd{center_}; }

/* @return Volume of the bounding box. */
/** @return Volume of the bounding box. */
double CalcVolume() const {
// Double the three half widths using * 8 instead of repeating * 2 three
// times to help the compiler out.
return half_width_[0] * half_width_[1] * half_width_[2] * 8;
}

/* Reports whether the two axis-aligned bounding boxes `a_G` and `b_H`
/** Reports whether the two axis-aligned bounding boxes `a_G` and `b_H`
intersect. The poses of `a_G` and `b_H` are defined in their corresponding
hierarchy frames G and H, respectively.

Expand All @@ -108,7 +112,7 @@ class Aabb {
static bool HasOverlap(const Aabb& a_G, const Aabb& b_H,
const math::RigidTransformd& X_GH);

/* Reports whether axis-aligned bounding box `aabb_G` intersects the given
/** Reports whether axis-aligned bounding box `aabb_G` intersects the given
oriented bounding box `obb_H`. The poses of `aabb_G` and `obb_H` are defined
in their corresponding hierarchy frames G and H, respectively.

Expand All @@ -123,7 +127,7 @@ class Aabb {
// TODO(SeanCurtis-TRI): Support collision with primitives as appropriate
// (see obb.h for an example).

/* Compares the values of the two Aabb instances for exact equality down to
/** Compares the values of the two Aabb instances for exact equality down to
the last bit. Assumes that the quantities are measured and expressed in
the same frame. */
bool Equal(const Aabb& other) const {
Expand All @@ -134,7 +138,7 @@ class Aabb {
private:
// Allow the BvhUpdater access to set_bounds().
template <typename>
friend class BvhUpdater;
friend class internal::BvhUpdater;

// Provides access to the BvhUpdater to refit the Aabb. Sets the extents of
// the bounding box based on a box spanned by the given `lower` and `upper`
Expand All @@ -151,7 +155,7 @@ class Aabb {
Vector3<double> half_width_;
};

/* %AabbMaker implements the logic to fit an Aabb to a collection of points.
/** %AabbMaker implements the logic to fit an Aabb to a collection of points.
The points are the position of a subset of verties in a mesh. The Aabb will
be measured and expressed in the same frame as the mesh.

Expand All @@ -165,7 +169,7 @@ class AabbMaker {
public:
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(AabbMaker);

/* Constructs the maker with the reference mesh and the subset of vertices to
/** Constructs the maker with the reference mesh and the subset of vertices to
fit (indicated by corresponding index).

@param mesh_M The mesh frame M.
Expand All @@ -177,7 +181,7 @@ class AabbMaker {
DRAKE_DEMAND(vertices_.size() > 0);
}

/* Computes the bounding volume of the vertices specified in the constructor.
/** Computes the bounding volume of the vertices specified in the constructor.
@retval aabb_M The axis-aligned bounding box posed in mesh frame M. */
Aabb Compute() const;

Expand All @@ -186,6 +190,5 @@ class AabbMaker {
const std::set<int>& vertices_;
};

} // namespace internal
} // namespace geometry
} // namespace drake
16 changes: 7 additions & 9 deletions geometry/proximity/obb.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,6 @@

namespace drake {
namespace geometry {
namespace internal {

using Eigen::Matrix3d;
using Eigen::Vector3d;
Expand All @@ -33,7 +32,7 @@ bool Obb::HasOverlap(const Obb& a, const Obb& b, const RigidTransformd& X_GH) {
const RigidTransformd& X_GA = a.pose();
const RigidTransformd& X_HB = b.pose();
const RigidTransformd X_AB = X_GA.InvertAndCompose(X_GH * X_HB);
return BoxesOverlap(a.half_width(), b.half_width(), X_AB);
return internal::BoxesOverlap(a.half_width(), b.half_width(), X_AB);
}

bool Obb::HasOverlap(const Obb& obb_G, const Aabb& aabb_H,
Expand All @@ -51,10 +50,10 @@ bool Obb::HasOverlap(const Obb& obb_G, const Aabb& aabb_H,
const RigidTransformd X_HG = X_GH.inverse();
const RotationMatrixd R_AO = X_HG.rotation() * obb_G.pose().rotation();
const RigidTransformd X_AO(R_AO, X_HG * obb_G.center() - aabb_H.center());
return BoxesOverlap(aabb_H.half_width(), obb_G.half_width(), X_AO);
return internal::BoxesOverlap(aabb_H.half_width(), obb_G.half_width(), X_AO);
}

bool Obb::HasOverlap(const Obb& bv, const Plane<double>& plane_P,
bool Obb::HasOverlap(const Obb& bv, const internal::Plane<double>& plane_P,
const math::RigidTransformd& X_PH) {
// We want the two corners of the box that lie at the most extreme extents in
// the plane's normal direction. Then we can determine their heights
Expand Down Expand Up @@ -176,13 +175,13 @@ RotationMatrixd ObbMaker<MeshType>::CalcOrientationByPca() const {
// C is for centroid.
Vector3d p_MC = Vector3d::Zero();
for (int v : vertices_) {
p_MC += convert_to_double(mesh_M_.vertex(v));
p_MC += internal::convert_to_double(mesh_M_.vertex(v));
}
p_MC *= one_over_n;

Matrix3d covariance_M = Matrix3d::Zero();
for (int v : vertices_) {
const Vector3d& p_MV = convert_to_double(mesh_M_.vertex(v));
const Vector3d& p_MV = internal::convert_to_double(mesh_M_.vertex(v));
const Vector3d p_CV_M = p_MV - p_MC;
// covariance_M is a symmetric matrix because it's a sum of the
// 3x3 symmetric matrices V*Vᵀ of column vectors V.
Expand Down Expand Up @@ -276,7 +275,7 @@ Obb ObbMaker<MeshType>::CalcOrientedBox(const RotationMatrixd& R_MB) const {
for (int v : vertices_) {
// Since frame F is a rotation of frame M with the same origin, we can use
// the rotation R_FM for the transform X_FM.
const Vector3d p_FV = R_FM * convert_to_double(mesh_M_.vertex(v));
const Vector3d p_FV = R_FM * internal::convert_to_double(mesh_M_.vertex(v));
p_FL = p_FL.cwiseMin(p_FV);
p_FU = p_FU.cwiseMax(p_FV);
}
Expand Down Expand Up @@ -397,10 +396,9 @@ template class ObbMaker<VolumeMesh<double>>;
// TODO(SeanCurtis-TRI): Remove support for building a Bvh on an AutoDiff-valued
// mesh after we've cleaned up the scalar types in hydroelastics. Specifically,
// this is here to support the unit tests in mesh_intersection_test.cc. Also
// the calls to convert_to_double should be removed.
// the calls to internal::convert_to_double should be removed.
template class ObbMaker<TriangleSurfaceMesh<drake::AutoDiffXd>>;
template class ObbMaker<VolumeMesh<drake::AutoDiffXd>>;

} // namespace internal
} // namespace geometry
} // namespace drake
41 changes: 22 additions & 19 deletions geometry/proximity/obb.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,6 @@

namespace drake {
namespace geometry {
namespace internal {

// Forward declarations.
template <typename>
Expand All @@ -38,11 +37,12 @@ class Obb {
public:
DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(Obb);

/** The class used for various creation operations on this bounding volume. */
/** The class used for various creation operations on this bounding volume.
*/
template <typename MeshType>
using Maker = ObbMaker<MeshType>;

/* Constructs an oriented bounding box measured and expressed in frame H.
/** Constructs an oriented bounding box measured and expressed in frame H.

@param X_HB The pose of the box in the hierarchy frame H.
The box is centered on Bo and aligned with Bx, By,
Expand All @@ -53,26 +53,26 @@ class Obb {
*/
Obb(const math::RigidTransformd& X_HB, const Vector3<double>& half_width);

/* Returns the center of the box -- equivalent to the position vector from
/** Returns the center of the box -- equivalent to the position vector from
the hierarchy frame's origin Ho to `this` box's origin Bo: `p_HoBo_H`. */
const Vector3<double>& center() const { return pose_.translation(); }

/* Returns the half_width -- equivalent to the position vector from the
/** Returns the half_width -- equivalent to the position vector from the
box's center Bo to the box's first octant (+,+,+) corner U expressed in
the box's frame B: `p_BoU_B`. */
const Vector3<double>& half_width() const { return half_width_; }

/* Returns the pose X_HB of the box frame B in the hierarchy frame H */
/** Returns the pose X_HB of the box frame B in the hierarchy frame H */
const math::RigidTransformd& pose() const { return pose_; }

/* @return Volume of the bounding box. */
/** @return Volume of the bounding box. */
double CalcVolume() const {
// Double the three half widths using * 8 instead of repeating * 2 three
// times to help the compiler out.
return half_width_[0] * half_width_[1] * half_width_[2] * 8;
}

/* Reports whether the two oriented bounding boxes `a_G` and `b_H` intersect.
/** Reports whether the two oriented bounding boxes `a_G` and `b_H` intersect.
The poses of `a_G` and `b_H` are defined in their corresponding hierarchy
frames G and H, respectively.

Expand All @@ -84,7 +84,7 @@ class Obb {
static bool HasOverlap(const Obb& a_G, const Obb& b_H,
const math::RigidTransformd& X_GH);

/* Reports whether oriented bounding box `obb_G` intersects the given
/** Reports whether oriented bounding box `obb_G` intersects the given
axis-aligned bounding box `aabb_H`. The poses of `obb_G` and `aabb_H` are
defined in their corresponding hierarchy frames G and H, respectively.

Expand All @@ -96,9 +96,12 @@ class Obb {
static bool HasOverlap(const Obb& obb_G, const Aabb& aabb_H,
const math::RigidTransformd& X_GH);

/* Checks whether bounding volume `bv` intersects the given plane. The
bounding volume is centered on its canonical frame B, and B is posed in the
corresponding hierarchy frame H. The plane is defined in frame P.
// TODO(xuchenhan-tri): Move Plane out of internal namespace and make this
// non-internal only.
/** (Internal use only) Checks whether bounding volume `bv` intersects the
given plane. The bounding volume is centered on its canonical frame B, and B
is posed in the corresponding hierarchy frame H. The plane is defined in
frame P.

The box and plane intersect if _any_ point within the bounding volume has
zero height (see CalcHeight()).
Expand All @@ -111,10 +114,11 @@ class Obb {
@param X_PH The relative pose between the hierarchy frame H and the
plane frame P.
@returns `true` if the plane intersects the box. */
static bool HasOverlap(const Obb& bv_H, const Plane<double>& plane_P,
static bool HasOverlap(const Obb& bv_H,
const internal::Plane<double>& plane_P,
const math::RigidTransformd& X_PH);

/* Checks whether bounding volume `bv` intersects the given half space. The
/** Checks whether bounding volume `bv` intersects the given half space. The
bounding volume is centered on its canonical frame B, and B is posed in the
corresponding hierarchy frame H. The half space is defined in its
canonical frame C (such that the boundary plane of the half space is
Expand All @@ -131,7 +135,7 @@ class Obb {
static bool HasOverlap(const Obb& bv_H, const HalfSpace& hs_C,
const math::RigidTransformd& X_CH);

/* Compares the values of the two Obb instances for exact equality down to
/** Compares the values of the two Obb instances for exact equality down to
the last bit. Assumes that the quantities are measured and expressed in
the same frame. */
bool Equal(const Obb& other) const {
Expand Down Expand Up @@ -165,7 +169,7 @@ class Obb {
template <typename MeshType>
class ObbMakerTester;

/* %ObbMaker performs an algorithm to create an oriented bounding box that
/** %ObbMaker performs an algorithm to create an oriented bounding box that
fits a specified set of vertices in a mesh.

@tparam MeshType is either TriangleSurfaceMesh<T> or VolumeMesh<T>, where T is
Expand All @@ -178,7 +182,7 @@ class ObbMaker {
// and unique before passing it to ObbMaker. Repeated vertices can harm
// PCA and slow down the rest of ObbMaker.

/* Specifies the input mesh with frame M and a set of vertices to fit.
/** Specifies the input mesh with frame M and a set of vertices to fit.
@param mesh_M The mesh that owns the vertices expressed in frame M.
@param vertices The vertices to fit.
@pre `vertices` is not empty, and each of its entry is in the
Expand All @@ -188,7 +192,7 @@ class ObbMaker {
DRAKE_DEMAND(vertices_.size() > 0);
}

/* Computes the bounding volume of the vertices specified in the constructor.
/** Computes the bounding volume of the vertices specified in the constructor.
@retval obb_M The oriented bounding box posed in frame M. */
Obb Compute() const;

Expand Down Expand Up @@ -236,6 +240,5 @@ class ObbMaker {
friend class ObbMakerTester<MeshType>;
};

} // namespace internal
} // namespace geometry
} // namespace drake
Loading