params.traj = traj.copy()
trajs.append(traj)
tend = time.time()
trajs = np.concatenate(trajs)
print('===============================================================')
print(f'Total computation time for 1000 vehicles: {tend-tstart}')
print('===============================================================')
# Plot the trajectories
plt.close('all')
temp = Bernstein(trajs[0:NDIM, :])
vehList = [temp]
ax = temp.plot(showCpts=False)
plt.plot([temp.cpts[0, -1]], [temp.cpts[1, -1]], [temp.cpts[2, -1]],
'k.',
markersize=15,
zorder=10)
for i in range(1000):
temp = Bernstein(trajs[i * NDIM:(i + 1) * NDIM, :])
vehList.append(temp)
temp.plot(ax, showCpts=False)
plt.plot([temp.cpts[0, -1]], [temp.cpts[1, -1]], [temp.cpts[2, -1]],
'k.',
markersize=15,
zorder=10)
plt.show()
from polynomial.bernstein import Bernstein
cpts1 = np.array([[0, 1, 2, 3, 4, 5], [4, 7, 3, 9, 4, 5]], dtype=float)
cpts2 = np.array([[9, 8, 9, 7, 9, 8], [0, 1, 2, 3, 4, 5]], dtype=float)
cpts3 = np.array([[2, 3, 4, 5, 6, 6], [10, 11, 12, 13, 14, 2]],
dtype=float)
cpts4 = np.array([[0, 1, 2, 3, 4, 5], [1, 1, 1, 1, 1, 1]], dtype=float)
c1 = Bernstein(cpts1)
c2 = Bernstein(cpts2)
c3 = Bernstein(cpts3)
c4 = Bernstein(cpts4)
bpList = [c1, c2, c3, c4]
# Testing whether temporal separation is working properly. Note that
# negative distances do make sense since it corresponds to the control
# points and not the actual curve. However, if a negative distance is found
# and the curves do not intersect, the degree elevation should be increased
distVeh = temporalSeparation(bpList)
print(distVeh)
if np.any(distVeh < 0):
print('[!] Warning, negative distance!')
plt.close('all')
ax = c1.plot()
c2.plot(ax)
c3.plot(ax)
c4.plot(ax)