E-Dreamer

脚踏实地，仰望星空
  博客园  :: 首页  :: 新随笔  :: 联系 :: 订阅 订阅  :: 管理
  69 随笔 :: 0 文章 :: 17 评论 :: 11万 阅读
碰撞检测技术
常用方法： AABB检测、OBB检测、python shapely 库方法等

AABB 与 OBB区别

OBB

AABB

重点： 是否 带 旋转

obb检测原理

OBB间的相交测试基于分离轴理论（separating axis theory）。若两个OBB在一条轴线上（不一定是坐标轴）上的投影不重叠，则这条轴称为分离轴。若一对OBB间存在一条分离轴，则可以判定这两个OBB不相交。对任何两个不相交的凸三维多面体，其分离轴要么垂直于任何一个多面体的某一个面，要么同时垂直于每个多面体的某一条边。因此，对一对OBB，只需测试15条可能是分离轴的轴（每个OBB的3个面方向再加上每个OBB的3个边方面的两两组合），只要找到一条这样的分离轴，就可以判定这两个OBB是不相交的，如果这15条轴都不能将这两个OBB分离，则它们是相交的。

Shapely api

https://www.osgeo.cn/shapely/manual.html

Shapely （2d） 与 OBB (3d) 程序及可视化包围框实现

注：代码只是个人项目使用，这里只做记录，读者可能拷贝不能直接使用。

?
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
import numpy as np
import pandas as pd
import random
import os
import sys ,cv2, mmcv
from shapely.geometry import Polygon
import math
 
class OBB:
    def __init__(self,point_set:np.ndarray):    # （point_set: [8,3]）
        self.pos=(point_set[0]+point_set[6])/2 #包围盒中心点位置
        self.axisX=self.__norm(point_set[1]-point_set[0]) #包围盒x轴的方向向量
        self.axisY=self.__norm(point_set[3]-point_set[0]) #包围盒y轴的方向向量
        self.axisZ=self.__norm(point_set[0]-point_set[4]) #包围盒z轴的方向向量
        self.half_size=np.array([self.__get_distance(point_set[0],point_set[1])/2,
                                 self.__get_distance(point_set[0],point_set[3])/2,
                                 self.__get_distance(point_set[0],point_set[4])/2])
 
    def __norm(self,vector): #将向量归一化为标准向量
        s=0
        for e in vector:
            s+=e*e
        return vector/(s**0.5)
 
    def __get_distance(self,point_1,point_2): #计算两个点的距离
        return ((point_1[0]-point_2[0])**2+(point_1[1]-point_2[1])**2+(point_1[2]-point_2[2])**2)**0.5
 
def cross_product(vector1,vector2): #向量积
    return np.array([vector1[1]*vector2[2]-vector1[2]*vector2[1],vector1[2]*vector2[0]-vector1[0]*vector2[2],vector1[0]*vector2[1]-vector1[1]*vector2[0]])
 
def getSeparatingPlane(r_pos,plane,box1:OBB,box2:OBB): #判断在选定的坐标平面是否有分割平面
    return ((abs(sum(r_pos*plane)) >
        (abs(sum((box1.axisX*box1.half_size[0])*plane)) +
        abs(sum((box1.axisY*box1.half_size[1])*plane)) +
        abs(sum((box1.axisZ*box1.half_size[2])*plane)) +
        abs(sum((box2.axisX*box2.half_size[0])*plane)) +
        abs(sum((box2.axisY*box2.half_size[1])*plane)) +
        abs(sum((box2.axisZ*box2.half_size[2])*plane)))))
 
def isCollision(box1:OBB,box2:OBB): #判断两个OBB是否发生碰撞
    r_pos=box2.pos-box1.pos
    if not (getSeparatingPlane(r_pos, box1.axisX, box1, box2) or getSeparatingPlane(r_pos, box1.axisY, box1, box2) or
            getSeparatingPlane(r_pos, box1.axisZ, box1, box2) or getSeparatingPlane(r_pos, box2.axisX, box1, box2) or
            getSeparatingPlane(r_pos, box2.axisY, box1, box2) or getSeparatingPlane(r_pos, box2.axisZ, box1, box2) or
            getSeparatingPlane(r_pos, cross_product(box1.axisX,box2.axisX), box1, box2) or
            getSeparatingPlane(r_pos, cross_product(box1.axisX,box2.axisY), box1, box2) or
            getSeparatingPlane(r_pos, cross_product(box1.axisX,box2.axisZ), box1, box2) or
            getSeparatingPlane(r_pos, cross_product(box1.axisY,box2.axisX), box1, box2) or
            getSeparatingPlane(r_pos, cross_product(box1.axisY,box2.axisY), box1, box2) or
            getSeparatingPlane(r_pos, cross_product(box1.axisY,box2.axisZ), box1, box2) or
            getSeparatingPlane(r_pos, cross_product(box1.axisZ,box2.axisX), box1, box2) or
            getSeparatingPlane(r_pos, cross_product(box1.axisZ,box2.axisY), box1, box2) or
            getSeparatingPlane(r_pos, cross_product(box1.axisZ,box2.axisZ), box1, box2)):
        return True
    else:
        return False
 
