History: Nearest point on a Spline

Source of version: 3 (current)

!!Quadratic Minimization Method combined with Newton Raphson

Pilfered from "Robust and Efficient Computation of the Closest Point on a Spline Curve"
''Hongling Wang, Joseph Kearney, and Kendall Atkinson''

Using 3 estimates apply the 2 above methods we chose the best 3 by eliminating the value which gives the largest P(s).

How to chose the 3 estimates? If you know the segment you could use a ''~np~0<s<1~/np~'' value thats ''si'', ''si+1'' and ''~np~(si + si+1)/2~/np~'', ''si'' the segment can be pretty approximate with this.

Added for clarity ''si'' is the segment your on, a number between 0 and 1, ''si+1'' is the next segment on (a number between 0 and 1). So for the 3 starting estimates you could use the segment your on (or even the segment you think your on) its start, half way on to the start of the next segment and the start of the next segment. Its just a suggestion they give.

Assumptions made in code, all splines of equal length or approximately equal length (see ((Equal Length Spline))).

{CODE(wrap="1", colors="c++")}double clamp (double value, double lowerLimit, double upperLimit){
  if (value < lowerLimit){ return lowerLimit; }
  else if (value > upperLimit){ return upperLimit; }
  else { return value; }
}

Real getClosestPointOnSpline(SimpleSpline& spline, Vector3 testPoint, Real s1, Real s2, Real s3, int maxIterations = 20){

  Real s[3];  // The estimates
  s[0] = s1; s[1] = s2; s[2] = s3; 
  
  Real Ds[3]; // The distances squared to the estimates
  
  Real sk, skLast; // sk is the "hopefully" converging value generated, skLast is the previous one
  
  Real Ps[4]; // The function P(s) evaluated for the 4 values
  
  // For gradient and curviture approximation
  Real width = 1.0/(spline.getNumPoints() * 1000); // step would be 1/1000 of a spline length 
  
  // The range of the parameter value for a spline segment * proportion of it is used to test for an exit condition 
  Real termCond = 1.0/(spline.getNumPoints() * 1000);

  for (int i=0; i<maxIterations; i++){ // its typically done in under 10

    Ds[0] = (spline.interpolate(s[0]) - testPoint).squaredLength();
    Ds[1] = (spline.interpolate(s[1]) - testPoint).squaredLength();
    Ds[2] = (spline.interpolate(s[2]) - testPoint).squaredLength();
    
    // Quadratic Minimization Bit
    sk = 0.5 * ( (s[1]*s[1] - s[2]*s[2]) * Ds[0] + (s[2]*s[2] - s[0]*s[0]) * Ds[1] + (s[0]*s[0] - s[1]*s[1]) * Ds[2] ) /
               ( (s[1]-s[2]) * Ds[0]         + (s[2] - s[0]) * Ds[1]       + (s[0] - s[1]) * Ds[2] );
    
    if (isnan (sk)){ // denominator = 0, how unfortunate
      //printf ("isnan %d %f\n", i, skLast);
      
      sk = skLast; // keep going?
      
      //return skLast;
      //return true;
    }

    // Newton Bit
    sk = clamp(sk, width, 1.0-width); // so can interpolate points for Newtons method
    
    Real grad, curv; // 1st 2nd derivatives
    Real Ds_pt1 = (spline.interpolate(sk - width) - testPoint).squaredLength();
    Real Ds_pt2 = (spline.interpolate(sk)         - testPoint).squaredLength();
    Real Ds_pt3 = (spline.interpolate(sk + width) - testPoint).squaredLength();
    
    Real g1 = (Ds_pt2 - Ds_pt1)/width;
    Real g2 = (Ds_pt3 - Ds_pt2)/width;
    
    grad = (Ds_pt3 - Ds_pt1)/(2*width);
    
    curv = (g2 - g1)/width;

    if (curv != 0.0){ 
      sk = sk - grad/curv;
      sk = clamp(sk, 0.0, 1.0);
    }

    // termination criteria
    // difference between skLast and sk <= range of s over the segment x small constant
    if (i > 0){
      if (Math::Abs(sk - skLast) <= termCond){
        //printf ("exit condition met %d %f %f\n", i, Math::Abs(sk - skLast), termCond);
        return sk;
        //return true;
      }
    }
    skLast = sk;

    // chose the best 3 from their Ps values (the closest ones we keep)
    // general Ps equation
    // Ps =    ((s-s2)*(s-s3))/((s1-s2)*(s1-s3)) * Ds1 + 
    //        ((s-s1)*(s-s3))/((s2-s1)*(s2-s3)) * Ds2 + 
    //        ((s-s1)*(s-s2))/((s3-s1)*(s3-s2)) * Ds3;
    
    Ps[0] = ((s[0]-s[1])*(s[0]-s[2]))/((s[0]-s[1])*(s[0]-s[2])) * Ds[0];
    
    Ps[1] = ((s[1]-s[0])*(s[1]-s[2]))/((s[1]-s[0])*(s[1]-s[2])) * Ds[1];

    Ps[2] = ((s[2]-s[0])*(s[2]-s[1]))/((s[2]-s[0])*(s[2]-s[1])) * Ds[2];
            
    Ps[3] = ((sk-s[1])*(sk-s[2]))/((s[0]-s[1])*(s[0]-s[2])) * Ds[0] + 
            ((sk-s[0])*(sk-s[2]))/((s[1]-s[0])*(s[1]-s[2])) * Ds[1] + 
            ((sk-s[0])*(sk-s[1]))/((s[2]-s[0])*(s[2]-s[1])) * Ds[2];                
        
    
    // find the worest one
    int biggest = 0;
    for (int i=1; i<4; i++){
      if (Ps[i]>Ps[biggest]){ biggest = i; }
    }
    
    if (biggest <= 2){ // update one of the estimates
      // equations will blow up if any of the estimates are the same

      s[biggest] = sk;
      
      // make them unique values
      for (int i=0; i<3; i++){
        for (int j=i+1; j<3; j++){
          if (s[i] == s[j]){
            if (s[j] < 0.5){ s[j] = s[j] + 0.0001; }
            else {           s[j] = s[j] - 0.0001; }
          }
        }
      }
    }
  }
  
  return sk;
  //return false;
}{CODE}

It'll usually converges in less than 10 iterations, I arbitarily chose 20... converges to a local minimum. Be careful with that, also does best on not too bendy splines. In the article they talk about a road surface and uses for AI.
History: Nearest point on a Spline

Source of version: 3 (current)

Search by Tags

Latest Changes

Search

Online Users
