Untitled

mail@pastecode.io avatarunknown
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)