def create_bb_3d(Um_L,Um_W,Um_H,trans_pos,trans_rot):
    T_x = pos_to_transmatrix(trans_rot, trans_pos)
    R_mat = T_x[0:3, 0:3]
    T_mat = T_x[0:3, 3]
    l = Um_L/2
    w = Um_W /2
    h = Um_H / 2
    bb = np.zeros((8,3))
    xm = 0
    ym = 0
    zm = 0
    bb[0] = np.array([xm + l,ym -w, zm + h])
    bb[1] = np.array([xm + l,ym +w, zm + h])
    bb[2] = np.array([xm - l,ym +w, zm + h])
    bb[3] = np.array([xm - l,ym -w, zm + h])
    bb[4] = np.array([xm + l,ym -w, zm - h])
    bb[5] = np.array([xm + l,ym +w, zm - h])
    bb[6] = np.array([xm - l,ym +w, zm - h])
    bb[7] = np.array([xm - l,ym -w, zm - h])
    bb = R_mat.dot(bb.T).T + T_mat
    return bb
 
def create_bb_2d(Um_L,Um_W,Um_H,trans_pos,trans_rot):
    T_x = pos_to_transmatrix(trans_rot, trans_pos)
    R_mat = T_x[0:3, 0:3]
    T_mat = T_x[0:3, 3]
    l = Um_L / 2
    w = Um_W / 2
    h = Um_H / 2
    res = []
    bb = np.zeros((8,3))
    xm = 0
    ym = 0
    bb[0] = np.array([xm + l,ym -w, 0])
    bb[1] = np.array([xm + l,ym +w, 0])
    bb[2] = np.array([xm - l,ym +w, 0])
    bb[3] = np.array([xm - l,ym -w, 0])
    bb = R_mat.dot(bb.T).T + T_mat
    res.append((bb[0][0],bb[0][1]))
    res.append((bb[1][0],bb[1][1]))
    res.append((bb[2][0],bb[2][1]))
    res.append((bb[3][0],bb[3][1]))
    return res
def InitCanvas(width, height, color=(255, 255, 255)):
    canvas = np.ones((height, width, 3), dtype="uint8")
    canvas[:] = color
    return canvas
COLOR_MAP = {
    "white": (255, 255, 255),
    "green": (0, 255, 0),
    "red": (0, 0, 255),
    "blue" :(255, 0, 0),
 
}
canvas = InitCanvas(600, 600, color=COLOR_MAP['white'])
 
def vis(box,img,angle):
    bounds = box.bounds
    cx = (bounds[2] + bounds[0]) / 2
    cy = (bounds[1] + bounds[3]) / 2
    w = bounds[2] - bounds[0]
    h = bounds[3] - bounds[1]
    cosA = math.cos(angle)
    sinA = math.sin(angle)
    x1 = cx - 0.5 * w
    y1 = cy - 0.5 * h
    x0 = cx + 0.5 * w
    y0 = y1
    x2 = x1
    y2 = cy + 0.5 * h
    x3 = x0
    y3 = y2
 
    x0n = (x0 - cx) * cosA - (y0 - cy) * sinA + cx
    y0n = (x0 - cx) * sinA + (y0 - cy) * cosA + cy
    x1n = (x1 - cx) * cosA - (y1 - cy) * sinA + cx
    y1n = (x1 - cx) * sinA + (y1 - cy) * cosA + cy
    x2n = (x2 - cx) * cosA - (y2 - cy) * sinA + cx
    y2n = (x2 - cx) * sinA + (y2 - cy) * cosA + cy
    x3n = (x3 - cx) * cosA - (y3 - cy) * sinA + cx
    y3n = (x3 - cx) * sinA + (y3 - cy) * cosA + cy
 
    # 根据得到的点，画出矩形框
    r = random.randint(0, 255)
    g = random.randint(0, 255)
    b = random.randint(0, 255)
    c = (r, g, b)
    thickness = 1
    cv2.line(img, (int(x0n), int(y0n)), (int(x1n), int(y1n)), c,thickness,lineType=cv2.LINE_AA)
    cv2.line(img, (int(x1n), int(y1n)), (int(x2n), int(y2n)), c,thickness,lineType=cv2.LINE_AA)
    cv2.line(img, (int(x2n), int(y2n)), (int(x3n), int(y3n)), c,thickness,lineType=cv2.LINE_AA)
    cv2.line(img, (int(x0n), int(y0n)), (int(x3n), int(y3n)), c,thickness,lineType=cv2.LINE_AA)
 
