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.

135 lines
3.6 KiB
Python

from lib import semicircle_generator, semicircle2_generator
from lib import path_rotate_z
import numpy as np
from collections import deque
def gen_forward_path():
# assert (g_steps % 4) == 0
g_steps = 20
g_radius = 25
halfsteps = int(g_steps/2)
path = np.zeros((g_steps, 6, 3))
path[:, 0, :] = semicircle_generator(g_radius, g_steps)
mir_path = np.roll(path[:, 0, :], halfsteps, axis=0)
path[:, 2, :] = path[:, 0, :]
path[:, 4, :] = path[:, 0, :]
path[:, 1, :] = mir_path
path[:, 3, :] = mir_path
path[:, 5, :] = mir_path
return path
def gen_backward_path():
# assert (g_steps % 4) == 0
g_steps = 20
g_radius = 25
halfsteps = int(g_steps/2)
path = np.zeros((g_steps, 6, 3))
path[:, 0, :] = semicircle_generator(g_radius, g_steps, reverse=True)
mir_path = np.roll(path[:, 0, :], halfsteps, axis=0)
path[:, 2, :] = path[:, 0, :]
path[:, 4, :] = path[:, 0, :]
path[:, 1, :] = mir_path
path[:, 3, :] = mir_path
path[:, 5, :] = mir_path
return path
def gen_fastforward_path():
g_steps = 20
y_radius = 50
z_radius = 30
x_radius = 10
halfsteps = int(g_steps/2)
path = np.zeros((g_steps, 6, 3))
path[:, 0, :] = semicircle2_generator(
g_steps, y_radius, z_radius, x_radius)
path[:, 4, :] = semicircle2_generator(
g_steps, y_radius, z_radius, -x_radius)
mir_rpath = np.roll(path[:, 0, :], halfsteps, axis=0)
path[:, 1, :] = mir_rpath
path[:, 2, :] = path[:, 0, :]
mir_lpath = np.roll(path[:, 4, :], halfsteps, axis=0)
path[:, 3, :] = mir_lpath
path[:, 5, :] = mir_lpath
return path
def gen_fastbackward_path():
g_steps = 20
y_radius = 50
z_radius = 30
x_radius = 10
halfsteps = int(g_steps/2)
path = np.zeros((g_steps, 6, 3))
path[:, 0, :] = semicircle2_generator(
g_steps, y_radius, z_radius, x_radius, reverse=True)
path[:, 4, :] = semicircle2_generator(
g_steps, y_radius, z_radius, -x_radius, reverse=True)
mir_rpath = np.roll(path[:, 0, :], halfsteps, axis=0)
path[:, 1, :] = mir_rpath
path[:, 2, :] = path[:, 0, :]
mir_lpath = np.roll(path[:, 4, :], halfsteps, axis=0)
path[:, 3, :] = mir_lpath
path[:, 5, :] = mir_lpath
return path
def gen_leftturn_path():
g_steps = 20
g_radius = 25
assert (g_steps % 4) == 0
halfsteps = int(g_steps/2)
path = semicircle_generator(g_radius, g_steps)
mir_path = np.roll(path, halfsteps, axis=0)
leftturn = np.zeros((g_steps, 6, 3))
leftturn[:, 0, :] = np.array(path_rotate_z(path, 45))
leftturn[:, 1, :] = np.array(path_rotate_z(mir_path, 0))
leftturn[:, 2, :] = np.array(path_rotate_z(path, 315))
leftturn[:, 3, :] = np.array(path_rotate_z(mir_path, 225))
leftturn[:, 4, :] = np.array(path_rotate_z(path, 180))
leftturn[:, 5, :] = np.array(path_rotate_z(mir_path, 135))
return leftturn
def gen_rightturn_path():
g_steps = 20
g_radius = 25
assert (g_steps % 4) == 0
halfsteps = int(g_steps/2)
path = semicircle_generator(g_radius, g_steps)
mir_path = np.roll(path, halfsteps, axis=0)
rightturn = np.zeros((g_steps, 6, 3))
rightturn[:, 0, :] = np.array(path_rotate_z(path, 45+180))
rightturn[:, 1, :] = np.array(path_rotate_z(mir_path, 0+180))
rightturn[:, 2, :] = np.array(path_rotate_z(path, 315+180))
rightturn[:, 3, :] = np.array(path_rotate_z(mir_path, 225+180))
rightturn[:, 4, :] = np.array(path_rotate_z(path, 180+180))
rightturn[:, 5, :] = np.array(path_rotate_z(mir_path, 135+180))
return rightturn