You cannot select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

110 lines
2.9 KiB
Python

3 years ago
import numpy as np
def semicircle_generator(radius, steps, reverse=False):
assert (steps % 4) == 0
halfsteps = int(steps/2)
3 years ago
step_angle = np.pi / halfsteps
3 years ago
3 years ago
result = np.zeros((steps, 3))
halfsteps_array = np.arange(halfsteps)
3 years ago
# first half, move backward (only y change)
3 years ago
result[:halfsteps, 1] = radius - halfsteps_array*radius*2/(halfsteps)
3 years ago
# second half, move forward in semicircle shape (y, z change)
3 years ago
angle = np.pi - step_angle*halfsteps_array
3 years ago
result[halfsteps:, 1] = radius * np.cos(angle)
result[halfsteps:, 2] = radius * np.sin(angle)
3 years ago
3 years ago
result = np.roll(result, int(steps/4), axis=0)
3 years ago
if reverse:
3 years ago
result = np.flip(result, axis=0)
result = np.roll(result, 1, axis=0)
3 years ago
return result
3 years ago
3 years ago
def semicircle2_generator(steps, y_radius, z_radius, x_radius, reverse=False):
assert (steps % 4) == 0
halfsteps = int(steps/2)
3 years ago
step_angle = np.pi / halfsteps
3 years ago
3 years ago
result = np.zeros((steps, 3))
halfsteps_array = np.arange(halfsteps)
3 years ago
# first half, move backward (only y change)
3 years ago
result[:halfsteps, 1] = y_radius - halfsteps_array*y_radius*2/(halfsteps)
3 years ago
3 years ago
# second half, move forward in semicircle shape (x, y, z change)
3 years ago
angle = np.pi - step_angle*halfsteps_array
3 years ago
result[halfsteps:, 0] = x_radius * np.sin(angle)
result[halfsteps:, 1] = y_radius * np.cos(angle)
result[halfsteps:, 2] = z_radius * np.sin(angle)
3 years ago
3 years ago
result = np.roll(result, int(steps/4), axis=0)
3 years ago
if reverse:
3 years ago
result = np.flip(result, axis=0)
result = np.roll(result, 1, axis=0)
3 years ago
return result
3 years ago
3 years ago
def get_rotate_x_matrix(angle):
3 years ago
angle = angle * np.pi / 180
3 years ago
return np.matrix([
[1, 0, 0, 0],
[0, np.cos(angle), -np.sin(angle), 0],
[0, np.sin(angle), np.cos(angle), 0],
3 years ago
[0, 0, 0, 1],
])
3 years ago
3 years ago
def get_rotate_y_matrix(angle):
3 years ago
angle = angle * np.pi / 180
3 years ago
return np.matrix([
[np.cos(angle), 0, np.sin(angle), 0],
3 years ago
[0, 1, 0, 0],
[-np.sin(angle), 0, np.cos(angle), 0],
3 years ago
[0, 0, 0, 1],
])
3 years ago
3 years ago
def get_rotate_z_matrix(angle):
3 years ago
angle = angle * np.pi / 180
3 years ago
return np.matrix([
[np.cos(angle), -np.sin(angle), 0, 0],
[np.sin(angle), np.cos(angle), 0, 0],
3 years ago
[0, 0, 1, 0],
[0, 0, 0, 1],
])
3 years ago
3 years ago
def matrix_mul(m, pt):
ptx = list(pt) + [1]
return list((m * np.matrix(ptx).T).T.flat)[:-1]
3 years ago
3 years ago
def path_rotate_x(path, angle):
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]
3 years ago
3 years ago
3 years ago
def path_rotate_y(path, angle):
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]
3 years ago
3 years ago
3 years ago
def path_rotate_z(path, angle):
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]
3 years ago
3 years ago
3 years ago
if __name__ == '__main__':
pt = [0, 1, 0]
3 years ago