Fix selection nearest point on init_guess to nearest edge.

utils/manifolds.py

@@ -115,7 +115,39 @@ def init_guess(curve, points_from):
         nearest_out = []
         for point_from in points_from:
             nearest, normal, i, distance = tree.find_nearest( point_from )
-            us_out.append( (us[i], us[i+1]) ) # interval to search minimum
+
+            # find t of arc:
+            points_i0 = points[i]
+            points_i1 = points[i+1]
+            dist_ix = abs(points_i1[0] - points_i0[0])
+            dist_iy = abs(points_i1[1] - points_i0[1])
+            dist_iz = abs(points_i1[2] - points_i0[2])
+            max_axis = 0
+            max_dist = dist_ix
+            if max_dist < dist_iy:
+                max_dist = dist_iy
+                max_axis = 1
+            if max_dist < dist_iz:
+                max_dist = dist_iz
+                max_axis = 2
+            dist_nearest_to_p0 = abs(nearest[max_axis]-points_i0[max_axis])
+            if dist_nearest_to_p0==0:
+                t01=0
+                raw_t = us[i]
+                nearest_t = points[i]
+            elif dist_nearest_to_p0==1:
+                t01 = 1
+                raw_t = us[i+1]
+                nearest_t = points[i+1]
+            else:
+                t01 = dist_nearest_to_p0/max_dist
+                raw_t = us[i] + t01*(us[i+1]-us[i]) # approximate nearest t by chorda
+                nearest_t = None #curve.evaluate(raw_t).tolist()  # later
+
+            # t0    - [0-1] in interval us[i]-us[i+1]
+            # raw_t - translate t0 to curve t
+            # nearest_t - if t0==0 or 1 then use points, else None and calc later
+            us_out.append( (us[i], us[i+1], t01, raw_t, points[i], nearest_t, points[i+1] ) ) # interval to search minimum
             nearest_out.append(tuple(nearest))
 
         return us_out, np.array(nearest_out)
@@ -126,43 +158,56 @@ def goal(t):
 
     intervals, init_points = init_guess(curve, src_points)
     result_ts = []
-    if precise:
-        for src_point, interval, init_point in zip(src_points, intervals, init_points):
-
-            init_t  = (interval[0]+interval[1])/2
-            bracket = (interval[0], init_t, interval[1])
-            bounds  = (interval[0], interval[1])            
-
-            logger.debug("T_min %s, T_max %s, init_t %s", t_min, t_max, init_t)
-
-            result = minimize_scalar(goal,
-                        bounds = bounds,
-                        bracket = bracket,
-                        method = method
-                    )
+    result_points = []
+    for src_point, interval, init_point in zip(src_points, intervals, init_points):
 
-            if not result.success:
-                if hasattr(result, 'message'):
-                    message = result.message
-                else:
-                    message = repr(result)
-                raise Exception("Can't find the nearest point for {}: {}".format(src_point, message))
+        t01       = interval[2]
+        res_t     = interval[3]
+        res_point = interval[5]  # remark: may be None. Calc of None later after getting final t.
 
-            t0 = result.x
-            if t0 < t_min:
-                t0 = t_min
-            elif t0 > t_max:
-                t0 = t_max
-            result_ts.append(t0)
-    else:
-        result_ts = init_ts
+        if precise==True:
+            if t01==0 or t01==1:
+                pass
+            else:
+                raw_t  = interval[3]
+                bracket = (interval[0], raw_t, interval[1])
+                bounds  = (interval[0], interval[1])            
+
+                logger.debug("T_min %s, T_max %s, init_t %s", t_min, t_max, raw_t)
+
+                result = minimize_scalar(goal,
+                            bounds = bounds,
+                            bracket = bracket,
+                            method = method
+                        )
+
+                if not result.success:
+                    if hasattr(result, 'message'):
+                        message = result.message
+                    else:
+                        message = repr(result)
+                    raise Exception("Can't find the nearest point for {}: {}".format(src_point, message))
+
+                res_t = result.x
+
+        result_ts.append(res_t)
+        result_points.append(res_point)
 
     if output_points:
-        if precise:
-            result_points = curve.evaluate_array(np.array(result_ts))
-            return list(zip(result_ts, result_points))
-        else:
-            return list(zip(result_ts, init_points))
+        result_ts_none = []
+        # get t where points is None value
+        for i in range(len(result_points)):
+            if result_points[i]==None:
+                result_ts_none.append(result_ts[i])
+
+        if len(result_ts_none)>0:
+            # evaluate that points and save values:
+            result_points_none = curve.evaluate_array(np.array(result_ts_none)).tolist()
+            for i in range(len(result_points)):
+                if result_points[i]==None:
+                    result_points[i] = result_points_none.pop(0)
+
+        return list(zip(result_ts, np.array(result_points) ))
     else:
         return result_ts