Skip to content

Refactor is_close: fix broken overloads, add operator==, centralize eps #69

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
gallantandre merged 2 commits into from
Jun 14, 2026
Merged

Refactor is_close: fix broken overloads, add operator==, centralize eps #69

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
18c5d85
Refactor is_close: fix broken overloads, add operator==, centralize eps
gallantandre Jun 14, 2026
c9a48e2
Address review: eps for limits, guarded Result deref, simpler is_clos…
gallantandre Jun 14, 2026
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
19 changes: 10 additions & 9 deletions blast/blast_math.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -32,7 +32,7 @@ static_assert(sizeof(real) == BLAST_SIZEOF_REAL);

// Constants
constexpr u32 ALIGN = 64;
constexpr real BLAST_EPSILON = 0.00001;
constexpr real BLAST_EPSILON = 0.00001; // default tolerance for is_close / is_small / operator==
constexpr real PI = 3.141592653589793;
#ifdef __CUDA_ARCH__
#if BLAST_USE_DOUBLES
Expand Down Expand Up @@ -75,8 +75,8 @@ inline blast_fn Vec3 cross(Vec3, Vec3);
inline blast_fn Vec3& zero(Vec3&);
inline blast_fn real dot(Vec3, Vec3);
inline blast_fn real norm(Vec3);
inline blast_fn bool is_small(const Vec3&, real eps = 1e-05);
inline blast_fn bool is_close(const Vec3&, const Vec3&, real eps = 1e-05);
inline blast_fn bool is_small(const Vec3&, real eps = BLAST_EPSILON);
inline blast_fn bool is_close(const Vec3&, const Vec3&, real eps = BLAST_EPSILON);
inline blast_fn Vec3& constant(Vec3&, real val);


Expand Down Expand Up @@ -109,8 +109,9 @@ inline blast_fn Mat3& transpose_inplace(Mat3& m);
inline blast_fn Mat3 transpose(Mat3 m);
inline blast_fn Mat3 eye();
inline blast_fn Mat3& constant(Mat3&, real val);
inline blast_fn bool is_close(const Mat3&, const Mat3&, real eps = 1e-05);
inline blast_fn bool is_small(const Mat3&, real eps = 1e-05);
inline blast_fn bool operator==(const Mat3&, const Mat3&);
inline blast_fn bool is_close(const Mat3&, const Mat3&, real eps = BLAST_EPSILON);
inline blast_fn bool is_small(const Mat3&, real eps = BLAST_EPSILON);
inline blast_fn Mat3 rpy2rotation(Vec3 rpy);


