1. AI技术聚合首页
  2. IT

计算几何学 | 实用计算几何学知识c++代码实现

青葱年少 • • IT • 阅读 25

实用计算几何学

    • 前言
    • Geometry
      • Point
      • Line
      • Segment
      • Polyline
    • Algorithms
      • 基本运算
      • Projection-投影
      • Distance-求距离
      • Side-求相对位置关系
      • Intersection-相交
      • Curvature-曲率
      • Find closest segment-求polyline上距离给定点最近的线段

前言

前段时间在b站发布了关于二维平面下一些计算几何学知识的讲解，有许多小伙伴私戳我说能不能出个代码实现，所以这段时间就抽个时间用c++实现下视频里面讲的内容。

注： 本篇博客不再具体讲解理论内容，而是实现相关算法。想要进一步深入了解理论内容的小伙伴可以去回顾之前的视频讲解：bilibili。

代码仓库：https://github.com/CHH3213/Math_Geometry/

Geometry

Point

point的struct定义如下：

// Define point struct.
struct Point {
    Point()=default;
    Point(double x_in, double y_in) : x(x_in), y(y_in) {}

    Point(const Point& p) : x(p.x), y(p.y) {}

    Point& operator=(const Point& p) {
        x = p.x;
        y = p.y;
        return *this;
    }
    Point operator+(const Point& p) const{
        return {x + p.x, y + p.y};
    }
    Point operator-(const Point& p) const{
        return {x - p.x, y - p.y};
    }
    double operator*(const Point& p) const{
        return x * p.x+ y * p.y;
    }
    Point operator*(double k)const {
        return {x *k, y * k};
    }
    friend Point operator*(double k, const Point& p) {
        return {p.x * k, p.y * k};
    }
    bool operator==(const Point& p)const{
        return p.x==x&&p.y==y;
    }
    bool operator!=(const Point& p)const{
        return !(p.x==x&&p.y==y);
    }


    double modulus()const {
        return sqrt(x * x + y * y);
    }

    double DistanceTo(const Point& other)const {
        double dx = x - other.x;
        double dy = y - other.y;
        return sqrt(dx * dx + dy * dy);
    }

    friend std::ostream& operator<<(std::ostream& out, const Point& p) {
        out << "(" << p.x << ", " << p.y << ")";
        return out;
    }

    double x;
    double y;
};

Line

line就是直线，直线的struct我们可以定义成：

//Define line segment.
struct Line {

    Line()=default;
    Line(Point p1_in, Point p2_in) : p1(p1_in), p2(p2_in),direction(p2_in-p1_in) {
    }

    friend std::ostream& operator<<(std::ostream& out, const Line& line) {
        out << "Line: " << line.p1 << " ---> " << line.p2;
        return out;
    }

    // A point in Line.
    Point p1;
    // Another point in line.
    Point p2;
    Point direction;


};

Segment

线段的struct如下：

// Define segment struct.
struct Segment {
    Segment()=default;
    Segment(Point start_in, Point end_in) : start(start_in), end(end_in),direction(end-start) {
    }

    Segment(const Segment& s) : start(s.start), end(s.end),direction(end-start) {}

    Segment& operator=(const Segment& s) {
        start = s.start;
        end = s.end;
        return *this;
    }
    Segment operator+(const Segment& rhs)const {
        return {start + rhs.start, end + rhs.end};
    }
    Segment operator-(const Segment& rhs)const {
        return {start - rhs.start, end - rhs.end};
    }


    double length() const{
        return direction.modulus();
    }

    Point unit_direction() const{
        double len = length();
        if (len != 0) {
            return {direction.x / len, direction.y / len};
        } else {
            // Handle the case where the length is zero (avoid division by zero).
            throw std::runtime_error("Cannot calculate unit direction for a segment with zero length.");
        }
    }

    friend std::ostream& operator<<(std::ostream& out, const Segment& s) {
        out << "Segment: " << s.start << " ---> " << s.end;
        return out;
    }

    Point start;
    Point end;
    Point direction;

};

Polyline

线段彼此相连便组成了折线段，也就是polyline，我们可以这样定义struct

// Define polyline struct.
// Note that a polyline we can use points vector or segments vector to represent.
struct Polyline {
    Polyline()=default;

    Polyline(const std::vector<Point>& pts):points(pts){
        for(int i = 1;i<points.size();++i){
            segs.push_back(Segment(points[i-1],points[i]));
        }
    }

