计算几何学（工程版）

昨天写了计算几何学的东西，今天主要是工程化一下，工程借鉴B站up主

【自动驾驶】自动驾驶planning方向中常用的计算几何学知识 01_哔哩哔哩_bilibili

#pragma once

#include <cmath>
#include <iostream>

class Point{
public:
    Point() = default;
    Point(double x_in, double y_in) : x(x_in), y(y_in) {}

    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};
    }
    Point operator*(double k)const {
        return {x * k, y * k};
    }

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

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

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

    double x;
    double y;
};

class Segment{
public:
    Segment() = default;
    Segment(Point start_in, Point end_in) : start(start_in), end(end_in), 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.");
        }
    }

    Point start;
    Point end;
    Point direction;
};

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

    Point p1;
    Point p2;
    Point direction;
};

#pragma once

#include "Geometry.h"
#include "utils.h"

double ComputeProjectionLength(const Point& p, const Segment& segement){
    const auto& p1p = p - segement.start;
    return DotProduct(p1p, segement.unit_direction());
}

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

#pragma once

#include "Geometry.h"
#include "utils.h"

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

// 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();
}

// 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();
}

#pragma once

#include <iostream>
#include "Geometry.h"

// Calculates dot product.
double DotProduct(const Point& p1, const Point& p2){
    return p1.x * p2.x + p1.y * p2.y;
}

// Calculates cross product.
double CrossProduct(const Point& p1, const Point& p2) {
    return p1.x * p2.y - p2.x * p1.y;
}

#include <iostream>
#include "Geometry.h"
#include "Projection.h"
#include "Distance.h"

int main(){
    Point point(3, 4);
    Segment segment(Point(0.0, 0.0), Point(10.0, 0.0));
    Point projection = ComputeProjection(point, segment);
    std::cout << "projection: " << projection << std::endl;

    Point p1(1, 1);
    Point p2(3, 5);
    std::cout << "Distance between p1 and p2: "
              << GetDistance(p1, p2) << std::endl;

    Line line(Point(0, 0), Point(10, 0));
    Point p(3, 4);
    std::cout << "Distance between p and line: "
              << GetDistance(p, line) << std::endl; 

    Segment seg(Point(5, 5), Point(9, 6));
    std::cout << "Distance between p and segment: "
              << GetDistance(p, seg) << std::endl;
    return 0;
}

TODO