use matrix calculation

software/rpi/lib.py

@@ -1,5 +1,3 @@
-from collections import deque
-import math
 import numpy as np
 
 
@@ -60,8 +58,8 @@ def get_rotate_x_matrix(angle):
     angle = angle * np.pi / 180
     return np.matrix([
         [1, 0, 0, 0],
-        [0, math.cos(angle), -math.sin(angle), 0],
-        [0, math.sin(angle), math.cos(angle), 0],
+        [0, np.cos(angle), -np.sin(angle), 0],
+        [0, np.sin(angle), np.cos(angle), 0],
         [0, 0, 0, 1],
     ])
 
@@ -69,9 +67,9 @@ def get_rotate_x_matrix(angle):
 def get_rotate_y_matrix(angle):
     angle = angle * np.pi / 180
     return np.matrix([
-        [math.cos(angle), 0, math.sin(angle), 0],
+        [np.cos(angle), 0, np.sin(angle), 0],
         [0, 1, 0, 0],
-        [-math.sin(angle), 0, math.cos(angle), 0],
+        [-np.sin(angle), 0, np.cos(angle), 0],
         [0, 0, 0, 1],
     ])
 
@@ -79,8 +77,8 @@ def get_rotate_y_matrix(angle):
 def get_rotate_z_matrix(angle):
     angle = angle * np.pi / 180
     return np.matrix([
-        [math.cos(angle), -math.sin(angle), 0, 0],
-        [math.sin(angle), math.cos(angle), 0, 0],
+        [np.cos(angle), -np.sin(angle), 0, 0],
+        [np.sin(angle), np.cos(angle), 0, 0],
         [0, 0, 1, 0],
         [0, 0, 0, 1],
     ])
@@ -91,31 +89,19 @@ def matrix_mul(m, pt):
     return list((m * np.matrix(ptx).T).T.flat)[:-1]
 
 
-def point_rotate_x(pt, angle):
-    ptx = list(pt) + [1]
-    return list((get_rotate_x_matrix(angle) * np.matrix(ptx).T).T.flat)[:-1]
-
-
-def point_rotate_y(pt, angle):
-    ptx = list(pt) + [1]
-    return list((get_rotate_y_matrix(angle) * np.matrix(ptx).T).T.flat)[:-1]
-
-
-def point_rotate_z(pt, angle):
-    ptx = list(pt) + [1]
-    return list((get_rotate_z_matrix(angle) * np.matrix(ptx).T).T.flat)[:-1]
-
-
 def path_rotate_x(path, angle):
-    return [point_rotate_x(p, angle) for p in path]
+    ptx = np.append(path, np.ones((np.shape(path)[0], 1)), axis=1)
+    return ((get_rotate_x_matrix(angle) * np.matrix(ptx).T).T)[:, :-1]
 
 
 def path_rotate_y(path, angle):
-    return [point_rotate_y(p, angle) for p in path]
+    ptx = np.append(path, np.ones((np.shape(path)[0], 1)), axis=1)
+    return ((get_rotate_y_matrix(angle) * np.matrix(ptx).T).T)[:, :-1]
 
 
 def path_rotate_z(path, angle):
-    return [point_rotate_z(p, angle) for p in path]
+    ptx = np.append(path, np.ones((np.shape(path)[0], 1)), axis=1)
+    return ((get_rotate_z_matrix(angle) * np.matrix(ptx).T).T)[:, :-1]
 
 
 if __name__ == '__main__':

software/rpi/path_generator.py

@@ -89,24 +89,25 @@ def gen_fastbackward_path(standby_coordinate,
     return path+np.tile(standby_coordinate, (g_steps, 1, 1))
 
 
-def gen_leftturn_path(standby_coordinate):
-    g_steps = 20
-    g_radius = 25
+def gen_leftturn_path(standby_coordinate,
+                        g_steps = 20,
+                        g_radius = 25):
     assert (g_steps % 4) == 0
     halfsteps = int(g_steps/2)
 
-    path = semicircle_generator(g_radius, g_steps)
-    mir_path = np.roll(path, halfsteps, axis=0)
+    path = np.zeros((g_steps, 6, 3))
+
+    semi_circle = semicircle_generator(g_radius, g_steps)
+    mir_path = np.roll(semi_circle, halfsteps, axis=0)
 
-    leftturn = np.zeros((g_steps, 6, 3))
-    leftturn[:, 0, :] = np.array(path_rotate_z(path, 45))
-    leftturn[:, 1, :] = np.array(path_rotate_z(mir_path, 0))
-    leftturn[:, 2, :] = np.array(path_rotate_z(path, 315))
-    leftturn[:, 3, :] = np.array(path_rotate_z(mir_path, 225))
-    leftturn[:, 4, :] = np.array(path_rotate_z(path, 180))
-    leftturn[:, 5, :] = np.array(path_rotate_z(mir_path, 135))
+    path[:, 0, :] = path_rotate_z(semi_circle, 45)
+    path[:, 1, :] = path_rotate_z(mir_path, 0)
+    path[:, 2, :] = path_rotate_z(semi_circle, 315)
+    path[:, 3, :] = path_rotate_z(mir_path, 225)
+    path[:, 4, :] = path_rotate_z(semi_circle, 180)
+    path[:, 5, :] = path_rotate_z(mir_path, 135)
 
-    return leftturn+np.tile(standby_coordinate, (g_steps, 1, 1))
+    return path+np.tile(standby_coordinate, (g_steps, 1, 1))
 
 
 def gen_rightturn_path(standby_coordinate):
@@ -119,12 +120,12 @@ def gen_rightturn_path(standby_coordinate):
     mir_path = np.roll(path, halfsteps, axis=0)
 
     rightturn = np.zeros((g_steps, 6, 3))
-    rightturn[:, 0, :] = np.array(path_rotate_z(path, 45+180))
-    rightturn[:, 1, :] = np.array(path_rotate_z(mir_path, 0+180))
-    rightturn[:, 2, :] = np.array(path_rotate_z(path, 315+180))
-    rightturn[:, 3, :] = np.array(path_rotate_z(mir_path, 225+180))
-    rightturn[:, 4, :] = np.array(path_rotate_z(path, 180+180))
-    rightturn[:, 5, :] = np.array(path_rotate_z(mir_path, 135+180))
+    rightturn[:, 0, :] = path_rotate_z(path, 45+180)
+    rightturn[:, 1, :] = path_rotate_z(mir_path, 0+180)
+    rightturn[:, 2, :] = path_rotate_z(path, 315+180)
+    rightturn[:, 3, :] = path_rotate_z(mir_path, 225+180)
+    rightturn[:, 4, :] = path_rotate_z(path, 180+180)
+    rightturn[:, 5, :] = path_rotate_z(mir_path, 135+180)
 
     return rightturn+np.tile(standby_coordinate, (g_steps, 1, 1))