Untitled
python
a month ago
1.3 kB
3
Indexable
Never
import numpy as np from copy import deepcopy # INPUTS PATH_LENGTH = 10 PATH_STEP_LENGTH = 5.0 START_POINT = [0.0, 0.0] Q_STEP_LENGTH = 3 # generate random path array with shape (PATH_LENGTH, 2) path = [deepcopy(START_POINT)] for i in range(PATH_LENGTH): x = np.abs(np.random.rand() * PATH_STEP_LENGTH) y = np.sqrt(PATH_STEP_LENGTH*PATH_STEP_LENGTH - x*x) START_POINT[0] += x START_POINT[1] += y path.append(deepcopy(START_POINT)) path = np.array(path) print(path) # ====================================================== # def dist(a, b): return np.sqrt(np.sum((a - b) ** 2)) q = [deepcopy(path[0])] p_idx = 0 p_max = len(path) - 1 remain = dist(path[p_idx+1], path[p_idx]) vector = path[p_idx+1] - path[p_idx] vector_length = dist(path[p_idx+1], path[p_idx]) q_remain = Q_STEP_LENGTH last_point = deepcopy(q[0]) while True: if remain >= q_remain: q.append(vector / vector_length * q_remain + last_point) remain -= q_remain q_remain = Q_STEP_LENGTH last_point = deepcopy(q[-1]) continue q_remain -= remain p_idx += 1 if p_idx+1 > p_max: break last_point = deepcopy(path[p_idx]) remain = dist(path[p_idx+1], path[p_idx]) vector = path[p_idx+1] - path[p_idx] vector_length = dist(path[p_idx+1], path[p_idx]) q = np.array(q) print(q)