    Polyline(const std::vector<Segment>& segs_) : segs(segs_) {
        for(int i = 0;i<segs.size();++i){
            points.push_back(segs[i].start);
        }
        points.push_back(segs[segs.size()-1].end);
    }

    void append(const Segment& seg) {
        if(!segs.empty() && segs.back().end != seg.start) {
            throw std::invalid_argument("Disconnected Segment");
        }
        segs.push_back(seg);
        points.push_back(seg.end);
    }
    void append(const Point& p) {
        const auto seg = Segment(points.back(),p);
        points.push_back(p);
        segs.push_back(seg);
    }

    Polyline operator+(const Polyline& other) const {
        Polyline result;
        result.segs = this->segs;
        result.points = this->points;
        for(auto& seg : other.segs) {
            result.append(seg);
        }

        return result;
    }

    Segment GetSegmentByIndex(int index) {
        if(index < 0 || index >= segs.size()) {
            throw std::out_of_range("Index out of range");
        }
        return segs[index];
    }
    std::vector<Point> Points() const{
        return points;
    }
    std::vector<Segment> Segments()const{
        return segs;
    }
private:
    std::vector<Segment> segs;
    std::vector<Point> points;

};

Algorithms

基本运算

  1. 点积

    // Calculates dot product.
double DotProduct(const Vec& v1,const Vec& v2){
    return v1.x*v2.x+v1.y*v2.y;
}

  2. 叉积

    // Calculates cross product.
double CrossProduct(const Vec& v1,const Vec& v2) {
    return v1.x*v2.y-v2.x*v1.y;
}

Projection-投影

投影这里指的是求点到线段的投影点、投影长度。

  1. 点到线段的投影长度

    // Compute projection length of point p.
double ComputeProjectionLength(const Point& p,const Segment& segment){
    const auto& p1p = p-segment.start;
    return DotProduct(p1p,segment.unit_direction());
}

  2. 点到线段的投影点

    // Compute projection point of point p.
Point ComputeProjection(const Point& p,const Segment& segment){
    double projection_length = ComputeProjectionLength(p,segment);
    return segment.start + segment.unit_direction()*projection_length;
}

Distance-求距离

距离包括点-点，点-直线，点-线段，线段-线段等之间的距离。

  1. point to point

    // Get distance between point p1 and point p2.
double GetDistance(const Point& p1,  const Point& p2){
    return p1.DistanceTo(p2);
}

  2. point to line

    // Get distance between point p and a straight line.
double GetDistance(const Point& p, const Line& line){
    Segment p1p2(line.p1,line.p2);
    Segment p1p(line.p1,p);
    return std::abs(CrossProduct(p1p2.direction,p1p.direction))/p1p2.length();
}

  3. point to segment

    // Get distance between point p and segment(p1,p2).
double GetDistance(const Point& p, const Segment& segment){
    Segment p1p(segment.start,p);
    Segment p2p(segment.end,p);
    const auto c1 = DotProduct(p1p.direction,segment.direction);
    const auto c2 = DotProduct(p2p.direction,segment.direction);
    if(c1<=0){
        //distance(p,segment)=distacne(p1,p).
        return GetDistance(segment.start,p);
    }
    if(c2>=0){
        //distance(p,segment)=distacne(p2,p).
        return GetDistance(segment.end,p);
    }
    return std::abs(CrossProduct(segment.direction,p1p.direction))/segment.length();
}

  4. segment to segment

    // Get distance between segment and segment (method 1), method 2 is to be determined.
double GetDistance(const Segment& s1,const Segment& s2){
    const double d1 = GetDistance(s1.start, s2);
    const double d2 = GetDistance(s1.end, s2);
    const double d3 = GetDistance(s2.start, s1);
    const double d4 = GetDistance(s2.end, s1);
    return std::min(std::min(d1, d2), std::min(d3, d4));
}

    注：视频中讲解的另外一种方法暂时未实现，留个todo。

Side-求相对位置关系

对于一个点与线段之间的位置关系，我们可以定义一个enum来表示

enum class Side{
    // When the segment's length is zero, it's useless to determine the side, so we use DEFAULT_NO_SIDE to show.
    DEFAULT_NO_SIDE=0,
    LEFT,
    RIGHT,
    // The three below states mean that the point is in line.
    ON_AFTER,
    ON_BEFORE,
    WITHIN
};

也就是说点与线段的相对位置关系包括以下几种：

  • 点在线段的左边
  • 点在线段的右边
  • 点在线段所在的直线上
    • 点在线段前面
    • 点在线段后面
    • 点在线段内部

  1. 判断点跟一条线段的相对位置关系

    // Determine which side of the segment the point is.
