saberbud/PathPlanning
1
0
Fork 0
mirror of https://github.com/zhm-real/PathPlanning.git synced 2026-08-29 08:34:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

'OBB'

Browse Source
This commit is contained in:
yue qi
2020-07-16 23:16:21 -07:00
parent 15bed564a7
commit 3fea283419
1 changed files with 116 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Search-based Planning/Search_3D/utils3D.py
+116 -2
View File
@@ -71,6 +71,7 @@ def lineSphere(p0, p1, ball):
def lineAABB(p0, p1, dist, aabb):
# https://www.gamasutra.com/view/feature/131790/simple_intersection_tests_for_games.php?print=1
# aabb should have the attributes of P, E as center point and extents
mid = [(p0[0] + p1[0]) / 2, (p0[1] + p1[1]) / 2, (p0[2] + p1[2]) / 2] # mid point
I = [(p1[0] - p0[0]) / dist, (p1[1] - p0[1]) / dist, (p1[2] - p0[2]) / dist] # unit direction
hl = dist / 2 # radius
@@ -93,6 +94,113 @@ def lineAABB(p0, p1, dist, aabb):
return True
def OBBOBB(obb1, obb2):
# https://www.gamasutra.com/view/feature/131790/simple_intersection_tests_for_games.php?print=1
# each obb class should contain attributes:
# E: extents along three principle axis in R3
# P: position of the center axis in R3
# O: orthornormal basis in R3*3
a , b = np.array(obb1.E), np.array(obb2.E)
Pa, Pb = np.array(obb1.P), np.array(obb2.P)
A , B = np.array(obb1.O), np.array(obb2.O)
# check if two oriented bounding boxes overlap
# translation, in parent frame
v = Pb - Pa
# translation, in A's frame
# vdotA[0],vdotA[1],vdotA[2]
T = [v@B[0], v@B[1], v@B[2]]
R = np.zeros([3,3])
for i in range(0,3):
for k in range(0,3):
R[i][k] = A[i]@B[k]
# use separating axis thm for all 15 separating axes
# if the separating axis cannot be found, then overlap
# A's basis vector
for i in range(0,3):
ra = a[i]
rb = b[0]*abs(R[i][0]) + b[1]*abs(R[i][1]) + b[2]*abs(R[i][2])
t = abs(T[i])
if t > ra + rb:
return False
for k in range(0,3):
ra = a[0]*abs(R[0][k]) + a[1]*abs(R[1][k]) + a[2]*abs(R[2][k])
rb = b[k]
t = abs(T[0]*R[0][k] + T[1]*R[1][k] + T[2]*R[2][k])
if t > ra + rb:
return False
#9 cross products
#L = A0 x B0
ra = a[1]*abs(R[2][0]) + a[2]*abs(R[1][0])
rb = b[1]*abs(R[0][2]) + b[2]*abs(R[0][1])
t = abs(T[2]*R[1][0] - T[1]*R[2][0])
if t > ra + rb:
return False
#L = A0 x B1
ra = a[1]*abs(R[2][1]) + a[2]*abs(R[1][1])
rb = b[0]*abs(R[0][2]) + b[2]*abs(R[0][0])
t = abs(T[2]*R[1][1] - T[1]*R[2][1])
if t > ra + rb:
return False
#L = A0 x B2
ra = a[1]*abs(R[2][2]) + a[2]*abs(R[1][2])
rb = b[0]*abs(R[0][1]) + b[1]*abs(R[0][0])
t = abs(T[2]*R[1][2] - T[1]*R[2][2])
if t > ra + rb:
return False
#L = A1 x B0
ra = a[0]*abs(R[2][0]) + a[2]*abs(R[0][0])
rb = b[1]*abs(R[1][2]) + b[2]*abs(R[1][1])
t = abs( T[0]*R[2][0] - T[2]*R[0][0] )
if t > ra + rb:
return False
# L = A1 x B1
ra = a[0]*abs(R[2][1]) + a[2]*abs(R[0][1])
rb = b[0]*abs(R[1][2]) + b[2]*abs(R[1][0])
t = abs( T[0]*R[2][1] - T[2]*R[0][1] )
if t > ra + rb:
return False
#L = A1 x B2
ra = a[0]*abs(R[2][2]) + a[2]*abs(R[0][2])
rb = b[0]*abs(R[1][1]) + b[1]*abs(R[1][0])
t = abs( T[0]*R[2][2] - T[2]*R[0][2] )
if t > ra + rb:
return False
#L = A2 x B0
ra = a[0]*abs(R[1][0]) + a[1]*abs(R[0][0])
rb = b[1]*abs(R[2][2]) + b[2]*abs(R[2][1])
t = abs( T[1]*R[0][0] - T[0]*R[1][0] )
if t > ra + rb:
return False
# L = A2 x B1
ra = a[0]*abs(R[1][1]) + a[1]*abs(R[0][1])
rb = b[0] *abs(R[2][2]) + b[2]*abs(R[2][0])
t = abs( T[1]*R[0][1] - T[0]*R[1][1] )
if t > ra + rb:
return False
#L = A2 x B2
ra = a[0]*abs(R[1][2]) + a[1]*abs(R[0][2])
rb = b[0]*abs(R[2][1]) + b[1]*abs(R[2][0])
t = abs( T[1]*R[0][2] - T[0]*R[1][2] )
if t > ra + rb:
return False
# no separating axis found,
# the two boxes overlap
return True
def StateSpace(env, factor=0):
boundary = env.boundary
@@ -195,7 +303,13 @@ def initcost(initparams):
c[xi][child] = cost(initparams, xi, child)
return c
class obb(object):
def __init__(self, P, E, O):
self.P = P
self.E = E
self.O = O
if __name__ == "__main__":
a = '()'
print(list(a))
obb1 = obb([0,0,0],[1,1,1],[[1,0,0],[0,1,0],[0,0,1]])
obb2 = obb([1,1,0],[1,1,1],[[1/np.sqrt(3)*1,1/np.sqrt(3)*1,1/np.sqrt(3)*1],[np.sqrt(3/2)*(-1/3),np.sqrt(3/2)*2/3,np.sqrt(3/2)*(-1/3)],[np.sqrt(1/8)*(-2),0,np.sqrt(1/8)*2]])
print(OBBOBB(obb1, obb2))