shark/hexapod
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Update lib.py

Browse Source
This commit is contained in:
Zhengyu Peng 2021-12-09 07:56:55 -05:00
parent fd48127316
commit 922eb50b72
1 changed files with 20 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
software/rpi/lib.py
View File

@ -2,13 +2,12 @@ from collections import deque
import math import math
import numpy as np import numpy as np
pi = math.acos(-1)
def semicircle_generator(radius, steps, reverse=False): def semicircle_generator(radius, steps, reverse=False):
assert (steps % 4) == 0 assert (steps % 4) == 0
halfsteps = int(steps/2) halfsteps = int(steps/2)
step_angle = pi / halfsteps step_angle = np.pi / halfsteps
result = [] result = []
@ -18,7 +17,7 @@ def semicircle_generator(radius, steps, reverse=False):
# second half, move forward in semicircle shape (y, z change) # second half, move forward in semicircle shape (y, z change)
for i in range(halfsteps): for i in range(halfsteps):
angle = pi - step_angle*i angle = np.pi - step_angle*i
y = radius * math.cos(angle) y = radius * math.cos(angle)
z = radius * math.sin(angle) z = radius * math.sin(angle)
result.append((0, y, z)) result.append((0, y, z))
@ -32,11 +31,12 @@ def semicircle_generator(radius, steps, reverse=False):
return result return result
def semicircle2_generator(steps, y_radius, z_radius, x_radius, reverse=False): def semicircle2_generator(steps, y_radius, z_radius, x_radius, reverse=False):
assert (steps % 4) == 0 assert (steps % 4) == 0
halfsteps = int(steps/2) halfsteps = int(steps/2)
step_angle = pi / halfsteps step_angle = np.pi / halfsteps
result = [] result = []
@ -46,7 +46,7 @@ def semicircle2_generator(steps, y_radius, z_radius, x_radius, reverse=False):
# second half, move forward in semicircle shape (y, z change) # second half, move forward in semicircle shape (y, z change)
for i in range(halfsteps): for i in range(halfsteps):
angle = pi - step_angle*i angle = np.pi - step_angle*i
y = y_radius * math.cos(angle) y = y_radius * math.cos(angle)
z = z_radius * math.sin(angle) z = z_radius * math.sin(angle)
x = x_radius * math.sin(angle) x = x_radius * math.sin(angle)
@ -61,8 +61,9 @@ def semicircle2_generator(steps, y_radius, z_radius, x_radius, reverse=False):
return result return result
def get_rotate_x_matrix(angle): def get_rotate_x_matrix(angle):
angle = angle * pi / 180 angle = angle * np.pi / 180
return np.matrix([ return np.matrix([
[1, 0, 0, 0], [1, 0, 0, 0],
[0, math.cos(angle), -math.sin(angle), 0], [0, math.cos(angle), -math.sin(angle), 0],
@ -70,8 +71,9 @@ def get_rotate_x_matrix(angle):
[0, 0, 0, 1], [0, 0, 0, 1],
]) ])
def get_rotate_y_matrix(angle): def get_rotate_y_matrix(angle):
angle = angle * pi / 180 angle = angle * np.pi / 180
return np.matrix([ return np.matrix([
[math.cos(angle), 0, math.sin(angle), 0], [math.cos(angle), 0, math.sin(angle), 0],
[0, 1, 0, 0], [0, 1, 0, 0],
@ -79,8 +81,9 @@ def get_rotate_y_matrix(angle):
[0, 0, 0, 1], [0, 0, 0, 1],
]) ])
def get_rotate_z_matrix(angle): def get_rotate_z_matrix(angle):
angle = angle * pi / 180 angle = angle * np.pi / 180
return np.matrix([ return np.matrix([
[math.cos(angle), -math.sin(angle), 0, 0], [math.cos(angle), -math.sin(angle), 0, 0],
[math.sin(angle), math.cos(angle), 0, 0], [math.sin(angle), math.cos(angle), 0, 0],
@ -88,31 +91,39 @@ def get_rotate_z_matrix(angle):
[0, 0, 0, 1], [0, 0, 0, 1],
]) ])
def matrix_mul(m, pt): def matrix_mul(m, pt):
ptx = list(pt) + [1] ptx = list(pt) + [1]
return list((m * np.matrix(ptx).T).T.flat)[:-1] return list((m * np.matrix(ptx).T).T.flat)[:-1]
def point_rotate_x(pt, angle): def point_rotate_x(pt, angle):
ptx = list(pt) + [1] ptx = list(pt) + [1]
return list((get_rotate_x_matrix(angle) * np.matrix(ptx).T).T.flat)[:-1] return list((get_rotate_x_matrix(angle) * np.matrix(ptx).T).T.flat)[:-1]
def point_rotate_y(pt, angle): def point_rotate_y(pt, angle):
ptx = list(pt) + [1] ptx = list(pt) + [1]
return list((get_rotate_y_matrix(angle) * np.matrix(ptx).T).T.flat)[:-1] return list((get_rotate_y_matrix(angle) * np.matrix(ptx).T).T.flat)[:-1]
def point_rotate_z(pt, angle): def point_rotate_z(pt, angle):
ptx = list(pt) + [1] ptx = list(pt) + [1]
return list((get_rotate_z_matrix(angle) * np.matrix(ptx).T).T.flat)[:-1] return list((get_rotate_z_matrix(angle) * np.matrix(ptx).T).T.flat)[:-1]
def path_rotate_x(path, angle): def path_rotate_x(path, angle):
return [point_rotate_x(p, angle) for p in path] return [point_rotate_x(p, angle) for p in path]
def path_rotate_y(path, angle): def path_rotate_y(path, angle):
return [point_rotate_y(p, angle) for p in path] return [point_rotate_y(p, angle) for p in path]
def path_rotate_z(path, angle): def path_rotate_z(path, angle):
return [point_rotate_z(p, angle) for p in path] return [point_rotate_z(p, angle) for p in path]
if __name__ == '__main__': if __name__ == '__main__':
pt = [0, 1, 0] pt = [0, 1, 0]
print(point_rotate_z(pt, 270)) print(point_rotate_z(pt, 270))