Fix to find_length to correctly compute parametric time along straight segments

panda/src/parametrics/parametricCurve.cxx

@@ -784,11 +784,18 @@ r_find_length(float target_length, float &found_t,
     right = (pmid - p2).length();
 
     if ((left + right) - seglength < length_tolerance) {
-      // No.  We're done.
+      // No.  Curve is relatively straight at this point.
       if (target_length <= seglength) {
-        found_t = t1 + (target_length / seglength) * (t2 - t1);
+        // Compute t value that corresponds to target_length
+        // Maybe the point is in the left half of the segment?
+        if (r_find_t(target_length, found_t, t1, tmid, p1, pmid)) {
           return true;
         }
+        // Maybe it's on the right half?
+        if (r_find_t(target_length - left, found_t, tmid, t2, pmid, p2)) {
+          return true;
+        }
+      }
       return false;
 
     } else {
@@ -813,6 +820,70 @@ r_find_length(float target_length, float &found_t,
 
 
 
+////////////////////////////////////////////////////////////////////
+//     Function: ParametricCurve::r_find_t
+//       Access: Private
+//  Description: The recursive function to compute t value of a 
+//               target point along a staight section of a curve.
+//               This is similar to r_calc_length, above.  
+//               target_length is the length along the curve past t1 
+//               that we hope to find.  If the indicated target_length 
+//               falls within this segment, returns true and sets 
+//               found_t to the point along the segment.  
+////////////////////////////////////////////////////////////////////
+bool ParametricCurve::
+r_find_t(float target_length, float &found_t,
+         float t1, float t2,
+         const LPoint3f &p1, const LPoint3f &p2) const {
+  static const float t_tolerance = 0.00001f;
+
+  parametrics_cat.debug()
+    << "target_length " << target_length << " t1 " << t1 << " t2 " << t2 << "\n";
+
+  if (target_length < t_tolerance) {
+    // Stop recursing--we've just walked off the limit for
+    // representing smaller values of t.
+    found_t = t1;
+    return true;
+  } 
+
+  // Compute distance between two endpoints
+  float point_dist;
+  point_dist = (p2 - p1).length();
+
+  // Is point past endpoint?
+  if (point_dist < target_length) {
+    return false;
+  }
+   
+  // Is point close to far endpoint?
+  if ( (point_dist - target_length ) < t_tolerance ) {
+    found_t = t2;
+    return true;
+  }
+
+  // Nope, subdivide and continue
+  float tmid;
+  LPoint3f pmid;
+  float left;
+  
+  // Calculate the point on the curve midway between the two
+  // endpoints.
+  tmid = (t1+t2)*0.5f;
+  get_point(tmid, pmid);
+  
+  // Maybe the point is in the left half of the segment?
+  if (r_find_t(target_length, found_t, t1, tmid, p1, pmid)) {
+    return true;
+  }
+  // Nope, must be in the right half
+  left = (p1 - pmid).length();
+  if (r_find_t(target_length - left, found_t, tmid, t2, pmid, p2)) {
+    return true;
+  }
+}
+
+
 ////////////////////////////////////////////////////////////////////
 //     Function: ParametricCurve::write_datagram
 //       Access: Protected, Virtual

panda/src/parametrics/parametricCurve.h

@@ -136,6 +136,9 @@ private:
                      float t1, float t2,
                      const LPoint3f &p1, const LPoint3f &p2,
                      float &seglength) const;
+  bool r_find_t(float target_length, float &found_t,
+                float t1, float t2,
+                const LPoint3f &p1, const LPoint3f &p2) const;
 
 protected:
   int _curve_type;