Side GetSide(const Point& p,const Segment& s){
    const auto& p0 = s.start;
    const auto& p2 = s.end;
    const auto& a = p-p0;
    const auto& b = p2-p0;
    const auto cross_product = CrossProduct(a,b);
    if(cross_product!=0){
        // Returns LEFT if p0,p,p2 are clockwise position, returns RIGHT means p0,p,p2 are counter-clockwise position.
        return cross_product<0?Side::LEFT:Side::RIGHT;
    }
    const auto dot_product = DotProduct(a,b);
    if(dot_product<0){
        return Side::ON_BEFORE;
    }else if(dot_product>0){
        if(b.modulus()<a.modulus()){
            return Side::ON_AFTER;
        }else{
            return Side::WITHIN;
        }
    }else{
        if(a.modulus()==0){
            return Side::WITHIN;
        }
        return Side::DEFAULT_NO_SIDE;
    }
}

  2. 判断点与两条相连的线段的相对位置关系——法一

    // Determine which side of two segments the point is.
//Method 1: directly use cross product to determine and only have left/right options.
Side GetSide(const Point& p, const Segment& s1,const Segment& s2) {
    //DCHECK(s1.end==s2.start)<<"please ensure the two segments are connected.";
    if (s1.end != s2.start) {
        throw std::runtime_error("Error: The two segments are not connected.");
    }
    const auto& p0p = p-s1.start;
    const auto& p1p = p-s2.start;
    const auto c1 = CrossProduct(s1.direction,p0p);
    const auto c2 = CrossProduct(s2.direction,p1p);

    if(c1>0&&c2>0){
        return Side::LEFT;
    }
    if(c1<0&&c2<0){
        return Side::RIGHT;
    }
    return std::abs(c1)>std::abs(c2)?Side::LEFT:Side::RIGHT;
}

  3. 判断点与两条相连的线段的相对位置关系——法二

    // Determine which side of two segments the point is.
// Method 2: through the side of p to one segment to determine, and except left/right side, we also provide other options.
Side GetSide(const Point& p, const Segment& s1,const Segment& s2) {
    //DCHECK(s1.end==s2.start)<<"please ensure the two segments are connected.";
    if (s1.end != s2.start) {
        throw std::runtime_error("Error: The two segments are not connected.");
    }
    const auto side_1 = GetSide(p,s1);
    const auto side_2 = GetSide(p,s2);
    if(side_1==side_2){
        return side_1;
    }
    if(side_1==Side::WITHIN||side_2==Side::WITHIN){
        return Side::WITHIN;
    }
    const auto& p0p = p-s1.start;
    const auto& p1p = p-s2.start;
    const auto c1 = CrossProduct(s1.direction,p0p);
    const auto c2 = CrossProduct(s2.direction,p1p);
    return std::abs(c2) > std::abs(c1) ? side_2 : side_1;
}

Intersection-相交

这里相交主要指的是线段与线段之间的相交。很显然相交分为两步：

  1. 判断是否相交

    // Determine whether segment 1 intersects segment 2.
bool IsIntersection(const Segment& s1, const Segment& s2){
    const double o1 = CrossProduct(s2.start - s1.start, s1.direction);
    const double o2 = CrossProduct(s2.end - s1.start, s1.direction);
    const double o3 = CrossProduct(s1.start - s2.start, s2.direction);
    const double o4 = CrossProduct(s1.end - s2.start, s2.direction);
    // Segments are considered intersecting if they have different orientations.
    if(o1 * o2 < 0 && o3 * o4 < 0){
        return true;
    }

    auto on_segment = [](const Point &p, const Point &q, const Point &r){
        return (q.x <= std::max(p.x, r.x) && q.x >= std::min(p.x, r.x) &&
                q.y <= std::max(p.y, r.y) && q.y >= std::min(p.y, r.y));
    };
    // Additional checks for collinear points.
    if(o1 == 0 && on_segment(s1.start, s2.start, s1.end)){
        return true;
    }

    if(o2 == 0 && on_segment(s1.start, s2.end, s1.end)){
        return true;
    }

    if(o3 == 0 && on_segment(s2.start, s1.start, s2.end)){
        return true;
    }

    if(o4 == 0 && on_segment(s2.start, s1.end, s2.end)){
        return true;
    }

    return false;
}

  2. 若相交则求出相交点

    	//Calculate the intersection between segment 𝑝0𝑝1 and segment 𝑝2𝑝3.
