diff --git a/panda/src/parametrics/parametricCurve.cxx b/panda/src/parametrics/parametricCurve.cxx index 2920a2186d..c4a062312f 100644 --- a/panda/src/parametrics/parametricCurve.cxx +++ b/panda/src/parametrics/parametricCurve.cxx @@ -784,10 +784,17 @@ 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); - return true; + // 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; @@ -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 diff --git a/panda/src/parametrics/parametricCurve.h b/panda/src/parametrics/parametricCurve.h index d6fa45eca9..9a5e6b17f6 100644 --- a/panda/src/parametrics/parametricCurve.h +++ b/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;