file_names = glob.glob("/home/liuq13/Documents/xworld_drive/xtraffic/cross/*.wpb")
world_proto = xworld_data.Xworld_session()
python_ue5_data = pd.read_csv("/home/liuq13/Documents/xworld_drive/python_ue5_data.csv")
obj_list_all = python_ue5_data['python_path'].to_list()
for file_name in file_names:
    print(file_name)
    with open(file_name, "rb") as f:
        msg = f.read()
 
    world_proto.ParseFromString(msg)
    # print('-----------------obj len', len(world_proto.obj_firm_data))
    flag_2d = False
    flag_3d = False
    bb_list = []
    bb_list_2d = []
    angle_list =[]
 
    if "right" in file_name:
        offset = 180
    else:
        offset = 0
 
    for i in range(0,len(world_proto.obj_firm_data)):
        item = world_proto.obj_firm_data[i]
        index = obj_list_all.index(item.obj_path)
        # print(item.obj_path)
        # print(python_ue5_data.iloc[index, 4])
        Um_L , Um_H,Um_W = python_ue5_data.iloc[index, 9],python_ue5_data.iloc[index, 10],python_ue5_data.iloc[index, 11]
        if i == 0:  # 自车
            Um_L, Um_H, Um_W = 4.860 ,1.450,2.123
 
        trans_rot = vec2ndpos(item.trans_rot)
        trans_pos = vec2ndpos(item.trans_pos)
        trans_rot[2] += offset
        bb = create_bb_3d(Um_L,Um_W,Um_H,trans_pos, trans_rot)
        bb_list.append(bb)
        bb_2d = create_bb_2d(Um_L,Um_W,Um_H,trans_pos,trans_rot)
        bb_list_2d.append(bb_2d)
        angle_list.append(trans_rot[2])
 
    for i in range(len(bb_list)-1):
        if flag_3d == True:
            break
        for j in range(i+1,len(bb_list)):
            # print("i, j:",i,j)
            box1 = OBB(bb_list[i])
            box2 = OBB(bb_list[j])
            flag_3d = isCollision(box1,box2)
            if flag_3d == True:
                break
 
    for box,angle  in zip(bb_list_2d,angle_list):
        # print(angle )
        bb = Polygon(box)
        vis(bb, canvas, angle)
        # cv2.imshow("Canvas", canvas)
        # cv2.waitKey(0)
 
    for i in range(len(bb_list_2d)-1):
        if flag_2d == True:
            break
        box1 = Polygon(bb_list_2d[i])
        for j in range(i+1,len(bb_list_2d)):
            box2 = Polygon(bb_list_2d[j])
            flag_2d = box1.intersects(box2)
            # print(flag_2d)
            if flag_2d == True:
                break
 
    print("flag_2d,flag_3d：",flag_2d,flag_3d)
 
    # cv2.imshow("Canvas", canvas)
    # mmcv.imshow(canvas,"res",wait_time=100000)
    # cv2.imwrite("draw_rectangle.png", canvas)
    # cv2.waitKey(0)   

　　

 
 
posted on   E-Dreamer  阅读(137)  评论(0)  编辑  收藏  举报
相关博文：
·  pyqt 实现双y轴曲线显示
·  可视化——将一个变量转换为颜色
·  自动驾驶运动规划:碰撞检测算法之分离轴定理
·  边缘碰撞检测
·  【Unity TIL】6. 如何判断两条线段是否相交
阅读排行：
· winform 绘制太阳，地球，月球 运作规律
· AI与.NET技术实操系列（五）：向量存储与相似性搜索在 .NET 中的实现
· 超详细：普通电脑也行Windows部署deepseek R1训练数据并当服务器共享给他人
· 【硬核科普】Trae如何「偷看」你的代码？零基础破解AI编程运行原理
· 上周热点回顾（3.3-3.9）
历史上的今天：
2020-04-19 位运算在编程题的一些作用
点击右上角即可分享
微信分享提示
AI IDE Trae