435 lines
21 KiB
C++
435 lines
21 KiB
C++
// ----------------------------------------------------------------------------
|
|
// - Open3D: www.open3d.org -
|
|
// ----------------------------------------------------------------------------
|
|
// Copyright (c) 2018-2023 www.open3d.org
|
|
// SPDX-License-Identifier: MIT
|
|
// ----------------------------------------------------------------------------
|
|
|
|
#pragma once
|
|
|
|
#include <Eigen/Core>
|
|
#include <Eigen/Geometry>
|
|
#include <limits>
|
|
|
|
#include "open3d/geometry/BoundingVolume.h"
|
|
#include "open3d/geometry/Geometry.h"
|
|
#include "open3d/utility/Optional.h"
|
|
|
|
#pragma once
|
|
|
|
namespace open3d {
|
|
namespace geometry {
|
|
|
|
/// \class Line3D
|
|
///
|
|
/// \brief Line3D is a class which derives from
|
|
/// Eigen::ParametrizedLine<double, 3> in order to capture the semantic
|
|
/// differences between a "line", "ray", and "line segment" for operations in
|
|
/// which the difference is important, such as intersection and distance tests.
|
|
/// The underlying Eigen object can always be retrieved with the .Line() method.
|
|
///
|
|
/// \details The taxonomy of the Line3D class and its derived classes, Ray3D
|
|
/// and Segment3D, was created in order to find a compromise between the
|
|
/// goals of providing an easy-to-use API, enforcing obvious correctness
|
|
/// when dealing with operations in which line semantics are important, and
|
|
/// maintaining reasonably high performance.
|
|
///
|
|
/// The underlying motivation is to enforce correctness when performing line
|
|
/// operations based on Eigen::ParametrizedLine<double, 3> even as subtleties
|
|
/// about how a line is represented begin to affect the outcomes of different
|
|
/// operations. Some performance is sacrificed in the use of virtual functions
|
|
/// for a clean API in cases where the compiler cannot determine at compile time
|
|
/// which function will be called and cannot de-virtualize the call.
|
|
///
|
|
/// In such cases where performance is extremely important, avoid iterating
|
|
/// through a list of derived objects stored as Line3D and calling virtual
|
|
/// functions on them so that the compiler can hopefully remove the vtable
|
|
/// lookup, or consider a hand implementation of your problem in which you
|
|
/// carefully account for the semantics yourself.
|
|
class Line3D : protected Eigen::ParametrizedLine<double, 3> {
|
|
public:
|
|
/// \brief Creates a line through two points. The line origin will take the
|
|
/// value of p0, and the line direction will be a normalized vector from
|
|
/// p0 to p1
|
|
static Line3D Through(const Eigen::Vector3d& p0,
|
|
const Eigen::Vector3d& p1) {
|
|
return {p0, (p1 - p0).normalized()};
|
|
}
|
|
|
|
/// \enum LineType
|
|
///
|
|
/// \brief Specifies different semantic interpretations of 3d lines
|
|
enum class LineType {
|
|
/// Lines extend infinitely in both directions
|
|
Line = 0,
|
|
|
|
/// Rays have an origin and a direction, and extend to infinity in
|
|
/// that direction
|
|
Ray = 1,
|
|
|
|
/// Segments have both an origin and an endpoint and are finite in
|
|
/// nature
|
|
Segment = 2,
|
|
};
|
|
|
|
/// \brief Default user constructor
|
|
Line3D(const Eigen::Vector3d& origin, const Eigen::Vector3d& direction);
|
|
|
|
virtual ~Line3D() = default;
|
|
|
|
/// \brief Gets the semantic type of the line
|
|
LineType GetLineType() const { return line_type_; }
|
|
|
|
/// \brief Gets the line's origin point
|
|
const Eigen::Vector3d& Origin() const { return m_origin; }
|
|
|
|
/// \brief Gets the line's direction vector
|
|
const Eigen::Vector3d& Direction() const { return m_direction; }
|
|
|
|
/// \brief Gets the length of the line, which for lines and rays will return
|
|
/// positive infinity, but for segments will return a finite positive value.
|
|
virtual double Length() const {
|
|
return std::numeric_limits<double>::infinity();
|
|
}
|
|
|
|
/// \brief Transform the Line3D by the given matrix
|
|
virtual void Transform(const Eigen::Transform<double, 3, Eigen::Affine>& t);
|
|
|
|
/// \brief Returns a const reference to the underlying
|
|
/// Eigen::ParametrizedLine object
|
|
const Eigen::ParametrizedLine<double, 3>& Line() const { return *this; }
|
|
|
|
/// \brief Calculates the intersection parameter between the line and a
|
|
/// plane taking into account line semantics. Returns an empty result if
|
|
/// there is no intersection. On a Line3D this returns the same result as
|
|
/// .Line().intersectionParameter(plane)
|
|
virtual utility::optional<double> IntersectionParameter(
|
|
const Eigen::Hyperplane<double, 3>& plane) const;
|
|
|
|
/// \brief Calculates the parameter of a point projected onto the line
|
|
/// taking into account special semantics.
|
|
///
|
|
/// \details On a Line3D this is the point directly projected onto the
|
|
/// infinite line, and represents the closest point on the line to the test
|
|
/// point. A negative value indicates the projection lies behind the origin,
|
|
/// a positive value is in front of the origin. On a Ray3D this will be a
|
|
/// positive value only, since rays do not exist in the negative direction.
|
|
/// On a Segment3D this will be a positive value which is less than or equal
|
|
/// to the segment length.
|
|
double ProjectionParameter(const Eigen::Vector3d& point) const;
|
|
|
|
/// \brief Calculates a point projected onto the line, taking into account
|
|
/// special semantics.
|
|
///
|
|
/// \details On a Line3D this is the point directly projected onto
|
|
/// the infinite line, and represents the closest point on the line to the
|
|
/// test point. On a Ray3D this will either be a point on the ray's
|
|
/// positive direction or the ray origin, whichever is closer. On a
|
|
/// Segment3D this will be either one of the segment's endpoints or a point
|
|
/// between them, whichever is closest to the test point.
|
|
virtual Eigen::Vector3d Projection(const Eigen::Vector3d& point) const;
|
|
|
|
/// \brief Returns the lower intersection parameter for a line with an
|
|
/// axis aligned bounding box or empty if no intersection. Uses the slab
|
|
/// method, see warning below.
|
|
///
|
|
/// \details Calculates the lower intersection parameter of a parameterized
|
|
/// line with an axis aligned bounding box. The intersection point can be
|
|
/// recovered with .Line().pointAt(...). If the line does not intersect the
|
|
/// box the optional return value will be empty. Also note that if the AABB
|
|
/// is behind the line's origin point, the value returned will still be of
|
|
/// the lower intersection, which is the first intersection in the direction
|
|
/// of the line, not the intersection closer to the origin.
|
|
///
|
|
/// This implementation is based off of Tavian Barnes' optimized branchless
|
|
/// slab method. https://tavianator.com/2011/ray_box.html. It runs in
|
|
/// roughly 5% of the time as the the naive exact method, but can degenerate
|
|
/// in specific conditions where a line lies exactly in one of the AABB's
|
|
/// planes.
|
|
///
|
|
/// \warning A line that lies exactly in one of the AABB's planes within the
|
|
/// double floating point precision will not intersect correctly by this
|
|
/// method
|
|
virtual utility::optional<double> SlabAABB(
|
|
const AxisAlignedBoundingBox& box) const;
|
|
|
|
/// \brief Returns the lower intersection parameter for a line with an
|
|
/// axis aligned bounding box or empty if no intersection. This method is
|
|
/// about 20x slower than the slab method, see details to know when to use.
|
|
///
|
|
/// \details Calculates the lower intersection parameter of a parameterized
|
|
/// line with an axis aligned bounding box. The intersection point can be
|
|
/// recovered with .Line().pointAt(...). If the line does not intersect the
|
|
/// box the return value will be empty. Also note that if the AABB is behind
|
|
/// the line's origin point, the value returned will still be of the lower
|
|
/// intersection, which is the first intersection in the direction of the
|
|
/// line, not the intersection closer to the origin.
|
|
///
|
|
/// This implementation is a naive exact method that considers intersections
|
|
/// with all six bounding box planes. It is not optimized for speed and
|
|
/// should only be used when a problem is conditioned such that the slab
|
|
/// method is unacceptable. Use this when a line is likely to lie exactly
|
|
/// in one of the AABB planes and false negatives are unacceptable.
|
|
/// Typically this will only happen when lines are axis-aligned and both
|
|
/// lines and bounding volumes are regularly spaced, and every intersection
|
|
/// is important. In such cases if performance is important, a simple
|
|
/// custom implementation based on the problem directionality will likely
|
|
/// outperform even the slab method.
|
|
virtual utility::optional<double> ExactAABB(
|
|
const AxisAlignedBoundingBox& box) const;
|
|
|
|
/// \brief Computes the two corresponding parameters of the closest distance
|
|
/// between two Line3D objects, including derived types Ray3D and Segment3D,
|
|
/// respecting the semantics of the line type.
|
|
std::pair<double, double> ClosestParameters(const Line3D& other) const;
|
|
|
|
/// \brief Computes the two closest points between this Line3D object and
|
|
/// the other, including of derived types Ray3D and Segment3D, respecting
|
|
/// the semantics of the line types.
|
|
std::pair<Eigen::Vector3d, Eigen::Vector3d> ClosestPoints(
|
|
const Line3D& other) const;
|
|
|
|
/// \brief Gets the closest distance between two Line3D objects, including
|
|
/// derived types Ray3D and Segment3D, respecting the semantics of the line
|
|
/// type.
|
|
double DistanceTo(const Line3D& other) const;
|
|
|
|
/// \brief Clamps/bounds a parameter value to the closest valid place where
|
|
/// the entity exists. On a Line3D, the value will be unchanged, on a Ray3D
|
|
/// a negative value will be made 0, and on a Segment3D a negative value
|
|
/// will be made 0 and a positive value greater than Length() will take
|
|
/// the value of Length()
|
|
virtual double ClampParameter(double parameter) const { return parameter; }
|
|
|
|
/// \brief Verifies that a given parameter value is valid for the semantics
|
|
/// of the line object. For lines, any parameter is valid, for rays any
|
|
/// positive parameter is valid, and for segments any parameter between 0
|
|
/// and the segment length is valid.
|
|
virtual bool IsParameterValid(double parameter) const { return true; }
|
|
|
|
protected:
|
|
/// \brief Internal constructor for inherited classes that allows the
|
|
/// setting of the LineType
|
|
Line3D(const Eigen::Vector3d& origin,
|
|
const Eigen::Vector3d& direction,
|
|
LineType type);
|
|
|
|
/// \brief Calculates the common t_min and t_max values of the slab AABB
|
|
/// intersection method. These values are computed identically for any
|
|
/// semantic interpretation of the line, it's up to the derived classes
|
|
/// to use them in conjunction with other information to determine what the
|
|
/// intersection parameter is.
|
|
std::pair<double, double> SlabAABBBase(
|
|
const AxisAlignedBoundingBox& box) const;
|
|
|
|
private:
|
|
const LineType line_type_ = LineType::Line;
|
|
|
|
// Pre-calculated inverse values for the line's direction used to
|
|
// accelerate the slab method
|
|
double x_inv_;
|
|
double y_inv_;
|
|
double z_inv_;
|
|
};
|
|
|
|
/// \class Ray3D
|
|
///
|
|
/// \brief A ray is a semantic interpretation of Eigen::ParametrizedLine which
|
|
/// has an origin and a direction and extends infinitely only in that specific
|
|
/// direction.
|
|
class Ray3D : public Line3D {
|
|
public:
|
|
/// \brief Creates a Ray3D through two points. The ray origin will take the
|
|
/// value of p0, and the direction will be a normalized vector from p0 to p1
|
|
static Ray3D Through(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1) {
|
|
return {p0, (p1 - p0).normalized()};
|
|
}
|
|
|
|
/// \brief Default constructor, requires point and direction
|
|
Ray3D(const Eigen::Vector3d& origin, const Eigen::Vector3d& direction);
|
|
|
|
/// \brief Gets the length of the line, which for lines and rays will return
|
|
/// positive infinity, but for segments will return a finite positive value.
|
|
double Length() const override {
|
|
return std::numeric_limits<double>::infinity();
|
|
}
|
|
|
|
/// \brief Calculates the intersection parameter between the line and a
|
|
/// plane taking into account ray semantics. Returns an empty result if
|
|
/// there is no intersection. On a Ray3D this means that intersections
|
|
/// behind the origin are invalid, so the return value will always be
|
|
/// positive.
|
|
utility::optional<double> IntersectionParameter(
|
|
const Eigen::Hyperplane<double, 3>& plane) const override;
|
|
|
|
/// \brief Returns the lower intersection parameter for a ray with an
|
|
/// axis aligned bounding box or empty if no intersection. Uses the slab
|
|
/// method, see warning below.
|
|
///
|
|
/// \details Calculates the lower intersection parameter of a parameterized
|
|
/// ray with an axis aligned bounding box. The intersection point can be
|
|
/// recovered with .Line().pointAt(...). If the ray does not intersect the
|
|
/// box the optional return value will be empty. No intersection behind the
|
|
/// ray origin will be counted, and if the ray originates from within the
|
|
/// bounding box the parameter value will be 0.
|
|
///
|
|
/// This implementation is based off of Tavian Barnes' optimized branchless
|
|
/// slab method. https://tavianator.com/2011/ray_box.html. It runs in
|
|
/// roughly 5% of the time as the the naive exact method, but can degenerate
|
|
/// in specific conditions where a ray lies exactly in one of the AABB's
|
|
/// planes.
|
|
///
|
|
/// \warning A ray that lies exactly in one of the AABB's planes within the
|
|
/// double floating point precision will not intersect correctly by this
|
|
/// method
|
|
utility::optional<double> SlabAABB(
|
|
const AxisAlignedBoundingBox& box) const override;
|
|
|
|
/// \brief Clamps/bounds a parameter value to the closest valid place where
|
|
/// the entity exists. On a Line3D, the value will be unchanged, on a Ray3D
|
|
/// a negative value will be made 0, and on a Segment3D a negative value
|
|
/// will be made 0 and a positive value greater than Length() will take
|
|
/// the value of Length()
|
|
double ClampParameter(double parameter) const override {
|
|
return std::max(parameter, 0.);
|
|
}
|
|
|
|
/// \brief Verifies that a given parameter value is valid for the semantics
|
|
/// of the line object. For lines, any parameter is valid, for rays any
|
|
/// positive parameter is valid, and for segments any parameter between 0
|
|
/// and the segment length is valid.
|
|
bool IsParameterValid(double parameter) const override {
|
|
return parameter >= 0;
|
|
}
|
|
};
|
|
|
|
/// \class Segment3D
|
|
///
|
|
/// \brief A segment is a semantic interpretation of Eigen::ParametrizedLine
|
|
/// which has an origin and an endpoint and exists finitely between them.
|
|
///
|
|
/// \details One of the main motivations behind this class, and the Line3D
|
|
/// taxonomy in general, is the ambiguity of the Eigen documentation with
|
|
/// regards to the ParametrizedLine's direction. The documentation warns that
|
|
/// the direction vector is expected to be normalized and makes no guarantees
|
|
/// about behavior when this expectation is not met. However, ParametrizedLine
|
|
/// does behave correctly when the direction vector is scaled. This class
|
|
/// exists as a seam to ensure the correct behavior can be produced regardless
|
|
/// of what happens in the underlying Eigen implementation without changing
|
|
/// the api surface for client code.
|
|
class Segment3D : public Line3D {
|
|
public:
|
|
/// \brief Creates a Segment3D through two points. The origin will take the
|
|
/// value of p0, and the endpoint be p1. The direction will be a normalized
|
|
/// vector from p0 to p1.
|
|
static Segment3D Through(const Eigen::Vector3d& p0,
|
|
const Eigen::Vector3d& p1) {
|
|
return {p0, p1};
|
|
}
|
|
|
|
/// \brief Default constructor for Segment3D takes the start and end points
|
|
/// of the segment \p start_point \p end_point
|
|
Segment3D(const Eigen::Vector3d& start_point,
|
|
const Eigen::Vector3d& end_point);
|
|
|
|
/// \brief Takes a std::pair of points, the first to be used as the start
|
|
/// point/origin and the second to be the end point
|
|
explicit Segment3D(const std::pair<Eigen::Vector3d, Eigen::Vector3d>& pair);
|
|
|
|
/// \brief Get the scalar length of the segment as the distance between the
|
|
/// start point (origin) and the end point.
|
|
double Length() const override { return length_; }
|
|
|
|
/// \brief Calculates the midpoint of the segment
|
|
Eigen::Vector3d MidPoint() const { return 0.5 * (origin() + end_point_); }
|
|
|
|
/// \brief Get the end point of the segment
|
|
const Eigen::Vector3d& EndPoint() const { return end_point_; }
|
|
|
|
/// \brief Transform the segment by the given matrix
|
|
void Transform(
|
|
const Eigen::Transform<double, 3, Eigen::Affine>& t) override;
|
|
|
|
/// \brief Get an axis-aligned bounding box representing the enclosed volume
|
|
/// of the line segment.
|
|
AxisAlignedBoundingBox GetBoundingBox() const;
|
|
|
|
/// \brief Calculates the intersection parameter between the line and a
|
|
/// plane taking into account segment semantics. Returns an empty result if
|
|
/// there is no intersection. On a Segment3D this means that intersections
|
|
/// behind the origin and beyond the endpoint are invalid.
|
|
utility::optional<double> IntersectionParameter(
|
|
const Eigen::Hyperplane<double, 3>& plane) const override;
|
|
|
|
/// \brief Returns the lower intersection parameter for a segment with an
|
|
/// axis aligned bounding box or empty if no intersection. Uses the slab
|
|
/// method, see warning below.
|
|
///
|
|
/// \details Calculates the lower intersection parameter of a parameterized
|
|
/// segment with an axis aligned bounding box. The intersection point can be
|
|
/// recovered with .Line().pointAt(...). If the segment does not
|
|
/// intersect the box the optional return value will be empty. No
|
|
/// intersection behind the segment origin will be counted, and if the
|
|
/// segment originates from within the bounding box the parameter value will
|
|
/// be 0. No intersection beyond the endpoint of the segment will be
|
|
/// considered either.
|
|
///
|
|
/// This implementation is based off of Tavian Barnes' optimized branchless
|
|
/// slab method. https://tavianator.com/2011/segment_box.html. It runs in
|
|
/// roughly 5% of the time as the the naive exact method, but can degenerate
|
|
/// in specific conditions where a segment lies exactly in one of the AABB's
|
|
/// planes.
|
|
///
|
|
/// \warning A segment that lies exactly in one of the AABB's planes within
|
|
/// the double floating point precision will not intersect correctly by this
|
|
/// method
|
|
utility::optional<double> SlabAABB(
|
|
const AxisAlignedBoundingBox& box) const override;
|
|
|
|
/// \brief Returns the lower intersection parameter for a segment with an
|
|
/// axis aligned bounding box or empty if no intersection. This method is
|
|
/// about 20x slower than the slab method, see details to know when to use.
|
|
///
|
|
/// \details Calculates the lower intersection parameter of a parameterized
|
|
/// segment with an axis aligned bounding box. The intersection point can be
|
|
/// recovered with .Line().pointAt(...). If the segment does not
|
|
/// intersect the box the return value will be empty.
|
|
///
|
|
/// This implementation is a naive exact method that considers intersections
|
|
/// with all six bounding box planes. It is not optimized for speed and
|
|
/// should only be used when a problem is conditioned such that the slab
|
|
/// method is unacceptable. Use this when a segment is likely to lie exactly
|
|
/// in one of the AABB planes and false negatives are unacceptable.
|
|
/// Typically this will only happen when segments are axis-aligned and both
|
|
/// segments and bounding volumes are regularly spaced, and every
|
|
/// intersection is important. In such cases if performance is important, a
|
|
/// simple custom implementation based on the problem directionality will
|
|
/// likely outperform even the slab method.
|
|
utility::optional<double> ExactAABB(
|
|
const AxisAlignedBoundingBox& box) const override;
|
|
|
|
/// \brief Clamps/bounds a parameter value to the closest valid place where
|
|
/// the entity exists. On a Line3D, the value will be unchanged, on a Ray3D
|
|
/// a negative value will be made 0, and on a Segment3D a negative value
|
|
/// will be made 0 and a positive value greater than Length() will take
|
|
/// the value of Length()
|
|
double ClampParameter(double parameter) const override {
|
|
return std::max(std::min(parameter, length_), 0.);
|
|
}
|
|
|
|
/// \brief Verifies that a given parameter value is valid for the semantics
|
|
/// of the line object. For lines, any parameter is valid, for rays any
|
|
/// positive parameter is valid, and for segments any parameter between 0
|
|
/// and the segment length is valid.
|
|
bool IsParameterValid(double parameter) const override {
|
|
return parameter >= 0 && parameter <= length_;
|
|
}
|
|
|
|
private:
|
|
Eigen::Vector3d end_point_;
|
|
double length_;
|
|
};
|
|
|
|
} // namespace geometry
|
|
} // namespace open3d
|