I am trying to find a way to create a trajectory between two points with specified speeds in a vectorized way.
For example start point is [0, 0] and end point is [25, 15].
Next I need to specified speeds [25, 4, 8, 2] and their corresponding probabilities [0.4, 0.2, 0.3, 0.1]. So for each time interval, speed can be 25 m/s (with probability 40%), or 4 m/s (probability 20%), etc.
Here is an example of desired output:
[[0,0], [3,1], [6,3.5], [12,7], [14,8], [19,11], [25,15]]
As you see object moved from [0,0] to [3,1] with speed e.g. 4 m/s, next from [3,1] to [6,3.5] with speed e.g. 8 m/s, etc.
(note it is just example with approximate coordinates)
Here is my attempt to create a script to generate such trajectories:
from math import asin, degrees
ue_speed = [3, 4, 8, 25]
ue_speed_prob = [0.4, 0.2, 0.3, 0.1]
steps = 20 # this parameter hardcoded and should be removed
square_size = 100 # need for scaling
time_interval = 100 # need for scaling
start_coord = [0, 0]
end_coord = [25, 15]
b = end_coord[0] - start_coord[0]
a = end_coord[1] - start_coord[1]
c = (a**2 + b**2)**0.5
theta = [degrees(asin(a/c))] * (steps - 1)
start = [start_coord]
v = np.random.choice(ue_speed, size=steps-1, p=ue_speed_prob)
R = np.expand_dims(((v * time_interval) / square_size), axis=-1)
xy = np.dstack((np.cos(theta), np.sin(theta))) * R
trajectory_ = np.hstack((np.zeros((1, 1, 2)), np.cumsum(xy, axis=1)))
trajectory = np.abs(trajectory_[0] + start )
Output:
array([[ 0. , 0. ],
[ 2.69850332, 1.31075545],
[ 5.39700664, 2.62151089],
[ 8.09550996, 3.93226634],
[10.79401327, 5.24302179],
[14.3920177 , 6.99069571],
[21.58802655, 10.48604357],
[24.28652987, 11.79679902],
[26.98503318, 13.10755446],
[30.58303761, 14.85522839],
[33.28154093, 16.16598384],
[35.98004425, 17.47673929],
[38.67854756, 18.78749473],
[45.87455641, 22.28284259],
[49.47256084, 24.03051652],
[56.66856969, 27.52586437],
[59.36707301, 28.83661982],
[62.06557632, 30.14737527],
[65.66358075, 31.8950492 ],
[69.26158517, 33.64272312]])
Output is not correct. End point should be [25, 15].
Is it possible to change somehow code above to generate correct results?