diff --git a/Sampling-based Planning/rrt_3D/__pycache__/env3D.cpython-37.pyc b/Sampling-based Planning/rrt_3D/__pycache__/env3D.cpython-37.pyc index 87f168d..66c29a4 100644 Binary files a/Sampling-based Planning/rrt_3D/__pycache__/env3D.cpython-37.pyc and b/Sampling-based Planning/rrt_3D/__pycache__/env3D.cpython-37.pyc differ diff --git a/Sampling-based Planning/rrt_3D/__pycache__/plot_util3D.cpython-37.pyc b/Sampling-based Planning/rrt_3D/__pycache__/plot_util3D.cpython-37.pyc index 227df59..754c2e4 100644 Binary files a/Sampling-based Planning/rrt_3D/__pycache__/plot_util3D.cpython-37.pyc and b/Sampling-based Planning/rrt_3D/__pycache__/plot_util3D.cpython-37.pyc differ diff --git a/Sampling-based Planning/rrt_3D/__pycache__/utils3D.cpython-37.pyc b/Sampling-based Planning/rrt_3D/__pycache__/utils3D.cpython-37.pyc index d4129a7..cadb6c7 100644 Binary files a/Sampling-based Planning/rrt_3D/__pycache__/utils3D.cpython-37.pyc and b/Sampling-based Planning/rrt_3D/__pycache__/utils3D.cpython-37.pyc differ diff --git a/Sampling-based Planning/rrt_3D/plot_util3D.py b/Sampling-based Planning/rrt_3D/plot_util3D.py index a05ca61..5aea9a6 100644 --- a/Sampling-based Planning/rrt_3D/plot_util3D.py +++ b/Sampling-based Planning/rrt_3D/plot_util3D.py @@ -6,21 +6,24 @@ import mpl_toolkits.mplot3d as plt3d from mpl_toolkits.mplot3d import proj3d import numpy as np -def CreateSphere(center,r): - u = np.linspace(0,2* np.pi,30) - v = np.linspace(0,np.pi,30) - x = np.outer(np.cos(u),np.sin(v)) - y = np.outer(np.sin(u),np.sin(v)) - z = np.outer(np.ones(np.size(u)),np.cos(v)) - x, y, z = r*x + center[0], r*y + center[1], r*z + center[2] - return (x,y,z) -def draw_Spheres(ax,balls): +def CreateSphere(center, r): + u = np.linspace(0, 2 * np.pi, 30) + v = np.linspace(0, np.pi, 30) + x = np.outer(np.cos(u), np.sin(v)) + y = np.outer(np.sin(u), np.sin(v)) + z = np.outer(np.ones(np.size(u)), np.cos(v)) + x, y, z = r * x + center[0], r * y + center[1], r * z + center[2] + return (x, y, z) + + +def draw_Spheres(ax, balls): for i in balls: - (xs,ys,zs) = CreateSphere(i[0:3],i[-1]) - ax.plot_wireframe(xs, ys, zs, alpha=0.15,color="b") + (xs, ys, zs) = CreateSphere(i[0:3], i[-1]) + ax.plot_wireframe(xs, ys, zs, alpha=0.15, color="b") -def draw_block_list(ax, blocks ,color=None,alpha=0.15): + +def draw_block_list(ax, blocks, color=None, alpha=0.15): ''' drawing the blocks on the graph ''' @@ -42,18 +45,19 @@ def draw_block_list(ax, blocks ,color=None,alpha=0.15): h = ax.add_collection3d(pc) return h + def obb_verts(obb): # 0.017004013061523438 for 1000 iters - ori_body = np.array([[1,1,1],[-1,1,1],[-1,-1,1],[1,-1,1],\ - [1,1,-1],[-1,1,-1],[-1,-1,-1],[1,-1,-1]]) + ori_body = np.array([[1, 1, 1], [-1, 1, 1], [-1, -1, 1], [1, -1, 1], \ + [1, 1, -1], [-1, 1, -1], [-1, -1, -1], [1, -1, -1]]) # P + (ori * E) - ori_body = np.multiply(ori_body,obb.E) + ori_body = np.multiply(ori_body, obb.E) # obb.O is orthornormal basis in {W}, aka rotation matrix in SO(3) - verts = (obb.O@ori_body.T).T + obb.P + verts = (obb.O @ ori_body.T).T + obb.P return verts -def draw_obb(ax, OBB, color=None,alpha=0.15): +def draw_obb(ax, OBB, color=None, alpha=0.15): f = np.array([[0, 1, 5, 4], [1, 2, 6, 5], [2, 3, 7, 6], [3, 0, 4, 7], [0, 1, 2, 3], [4, 5, 6, 7]]) n = OBB.shape[0] vl = np.zeros((8 * n, 3)) @@ -70,7 +74,7 @@ def draw_obb(ax, OBB, color=None,alpha=0.15): return h -def draw_line(ax,SET,visibility=1,color=None): +def draw_line(ax, SET, visibility=1, color=None): if SET != []: for i in SET: xs = i[0][0], i[1][0] @@ -79,8 +83,9 @@ def draw_line(ax,SET,visibility=1,color=None): line = plt3d.art3d.Line3D(xs, ys, zs, alpha=visibility, color=color) ax.add_line(line) + def visualization(initparams): - if initparams.ind % 20 == 0 or initparams.done: + if initparams.ind % 100 == 0 or initparams.done: V = np.array(list(initparams.V)) E = initparams.E Path = np.array(initparams.Path) @@ -89,33 +94,34 @@ def visualization(initparams): edges = E.get_edge() # generate axis objects ax = plt.subplot(111, projection='3d') - #ax.view_init(elev=0.+ 0.03*initparams.ind/(2*np.pi), azim=90 + 0.03*initparams.ind/(2*np.pi)) - #ax.view_init(elev=0., azim=90.) + # ax.view_init(elev=0.+ 0.03*initparams.ind/(2*np.pi), azim=90 + 0.03*initparams.ind/(2*np.pi)) + # ax.view_init(elev=0., azim=90.) ax.view_init(elev=8., azim=120.) - #ax.view_init(elev=-8., azim=180) + # ax.view_init(elev=-8., azim=180) ax.clear() # drawing objects draw_Spheres(ax, initparams.env.balls) draw_block_list(ax, initparams.env.blocks) if initparams.env.OBB is not None: - draw_obb(ax,initparams.env.OBB) - draw_block_list(ax, np.array([initparams.env.boundary]),alpha=0) - draw_line(ax,edges,visibility=0.25) - draw_line(ax,Path,color='r') + draw_obb(ax, initparams.env.OBB) + draw_block_list(ax, np.array([initparams.env.boundary]), alpha=0) + draw_line(ax, edges, visibility=0.25) + draw_line(ax, Path, color='r') if len(V) > 0: - ax.scatter3D(V[:, 0], V[:, 1], V[:, 2], s=2, color='g',) + ax.scatter3D(V[:, 0], V[:, 1], V[:, 2], s=2, color='g', ) ax.plot(start[0:1], start[1:2], start[2:], 'go', markersize=7, markeredgecolor='k') - ax.plot(goal[0:1], goal[1:2], goal[2:], 'ro', markersize=7, markeredgecolor='k') + ax.plot(goal[0:1], goal[1:2], goal[2:], 'ro', markersize=7, markeredgecolor='k') # adjust the aspect ratio xmin, xmax = initparams.env.boundary[0], initparams.env.boundary[3] ymin, ymax = initparams.env.boundary[1], initparams.env.boundary[4] zmin, zmax = initparams.env.boundary[2], initparams.env.boundary[5] - dx, dy, dz = xmax-xmin, ymax-ymin, zmax-zmin - ax.get_proj = make_get_proj(ax,1*dx, 1*dy, 2*dy) + dx, dy, dz = xmax - xmin, ymax - ymin, zmax - zmin + ax.get_proj = make_get_proj(ax, 1 * dx, 1 * dy, 2 * dy) plt.xlabel('x') plt.ylabel('y') plt.pause(0.0001) + def make_get_proj(self, rx, ry, rz): ''' Return a variation on :func:`~mpl_toolkit.mplot2d.axes3d.Axes3D.getproj` that @@ -123,12 +129,14 @@ def make_get_proj(self, rx, ry, rz): ''' rm = max(rx, ry, rz) - kx = rm / rx; ky = rm / ry; kz = rm / rz + kx = rm / rx; + ky = rm / ry; + kz = rm / rz # Copied directly from mpl_toolkit/mplot3d/axes3d.py. New or modified lines are # marked by ## def get_proj(): - relev, razim = np.pi * self.elev/180, np.pi * self.azim/180 + relev, razim = np.pi * self.elev / 180, np.pi * self.azim / 180 xmin, xmax = self.get_xlim3d() ymin, ymax = self.get_ylim3d() @@ -140,35 +148,37 @@ def make_get_proj(self, rx, ry, rz): zmin, zmax) ratio = 0.5 # adjust the aspect ratio ## - aspectM = proj3d.world_transformation(-kx + 1, kx, ## - -ky + 1, ky, ## - -kz + 1, kz) ## + aspectM = proj3d.world_transformation(-kx + 1, kx, ## + -ky + 1, ky, ## + -kz + 1, kz) ## # look into the middle of the new coordinates R = np.array([0.5, 0.5, 0.5]) - xp = R[0] + np.cos(razim) * np.cos(relev) * self.dist *ratio - yp = R[1] + np.sin(razim) * np.cos(relev) * self.dist *ratio - zp = R[2] + np.sin(relev) * self.dist *ratio + xp = R[0] + np.cos(razim) * np.cos(relev) * self.dist * ratio + yp = R[1] + np.sin(razim) * np.cos(relev) * self.dist * ratio + zp = R[2] + np.sin(relev) * self.dist * ratio E = np.array((xp, yp, zp)) self.eye = E self.vvec = R - E self.vvec = self.vvec / np.linalg.norm(self.vvec) - if abs(relev) > np.pi/2: + if abs(relev) > np.pi / 2: # upside down V = np.array((0, 0, -1)) else: V = np.array((0, 0, 1)) - zfront, zback = -self.dist *ratio, self.dist *ratio + zfront, zback = -self.dist * ratio, self.dist * ratio viewM = proj3d.view_transformation(E, R, V) perspM = proj3d.persp_transformation(zfront, zback) - M0 = np.dot(viewM, np.dot(aspectM, worldM)) ## + M0 = np.dot(viewM, np.dot(aspectM, worldM)) ## M = np.dot(perspM, M0) return M + return get_proj + if __name__ == '__main__': - pass \ No newline at end of file + pass diff --git a/Sampling-based Planning/rrt_3D/rrt3D.py b/Sampling-based Planning/rrt_3D/rrt3D.py index 0261b2b..3ea9729 100644 --- a/Sampling-based Planning/rrt_3D/rrt3D.py +++ b/Sampling-based Planning/rrt_3D/rrt3D.py @@ -14,7 +14,8 @@ import sys sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling-based Planning/") from rrt_3D.env3D import env -from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path, edgeset, hash3D, dehash +from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path, edgeset, \ + hash3D, dehash class rrtstar(): @@ -25,18 +26,18 @@ class rrtstar(): self.E = edgeset() self.i = 0 self.maxiter = 10000 - self.stepsize = 0.5 + self.stepsize = 1.0 self.Path = [] self.done = False def wireup(self, x, y): - self.E.add_edge([x,y]) # add edge + self.E.add_edge([x, y]) # add edge self.Parent[hash3D(x)] = y def run(self): self.V.append(self.env.start) self.ind = 0 - self.fig = plt.figure(figsize = (10,8)) + self.fig = plt.figure(figsize=(10, 8)) xnew = self.env.start while self.ind < self.maxiter and getDist(xnew, self.env.goal) > 1: xrand = sampleFree(self) @@ -45,7 +46,7 @@ class rrtstar(): if not isCollide(self, xnearest, xnew): self.V.append(xnew) # add point self.wireup(xnew, xnearest) - # visualization(self) + visualization(self) self.i += 1 self.ind += 1 if getDist(xnew, self.env.goal) <= 1: diff --git a/Sampling-based Planning/rrt_3D/utils3D.py b/Sampling-based Planning/rrt_3D/utils3D.py index a63385e..ac7c45f 100644 --- a/Sampling-based Planning/rrt_3D/utils3D.py +++ b/Sampling-based Planning/rrt_3D/utils3D.py @@ -4,13 +4,16 @@ import pyrr as pyrr import os import sys -sys.path.append(os.path.dirname(os.path.abspath(__file__))+"/../../Sampling-based Planning/") + +sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling-based Planning/") from rrt_3D.plot_util3D import visualization + def getRay(x, y): direc = [y[0] - x[0], y[1] - x[1], y[2] - x[2]] return np.array([x, direc]) + def getAABB(blocks): AABB = [] for i in blocks: @@ -21,6 +24,7 @@ def getAABB(blocks): def getDist(pos1, pos2): return np.sqrt(sum([(pos1[0] - pos2[0]) ** 2, (pos1[1] - pos2[1]) ** 2, (pos1[2] - pos2[2]) ** 2])) + ''' The following utils can be used for rrt or rrt*, required param initparams should have env, environement generated from env3D @@ -40,9 +44,10 @@ def sampleFree(initparams): if isinside(initparams, x): return sampleFree(initparams) else: - if i < 0.05: - return initparams.env.goal+0.01 - else: return np.array(x) + if i < 0.1: + return initparams.env.goal + 1 + else: + return np.array(x) return np.array(x) @@ -53,16 +58,18 @@ def isinside(initparams, x): return True return False + def isinbound(i, x): if i[0] <= x[0] < i[3] and i[1] <= x[1] < i[4] and i[2] <= x[2] < i[5]: return True return False + def isCollide(initparams, x, y): '''see if line intersects obstacle''' ray = getRay(x, y) dist = getDist(x, y) - if not isinbound(initparams.env.boundary,y): + if not isinbound(initparams.env.boundary, y): return True for i in getAABB(initparams.env.blocks): shot = pyrr.geometric_tests.ray_intersect_aabb(ray, i) @@ -98,7 +105,7 @@ def near(initparams, x): cardV = len(initparams.V) eta = initparams.eta gamma = initparams.gamma - r = min(gamma*(np.log(cardV)/cardV),eta) + r = min(gamma * (np.log(cardV) / cardV), eta) if initparams.done: r = 1 V = np.array(initparams.V) if initparams.i == 0: @@ -126,31 +133,34 @@ def path(initparams, Path=[], dist=0): x = x2 return Path, dist + def hash3D(x): - return str(x[0])+' '+str(x[1])+' '+str(x[2]) + return str(x[0]) + ' ' + str(x[1]) + ' ' + str(x[2]) + def dehash(x): return np.array([float(i) for i in x.split(' ')]) + class edgeset(object): def __init__(self): self.E = {} - def add_edge(self,edge): - x, y = hash3D(edge[0]),hash3D(edge[1]) + def add_edge(self, edge): + x, y = hash3D(edge[0]), hash3D(edge[1]) if x in self.E: self.E[x].append(y) else: self.E[x] = [y] - def remove_edge(self,edge): - x, y = edge[0],edge[1] + def remove_edge(self, edge): + x, y = edge[0], edge[1] self.E[hash3D(x)].remove(hash3D(y)) def get_edge(self): edges = [] for v in self.E: for n in self.E[v]: - #if (n,v) not in edges: - edges.append((dehash(v),dehash(n))) - return edges \ No newline at end of file + # if (n,v) not in edges: + edges.append((dehash(v), dehash(n))) + return edges diff --git a/Search-based Planning/Search_3D/env3D.py b/Search-based Planning/Search_3D/env3D.py index 1204413..1d1a59a 100644 --- a/Search-based Planning/Search_3D/env3D.py +++ b/Search-based Planning/Search_3D/env3D.py @@ -5,9 +5,11 @@ @author: yue qi """ import numpy as np + + # from utils3D import OBB2AABB -def R_matrix(z_angle,y_angle,x_angle): +def R_matrix(z_angle, y_angle, x_angle): # x angle: row; y angle: pitch; z angle: yaw # generate rotation matrix in SO3 # RzRyRx = R, ZYX intrinsic rotation @@ -15,17 +17,21 @@ def R_matrix(z_angle,y_angle,x_angle): # used in obb.O # [[R p] # [0T 1]] gives transformation from body to world - return np.array([[np.cos(z_angle), -np.sin(z_angle), 0.0], [np.sin(z_angle), np.cos(z_angle), 0.0], [0.0, 0.0, 1.0]])@ \ - np.array([[np.cos(y_angle), 0.0, np.sin(y_angle)], [0.0, 1.0, 0.0], [-np.sin(y_angle), 0.0, np.cos(y_angle)]])@ \ - np.array([[1.0, 0.0, 0.0], [0.0, np.cos(x_angle), -np.sin(x_angle)], [0.0, np.sin(x_angle), np.cos(x_angle)]]) + return np.array( + [[np.cos(z_angle), -np.sin(z_angle), 0.0], [np.sin(z_angle), np.cos(z_angle), 0.0], [0.0, 0.0, 1.0]]) @ \ + np.array( + [[np.cos(y_angle), 0.0, np.sin(y_angle)], [0.0, 1.0, 0.0], [-np.sin(y_angle), 0.0, np.cos(y_angle)]]) @ \ + np.array( + [[1.0, 0.0, 0.0], [0.0, np.cos(x_angle), -np.sin(x_angle)], [0.0, np.sin(x_angle), np.cos(x_angle)]]) + def getblocks(): # AABBs block = [[3.10e+00, 0.00e+00, 2.10e+00, 3.90e+00, 5.00e+00, 6.00e+00], [9.10e+00, 0.00e+00, 2.10e+00, 9.90e+00, 5.00e+00, 6.00e+00], - #[1.51e+01, 0.00e+00, 2.10e+00, 1.59e+01, 5.00e+00, 6.00e+00], - #[1.00e-01, 0.00e+00, 0.00e+00, 9.00e-01, 5.00e+00, 3.90e+00], - #[6.10e+00, 0.00e+00, 0.00e+00, 6.90e+00, 5.00e+00, 3.90e+00], + # [1.51e+01, 0.00e+00, 2.10e+00, 1.59e+01, 5.00e+00, 6.00e+00], + # [1.00e-01, 0.00e+00, 0.00e+00, 9.00e-01, 5.00e+00, 3.90e+00], + # [6.10e+00, 0.00e+00, 0.00e+00, 6.90e+00, 5.00e+00, 3.90e+00], [1.21e+01, 0.00e+00, 0.00e+00, 1.29e+01, 5.00e+00, 3.90e+00], [1.81e+01, 0.00e+00, 0.00e+00, 1.89e+01, 5.00e+00, 3.90e+00]] Obstacles = [] @@ -34,6 +40,7 @@ def getblocks(): Obstacles.append([j for j in i]) return np.array(Obstacles) + def getAABB(blocks): # used for Pyrr package for detecting collision AABB = [] @@ -41,15 +48,17 @@ def getAABB(blocks): AABB.append(np.array([np.add(i[0:3], -0), np.add(i[3:6], 0)])) # make AABBs alittle bit of larger return AABB + class aabb(object): # make AABB out of blocks, # P: center point # E: extents # O: Rotation matrix in SO(3), in {w} - def __init__(self,AABB): - self.P = [(AABB[3] + AABB[0])/2, (AABB[4] + AABB[1])/2, (AABB[5] + AABB[2])/2]# center point - self.E = [(AABB[3] - AABB[0])/2, (AABB[4] - AABB[1])/2, (AABB[5] - AABB[2])/2]# extents - self.O = [[1,0,0],[0,1,0],[0,0,1]] + def __init__(self, AABB): + self.P = [(AABB[3] + AABB[0]) / 2, (AABB[4] + AABB[1]) / 2, (AABB[5] + AABB[2]) / 2] # center point + self.E = [(AABB[3] - AABB[0]) / 2, (AABB[4] - AABB[1]) / 2, (AABB[5] - AABB[2]) / 2] # extents + self.O = [[1, 0, 0], [0, 1, 0], [0, 0, 1]] + class obb(object): # P: center point @@ -59,7 +68,8 @@ class obb(object): self.P = P self.E = E self.O = O - self.T = np.vstack([np.column_stack([self.O.T,-self.O.T@self.P]),[0,0,0,1]]) + self.T = np.vstack([np.column_stack([self.O.T, -self.O.T @ self.P]), [0, 0, 0, 1]]) + def getAABB2(blocks): # used in lineAABB @@ -68,96 +78,101 @@ def getAABB2(blocks): AABB.append(aabb(i)) return AABB + def getballs(): - spheres = [[16,2.5,4,2],[10,2.5,1,1]] + spheres = [[16, 2.5, 4, 2], [10, 2.5, 1, 1]] Obstacles = [] for i in spheres: Obstacles.append([j for j in i]) return np.array(Obstacles) -def add_block(block = [1.51e+01, 0.00e+00, 2.10e+00, 1.59e+01, 5.00e+00, 6.00e+00]): + +def add_block(block=[1.51e+01, 0.00e+00, 2.10e+00, 1.59e+01, 5.00e+00, 6.00e+00]): return block + class env(): def __init__(self, xmin=0, ymin=0, zmin=0, xmax=20, ymax=5, zmax=6, resolution=1): self.resolution = resolution - self.boundary = np.array([xmin, ymin, zmin, xmax, ymax, zmax]) + self.boundary = np.array([xmin, ymin, zmin, xmax, ymax, zmax]) self.blocks = getblocks() self.AABB = getAABB2(self.blocks) self.AABB_pyrr = getAABB(self.blocks) self.balls = getballs() - self.OBB = np.array([obb([2.6,2.5,1],[0.2,2,1],R_matrix(0,0,45))]) - #self.OBB = np.squeeze(np.vstack([self.OBB,OBB2AABB(self.OBB[0])])) - #print(self.OBB) + self.OBB = np.array([obb([2.6, 2.5, 1], [0.2, 2, 1], R_matrix(0, 0, 45))]) + # self.OBB = np.squeeze(np.vstack([self.OBB,OBB2AABB(self.OBB[0])])) + # print(self.OBB) # self.OBB = [] self.start = np.array([0.5, 2.5, 5.5]) self.goal = np.array([19.0, 2.5, 5.5]) - self.t = 0 # time + self.t = 0 # time def New_block(self): newblock = add_block() - self.blocks = np.vstack([self.blocks,newblock]) + self.blocks = np.vstack([self.blocks, newblock]) self.AABB = getAABB2(self.blocks) self.AABB_pyrr = getAABB(self.blocks) def move_start(self, x): self.start = x - def move_block(self, a = [0,0,0], s = 0, v = [0.1,0,0], theta = [0,0,0], block_to_move = 0, obb_to_move = 0, mode = 'uniform'): + def move_block(self, a=[0, 0, 0], s=0, v=[0.1, 0, 0], theta=[0, 0, 0], block_to_move=0, obb_to_move=0, + mode='uniform'): # t is time , v is velocity in R3, a is acceleration in R3, s is increment ini time, # R is an orthorgonal transform in R3*3, is the rotation matrix # (x',t') = (x + tv, t) is uniform transformation if mode == 'uniform': ori = np.array(self.blocks[block_to_move]) self.blocks[block_to_move] = \ - np.array([ori[0] + self.t * v[0],\ - ori[1] + self.t * v[1],\ - ori[2] + self.t * v[2],\ - ori[3] + self.t * v[0],\ - ori[4] + self.t * v[1],\ - ori[5] + self.t * v[2]]) + np.array([ori[0] + self.t * v[0], + ori[1] + self.t * v[1], + ori[2] + self.t * v[2], + ori[3] + self.t * v[0], + ori[4] + self.t * v[1], + ori[5] + self.t * v[2]]) self.AABB[block_to_move].P = \ - [self.AABB[block_to_move].P[0] + self.t * v[0], \ - self.AABB[block_to_move].P[1] + self.t * v[1], \ - self.AABB[block_to_move].P[2] + self.t * v[2]] + [self.AABB[block_to_move].P[0] + self.t * v[0], + self.AABB[block_to_move].P[1] + self.t * v[1], + self.AABB[block_to_move].P[2] + self.t * v[2]] # return a range of block that the block might moved a = self.blocks[block_to_move] # return np.array([a[0] - self.resolution, a[1] - self.resolution, a[2] - self.resolution, \ # a[3] + self.resolution, a[4] + self.resolution, a[5] + self.resolution]). \ - # np.array([ori[0] - self.resolution, ori[1] - self.resolution, ori[2] - self.resolution, \ - # ori[3] + self.resolution, ori[4] + self.resolution, ori[5] + self.resolution]) - return a,ori + # np.array([ori[0] - self.resolution, ori[1] - self.resolution, ori[2] - self.resolution, \ + # ori[3] + self.resolution, ori[4] + self.resolution, ori[5] + self.resolution]) + return a, ori # (x',t') = (x + a, t + s) is a translation if mode == 'translation': ori = np.array(self.blocks[block_to_move]) self.blocks[block_to_move] = \ - np.array([ori[0] + a[0],\ - ori[1] + a[1],\ - ori[2] + a[2],\ - ori[3] + a[0],\ - ori[4] + a[1],\ - ori[5] + a[2]]) + np.array([ori[0] + a[0], + ori[1] + a[1], + ori[2] + a[2], + ori[3] + a[0], + ori[4] + a[1], + ori[5] + a[2]]) self.AABB[block_to_move].P = \ - [self.AABB[block_to_move].P[0] + a[0], \ - self.AABB[block_to_move].P[1] + a[1], \ - self.AABB[block_to_move].P[2] + a[2]] + [self.AABB[block_to_move].P[0] + a[0], + self.AABB[block_to_move].P[1] + a[1], + self.AABB[block_to_move].P[2] + a[2]] self.t += s # return a range of block that the block might moved a = self.blocks[block_to_move] - return np.array([a[0] - self.resolution, a[1] - self.resolution, a[2] - self.resolution, \ - a[3] + self.resolution, a[4] + self.resolution, a[5] + self.resolution]), \ - np.array([ori[0] - self.resolution, ori[1] - self.resolution, ori[2] - self.resolution, \ - ori[3] + self.resolution, ori[4] + self.resolution, ori[5] + self.resolution]) + return np.array([a[0] - self.resolution, a[1] - self.resolution, a[2] - self.resolution, + a[3] + self.resolution, a[4] + self.resolution, a[5] + self.resolution]), \ + np.array([ori[0] - self.resolution, ori[1] - self.resolution, ori[2] - self.resolution, + ori[3] + self.resolution, ori[4] + self.resolution, ori[5] + self.resolution]) # return a,ori # (x',t') = (Rx, t) - if mode == 'rotation': # this makes an OBB rotate + if mode == 'rotation': # this makes an OBB rotate ori = [self.OBB[obb_to_move]] - self.OBB[obb_to_move].O = R_matrix(z_angle=theta[0],y_angle=theta[1],x_angle=theta[2]) - self.OBB[obb_to_move].T = np.vstack([np.column_stack([self.OBB[obb_to_move].O.T,-self.OBB[obb_to_move].O.T@self.OBB[obb_to_move].P]),[0,0,0,1]]) + self.OBB[obb_to_move].O = R_matrix(z_angle=theta[0], y_angle=theta[1], x_angle=theta[2]) + self.OBB[obb_to_move].T = np.vstack( + [np.column_stack([self.OBB[obb_to_move].O.T, -self.OBB[obb_to_move].O.T @ self.OBB[obb_to_move].P]), + [0, 0, 0, 1]]) return self.OBB[obb_to_move], ori[0] - if __name__ == '__main__':