helixish: debugging

Signed-off-by: Ian Jackson <ijackson@chiark.greenend.org.uk>

diff --git a/helixish.py b/helixish.py
index 3bb589338ad035af0144453bb6ccc11a53fd6227..dfe7e5ec7a9ed4abb29cdc3c50a6e299be55e0ce 100644 (file)
--- a/helixish.py
+++ b/helixish.py
@@ -87,7 +87,7 @@ class HelixishCurve():
     railway_phi   = atan2(*dq_plane[0:2]) - railway_angleoffset
     railway_cos_theta = cos(railway_theta)
     railway_cos_phi   = cos(railway_phi)
-    if railway_cos_theta**2 + railway_cos_phi**2 > 1E6:
+    if railway_cos_theta**2 + railway_cos_phi**2 > 1E-6:
       railway_roots = np.roots([
         2 * (1 + cos(railway_theta - railway_phi)),
         2 * (railway_cos_theta - railway_cos_phi),
@@ -95,22 +95,30 @@ class HelixishCurve():
         ])
       for railway_r in railway_roots:
         def railway_CPQ(pq, dpq, railway_r):
-          return pq + railway_r * [-dpq[1], dpq[0]]
+          CPQ = pq + railway_r * np.array([-dpq[1], dpq[0]])
+          dbg('railway_CPQ', railway_r, pq, dpq, CPQ)
+          return CPQ
 
-        railway_CP = railway_CPQ([0,0,0],       dp_plane, railway_r)
+        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)
 
         best_st = None
         def railway_ST(C, start, end, railway_r):
-          delta = atan2(*(end - C)[0:2]) - atan2(start - C)[0:2]
+          delta = atan2(*(end - C)[0:2]) - atan2(*(start - C)[0:2])
           s = delta * railway_r
+          dbg('railway_ST', C, start, end, railway_r, 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)
+        dbg('try_s, _t', try_s, try_t)
+        if try_s < 0 or try_t < 0:
+          continue
 
-        try_s = railway_ST(railway_CP, [0,0], midpt, railway_r)
-        try_t = railway_ST(railway_CP, midpt, q_plane, railway_r)
         try_st = try_s + try_t
         if best_st is None or try_st < best_st:
-          start_la = 1/r
+          start_la = 1/railway_r
           start_s = try_s
           start_t = try_t
           best_st = try_st
@@ -141,6 +149,9 @@ class HelixishCurve():
     findcurve_basis = matmatmultiply(dPQplane_basis, tilt_basis)
     findcurve_into = np.linalg.inv(findcurve_basis)
 
+    for ax in range(0,3):
+      vdbg().arrow(findcurve_basis[0:3,3], findcurve_basis[0:3,ax])
+
     q_findcurve = augmatmultiply(findcurve_into, q)
     dq_findcurve = augmatmultiply(findcurve_into, dq, augwith=0)