algorithms.h

Go to the documentation of this file.

#pragma once

#include <opencv2/opencv.hpp>

namespace vr {

class Plane;

class Line
{
public:
    friend class Plane;
    Line() : point_(0.0, 0.0, 0.0), direction_(1.0, 0.0, 0.0) {};
    Line(const cv::Vec3d& point, const cv::Vec3d& direction);
    const cv::Vec3d& getDirection() const;
    const cv::Vec3d& getPoint() const;
    cv::Vec3d getPoint(const double t) const;
    cv::Vec3d operator ()(const double t) const;
    void getIntersection(double& t, const Plane& plane) const;
    cv::Vec3d getIntersection(const Plane& plane) const;
    void getNearestPoint(double& t, const Line& line) const;
    cv::Vec3d getNearestPoint(const Line& line) const;
    double normalize();
private:
    cv::Vec3d point_;
    cv::Vec3d direction_;
};


class Plane
{
public:
    friend class Line;
    Plane(const cv::Vec3d& normal, const double lambda);
    Plane(const cv::Vec3d& direction1,
          const cv::Vec3d& direction2,
          const cv::Vec3d& point);
    cv::Vec3d getIntersection(const Line& line) const;
    Line getIntersection(const Plane& plane) const;
    const cv::Vec3d& getNormal() const;
    double getLambda() const;
private:
    cv::Vec3d normal_;
    double lambda_;
};

class AxisParallelEllipse
{
public:
    AxisParallelEllipse(cv::Point2d& center, cv::Point2d& radius);
    ~AxisParallelEllipse();
    bool contains(cv::Point2d& point)
    {
        double dx2 = point.x - center_.x;
        double dy2 = point.y - center_.y;

        double rx2 = radius_.x * radius_.x;
        double ry2 = radius_.y * radius_.y;

        return dx2 / rx2 + dy2 / ry2 <= 1.0;
    };
private:
    cv::Point2d center_;
    cv::Point2d radius_;
};


template <typename T> int sgn(T val) {
    return (T(0) < val) - (val < T(0));
}


int adaptedNewtonsMethod(cv::Vec3d& depth, double* residuum,
    double p_L, double p_R, double p_M,
    double q_LR, double q_LM, double q_RM,
    double d_LR, double d_LM, double d_RM,
    double epsilon);

// Gibt den Lösungsvektor
cv::Vec3d adaptedNewtonsMethodForBC(const cv::Vec3d& F_O, const cv::Vec3d& F_A, const cv::Vec3d& F_U,
    double rq, const cv::Vec3d& start, double epsilon);

template <int n>
cv::Vec<double, n> SP(const cv::Vec<double, n>& p, const cv::Vec<double, n>& v, const cv::Vec<double, n>& w)
{
    double t = (p - v).dot(w) / w.dot(w);
    return v + (w * t);
}

int calcLineCircleIntersections (double& result1, double& result2,
    const cv::Vec2d& linePoint, const cv::Vec2d& lineDirection, const cv::Vec2d& center, const double radius);

int calcLineCircleIntersections (cv::Vec2d& result1, cv::Vec2d& result2,
    const cv::Vec2d& linePoint, const cv::Vec2d& lineDirection, const cv::Vec2d& center, const double radius);

int calcLineSphereIntersections (cv::Vec3d& result1, cv::Vec3d& result2,
    const cv::Vec3d& linePoint, const cv::Vec3d& lineDirection, const cv::Vec3d& center, const double radius);


bool getPlaneNormalForm(cv::Vec3d& normal,
                    double& lambda,
                    const cv::Vec3d& direction1,
                    const cv::Vec3d& direction2,
                    const cv::Vec3d& planePoint);

void calcLinePlaneIntersection(double& result,
    const cv::Vec3d& linePoint, const cv::Vec3d& lineDirection,
    const cv::Vec3d& planeNormal, const double lambda);

cv::Vec3d calcLinePlaneIntersection(const cv::Vec3d& linePoint, const cv::Vec3d& lineDirection,
                                    const cv::Vec3d& planeNormal, const double lambda);

cv::Vec3d calcLineIntersection(
    const cv::Vec2d& line1Point1, const cv::Vec2d& line1Point2,
    const cv::Vec2d& line2Point1, const cv::Vec2d& line2Point2);

void calcLineIntersection(cv::Vec2d& result,
    const cv::Vec2d& line1Point1, const cv::Vec2d& line1Point2,
    const cv::Vec2d& line2Point1, const cv::Vec2d& line2Point2);

inline double deg2rad(double deg) {
    return (deg * CV_PI / 180.0);
}


inline double rad2deg(double rad) {
    return (rad * 180.0 / CV_PI);
}

inline bool IsPointInEllipse(const cv::Point2d p,
    const cv::Point2d& center, const cv::Point2d& radius)
{
    double dx2 = p.x - center.x;
    double dy2 = p.y - center.y;

    dx2 *= dx2;
    dy2 *= dy2;

    double rx2 = radius.x * radius.x;
    double ry2 = radius.y * radius.y;

    return dx2 / rx2 + dy2 / ry2 <= 1.0;
}

} // namespace vr