Expand Down Expand Up @@ -227,8 +228,8 @@ inline blast_fn real min(const Array&);
inline blast_fn real max(const Array&);
inline blast_fn u32 argmin(const Array&);
inline blast_fn u32 argmax(const Array&);
inline blast_fn bool is_close(const Array&, const Array&, real eps = 1e-05);
inline blast_fn bool is_small(const Array&, real eps = 1e-05);
inline blast_fn bool is_close(const Array&, const Array&, real eps = BLAST_EPSILON);
inline blast_fn bool is_small(const Array&, real eps = BLAST_EPSILON);
inline blast_fn real sum(const Array&);
inline blast_fn real mean(const Array&);
inline blast_fn real norm(const Array&);
Expand Down Expand Up @@ -339,8 +340,8 @@ inline blast_fn Matrix eye(int s);
inline blast_fn Matrix transpose(const Matrix& m);
inline blast_fn Matrix& zero(Matrix&);
inline blast_fn Matrix& constant(Matrix&, real val);
inline blast_fn bool is_close(const Matrix&, const Matrix&, real eps = 1e-05);
inline blast_fn bool is_small(const Matrix&, real eps = 1e-05);
inline blast_fn bool is_close(const Matrix&, const Matrix&, real eps = BLAST_EPSILON);
inline blast_fn bool is_small(const Matrix&, real eps = BLAST_EPSILON);
inline blast_fn real min(const Matrix&);
inline blast_fn real max(const Matrix&);
inline blast_fn std::tuple<u32, u32> argmin(const Matrix&); // return row, column of smallest element
Expand Down
56 changes: 20 additions & 36 deletions blast/blast_utilities.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -69,56 +69,40 @@ inline host_fn blast::Matrix read_csv_matrix_no_header(const std::string& filena

inline host_fn Trajectory read_csv_trajectory_no_header(const std::string& filename, const char* csv_sep);

template<typename T>
host_fn bool is_close(const T type1, const T type2, real eps = 1e-5);

template<typename T, std::size_t N>
host_fn bool is_close(const std::array<T, N>& a1, const std::array<T, N>& a2, real eps = 1e-5);
host_fn bool is_close(const std::array<T, N>& a1, const std::array<T, N>& a2, real eps = BLAST_EPSILON);

template<typename T>
host_fn bool is_close(const ObjMatrix<T>& a1, const ObjMatrix<T>& a2, real eps = 1e-5);
host_fn bool is_close(const ObjMatrix<T>& a1, const ObjMatrix<T>& a2, real eps = BLAST_EPSILON);

template<typename T>
host_fn bool is_close(const std::vector<T>& a1, const std::vector<T>& a2, real eps = 1e-5);

// note: Does not use eps = 1e-5 but necessary for consistency for usability with templates
inline blast_fn bool is_close(const u8 a1, const u8& a2, real eps = 1e-5);

// note: Does not use eps = 1e-5 but necessary for consistency for usability with templates
inline blast_fn bool is_close(u32 a1, u32 a2, real eps = 1e-5);

inline host_fn bool is_close(real r1, real r2, real eps = 1e-5);

inline host_fn bool is_close(const Box& box1, const Box& box2, real eps = 1e-5);

inline host_fn bool is_close(const DynamicBox& box1, const DynamicBox& box2, real eps = 1e-5);

inline host_fn bool is_close(const Sphere& sph1, const Sphere& sph2, real eps = 1e-5);

inline host_fn bool is_close(const DynamicSphere& sph1, const DynamicSphere& sph2, real eps = 1e-5);

inline host_fn bool is_close(const Capsule& capsule1, const Capsule& capsule2, real eps = 1e-5);

inline host_fn bool is_close(const DynamicCapsule& capsule1, const DynamicCapsule& capsule2, real eps = 1e-5);
host_fn bool is_close(const std::vector<T>& a1, const std::vector<T>& a2, real eps = BLAST_EPSILON);

inline host_fn bool is_close(const World& world1, const World& world2, real eps = 1e-5);
inline host_fn bool is_close(real r1, real r2, real eps = BLAST_EPSILON);

inline host_fn bool is_close(const CollisionModelCapsule& capsule1, const CollisionModelCapsule& capsule2, real eps = 1e-5);
inline host_fn bool is_close(const Box& box1, const Box& box2, real eps = BLAST_EPSILON);

inline host_fn bool is_close(const Manipulator& manip1, const Manipulator& manip2, real eps = 1e-5);
inline host_fn bool is_close(const DynamicBox& box1, const DynamicBox& box2, real eps = BLAST_EPSILON);

inline host_fn bool is_close(const ManipulatorTempData& manip_data1, const ManipulatorTempData& manip_data2, const u32 n_joints, const u32 n_caps, real eps = 1e-5);
inline host_fn bool is_close(const Sphere& sph1, const Sphere& sph2, real eps = BLAST_EPSILON);

inline host_fn bool is_close(const Bspline& spline1, const Bspline& spline2, real eps = 1e-5);
inline host_fn bool is_close(const DynamicSphere& sph1, const DynamicSphere& sph2, real eps = BLAST_EPSILON);

inline host_fn bool is_close(const ConstraintSelection& constraints1, const ConstraintSelection& constraints2, real eps = 1e-5);
inline host_fn bool is_close(const Capsule& capsule1, const Capsule& capsule2, real eps = BLAST_EPSILON);

inline host_fn bool is_close(const Objective& objective1, const Objective& objective2, real eps = 1e-5);
inline host_fn bool is_close(const DynamicCapsule& capsule1, const DynamicCapsule& capsule2, real eps = BLAST_EPSILON);

inline host_fn bool is_close(const Guess& guess1, const Guess& guess2, real eps = 1e-5);
inline host_fn bool is_close(const World& world1, const World& world2, real eps = BLAST_EPSILON);

inline host_fn bool is_close(const Optimization& opt1, const Optimization& opt2, real eps = 1e-5);
inline host_fn bool is_close(const ManipulatorTempData& manip_data1, const ManipulatorTempData& manip_data2, const u32 n_joints, const u32 n_caps, real eps = BLAST_EPSILON);

inline host_fn bool is_close(const Result& result1, Result& result2, real eps = 1e-5);
inline host_fn bool operator==(const CollisionModelCapsule& a, const CollisionModelCapsule& b);
inline host_fn bool operator==(const Manipulator& a, const Manipulator& b);
inline host_fn bool operator==(const Bspline& a, const Bspline& b);
inline host_fn bool operator==(const ConstraintSelection& a, const ConstraintSelection& b);
inline host_fn bool operator==(const Objective& a, const Objective& b);
inline host_fn bool operator==(const Guess& a, const Guess& b);
inline host_fn bool operator==(const Optimization& a, const Optimization& b);
inline host_fn bool operator==(const Result& a, const Result& b);

} // namespace blast
27 changes: 27 additions & 0 deletions blast/container/ObjMatrix.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,9 +1,16 @@
#pragma once
#include <blast>
#include <type_traits>
#include <vector>

