return CPQ
railway_CP = railway_CPQ([0,0], dp_plane, railway_r)
- railway_QP = railway_CPQ(q_plane[0:2], -dq_plane, railway_r)
- railway_midpt = 0.5 * (railway_CP + railway_QP)
+ railway_CQ = railway_CPQ(q_plane[0:2], -dq_plane, railway_r)
+ railway_midpt = 0.5 * (railway_CP + railway_CQ)
best_st = None
def railway_ST(C, start, end, railway_r):
dbg('railway_ST delta', delta, 'r', railway_r, 's', s)
return s
- try_s = railway_ST(railway_CP, [0,0], railway_midpt, railway_r)
- try_t = railway_ST(railway_CP, railway_midpt, q_plane[0:2], railway_r)
+ try_s = railway_ST(railway_CP, railway_midpt, [0,0], railway_r)
+ try_t = railway_ST(railway_CQ, railway_midpt, q_plane[0:2], railway_r)
dbg('try_s, _t', try_s, try_t)
try_st = try_s + try_t
if best_st is None or try_st < best_st:
- start_la = 1/railway_r
+ start_la = -1/railway_r
start_s = try_s
start_t = try_t
best_st = try_st