def compute_critical_values(P, Q, p, q, mdist, P_dist, Q_dist):
"""
Usage
-----
Compute all the critical values between trajectories P and Q
Parameters
----------
param P : px2 numpy_array, Trajectory P
param Q : qx2 numpy_array, Trajectory Q
param p : int, number of points in Trajectory P
param q : int, number of points in Trajectory Q
mdist : p x q numpy array, pairwise distance between points of trajectories t1 and t2
param P_dist: p x 1 numpy_array, distances between consecutive points in P
param Q_dist: q x 1 numpy_array, distances between consecutive points in Q
Returns
-------
cc : list, all critical values between trajectories P and Q
"""
origin = eucl_dist(P[0], Q[0])
end = eucl_dist(P[-1], Q[-1])
end_point = max(origin, end)
cc = set([end_point])
for i in range(p - 1):
for j in range(q - 1):
Lij = point_to_seg(Q[j], P[i], P[i + 1], mdist[i, j],
mdist[i + 1, j], P_dist[i])
if Lij > end_point:
cc.add(Lij)
Bij = point_to_seg(P[i], Q[j], Q[j + 1], mdist[i, j],
mdist[i, j + 1], Q_dist[j])
if Bij > end_point:
cc.add(Bij)
return sorted(list(cc))
def compute_critical_values(P, Q, p, q):
"""
Usage
-----
Compute all the critical values between trajectories P and Q
Parameters
----------
param P : px2 numpy_array, Trajectory P
param Q : qx2 numpy_array, Trajectory Q
param p : float, number of points in Trajectory P
param q : float, number of points in Trajectory Q
Returns
-------
cc : list, all critical values between trajectories P and Q
"""
origin = eucl_dist(P[0], Q[0])
end = eucl_dist(P[-1], Q[-1])
end_point = max(origin, end)
cc = set([end_point])
for i in range(p - 1):
for j in range(q - 1):
Lij = point_to_seg(Q[j], P[i], P[i + 1])
if Lij > end_point:
cc.add(Lij)
Bij = point_to_seg(P[i], Q[j], Q[j + 1])
if Bij > end_point:
cc.add(Bij)
return sorted(list(cc))
def compute_critical_values(P, Q, p, q):
"""
Usage
-----
Compute all the critical values between trajectories P and Q
Parameters
----------
param P : px2 numpy_array, Trajectory P
param Q : qx2 numpy_array, Trajectory Q
param p : float, number of points in Trajectory P
param q : float, number of points in Trajectory Q
Returns
-------
cc : list, all critical values between trajectories P and Q
"""
origin = eucl_dist(P[0], Q[0])
end = eucl_dist(P[-1], Q[-1])
end_point = max(origin, end)
cc = set([end_point])
for i in range(p - 1):
for j in range(q - 1):
Lij = point_to_seg(Q[j], P[i], P[i + 1])
if Lij > end_point:
cc.add(Lij)
Bij = point_to_seg(P[i], Q[j], Q[j + 1])
if Bij > end_point:
cc.add(Bij)
return sorted(list(cc))
def free_line(p, eps, s, dps1, dps2, ds):
"""
Usage
-----
Return the free space in the segment s, from point p.
This free space is the set of all point in s whose distance from p is at most eps.
Since s is a segment, the free space is also a segment.
We return a 1x2 array whit the fraction of the segment s which are in the free space.
If no part of s are in the free space, return [-1,-1]
Parameters
----------
param p : 1x2 numpy_array, centre of the circle
param eps : float, radius of the circle
param s : 2x2 numpy_array, line
Returns
-------
lf : 1x2 numpy_array
fraction of segment which is in the free space (i.e [0.3,0.7], [0.45,1], ...)
If no part of s are in the free space, return [-1,-1]
"""
px = p[0]
py = p[1]
s1x = s[0, 0]
s1y = s[0, 1]
s2x = s[1, 0]
s2y = s[1, 1]
if s1x == s2x and s1y == s2y:
if eucl_dist(p, s[0]) > eps:
lf = [-1, -1]
else:
lf = [0, 1]
else:
if point_to_seg(p, s[0], s[1], dps1, dps2, ds) > eps:
# print("No Intersection")
lf = [-1, -1]
else:
segl = eucl_dist(s[0], s[1])
segl2 = segl * segl
intersect = circle_line_intersection(px, py, s1x, s1y, s2x, s2y,
eps)
if intersect[0][0] != intersect[1][0] or intersect[0][
1] != intersect[1][1]:
i1x = intersect[0, 0]
i1y = intersect[0, 1]
u1 = (((i1x - s1x) * (s2x - s1x)) + ((i1y - s1y) *
(s2y - s1y))) / segl2
i2x = intersect[1, 0]
i2y = intersect[1, 1]
u2 = (((i2x - s1x) * (s2x - s1x)) + ((i2y - s1y) *
(s2y - s1y))) / segl2
ordered_point = sorted((0, 1, u1, u2))
lf = ordered_point[1:3]
else:
if px == s1x and py == s1y:
lf = [0, 0]
elif px == s2x and py == s2y:
lf = [1, 1]
else:
i1x = intersect[0][0]
i1y = intersect[0][1]
u1 = (((i1x - s1x) * (s2x - s1x)) + ((i1y - s1y) *
(s2y - s1y))) / segl2
if 0 <= u1 <= 1:
lf = [u1, u1]
else:
lf = [-1, -1]
return lf
def free_line(p, eps, s):
"""
Usage
-----
Return the free space in the segment s, from point p.
This free space is the set of all point in s whose distance from p is at most eps.
Since s is a segment, the free space is also a segment.
We return a 1x2 array whit the fraction of the segment s which are in the free space.
If no part of s are in the free space, return [-1,-1]
Parameters
----------
param p : 1x2 numpy_array, centre of the circle
param eps : float, radius of the circle
param s : 2x2 numpy_array, line
Returns
-------
lf : 1x2 numpy_array
fraction of segment which is in the free space (i.e [0.3,0.7], [0.45,1], ...)
If no part of s are in the free space, return [-1,-1]
"""
px = p[0]
py = p[1]
s1x = s[0, 0]
s1y = s[0, 1]
s2x = s[1, 0]
s2y = s[1, 1]
if s1x == s2x and s1y==s2y:
if eucl_dist(p, s[0]) > eps:
lf = [-1, -1]
else:
lf = [0, 1]
else:
if point_to_seg(p, s[0], s[1]) > eps:
#print("No Intersection")
lf = [-1, -1]
else:
segl = eucl_dist(s[0], s[1])
segl2 = segl * segl
intersect = circle_line_intersection(px, py, s1x, s1y, s2x, s2y, eps)
if intersect[0][0] != intersect[1][0] or intersect[0][1] != intersect[1][1]:
i1x = intersect[0, 0]
i1y = intersect[0, 1]
u1 = (((i1x - s1x) * (s2x - s1x)) + ((i1y - s1y) * (s2y - s1y))) / segl2
i2x = intersect[1, 0]
i2y = intersect[1, 1]
u2 = (((i2x - s1x) * (s2x - s1x)) + ((i2y - s1y) * (s2y - s1y))) / segl2
ordered_point = sorted((0, 1, u1, u2))
lf = ordered_point[1:3]
else:
if px == s1x and py==s1y:
lf = [0, 0]
elif px == s2x and py==s2y:
lf = [1, 1]
else:
i1x = intersect[0][0]
i1y = intersect[0][1]
u1 = (((i1x - s1x) * (s2x - s1x)) + ((i1y - s1y) * (s2y - s1y))) / segl2
if 0 <= u1 <= 1:
lf = [u1, u1]
else:
lf = [-1, -1]
return lf