dbg('railway poly, roots:', railway_polynomial, railway_roots)
vdbg_railway.circle([0,0,0], [0,0, dist_pq_plane], color=(.5,0,0))
+ vdbg_railway.circle([1,0,0], [0,0, 0.05], color=(.5,0,0))
+ #vdbg().circle(p, dPQplane_normal * dist_pq_plane, color=(.5,.5,0))
for railway_r_pq1 in railway_roots:
# roots for r are calculated based on coordinates where
)
vdbg_railway.circle([0,0,0], [0,0, railway_r], color=(1,0,0))
+ #vdbg().circle(p, dPQplane_normal * railway_r, color=(1,1,0))
def railway_CPQ(pq, dpq, railway_r):
CPQ = pq + railway_r * np.array([-dpq[1], dpq[0]])