Line3D.h

Go to the documentation of this file.

// ----------------------------------------------------------------------------
// -                        Open3D: www.open3d.org                            -
// ----------------------------------------------------------------------------
// The MIT License (MIT)
//
// Copyright (c) 2020 www.open3d.org
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
// IN THE SOFTWARE.
// ----------------------------------------------------------------------------

#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 : protected Eigen::ParametrizedLine<double, 3> {
public:
    static Line3D Through(const Eigen::Vector3d& p0,
                          const Eigen::Vector3d& p1) {
        return {p0, (p1 - p0).normalized()};
    }

    enum class LineType {
        Line = 0,

        Ray = 1,

        Segment = 2,
    };

    Line3D(const Eigen::Vector3d& origin, const Eigen::Vector3d& direction);

    virtual ~Line3D() = default;

    LineType GetLineType() const { return line_type_; }

    const Eigen::Vector3d& Origin() const { return m_origin; }

    const Eigen::Vector3d& Direction() const { return m_direction; }

    virtual double Length() const {
        return std::numeric_limits<double>::infinity();
    }

    virtual void Transform(
            const Eigen::Transform<double, 3, Eigen::Affine>& t) {
        transform(t);
    }

    const Eigen::ParametrizedLine<double, 3>& Line() const { return *this; }

    virtual utility::optional<double> IntersectionParameter(
            const Eigen::Hyperplane<double, 3>& plane) const;

    double ProjectionParameter(const Eigen::Vector3d& point) const;

    virtual Eigen::Vector3d Projection(const Eigen::Vector3d& point) const;

    virtual utility::optional<double> SlabAABB(
            const AxisAlignedBoundingBox& box) const;

    virtual utility::optional<double> ExactAABB(
            const AxisAlignedBoundingBox& box) const;

    std::pair<double, double> ClosestParameters(const Line3D& other) const;

    std::pair<Eigen::Vector3d, Eigen::Vector3d> ClosestPoints(
            const Line3D& other) const;

    double DistanceTo(const Line3D& other) const;

    virtual double ClampParameter(double parameter) const { return parameter; }

    virtual bool IsParameterValid(double parameter) const { return true; }

protected:
    Line3D(const Eigen::Vector3d& origin,
           const Eigen::Vector3d& direction,
           LineType type);

    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 : public Line3D {
public:
    static Ray3D Through(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1) {
        return {p0, (p1 - p0).normalized()};
    }

    Ray3D(const Eigen::Vector3d& origin, const Eigen::Vector3d& direction);

    double Length() const override {
        return std::numeric_limits<double>::infinity();
    }

    utility::optional<double> IntersectionParameter(
            const Eigen::Hyperplane<double, 3>& plane) const override;

    utility::optional<double> SlabAABB(
            const AxisAlignedBoundingBox& box) const override;

    double ClampParameter(double parameter) const override {
        return std::max(parameter, 0.);
    }

    bool IsParameterValid(double parameter) const override {
        return parameter >= 0;
    }
};

class Segment3D : public Line3D {
public:
    static Segment3D Through(const Eigen::Vector3d& p0,
                             const Eigen::Vector3d& p1) {
        return {p0, p1};
    }

    Segment3D(const Eigen::Vector3d& start_point,
              const Eigen::Vector3d& end_point);

    explicit Segment3D(const std::pair<Eigen::Vector3d, Eigen::Vector3d>& pair);

    double Length() const override { return length_; }

    Eigen::Vector3d MidPoint() const { return 0.5 * (origin() + end_point_); }

    const Eigen::Vector3d& EndPoint() const { return end_point_; }

    void Transform(
            const Eigen::Transform<double, 3, Eigen::Affine>& t) override {
        transform(t);
        end_point_ = t * end_point_;
    }

    AxisAlignedBoundingBox GetBoundingBox() const;

    utility::optional<double> IntersectionParameter(
            const Eigen::Hyperplane<double, 3>& plane) const override;

    utility::optional<double> SlabAABB(
            const AxisAlignedBoundingBox& box) const override;

    utility::optional<double> ExactAABB(
            const AxisAlignedBoundingBox& box) const override;

    double ClampParameter(double parameter) const override {
        return std::max(std::min(parameter, length_), 0.);
    }

    bool IsParameterValid(double parameter) const override {
        return parameter >= 0 && parameter <= length_;
    }

private:
    Eigen::Vector3d end_point_;
    double length_;
};

}  // namespace geometry
}  // namespace open3d