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
Download Patch File Download Diff File Expand all files Collapse all files

29
software/rpi/lib.py
View File

@@ -2,13 +2,12 @@ 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
step_angle = np.pi / halfsteps
result = []
@@ -18,7 +17,7 @@ def semicircle_generator(radius, steps, reverse=False):
# second half, move forward in semicircle shape (y, z change)
for i in range(halfsteps):
angle = pi - step_angle*i
angle = np.pi - step_angle*i
y = radius * math.cos(angle)
z = radius * math.sin(angle)
result.append((0, y, z))
@@ -32,11 +31,12 @@ def semicircle_generator(radius, steps, reverse=False):
return result
def semicircle2_generator(steps, y_radius, z_radius, x_radius, reverse=False):
assert (steps % 4) == 0
halfsteps = int(steps/2)
step_angle = pi / halfsteps
step_angle = np.pi / halfsteps
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)
for i in range(halfsteps):
angle = pi - step_angle*i
angle = np.pi - step_angle*i
y = y_radius * math.cos(angle)
z = z_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
def get_rotate_x_matrix(angle):
angle = angle * pi / 180
angle = angle * np.pi / 180
return np.matrix([
[1, 0, 0, 0],
[0, math.cos(angle), -math.sin(angle), 0],
@@ -70,8 +71,9 @@ def get_rotate_x_matrix(angle):
[0, 0, 0, 1],
])
def get_rotate_y_matrix(angle):
angle = angle * pi / 180
angle = angle * np.pi / 180
return np.matrix([
[math.cos(angle), 0, math.sin(angle), 0],
[0, 1, 0, 0],
@@ -79,8 +81,9 @@ def get_rotate_y_matrix(angle):
[0, 0, 0, 1],
])
def get_rotate_z_matrix(angle):
angle = angle * pi / 180
angle = angle * np.pi / 180
return np.matrix([
[math.cos(angle), -math.sin(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],
])
def matrix_mul(m, pt):
ptx = list(pt) + [1]
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]
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))
print(point_rotate_z(pt, 270))