fast_floor.hpp

// SPDX-FileCopyrightText: Copyright 2024 Kenji Koide
// SPDX-License-Identifier: MIT
#pragma once
 
#include <Eigen/Core>
 
namespace gtsam_points {
 
inline Eigen::Array4i fast_floor(const Eigen::Array4d& pt) {
  const Eigen::Array4i ncoord = pt.cast<int>();
  return ncoord - (pt < ncoord.cast<double>()).cast<int>();
};
 
}  // namespace gtsam_points