add sch and src

software/pathTool/src/path/backward.py

@@ -0,0 +1,15 @@
+from collections import deque
+
+from lib import semicircle_generator
+from forward import g_steps, g_radius
+
+def path_generator():
+    assert (g_steps % 4) == 0
+    halfsteps = int(g_steps/2)
+
+    path = semicircle_generator(g_radius, g_steps, reverse=True)
+
+    mir_path = deque(path)
+    mir_path.rotate(halfsteps)
+
+    return [path, mir_path, path, mir_path, path, mir_path, ], "shift", 20, (0, halfsteps)

software/pathTool/src/path/forward.py

@@ -0,0 +1,19 @@
+
+from collections import deque
+
+from lib import semicircle_generator
+
+g_steps = 20
+g_radius = 20
+
+def path_generator():
+    assert (g_steps % 4) == 0
+    halfsteps = int(g_steps/2)
+
+    path = semicircle_generator(g_radius, g_steps)
+
+    mir_path = deque(path)
+    mir_path.rotate(halfsteps)
+
+    return [path, mir_path, path, mir_path, path, mir_path, ], "shift", 20, (0, halfsteps)
+    

software/pathTool/src/path/lib.py

@@ -0,0 +1,85 @@
+from collections import deque
+import math
+import numpy as np
+
+pi = math.acos(-1)
+
+def semicircle_generator(radius, steps, reverse=False):
+    assert (steps % 4) == 0
+    halfsteps = int(steps/2)
+
+    step_angle = pi / halfsteps
+
+    result = []
+
+    # first half, move backward (only y change)
+    for i in range(halfsteps):
+        result.append((0, radius - i*radius*2/(halfsteps), 0))
+
+    # second half, move forward in semicircle shape (y, z change)
+    for i in range(halfsteps):
+        angle = pi - step_angle*i
+        y = radius * math.cos(angle)
+        z = radius * math.sin(angle)
+        result.append((0, y, z))
+
+    result = deque(result)
+    result.rotate(int(steps/4))
+
+    if reverse:
+        result = deque(reversed(result))
+        result.rotate(1)
+
+    return result
+
+def get_rotate_x_matrix(angle):
+    angle = angle * 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, 0, 0, 1],
+    ])
+
+def get_rotate_y_matrix(angle):
+    angle = angle * pi / 180
+    return np.matrix([
+        [math.cos(angle), 0, math.sin(angle), 0],
+        [0, 1, 0, 0],
+        [-math.sin(angle), 0, math.cos(angle), 0],
+        [0, 0, 0, 1],
+    ])
+
+def get_rotate_z_matrix(angle):
+    angle = angle * pi / 180
+    return np.matrix([
+        [math.cos(angle), -math.sin(angle), 0, 0],
+        [math.sin(angle), math.cos(angle), 0, 0],
+        [0, 0, 1, 0],
+        [0, 0, 0, 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]
+
+def path_rotate_y(path, angle):
+    return [point_rotate_y(p, angle) for p in path]
+
+def path_rotate_z(path, angle):
+    return [point_rotate_z(p, angle) for p in path]
+
+if __name__ == '__main__':
+    pt = [0, 1, 0]
+    print(point_rotate_z(pt, 270))

software/pathTool/src/path/rotatex.py

@@ -0,0 +1,42 @@
+
+from collections import deque
+import math
+
+from lib import get_rotate_x_matrix as get_matrix
+
+g_steps = 20
+
+swing_angle = 15
+y_radius = 15
+
+def path_generator():
+    assert (g_steps % 4) == 0
+    quarter = int(g_steps/4)
+
+    pi = math.acos(-1)
+
+    result = []
+    step_angle = swing_angle / quarter
+    step_offset = y_radius / quarter
+
+    for i in range(quarter):
+        m = get_matrix(swing_angle - i*step_angle)
+        m[1,3] = -i * step_offset
+        result.append(m)
+
+    for i in range(quarter):
+        m = get_matrix(-i*step_angle)
+        m[1,3] = -y_radius + i * step_offset
+        result.append(m)
+
+    for i in range(quarter):
+        m = get_matrix(i*step_angle-swing_angle)
+        m[1,3] = i * step_offset
+        result.append(m)
+
+    for i in range(quarter):
+        m = get_matrix(i*step_angle)
+        m[1,3] = y_radius-i * step_offset
+        result.append(m)
+
+    return result, "matrix", 50, (0, quarter*2)

software/pathTool/src/path/rotatey.py

@@ -0,0 +1,42 @@
+
+from collections import deque
+import math
+
+from lib import get_rotate_y_matrix as get_matrix
+
+g_steps = 20
+
+swing_angle = 15
+x_radius = 15
+
+def path_generator():
+    assert (g_steps % 4) == 0
+    quarter = int(g_steps/4)
+
+    pi = math.acos(-1)
+
+    result = []
+    step_angle = swing_angle / quarter
+    step_offset = x_radius / quarter
+
+    for i in range(quarter):
+        m = get_matrix(swing_angle - i*step_angle)
+        m[0,3] = -i * step_offset
+        result.append(m)
+
+    for i in range(quarter):
+        m = get_matrix(-i*step_angle)
+        m[0,3] = -x_radius + i * step_offset
+        result.append(m)
+
+    for i in range(quarter):
+        m = get_matrix(i*step_angle-swing_angle)
+        m[0,3] = i * step_offset
+        result.append(m)
+
+    for i in range(quarter):
+        m = get_matrix(i*step_angle)
+        m[0,3] = x_radius-i * step_offset
+        result.append(m)
+
+    return result, "matrix", 50, (0, quarter*2)

software/pathTool/src/path/rotatez.py

@@ -0,0 +1,25 @@
+
+from collections import deque
+import math
+
+from lib import get_rotate_x_matrix, get_rotate_y_matrix
+
+g_steps = 20
+
+z_lift = 4.5
+xy_radius = 1
+
+def path_generator():
+
+    pi = math.acos(-1)
+
+    result = []
+    step_angle = 2*pi / g_steps
+    for i in range(g_steps):
+        x = xy_radius * math.cos(i*step_angle)
+        y = xy_radius * math.sin(i*step_angle)
+
+        m = get_rotate_y_matrix(math.atan2(x, z_lift)*180/pi) * get_rotate_x_matrix(math.atan2(y, z_lift)*180/pi)
+        result.append(m)
+
+    return result, "matrix", 50, range(g_steps)

software/pathTool/src/path/shiftleft.py

@@ -0,0 +1,20 @@
+
+from collections import deque
+
+from lib import semicircle_generator
+from lib import path_rotate_z
+
+g_steps = 20
+g_radius = 20
+
+def path_generator():
+    assert (g_steps % 4) == 0
+    halfsteps = int(g_steps/2)
+
+    path = semicircle_generator(g_radius, g_steps)
+    path = path_rotate_z(path, 90)  # shift 90 degree to make the path "left" shift
+
+    mir_path = deque(path)
+    mir_path.rotate(halfsteps)
+
+    return [path, mir_path, path, mir_path, path, mir_path, ], "shift", 20, (0, halfsteps)

software/pathTool/src/path/shiftright.py

@@ -0,0 +1,20 @@
+
+from collections import deque
+
+from lib import semicircle_generator
+from lib import path_rotate_z
+
+g_steps = 20
+g_radius = 20
+
+def path_generator():
+    assert (g_steps % 4) == 0
+    halfsteps = int(g_steps/2)
+
+    path = semicircle_generator(g_radius, g_steps)
+    path = path_rotate_z(path, 270)  # shift 270 degree to make the path "right" shift
+
+    mir_path = deque(path)
+    mir_path.rotate(halfsteps)
+
+    return [path, mir_path, path, mir_path, path, mir_path, ], "shift", 20, (0, halfsteps)

software/pathTool/src/path/turnleft.py

@@ -0,0 +1,27 @@
+
+from collections import deque
+
+from lib import semicircle_generator
+from lib import path_rotate_z
+
+g_steps = 20
+g_radius = 20
+
+def path_generator():
+    assert (g_steps % 4) == 0
+    halfsteps = int(g_steps/2)
+
+    path = semicircle_generator(g_radius, g_steps)
+
+    mir_path = deque(path)
+    mir_path.rotate(halfsteps)
+
+    return [
+        path_rotate_z(path, 45), 
+        path_rotate_z(mir_path, 0), 
+        path_rotate_z(path, 315), 
+        path_rotate_z(mir_path, 225), 
+        path_rotate_z(path, 180), 
+        path_rotate_z(mir_path, 135), 
+    ], "shift", 20, (0, halfsteps)
+    

software/pathTool/src/path/turnright.py

@@ -0,0 +1,27 @@
+
+from collections import deque
+
+from lib import semicircle_generator
+from lib import path_rotate_z
+
+g_steps = 20
+g_radius = 20
+
+def path_generator():
+    assert (g_steps % 4) == 0
+    halfsteps = int(g_steps/2)
+
+    path = semicircle_generator(g_radius, g_steps)
+
+    mir_path = deque(path)
+    mir_path.rotate(halfsteps)
+
+    return [
+        path_rotate_z(path, 45+180), 
+        path_rotate_z(mir_path, 0+180), 
+        path_rotate_z(path, 315+180), 
+        path_rotate_z(mir_path, 225+180), 
+        path_rotate_z(path, 180+180), 
+        path_rotate_z(mir_path, 135+180), 
+    ], "shift", 20, (0, halfsteps)
+