from src.Vec import VecSE2
from src.Vec import VecE2
from src.Vec.geom import geom
[docs]class CurvePt:
def __init__(self, pos: VecSE2.VecSE2, s: float, k=None, kd=None):
'''
CurvePt
describes a point on a curve, associated with a curvature and the derivative of the curvature
- `pos::VecSE2` # global position and orientation
- `s` # distance along the curve
- `k` # curvature
- `kd` # derivative of curvature
'''
self.pos = pos
self.s = s
self.k = k
self.kd = kd
[docs] def show(self):
'''
:return: print out
'''
print("CurvePt({{:.3f}, {:.3f}, {:.3f}}, {:.3f}, {:.3f}, {:.3f})".format(self.pos.x, self.pos.y, self.pos.theta,
self.s, self.k, self.kd))
[docs]def lerp(a: CurvePt, b: CurvePt, t: float):
'''
:param a: start point
:param b: end point
:param t: `t::T` ∈ [0,1] for linear interpolation
:return: the result of interpolation
'''
return CurvePt(VecSE2.lerp(a.pos, b.pos, t), a.s + (b.s - a.s)*t, a.k + (b.k - a.k)*t, a.kd + (b.kd - a.kd)*t)
[docs]class CurveIndex:
'''
CurveIndex{I <: Integer, T <: Real}
Given a `Curve` object `curve` one can call `curve[ind]`
where `ind` is a `CurveIndex`. The field `t` can be used to interpolate between two
points in the curve.
# Fields
- `i`::I` index in the curve , ∈ [0:length(curve)-2]
- `t::T` ∈ [0,1] for linear interpolation
'''
def __init__(self, i: int, t: float):
'''
:param i: index in the curve , ∈ [0:length(curve)-2]
:param t: `t::T` ∈ [0,1] for linear interpolation
'''
self.i = i
self.t = t
def __eq__(self, other):
return self.t == other.t and self.i == other.i
def __ne__(self, other):
return self.t != other.t or self.i != other.i
[docs]def curveindex_end(curve: list):
'''
:param curve: curve list
:return: the last curve index of the curve list
'''
return CurveIndex(len(curve)-2, 1.0)
[docs]def get_curve_list_by_index(curve: list, ind: CurveIndex):
return lerp(curve[ind.i], curve[ind.i+1], ind.t)
CURVEINDEX_START = CurveIndex(0, 0.0)
[docs]class CurveProjection:
'''
CurveProjection{I <: Integer, T <: Real}
The result of a point projected to a Curve
# Fields
- `ind::CurveIndex{I, T}`
- `t::T` lane offset
- `ϕ::T` lane-relative heading [rad]
'''
def __init__(self, ind: CurveIndex, t: float, phi: float):
self.ind = ind
self.t = t
self.phi = phi
[docs]def div(a, b):
return int(a/b)
[docs]def index_closest_to_point(curve: list, target: VecSE2.VecSE2): # curve: list(CurvePt)
'''
index_closest_to_point(curve::Curve, target::posG(VecSE2))
returns the curve index closest to the point described by `target`.
`target` must be [x, y].
'''
a = 1
b = len(curve)
c = div(a+b, 2)
assert(len(curve) >= b)
sqdist_a = curve[a - 1].pos - target
sqdist_b = curve[b - 1].pos - target
sqdist_c = curve[c - 1].pos - target
# sqdist_a.show()
# sqdist_b.show()
# sqdist_c.show()
sqdist_a = VecE2.normsquared(VecE2.VecE2(sqdist_a.x, sqdist_a.y))
sqdist_b = VecE2.normsquared(VecE2.VecE2(sqdist_b.x, sqdist_b.y))
sqdist_c = VecE2.normsquared(VecE2.VecE2(sqdist_c.x, sqdist_c.y))
# print(target.x, target.y, sqdist_a, sqdist_b, sqdist_c)
# curve[a - 1].pos.show()
# curve[b - 1].pos.show()
# curve[c - 1].pos.show()
while True:
if b == a:
return a - 1
elif b == a + 1:
return (b - 1) if sqdist_b < sqdist_a else (a - 1)
elif c == a + 1 and c == b - 1:
if sqdist_a < sqdist_b and sqdist_a < sqdist_c:
return a - 1
elif sqdist_b < sqdist_a and sqdist_b < sqdist_c:
return b - 1
else:
return c - 1
left = div(a+c, 2)
sqdist_l = curve[left - 1].pos - target
sqdist_l = VecE2.normsquared(VecE2.VecE2(sqdist_l.x, sqdist_l.y))
right = div(c+b, 2)
sqdist_r = curve[right - 1].pos - target
sqdist_r = VecE2.normsquared(VecE2.VecE2(sqdist_r.x, sqdist_r.y))
if sqdist_l < sqdist_r:
b = c
sqdist_b = sqdist_c
c = left
sqdist_c = sqdist_l
else:
a = c
sqdist_a = sqdist_c
c = right
sqdist_c = sqdist_r
raise OverflowError("index_closest_to_point reached unreachable statement")
[docs]def get_lerp_time_unclamped_1(A: VecE2.VecE2, B: VecE2.VecE2, Q: VecE2.VecE2):
'''
:param A: VecE2 vector
:param B: VecE2 vector
:param Q: VecE2 vector
:return: Get the interpolation scalar t for the point on the line AB closest to Q, This point is P = A + (B-A)*t
'''
a = Q - A
# A.show()
# B.show()
b = B - A
c = VecE2.proj(a, b, VecE2.VecE2)
if b.x != 0.0:
t = c.x / b.x
elif b.y != 0.0:
t = c.y / b.y
else:
t = 0.0 # no lerping to be done
return t
[docs]def get_lerp_time_unclamped_2(A: CurvePt, B: CurvePt, Q: VecSE2.VecSE2):
return get_lerp_time_unclamped_1(A.pos.convert(), B.pos.convert(), Q.convert())
[docs]def get_lerp_time_unclamped_3(A: VecSE2.VecSE2, B: VecSE2.VecSE2, Q: VecSE2.VecSE2):
return get_lerp_time_unclamped_1(A.convert(), B.convert(), Q.convert())
[docs]def clamp(a, low, high):
return min(high, max(low, a))
[docs]def get_lerp_time_1(A: VecE2.VecE2, B: VecE2.VecE2, Q: VecE2.VecE2):
return clamp(get_lerp_time_unclamped_1(A, B, Q), 0.0, 1.0)
[docs]def get_lerp_time_2(A: CurvePt, B: CurvePt, Q: VecSE2.VecSE2):
return get_lerp_time_1(A.pos.convert(), B.pos.convert(), Q.convert())
[docs]def get_curve_projection(posG: VecSE2.VecSE2, footpoint: VecSE2.VecSE2, ind: CurveIndex):
F = geom.inertial2body(posG, footpoint)
return CurveProjection(ind, F.y, F.theta)
[docs]def proj(posG: VecSE2.VecSE2, curve: list):
'''
Vec.proj(posG::VecSE2, curve::list of CurvePt)
Return a CurveProjection obtained by projecting posG onto the curve
'''
ind = index_closest_to_point(curve, posG)
# if ind <= len(curve):
# print("project: ")
# print(ind, len(curve))
curveind = CurveIndex(-1, 0)
footpoint = VecSE2.VecSE2(0, 0, 0)
if 0 < ind < len(curve) - 1:
t_lo = get_lerp_time_2(curve[ind - 1], curve[ind], posG)
t_hi = get_lerp_time_2(curve[ind], curve[ind + 1], posG)
p_lo = VecSE2.lerp(curve[ind - 1].pos, curve[ind].pos, t_lo)
p_hi = VecSE2.lerp(curve[ind].pos, curve[ind + 1].pos, t_hi)
vec_lo = p_lo - posG
vec_hi = p_hi - posG
d_lo = VecE2.norm(VecE2.VecE2(vec_lo.x, vec_lo.y))
d_hi = VecE2.norm(VecE2.VecE2(vec_hi.x, vec_hi.y))
if d_lo < d_hi:
footpoint = p_lo
curveind = CurveIndex(ind - 1, t_lo)
else:
footpoint = p_hi
curveind = CurveIndex(ind, t_hi)
elif ind == 0:
t = get_lerp_time_2(curve[0], curve[1], posG)
footpoint = VecSE2.lerp(curve[0].pos, curve[1].pos, t)
curveind = CurveIndex(ind, t)
else: # ind == length(curve)
t = get_lerp_time_2(curve[-2], curve[-1], posG)
footpoint = VecSE2.lerp(curve[-2].pos, curve[-1].pos, t)
curveind = CurveIndex(ind - 1, t)
return get_curve_projection(posG, footpoint, curveind)
'''
Curve : list(CurvePt)
'''