Point GetIntersectionPoint(const Segment& s1, const Segment& s2){
    if(!IsIntersection(s1, s2)){
        std::cout << "No intersection, return a deafult point value:(0,0)!";
        return Point(0, 0);
    }
    const auto& u = s1.direction;
    const auto& v = s2.direction;
    const auto& w = s1.start - s2.end;
    const auto c1 = CrossProduct(w, v);
    const auto c2 = CrossProduct(v, u);
    if(c2 != 0){
        const double t = std::abs(c1 / c2);
        return s1.start + t * u;
    }
    // Address collinear and overlapping situation. If so, we return overlaping start or end.
    const auto side_1 = GetSide(s1.start, s2);
    const auto side_2 = GetSide(s1.end, s2);
    const auto side_3 = GetSide(s2.start, s1);
    const auto side_4 = GetSide(s2.end, s1);
    if(side_1 == Side::WITHIN){
        return s1.start;
    }
    if(side_2 == Side::WITHIN){
        return s1.end;
    }
    if(side_3 == Side::WITHIN){
        return s2.start;
    }
    if(side_4 == Side::WITHIN){
        return s2.end;
    }
    throw std::runtime_error("Something is wrong, please check.");
}

Curvature-曲率

通过三点求曲率：

// Obtain curvature according to p1,p2,p3.
// NOTE : curvature has a sign, not just an unsigned value.
double GetCurvature(const Point& p1, const Point& p2, const Point& p3){
    const auto& p1p2 = p2 - p1;
    const auto& p2p3 = p3 - p2;
    const auto& p1p3 = p3 - p1;
    const auto& a = p1p2.modulus();
    const auto& b = p2p3.modulus();
    const auto& c = p1p3.modulus();
    const auto cross_product = CrossProduct(p1p2, p2p3);
    return 2 * cross_product / (a * b * c);
}

Find closest segment-求polyline上距离给定点最近的线段

求距离给定点最近的线段。

  1. 实现方式1

    // Find the given point's closest segment in polyline using linear search.
// Option 1.
Segment FindClosestSegmentByLinearSearch(const Point& point, const Polyline& polyline){
    const auto points = polyline.Points();
    const auto segments = polyline.Segments();
    //Compute the square distance between given point and first point in polyline.
    double min_dist_sq = GetDistance(point,points[0]);
    int closest_segment_index = 0;
    for(int i=1;i<points.size();++i){
        const auto& p1 = points[i-1];
        const auto& p2 = points[i];
        const auto& seg = segments[i-1];
        const auto& v = seg.unit_direction();
        const auto& w = point to p1;
        const double c1 = DotProduct(w,v);
        if(c1<=0){
            continue;
        }
        double dist_sq= 0.0;
        const double c2 = seg.length();
        if(c2<=c1){
            dist_sq = GetDistance(point,p2);
        }else{
            dist_sq = GetDistance(point,seg);
        }

        if(dist_sq<min_dist_sq){
            min_dist_sq = dist_sq;
            closest_segment_index=i-1;
            if(min_dist_sq<Epsilon){
                break;
            }
        }

    }
    return segments[closest_segment_index];
}

  2. 实现方式2

    // Find the given point's closest segment in polyline using linear search.
// Option 2: since we have implemented distance function, we can directly use it.
Segment FindClosestSegmentByLinearSearch(const Point& point, const Polyline& polyline){
    const auto& points = polyline.Points();
    const auto& segments = polyline.Segments();
    //Compute the square distance  between given point and first point in polyline.
    double min_dist_sq = GetDistance(point,points[0]);
    int closest_segment_index = 0;
    for(int i=0;i<segments.size();++i){
        const auto& seg = segments[i];
        const double dist_sq = GetDistance(point,seg);
        if(dist_sq<min_dist_sq){
            min_dist_sq = dist_sq;
            closest_segment_index=i;
            if(min_dist_sq<Epsilon){
                break;
            }
        }
    }
    return segments[closest_segment_index];
}

视频中主要涉及的内容实现基本完成了，还有一些额外的没有实现，后续会把它完善。

以上所有代码均存放于github仓库中，欢迎访问：https://github.com/CHH3213/Math_Geometry/

版权声明：本文为博主作者：CHH3213原创文章，版权归属原作者，如果侵权，请联系我们删除！

原文链接：https://blog.csdn.net/weixin_42301220/article/details/135439512

共计人评分，平均分

到目前为止还没有投票！成为第一位评论此文章。

赞 (0)
青葱年少的头像青葱年少普通用户
0
上一篇 2024年4月22日
下一篇 2024年4月22日

相关推荐