]
railway_roots = np.roots(railway_polynomial)
dbg('railway poly, roots:', railway_polynomial, railway_roots)
- for railway_r in railway_roots:
- dbg(' twoarcs root r=',railway_r)
+ for railway_r_pq1 in railway_roots:
+ # roots for r are calculated based on coordinates where
+ # Q is at (1,0) but our PQ distance is different
+ railway_r = railway_r_pq1 * dist_pq_plane
+ dbg(' twoarcs root r_pq1=', railway_r_pq1, 'r=',railway_r,
+ railway_polynomial[0] * railway_r_pq1 * railway_r_pq1 +
+ railway_polynomial[1] * railway_r_pq1 +
+ railway_polynomial[2]
+ )
def railway_CPQ(pq, dpq, railway_r):
CPQ = pq + railway_r * np.array([-dpq[1], dpq[0]])