From 922eb50b7238648f7248476c652b2e29d927bbb4 Mon Sep 17 00:00:00 2001 From: Zhengyu Peng Date: Thu, 9 Dec 2021 07:56:55 -0500 Subject: [PATCH] Update lib.py --- software/rpi/lib.py | 29 ++++++++++++++++++++--------- 1 file changed, 20 insertions(+), 9 deletions(-) diff --git a/software/rpi/lib.py b/software/rpi/lib.py index 6d22749..f0d305c 100644 --- a/software/rpi/lib.py +++ b/software/rpi/lib.py @@ -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)) \ No newline at end of file + print(point_rotate_z(pt, 270))