dPQplane_basis = np.vstack((dPQplane_basis, [0,0,0,1]))
dbg(dPQplane_basis)
- #for ax in range(0,3):
- # vdbg().arrow(dPQplane_basis[0:3,3], dPQplane_basis[0:3,ax])
+ for ax in range(0,3):
+ vdbg().arrow(dPQplane_basis[0:3,3], dPQplane_basis[0:3,ax],
+ colour= [ c * (1.0 - ax*0.2) for c in (1,1,1) ])
dPQplane_into = np.linalg.inv(dPQplane_basis)
dbg(dPQplane_into)
railway_phi = atan2(*dq_plane[0:2]) - railway_angleoffset
railway_cos_theta = cos(railway_theta)
railway_cos_phi = cos(railway_phi)
+ dbg('railway:', railway_theta, railway_phi, railway_angleoffset)
if railway_cos_theta**2 + railway_cos_phi**2 > 1E-6:
railway_roots = np.roots([
2 * (1 + cos(railway_theta - railway_phi)),