diff --git a/sbnalg/CMakeLists.txt b/sbnalg/CMakeLists.txt index 3ca33b4..c576175 100644 --- a/sbnalg/CMakeLists.txt +++ b/sbnalg/CMakeLists.txt @@ -1,2 +1,3 @@ add_subdirectory(Geometry) +add_subdirectory(Utilities) add_subdirectory(gallery) diff --git a/sbnalg/Utilities/CMakeLists.txt b/sbnalg/Utilities/CMakeLists.txt new file mode 100644 index 0000000..4d7e9f0 --- /dev/null +++ b/sbnalg/Utilities/CMakeLists.txt @@ -0,0 +1,10 @@ +cet_make_library(INTERFACE + SOURCE + "PlaneCrossers.h" + LIBRARIES + larcorealg::Geometry + ) + +install_headers() +install_source() +install_fhicl() diff --git a/sbnalg/Utilities/PlaneCrossers.h b/sbnalg/Utilities/PlaneCrossers.h new file mode 100644 index 0000000..0c74b35 --- /dev/null +++ b/sbnalg/Utilities/PlaneCrossers.h @@ -0,0 +1,247 @@ +/** + * @file sbnalg/Utilities/PlaneCrossers.h + * @brief Defines the `util::PlaneCrossers` algorithm for plane/line intersection. + * @author Gianluca Petrillo (petrillo@slac.stanford.edu) + * @date June 12, 2025 + */ + +#ifndef SBNALG_UTILITIES_PLANECROSSERS_H +#define SBNALG_UTILITIES_PLANECROSSERS_H + +// LArSoft libraries +#include "larcorealg/Geometry/geo_vectors_utils.h" // geo::vect namespace + +// C/C++ standard libraries +#include // std::isinf(), std::abs() +#include // std::numeric_limits +#include // std::decay_t +#include // std::declval(), std::move() + + +//------------------------------------------------------------------------------ +namespace util { + template class PlaneCrossers; +} + +/** + * @brief Algorithm to intersect a plane and a line. + * @tparam Point type representing a point in space + * + * This algorithm computes the intersection between a 2D plane and a 1D straight + * line in a Cartesian metric space. + * + * Example: + * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~{.cpp} + * util::PlaneCrossers plane{ geo::origin(), geo::Xaxis(), 2. * geo::Zaxis() }; + * + * geo::Point_t const lineStart{ 0.0, 1.0, 0.0 }; + * geo::Vector_t const lineDir{ 0.0, 1.0, 1.0 }; + * util::PlaneCrossers::CrossingInfo const crossing = plane(lineStart, lineDir); + * std::cout << "Intersection point: "; + * if (crossing) std::cout << (lineStart + crossing.line * lineDir); + * else std::cout << " undefined."; + * std::cout << std::endl; + * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + * compute the intersection between the x/z plane and a line; it will print + * something like: + * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + * Intersection point: ( 0, 0, -1) + * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + * and the values in `crossing` will be `line = -1`, `u = 0` and `v = -0.5`. + * Note that both the second axis of the plane and the direction of the line + * were not unit vectors, and the coefficients `line` and `v` reflect that. + * Also note that the same intersection point is also returned by + * `plane.intersectionPoint(crossing)`. + * + */ +template +class util::PlaneCrossers { + + public: + + using InnerCoord_t = double; ///< Internally we always use double precision. + + using Point_t = Point; ///< Type of the geometry point. + + /// Type of the direction vector. + using Dir_t = decltype(std::declval() - std::declval()); + + static constexpr unsigned int Dim = geo::vect::dimension(); + + // --------------------------------------------------------------------------- + /// Information on the crossing point. + struct CrossingInfo { + + /// Special value identifying an invalid coefficient. + static constexpr InnerCoord_t Infinite + = std::numeric_limits::infinity(); + + /// Coefficient of the intersection point on the first plane axis (_u_). + InnerCoord_t u = Infinite; + + /// Coefficient of the intersection point on the second plane axis (_v_). + InnerCoord_t v = Infinite; + + /// Coefficient of the intersection point on the line direction. + InnerCoord_t line = Infinite; + + /// @{ + /// Returns whether there is a finite intersection point. + bool present() const noexcept { return !std::isinf(line); } + operator bool() const noexcept { return present(); } + /// @} + + }; // CrossingInfo + + + // --------------------------------------------------------------------------- + /** + * @brief Constructor: sets the plane. + * @param center reference point on the plane + * @param udir direction representing the first axis of the plane (and metric) + * @param vdir direction representing the second axis of the plane (and metric) + * + * The plane is defined by points in the set `center + u * Uaxis + v * Vaxis`, + * with `u` and `v` all possible real numbers. + */ + PlaneCrossers(Point_t center, Dir_t Uaxis, Dir_t Vaxis) + : fCenter{ std::move(center) } + , fUaxis{ std::move(Uaxis) } + , fVaxis{ std::move(Vaxis) } + {} + + // --- BEGIN --- Access to plane information ------------------------------- + /// @name Access to plane information + /// @{ + + /// Returns the reference point of the plane ("center"). + Point_t const& center() const noexcept { return fCenter; } + + /// Returns the vector of the first axis of the plane. + Dir_t const& Uaxis() const noexcept { return fUaxis; } + + /// Returns the vector of the second axis of the plane. + Dir_t const& Vaxis() const noexcept { return fVaxis; } + + /// Returns the normal to the plane (positive-defined with _u_ and _v_). + Dir_t normalAxis() const { return geo::vect::cross(Uaxis(), Vaxis()); } + + /// Returns a displacement from the plane origin by the specified coordinates. + Dir_t displacementOnPlane(InnerCoord_t u, InnerCoord_t v) const + { return u * Uaxis() + v * Vaxis(); } + + /// Returns the point on the plane with the specified coordinates. + Point_t pointOnPlane(InnerCoord_t u, InnerCoord_t v) const + { return center() + displacementOnPlane(u, v); } + + /** + * @brief Returns the point of intersection identified by the specified `crossing`. + * @param crossing a valid result from `this->findCrossing()` + * @return the intersection point represented by `crossing` + * + * The `crossing` result must come from this object (because it assumed its + * plane). + * If `crossing.present()` is `false`, the behaviour is undefined. + */ + Point_t intersectionPoint(CrossingInfo const& crossing) const + { return pointOnPlane(crossing.u, crossing.v); } + + /// @} + // --- END --- Access to plane information ------------------------------- + + + // --- BEGIN --- Algorithm execution --------------------------------------- + /// @name Algorithm execution + /// @{ + + /** + * @brief Returns the intersection of the plane with the specified line. + * @param point the origin of the line + * @param dir the positive direction of the line + * @return the intersection information (converts to `false` if no intersection) + * + * The intersection of the line in the argument and the plane of the object + * is computed and returned. + * If there is no intersection (the plane and line are parallel), the result's + * `present()` method returns `false` and the rest of the information is + * invalid. Otherwise, the returned values include `u`, `v` and `line` so that + * the intersection point can be written as `point + line * dir` and also as + * `center() + u * Uaxis() + v * Vaxis()`. + */ + CrossingInfo findCrossing(Point_t const& point, Dir_t const& dir) const; + + + /// Calls `util::PlaneCrossers::findCrossing()`. + CrossingInfo operator()(Point_t const& point, Dir_t const& dir) const + { return findCrossing(point, dir); } + + + /// @} + // --- END --- Algorithm execution --------------------------------------- + + + private: + + static_assert(Dim == 3, "Currently only dimension 3 is supported."); + + Point_t fCenter; ///< Center (reference point) of the plane. + Dir_t fUaxis; ///< Direction of the first axis of the plane. + Dir_t fVaxis; ///< Direction of the second axis of the plane. + +}; // util::PlaneCrossers + +namespace util { + template + PlaneCrossers(Point, Vector) -> PlaneCrossers>; +} + + +// ----------------------------------------------------------------------------- +// --- template implementation +// ----------------------------------------------------------------------------- +template +auto util::PlaneCrossers::findCrossing + (Point_t const& startPoint, Dir_t const& dir) const -> CrossingInfo +{ + /* + * Plane point: P(u,v) = C + u U + v V + * Line point: P(d) = S + d D (as in Start and Direction) + * + * P(u,v) = P(d) + * <=> C + u U + v V = S + d D + * <=> u U + v V - d D = S - C + * <=> { U, V, D } x { u, v, -d } = { S-C } + * with { U, V, D } = A the matrix with U, V and D as columns, + * { u, v, -d } = X and { S-C } = SminusC two column vectors. + * The following is the solution in X of A X = SminusC. + * + * We solve using Cramer's rule (strictly 3D) and the fact that the + * determinant of column-made matrix { U, V, D } is equivalent to the triple + * product ( U x V , D ). + */ + + // these "directions" are not built to be unit vectors. + Dir_t const& U = Uaxis(); + Dir_t const& V = Vaxis(); + Dir_t const& D = dir; + + CrossingInfo crossing; + + using geo::vect::mixedProduct; + + double const detA = mixedProduct(U, V, D); + if (std::abs(detA) < 1e-5) return {}; // return the default, invalid + + Dir_t const& SminusC = startPoint - center(); + return { + /* .u = */ mixedProduct(SminusC, V, D ) / detA, + /* .v = */ mixedProduct(U, SminusC, D ) / detA, + /* .line = */ -mixedProduct(U, V, SminusC) / detA + }; + +} // util::PlaneCrossers::findCrossing() + + +// ----------------------------------------------------------------------------- + +#endif // SBNALG_UTILITIES_PLANECROSSERS_H diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 06e962f..00e6313 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -5,3 +5,4 @@ include(CetTest) cet_enable_asserts() add_subdirectory(Geometry) +add_subdirectory(Utilities) diff --git a/test/Utilities/CMakeLists.txt b/test/Utilities/CMakeLists.txt new file mode 100644 index 0000000..59e594c --- /dev/null +++ b/test/Utilities/CMakeLists.txt @@ -0,0 +1,8 @@ +cet_test(PlaneCrossers_test USE_BOOST_UNIT + SOURCE PlaneCrossers_test.cxx + LIBRARIES PRIVATE + larcoreobj::headers + larcorealg::Geometry +) + +install_source() diff --git a/test/Utilities/PlaneCrossers_test.cxx b/test/Utilities/PlaneCrossers_test.cxx new file mode 100644 index 0000000..6cfa846 --- /dev/null +++ b/test/Utilities/PlaneCrossers_test.cxx @@ -0,0 +1,263 @@ +/** + * @file PlaneCrossers_test.cc + * @brief Unit test for `util::PlaneCrossers`. + * @date June 13, 2025 + * @author Gianluca Petrillo (petrillo@slac.stanford.edu) + */ + +// Boost test libraries; defining this symbol tells boost somehow to generate +// a main() function; Boost is pulled in by boost_unit_test_base.h +#define BOOST_TEST_MODULE PlaneCrossersTest + +// SBN software +#include "sbnalg/Utilities/PlaneCrossers.h" + +// LArSoft libraries +#include "larcorealg/Geometry/geo_vectors_utils.h" // geo::vect namespace +#include "larcoreobj/SimpleTypesAndConstants/geo_vectors.h" // geo::Point_t + +// Boost libraries +#include + +// C++ standard libraries +#include +#include +#include // std::abs() + + +//------------------------------------------------------------------------------ +//--- The tests +//--- +namespace details { + + template + constexpr bool isZero(T v, T tol = T{1e-8}) { return (v >= -tol) && (v <= tol); } + template + constexpr bool isNonzero(T v, T tol = T{1e-8}) { return !isZero(v, tol); } + + + template + constexpr T cycleUp(T var, T min, T max) { return (++var >= max)? min: var; } + template + constexpr T cycleUp(T var, T max) { return cycleUp(var, T{}, max); } + + template + std::size_t findSmallestCoord(Vector const& v) { + auto it = geo::vect::vector_cbegin(v), cend = geo::vect::vector_cend(v); + auto minCoordVal = std::abs(*it++); + std::size_t minCoord = 0; + for (std::size_t coord = 1; it != cend; ++it, ++coord) { + if (minCoordVal < std::abs(*it)) continue; + minCoord = coord; + minCoordVal = std::abs(*it); + } + return minCoord; + } // findSmallestCoord() + + + geo::Vector_t makeAnOrthogonalVector(geo::Vector_t const& v) { + using Vector_t = geo::Vector_t; + + using geo::vect::dimension, geo::vect::mag2, geo::vect::coord, + geo::vect::dot, geo::vect::normalize; + + constexpr std::size_t dim = dimension(); + + assert(!isZero(geo::vect::mag2(v))); // v must not be null + + // The most important step is to find a direction that is not parallel to v. + // If we exchange the two largest coordinates, we win (unless they are the same) + + // So: find the smallest coordinate, then the other two. + std::size_t smallestCoord = findSmallestCoord(v); + std::size_t largestCoord = cycleUp(smallestCoord, dim); + std::size_t middleCoord = cycleUp(largestCoord, dim); + if (std::abs(coord(v, largestCoord)) < std::abs(coord(v, middleCoord))) + std::swap(middleCoord, largestCoord); + + Vector_t o = v; // orthogonal + if (isZero(coord(v, largestCoord) - coord(v, middleCoord))) { + // the two largest coordinates have the same value; + // that value can't be 0 (or else the smallest would also be 0), + // so setting one to `0` will change the direction of the vector + coord(o, largestCoord) = 0.0; + } + else { + // the two largest coordinates have the different value; + // swapping them will change the direction of the vector + coord(o, largestCoord) = coord(v, middleCoord); + coord(o, middleCoord) = coord(v, largestCoord); + } + + o -= dot(o, v) * v; // remove the component of `o` along `v` + + return geo::vect::normalize(o); + } // makeAnOrthogonalVector() + + + std::array makeOrthogonalBase(geo::Vector_t const& normal) { + using Vector_t = geo::Vector_t; + + Vector_t const Uaxis = makeAnOrthogonalVector(normal); + Vector_t const Vaxis = geo::vect::normalize(geo::vect::cross(normal, Uaxis)); + return { Uaxis, Vaxis, geo::vect::normalize(normal) }; + } // makeOrthogonalBase() + +} // namespace details + + +// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - +void lineTest( + util::PlaneCrossers const& plane, + geo::Point_t const& lineStart, geo::Vector_t const& lineDir +) { + + bool const expectResult = !details::isZero + (geo::vect::mixedProduct(plane.Uaxis(), plane.Vaxis(), lineDir)); + + auto const& crossing = plane(lineStart, lineDir); + + BOOST_TEST(crossing.present() == expectResult); + BOOST_TEST(bool(crossing) == expectResult); + if (crossing && expectResult) { + geo::Point_t const intersectionPlane = plane.intersectionPoint(crossing); + geo::Point_t const intersectionLine = lineStart + crossing.line * lineDir; + + BOOST_CHECK_SMALL(intersectionPlane.X() - intersectionLine.X(), 1e-6); + BOOST_CHECK_SMALL(intersectionPlane.Y() - intersectionLine.Y(), 1e-6); + BOOST_CHECK_SMALL(intersectionPlane.Z() - intersectionLine.Z(), 1e-6); + } + +} // lineTest() + + +void planeTest(geo::Point_t const& center, geo::Vector_t const& dir) { + + auto const& [ Uaxis, Vaxis, planeDir ] = details::makeOrthogonalBase(dir); + + util::PlaneCrossers const plane{ center, Uaxis, Vaxis }; + + std::array const dirSteps { -2.0, -1.0, 0.0, +1.0, +2.0 }; + std::array const posSteps { -1.0, 0.0, +1.0 }; + + BOOST_TEST_CONTEXT( + "Test on plane at " << center << " with normal=" << plane.normalAxis() + << ", u=" << plane.Uaxis() << ", v=" << plane.Vaxis() + ) { + for (double const dirStepX: dirSteps) { + for (double const dirStepY: dirSteps) { + for (double const dirStepZ: dirSteps) { + + geo::Vector_t const lineDir{ dirStepX, dirStepY, dirStepZ }; + if (geo::vect::mag2(lineDir) < 1e-6) continue; + + for (double const posStepX: posSteps) { + for (double const posStepY: posSteps) { + for (double const posStepZ: posSteps) { + + geo::Point_t const lineStart{ posStepX, posStepY, posStepZ }; + + BOOST_TEST_CONTEXT("Test with line at " << lineStart << " with normal " << lineDir) { + lineTest(plane, lineStart, lineDir); + } + + } // for z + } // for y + } // for x + + } // for dz + } // for dy + } // for dx + } // BOOST_TEST_CONTEXT() + +} // planeTest() + + +void gridTest() { + /* + * We test with 124 plane orientations and 27 plane reference points. + * + * Each test will run different lines. + */ + + std::array const dirSteps { -2.0, -1.0, 0.0, +1.0, +2.0 }; + std::array const posSteps { -2.0, 0.0, +2.0 }; + + for (double const dirStepX: dirSteps) { + for (double const dirStepY: dirSteps) { + for (double const dirStepZ: dirSteps) { + + geo::Vector_t const planeDir{ dirStepX, dirStepY, dirStepZ }; + if (geo::vect::mag2(planeDir) < 1e-6) continue; + + for (double const posStepX: posSteps) { + for (double const posStepY: posSteps) { + for (double const posStepZ: posSteps) { + + geo::Point_t const planeReference{ posStepX, posStepY, posStepZ }; + + planeTest(planeReference, planeDir); + + } // for z + } // for y + } // for x + + } // for dz + } // for dy + } // for dx + +} // gridTest() + + +//------------------------------------------------------------------------------ +void PlaneCrossers_classDoc_1() { + + std::cout << "Documentation test (" << __func__ << ")" << std::endl; + std::cout << std::string(80, '-') << std::endl; + + // The code: + // --------------------------------------------------------------------------- + util::PlaneCrossers plane{ geo::origin(), geo::Xaxis(), 2. * geo::Zaxis() }; + + geo::Point_t const lineStart{ 0.0, 1.0, 0.0 }; + geo::Vector_t const lineDir{ 0.0, 1.0, 1.0 }; + util::PlaneCrossers::CrossingInfo const crossing = plane(lineStart, lineDir); + std::cout << "Intersection point: "; + if (crossing) std::cout << (lineStart + crossing.line * lineDir); + else std::cout << " undefined."; + std::cout << std::endl; + // --------------------------------------------------------------------------- + + std::cout << std::string(80, '-') << std::endl; + + static_assert(std::is_same_v>, + "Unexpected deduced type"); + + // the promise: ( 0, 0, -1) + geo::Point_t const expectedCrossingPoint{ 0, 0, -1 }; + BOOST_TEST(bool(crossing)); + if (crossing) { + geo::Point_t const crossingPoint = lineStart + crossing.line * lineDir; + BOOST_CHECK_SMALL(crossingPoint.X() - expectedCrossingPoint.X(), 1e-6); + BOOST_CHECK_SMALL(crossingPoint.Y() - expectedCrossingPoint.Y(), 1e-6); + BOOST_CHECK_SMALL(crossingPoint.Z() - expectedCrossingPoint.Z(), 1e-6); + } + +} // PlaneCrossers_classDoc_1() + + +//------------------------------------------------------------------------------ +BOOST_AUTO_TEST_CASE(GridTestCase) +{ + gridTest(); +} + + +//------------------------------------------------------------------------------ +BOOST_AUTO_TEST_CASE(DocumentationTestCase) +{ + PlaneCrossers_classDoc_1(); +} + + +//------------------------------------------------------------------------------