碰撞检测技术

常用方法： 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) 程序及可视化包围框实现

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

      
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)    

　　

 

 

TRANSLATE with x

English

Arabic	Hebrew	Polish
Bulgarian	Hindi	Portuguese
Catalan	Hmong Daw	Romanian
Chinese Simplified	Hungarian	Russian
Chinese Traditional	Indonesian	Slovak
Czech	Italian	Slovenian
Danish	Japanese	Spanish
Dutch	Klingon	Swedish
English	Korean	Thai
Estonian	Latvian	Turkish
Finnish	Lithuanian	Ukrainian
French	Malay	Urdu
German	Maltese	Vietnamese
Greek	Norwegian	Welsh
Haitian Creole	Persian

 

TRANSLATE with

COPY THE URL BELOW

Back

EMBED THE SNIPPET BELOW IN YOUR SITE

Enable collaborative features and customize widget: Bing Webmaster Portal

Back