昨天写了计算几何学的东西，今天主要是工程化一下，工程借鉴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