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.

129 lines
3.1 KiB
Python

3 years ago
from collections import deque
import math
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
y = radius * np.cos(angle)
z = radius * np.sin(angle)
result[halfsteps:, 1] = y
result[halfsteps:, 2] = z
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
result = []
# first half, move backward (only y change)
for i in range(halfsteps):
result.append((0, y_radius - i*y_radius*2/(halfsteps), 0))
# second half, move forward in semicircle shape (y, z change)
for i in range(halfsteps):
3 years ago
angle = np.pi - step_angle*i
3 years ago
y = y_radius * math.cos(angle)
z = z_radius * math.sin(angle)
x = x_radius * math.sin(angle)
result.append((x, y, z))
result = deque(result)
result.rotate(int(steps/4))
if reverse:
result = deque(reversed(result))
result.rotate(1)
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, math.cos(angle), -math.sin(angle), 0],
[0, math.sin(angle), math.cos(angle), 0],
[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([
[math.cos(angle), 0, math.sin(angle), 0],
[0, 1, 0, 0],
[-math.sin(angle), 0, math.cos(angle), 0],
[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([
[math.cos(angle), -math.sin(angle), 0, 0],
[math.sin(angle), math.cos(angle), 0, 0],
[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 point_rotate_x(pt, angle):
ptx = list(pt) + [1]
return list((get_rotate_x_matrix(angle) * np.matrix(ptx).T).T.flat)[:-1]
3 years ago
3 years ago
def point_rotate_y(pt, angle):
ptx = list(pt) + [1]
return list((get_rotate_y_matrix(angle) * np.matrix(ptx).T).T.flat)[:-1]
3 years ago
3 years ago
def point_rotate_z(pt, angle):
ptx = list(pt) + [1]
return list((get_rotate_z_matrix(angle) * np.matrix(ptx).T).T.flat)[:-1]
3 years ago
3 years ago
def path_rotate_x(path, angle):
return [point_rotate_x(p, angle) for p in path]
3 years ago
3 years ago
def path_rotate_y(path, angle):
return [point_rotate_y(p, angle) for p in path]
3 years ago
3 years ago
def path_rotate_z(path, angle):
return [point_rotate_z(p, angle) for p in path]
3 years ago
3 years ago
if __name__ == '__main__':
pt = [0, 1, 0]
3 years ago
print(point_rotate_z(pt, 270))