墨星写作网站,网络营销文案创作思路有哪些,阳江房产网0662,从化营销网站建设Planning模块#xff0c;路径和速度曲线抽象成折线#xff08;Polyline#xff09;#xff0c;障碍物抽象成多边形#xff08;Polygon#xff09;。在碰撞检测、投影计算距离、平滑曲线等方面应用广泛。
1 几何算法
1.1 线段
moudles/common/math/line_segment2d.h
n…Planning模块路径和速度曲线抽象成折线Polyline障碍物抽象成多边形Polygon。在碰撞检测、投影计算距离、平滑曲线等方面应用广泛。
1 几何算法
1.1 线段
moudles/common/math/line_segment2d.h
namespace apollo {
namespace common {
namespace math {
// 平面线段
class LineSegment2d {public:LineSegment2d();LineSegment2d(const Vec2d start, const Vec2d end);// 获取线段的起点const Vec2d start() const { return start_; }// 获取线段的终点const Vec2d end() const { return end_; }// 获取从起点到终点的方向const Vec2d unit_direction() const { return unit_direction_; }// 获取线段的中心Vec2d center() const { return (start_ end_) / 2.0; }// 旋转线段的终点Vec2d rotate(const double angle);// 返回线段的航向double heading() const { return heading_; }// cos(heading_)double cos_heading() const { return unit_direction_.x(); }// sin(heading_)double sin_heading() const { return unit_direction_.y(); }// 获取线段的长度double length() const;// 获取线段长度的平方double length_sqr() const;/*** brief Compute the shortest distance from a point on the line segment* to a point in 2-D.* param point The point to compute the distance to.* return The shortest distance from points on the line segment to point.*/// 计算线段上的点到2-D中的点的最短距离。double DistanceTo(const Vec2d point) const;/*** brief 计算线段上一点到二维中一点的最短距离得到线段上最近的点* param point The point to compute the distance to.* param nearest_pt The nearest point on the line segment* to the input point.* return 线段上的点到输入点的最短距离*/double DistanceTo(const Vec2d point, Vec2d *const nearest_pt) const;// 计算线段上的一点到2-D中的一点的最短距离的平方double DistanceSquareTo(const Vec2d point) const;// 计算二维中线段上一点到一点的最短距离的平方得到线段上最近的点。double DistanceSquareTo(const Vec2d point, Vec2d *const nearest_pt) const;/*** brief 检查一个点是否在线段内* param point 检查它是否在线段内的点* return 输入点是否在线段内*/bool IsPointIn(const Vec2d point) const;// 检查该线段是否与二维中的另一条线段相交bool HasIntersect(const LineSegment2d other_segment) const;// 计算与二维中另一条线段的交点如果有的话bool GetIntersect(const LineSegment2d other_segment,Vec2d *const point) const;// 计算矢量在线段上的投影double ProjectOntoUnit(const Vec2d point) const;// 计算向量与线段的叉积double ProductOntoUnit(const Vec2d point) const;// 计算从线段展开的直线上的二维点的垂直脚部double GetPerpendicularFoot(const Vec2d point,Vec2d *const foot_point) const;// 获取包含基本信息的调试字符串std::string DebugString() const;private:Vec2d start_;Vec2d end_;Vec2d unit_direction_;double heading_ 0.0;double length_ 0.0;
};} // namespace math
} // namespace common
} // namespace apollo
moudles/common/math/line_segment2d.cc 计算点到直线的距离
double LineSegment2d::DistanceTo(const Vec2d point) const {// 如果线段的长度小于等于一个阈值kMathEpsilon那么点一定在线段上直接返回点与线段起点的距离if (length_ kMathEpsilon) {return point.DistanceTo(start_);}const double x0 point.x() - start_.x();const double y0 point.y() - start_.y();// proj表示点在方向向量上的投影const double proj x0 * unit_direction_.x() y0 * unit_direction_.y();// 如果投影小于等于0说明点在直线段的同侧直接返回点到线段起点的距离if (proj 0.0) {return hypot(x0, y0);}// 如果投影大于等于线段长度说明点在直线段的延长线上直接返回点到线段终点的距离if (proj length_) {return point.DistanceTo(end_);}return std::abs(x0 * unit_direction_.y() - y0 * unit_direction_.x());
}给定一个点计算到线段的最短距离同时返回最近的点过给定点的垂线与原线段的交点或者线段的端点
double LineSegment2d::DistanceTo(const Vec2d point,Vec2d *const nearest_pt) const {// 检查nearest_pt是否为空CHECK_NOTNULL(nearest_pt);// 如果线段的长度小于等于一个阈值kMathEpsilon那么点一定在线段上直接返回点与线段起点的距离if (length_ kMathEpsilon) {*nearest_pt start_;return point.DistanceTo(start_);}// 计算点与线段起点的坐标差x0和y0const double x0 point.x() - start_.x();const double y0 point.y() - start_.y();// 计算proj表示点在方向向量上的投影const double proj x0 * unit_direction_.x() y0 * unit_direction_.y();// 如果投影小于等于0说明点在直线段的同侧直接返回点到线段起点的距离。如果投影大于等于线段长度说明点在直线段的延长线上直接返回点到线段终点的距离if (proj 0.0) {*nearest_pt start_;return hypot(x0, y0);}if (proj length_) {*nearest_pt end_;return point.DistanceTo(end_);}*nearest_pt start_ unit_direction_ * proj;// 计算点到线段的垂线与x轴正半轴的夹角即x0 * unit_direction_.y() - y0 * unit_direction_.x()取绝对值作为最终结果return std::abs(x0 * unit_direction_.y() - y0 * unit_direction_.x());
}rotate逆时针旋转angle度注意是弧度
Vec2d LineSegment2d::rotate(const double angle) {Vec2d diff_vec end_ - start_;diff_vec.SelfRotate(angle);return start_ diff_vec;
}GetPerpendicularFoot某个点到该线段垂点的距离
double LineSegment2d::GetPerpendicularFoot(const Vec2d point,Vec2d *const foot_point) const {CHECK_NOTNULL(foot_point);if (length_ kMathEpsilon) {*foot_point start_;return point.DistanceTo(start_);}const double x0 point.x() - start_.x();const double y0 point.y() - start_.y();const double proj x0 * unit_direction_.x() y0 * unit_direction_.y();*foot_point start_ unit_direction_ * proj;return std::abs(x0 * unit_direction_.y() - y0 * unit_direction_.x());
}1.2 包围盒
二维平面上Box特指矩形包围盒。Planning模块经常将自车和障碍物抽象成一个矩形框简化计算 bounding box Box2d普通矩形包围盒 文件路径modules/common/math/box2d.h IsPointIn函数检查给定点是否位于由其中心、方向、长度和宽度定义的二维框内 1首先计算点相对于长方体中心的x和y分量 2然后它计算点与长方体沿x和y轴的距离同时考虑长方体的航向。使用航向的余弦和正弦来计算距离。 3最后如果两个距离都小于或等于长方体长度和宽度的一半加上一个小的ε值kMathEpsilon则函数返回true。添加此ε值是为了说明潜在的舍入误差。如果任一距离大于长方体的一半长度或宽度则函数返回false。 总体思路就是 1以Box的Center为原点heading方向为X’建立车身坐标系 2计算点在车身坐标系下的坐标 假设点的坐标 ( x p , y p ) (x_p,y_p) (xp,yp)Box中心坐标 ( x c , y c ) (x_c,y_c) (xc,yc),Box的heading角度 θ \theta θ,在局部坐标系下的坐标 [ d x d y ] [ c o s θ s i n θ − s i n θ c o s θ ] [ x p − x 0 y p − y 0 ] \begin{bmatrix} dx\\ dy \end{bmatrix}\begin{bmatrix} cos\theta sin\theta\\ -sin\theta cos\theta \end{bmatrix}\begin{bmatrix} x_p-x_0 \\ y_p-y_0 \end{bmatrix} [dxdy][cosθ−sinθsinθcosθ][xp−x0yp−y0] 旋转矩阵是 [ c o s θ − s i n θ s i n θ c o s θ ] \begin{bmatrix} cos\theta -sin\theta\\ sin\theta cos\theta \end{bmatrix} [cosθsinθ−sinθcosθ] 3判断新坐标的x和y绝对值是否小于半个Box宽度和长度
bool Box2d::IsPointIn(const Vec2d point) const {const double x0 point.x() - center_.x();const double y0 point.y() - center_.y();const double dx std::abs(x0 * cos_heading_ y0 * sin_heading_);const double dy std::abs(-x0 * sin_heading_ y0 * cos_heading_);return dx half_length_ kMathEpsilon dy half_width_ kMathEpsilon;
}判断一个点是否在Boundary上思路和上面的一样
bool Box2d::IsPointOnBoundary(const Vec2d point) const {const double x0 point.x() - center_.x();const double y0 point.y() - center_.y();const double dx std::abs(x0 * cos_heading_ y0 * sin_heading_);const double dy std::abs(x0 * sin_heading_ - y0 * cos_heading_);return (std::abs(dx - half_length_) kMathEpsilon dy half_width_ kMathEpsilon) ||(std::abs(dy - half_width_) kMathEpsilon dx half_length_ kMathEpsilon);
}double DistanceTo(const Vec2d point)计算Box到一个点的距离 1计算点在局部坐标系下的值 2如果 x p ′ x^{}_p xp′绝对值小于半个车长则直接用dy作为距离 3如果 y p ′ y^{}_p yp′绝对值小于半个车宽则直接用dx作为距离 其他情况返回 d x 2 d y 2 \sqrt{dx^2dy^2} dx2dy2 到角点的距离作为最终的距离
double Box2d::DistanceTo(const Vec2d point) const {const double x0 point.x() - center_.x();const double y0 point.y() - center_.y();const double dx std::abs(x0 * cos_heading_ y0 * sin_heading_) - half_length_;const double dy std::abs(x0 * sin_heading_ - y0 * cos_heading_) - half_width_;if (dx 0.0) {return std::max(0.0, dy);}if (dy 0.0) {return dx;}return hypot(dx, dy);
}1.3 多边形
不规则障碍物使用包围盒表示精度低用多边形Polygon2d来抽象表示。 modules/common/math/polygon2d.h 多边形可以用多个点来表示也可以用多个边来表示
std::vectorVec2d points_;
std::vectorLineSegment2d line_segments_;此函数用于从给定的一组点构建多边形 num_points_多边形中的点数。 points_Vec2d对象的矢量表示多边形的点。 area_多边形的面积。 line_segments_LineSegment2d对象的矢量表示连接多边形各点的线段。 is_convex_一个布尔标志指示多边形是否是凸的。 min_x_多边形aabox的最小x值。 max_x_多边形aabox的最大x值。 min_y_多边形aabox的最小y值。 max_y_多边形的aabox的最大y值。 void Polygon2d::BuildFromPoints() {num_points_ static_castint(points_.size());// 检查点的数量是否至少为3CHECK_GE(num_points_, 3);// 保证点顺时针area_ 0.0;for (int i 1; i num_points_; i) {// 使用连接每对点的向量的叉积来计算多边形的面积area_ CrossProd(points_[0], points_[i - 1], points_[i]);}// 如果面积为负值则反转点的顺序以确保它们按顺时针ccw顺序排列if (area_ 0) {area_ -area_;std::reverse(points_.begin(), points_.end());}area_ / 2.0;CHECK_GT(area_, kMathEpsilon);// 构建线段line_segments_.reserve(num_points_);// 通过迭代点并将每个点按ccw顺序连接到下一个点来构建线段向量for (int i 0; i num_points_; i) {line_segments_.emplace_back(points_[i], points_[Next(i)]);}// 检查凸性质is_convex_ true;for (int i 0; i num_points_; i) {// 通过计算连接前一点、当前点和下一点的向量的叉积来检查多边形是否是凸如果叉积小于或等于-kMathEpsilon则多边形不是凸的if (CrossProd(points_[Prev(i)], points_[i], points_[Next(i)]) -kMathEpsilon) {is_convex_ false;break;}}// 计算aabox.// 最后它通过找到点的最小值和最大值x和y来计算多边形的轴对齐边界框aaboxmin_x_ points_[0].x();max_x_ points_[0].x();min_y_ points_[0].y();max_y_ points_[0].y();for (const auto point : points_) {min_x_ std::min(min_x_, point.x());max_x_ std::max(max_x_, point.x());min_y_ std::min(min_y_, point.y());max_y_ std::max(max_y_, point.y());}
}叉积CrossProd物理含义是向量组成的平行四边形的面积除以二就是三角形的面积将所有三角形面积累加可以得到area_如果area_为负数说明角点按顺时针CW排列需要进行reverse从而保证所有的点是逆时针排列的CCW 如果连续的三个点 P i − 1 , P i , P i 1 P_{i-1},P_i,P_{i1} Pi−1,Pi,Pi1有 说明多边形非凸比如右侧就是