namespace blast {

// Defined in utilities/is_close.hpp, which is included after this header in the <blast>
// umbrella. Forward-declared so operator== can compare floating-point elements with
// tolerance on strict two-phase-lookup compilers (Clang/GCC). No default arg here: the
// canonical declaration in blast_utilities.hpp supplies eps = BLAST_EPSILON.
inline host_fn bool is_close(real r1, real r2, real eps);

/// @brief A simple matrix class that stores objects of type T. Data are stored in a column-major order.
template<typename T>
struct ObjMatrix {
Expand All @@ -23,6 +30,8 @@ struct ObjMatrix {
inline T& operator()(int r, int c);
inline const T& operator()(int r, int c) const;

inline bool operator==(const ObjMatrix& other) const;

inline void resize(int rows, int cols);

inline std::vector<T> col_copy(int c) const;
Expand Down Expand Up @@ -122,6 +131,24 @@ inline const T& ObjMatrix<T>::operator()(int r, int c) const {
return data[c * rows + r];
}

template<typename T>
inline bool ObjMatrix<T>::operator==(const ObjMatrix& other) const {
if (rows != other.rows || cols != other.cols)
return false;
for (int i = 0; i < size; i++) {
if constexpr (std::is_floating_point_v<T>) {
// Floating-point elements compare with tolerance.
if (!is_close(data[i], other.data[i], BLAST_EPSILON))
return false;
} else {
// Integral and custom types use their own operator==.
if (!(data[i] == other.data[i]))
return false;
}
}
return true;
}

/// @brief Resizes the matrix to the specified number of columns and rows.
/// @param rows Number of rows.
/// @param cols Number of columns.
Expand Down
4 changes: 4 additions & 0 deletions blast/math/Mat3.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -168,6 +168,10 @@ inline blast_fn Mat3& constant(Mat3& m, real val) {
return m;
}

inline blast_fn bool operator==(const Mat3& a, const Mat3& b) {
return is_close(a, b);
}

inline blast_fn bool is_close(const Mat3& a, const Mat3& b, real eps) {
for (int i = 0; i < 9; i++)
if (std::abs(a.data[i] - b.data[i]) > eps)
Expand Down
Loading
Loading