【如果笔记对你有帮助，欢迎关注&点赞&收藏，收到正反馈会加快更新！谢谢支持！】
记录秋招遇到的一些手撕几何题
1. 判断一个点是否在多边形内
- 题意：判断 point (x, y)，是否在 polygon [(x1, y1), (x2, y2), ...] 内
- 方法：点向任意方向（通常水平向右）发射一条射线，统计射线与多边形边的交点数量。如果交点数量为奇数，则点在多边形内部；如果为偶数，则点在多边形外部
- 代码：
def is_point_in_polygon(point, polygon): """ 使用射线法判断点是否在多边形内部（包括边界）。 Args: point: 待判断的点，格式为 (x, y)。 polygon: 多边形的顶点列表，格式为 [(x1, y1), (x2, y2), ...]，顶点应按顺序连接。 Returns: bool: 如果点在多边形内或边界上，返回 True；否则返回 False。 """ x, y = point n = len(polygon) if n < 3: return False # 至少需要三个点才能构成多边形 inside = False p1x, p1y = polygon[0] for i in range(1, n + 1): p2x, p2y = polygon[i % n] # 取下一个顶点，实现首尾相连 # 首先检查点是否在当前边上（包括端点） if is_point_on_segment((x, y), (p1x, p1y), (p2x, p2y)): return True # 点在边界上，直接返回True # 核心逻辑：检查射线是否与当前边相交 # 1. 检查边的两个端点是否在射线（水平向右）的两侧（一个在y之上，一个在y之下或等于） if (p1y > y) != (p2y > y): # 利用不等式的异或效果判断是否跨越 # 2. 计算射线与边交点的x坐标 if p1y != p2y: # 避免除以零（水平边已由上面的条件排除） x_intersect = (y - p1y) * (p2x - p1x) / (p2y - p1y) + p1x # 3. 如果交点在当前点的右侧，则计数（翻转inside状态） if x_intersect > x: inside = not inside # 如果p1y == p2y，则为水平边，且由于y值相同，射线不会与其相交（除非重合，但重合已在边界检查中处理） p1x, p1y = p2x, p2y # 移动到下一条边 return inside def is_point_on_segment(point, segment_start, segment_end): """ 判断点是否在线段上（包括端点）。 Args: point: 待判断的点 (px, py)。 segment_start: 线段的起点 (x1, y1)。 segment_end: 线段的终点 (x2, y2)。 Returns: bool: 如果点在线段上返回 True，否则返回 False。 """ px, py = point x1, y1 = segment_start x2, y2 = segment_end # 先检查点是否在线段的轴对齐边界框内 if not (min(x1, x2) <= px <= max(x1, x2) and min(y1, y2) <= py <= max(y1, y2)): return False # 使用叉积原理判断点是否与线段共线 # 向量 (x2-x1, y2-y1) 与向量 (px-x1, py-y1) 的叉积应为0（或接近0，考虑浮点精度） cross_product = (px - x1) * (y2 - y1) - (py - y1) * (x2 - x1) # 使用一个很小的容差值来判断叉积是否接近0 return abs(cross_product) < 1e-10
2. 相对位置和朝向计算（坐标系变换）
- 题意：计算
Agent 1(x1, y1, theta1) 相对于Agent 0(x, y, theta) 的相对位置和朝向 - 方法：
- 相对位置：1. 计算位移向量；2. 旋转坐标系（使用旋转矩阵）
【关键代码】 dx = x1 - x0, dy = y1 - y0 rel_x = dx * cos(theta0) + dy * sin(theta0) rel_y = -dx * sin(theta0) + dy * cos(theta0) - 相对朝向：计算朝向角之差并归一化到 [-π, π] 范围
【关键代码】 rel_theta = (theta1 - theta0) rel_theta = (rel_theta + π) % (2*π) - π
- 相对位置：1. 计算位移向量；2. 旋转坐标系（使用旋转矩阵）
- 代码：
import math def calculate_relative_pose(agent0_pose, agent1_pose): """ 计算agent1相对于agent0的相对位置和朝向。 Args: agent0_pose: 元组 (x0, y0, theta0)，表示agent0的世界坐标和朝向（弧度）。 agent1_pose: 元组 (x1, y1, theta1)，表示agent1的世界坐标和朝向（弧度）。 Returns: 元组 (rel_x, rel_y, rel_theta)： rel_x, rel_y: agent1在agent0局部坐标系中的坐标。 rel_theta: agent1相对于agent0的朝向（弧度，范围[-π, π]）。 """ x0, y0, theta0 = agent0_pose x1, y1, theta1 = agent1_pose # 1. 计算从agent0指向agent1的位移向量 dx = x1 - x0 dy = y1 - y0 # 2. 计算相对位置：通过旋转矩阵将世界坐标变换到agent0的局部坐标系 cos_theta0 = math.cos(theta0) sin_theta0 = math.sin(theta0) rel_x = dx * cos_theta0 + dy * sin_theta0 rel_y = -dx * sin_theta0 + dy * cos_theta0 # 注意负号，这对应于逆时针旋转矩阵 # 3. 计算相对朝向：归一化到[-π, π]区间 rel_theta = theta1 - theta0 # 归一化步骤，确保角度在标准范围内 rel_theta = (rel_theta + math.pi) % (2 * math.pi) - math.pi return rel_x, rel_y, rel_theta
3. 矩阵的点到矩阵的最近距离
- 题意：有一个矩形（用中心点、长宽和旋转角表示），需要计算一个点到这个矩形的最短距离
- 方法：
- 坐标系变换：将点从世界坐标系变换到以矩形为中心的局部坐标系
- 计算局部距离：在局部坐标系中，计算点到轴对齐矩形的距离
- 代码：
import numpy as np import math def point_to_rotated_rectangle_distance(point, rect_center, rect_width, rect_height, rect_angle_deg): """ 计算点到旋转矩形的最近距离。 Args: point: 点的坐标，格式为 (x, y)。 rect_center: 矩形中心点坐标，格式为 (cx, cy)。 rect_width: 矩形的宽度（沿旋转前X轴方向）。 rect_height: 矩形的高度（沿旋转前Y轴方向）。 rect_angle_deg: 矩形的旋转角度（度），绕中心点逆时针旋转为正。 Returns: float: 点到矩形的最短距离。点在矩形内或边上时返回0。 """ # 将角度转换为弧度 angle_rad = np.radians(rect_angle_deg) cos_angle = np.cos(angle_rad) sin_angle = np.sin(angle_rad) # 1. 计算点相对于矩形中心的向量 dx = point[0] - rect_center[0] dy = point[1] - rect_center[1] # 2. 构造逆旋转矩阵，将点变换到矩形的局部坐标系（使矩形轴对齐） # 逆旋转矩阵是原旋转矩阵的转置 local_x = dx * cos_angle + dy * sin_angle local_y = -dx * sin_angle + dy * cos_angle # 3. 计算局部坐标系中矩形的半长半宽 half_width = rect_width / 2.0 half_height = rect_height / 2.0 # 4. 在局部坐标系中计算点到轴对齐矩形的距离 # 计算点在X和Y方向上到矩形边界的有符号距离 # 如果点在矩形内，距离为负值 dx_clamped = max(-half_width, min(local_x, half_width)) dy_clamped = max(-half_height, min(local_y, half_height)) # 最近点是在矩形边界上夹紧后的点 closest_x = dx_clamped closest_y = dy_clamped # 计算局部坐标系中点到最近点的向量 diff_x = local_x - closest_x diff_y = local_y - closest_y # 5. 计算欧氏距离 distance = math.sqrt(diff_x * diff_x + diff_y * diff_y) return distance
4. 判断两根线段是否相交
- 方法：结合快速排斥实验和跨立实验
- 快速排斥实验：如果两条线段相交，那么以它们为对角线的最小外接矩形在平面上必然有重叠部分
- 跨立实验：当A和点B在线段CD的两侧，且C和点D是否在线段AB的两侧时，两条线段相交
- 代码：
def cross_product(o, a, b): """ 计算向量oa与向量ob的叉积。 叉积 > 0：ob在oa的逆时针方向 叉积 < 0：ob在oa的顺时针方向 叉积 = 0：o, a, b 三点共线 """ return (a[0] - o[0]) * (b[1] - o[1]) - (b[0] - o[0]) * (a[1] - o[1]) def is_intersect_custom(A, B, C, D): """ 通过快速排斥实验和跨立实验判断线段AB与CD是否相交，并求交点。 参数: A, B: 线段AB的端点，格式为 (x, y) C, D: 线段CD的端点，格式为 (x, y) 返回: 如果相交，返回交点坐标 (x, y)；如果不相交，返回 None。 """ # 解构坐标，便于书写 x1, y1 = A x2, y2 = B x3, y3 = C x4, y4 = D # 1. 快速排斥实验：检查以两条线段为对角线的矩形是否相交 # 如果连矩形都不相交，线段必然不相交 if max(x1, x2) < min(x3, x4) or max(x3, x4) < min(x1, x2) or \ max(y1, y2) < min(y3, y4) or max(y3, y4) < min(y1, y2): return None # 2. 跨立实验：判断点C和D是否在线段AB的两侧，并且点A和B是否在线段CD的两侧 # 计算向量叉积 d1 = cross_product(A, B, C) # 向量AB × 向量AC d2 = cross_product(A, B, D) # 向量AB × 向量AD d3 = cross_product(C, D, A) # 向量CD × 向量CA d4 = cross_product(C, D, B) # 向量CD × 向量CB # 如果线段AB跨立线段CD，且线段CD也跨立线段AB，则两线段相交 if d1 * d2 <= 0 and d3 * d4 <= 0: # 计算交点坐标（基于参数方程求解）[3,6](@ref) denominator = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4) if denominator == 0: # 分母为0，说明线段平行或共线，本例中已由跨立实验排除不相交情况 return None # 计算交点坐标 t = ((x1 - x3) * (y3 - y4) - (y1 - y3) * (x3 - x4)) / denominator intersection_x = x1 + t * (x2 - x1) intersection_y = y1 + t * (y2 - y1) return (intersection_x, intersection_y) return None
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
