Results 1 to 2 of 2

Thread: Compute closest point on spline

  1. #1

    Compute closest point on spline

    Hey guys,

    I want to implement an efficient way to do this. I translated this snippet to pascal. That snippet was based on this article.

    Code:
    function TSpline.GetClosestN(aVec: TVector3f; s1,s2,s3: Single; MaxIterations: Integer = 20): Single;
    
      function Clamp(val, min, max: Single): Single;
      begin
        Result := val;
        if Result < Min then Result := min;
        if Result > Max then Result := max;
      end;
    
      function SplineInterpolate(s: Single): TVector3f;
      begin
        Result := GetPosition( GetCurveParameter(s) );
      end;
    
    var
      s, Ds: array[0..2] of Single;
      Ps: array[0..3] of Single;      // The function P(s) evaluated for the 4 values
      sk, skLast, grad, curv: Single;
      width, termCond: Single;
      Ds_pt1, Ds_pt2, Ds_pt3, g1,g2: Single;
      I, J, K, biggest: Integer;
    begin
      s[0] := s1;
      s[1] := s2;
      s[2] := s3;
    
      // For gradient and curviture approximation
      // CalculateLength() computes length of spline!
      width := 1.0/(CalculateLength() * 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
      termCond := width;
    
      // its typically done in under 10 iterations
      for I := 0 to maxIterations do
      begin
    
        Ds[0] := VecSqrMagnitude(vecSub( splineInterpolate(s[0]), aVec));
        Ds[1] := VecSqrMagnitude(vecSub( splineInterpolate(s[1]), aVec));
        Ds[2] := VecSqrMagnitude(vecSub( splineInterpolate(s[2]), aVec));
    
        // 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)) then
        begin
          // denominator = 0, how unfortunate
          sk := skLast; // keep going?
    
          Result := skLast;
          Exit;
        end;
    
        // Newton Bit
        sk := clamp(sk, width, 1.0-width); // so can interpolate points for Newtons method
    
        Ds_pt1 := VecSqrMagnitude(vecSub( splineInterpolate(sk - width), aVec));
        Ds_pt2 := VecSqrMagnitude(vecSub( splineInterpolate(sk        ), aVec));
        Ds_pt3 := VecSqrMagnitude(vecSub( splineInterpolate(sk + width), aVec));
    
        g1 := (Ds_pt2 - Ds_pt1)/width;
        g2 := (Ds_pt3 - Ds_pt2)/width;
    
        grad := (Ds_pt3 - Ds_pt1)/(2*width);
    
        curv := (g2 - g1)/width;
    
        //Check whether equal to zero
        if (Abs(curv) > 0.00001) then
        begin
          sk := sk - grad/curv;
          sk := clamp(sk, 0.0, 1.0);
        end;
    
        // termination criteria
        // difference between skLast and sk <= range of s over the segment x small constant
        if (i > 0) then
        begin
          if (Abs(sk - skLast) <= termCond) then
          begin
            //printf ("exit condition met %d %f %f\n", i, Math::Abs(sk - skLast), termCond);
            Result := sk;
            Exit;
          end;
        end;
        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
        biggest := 0;
        for J := 0 to 3 do
        begin
          if (Ps[J] > Ps[biggest]) then biggest := J;
        end;
    
        if (biggest <= 2) then
        begin
          // update one of the estimates
          // equations will blow up if any of the estimates are the same
    
          s[biggest] := sk;
    
          for J := 0 to 2 do
            for K := J+1 to 2 do
            begin
              if (s[j] = s[k]) then
              begin
                if (s[k] < 0.5) then  s[k] := s[k] + 0.0001
                else                  s[k] := s[k] - 0.0001;
              end;
            end;
        end;
      end;
    
      Result := sk;
    end;
    However, it doesn't seem to work. Does anyone see any differences between my code and the snippet? More importantly, does anybody understand how the "Quadratic minimization + Newton's method" works. My understanding of this is not good enough to debug this code. I'd really appreciate some help.

    Further information: I want to use this on multiple bezier spline segemnts (each segment consisting of 4 points, with a curve inbetween). The s1..s3 parameters are distances from the start of the spline (no 0..1 range). The output must also lie in this range. GetCurveParameter() translates a 0...Length value into a 0...1 range (and takes care of the "variable speed" along weirdly stetched bezier curves). If anyone needs a full project to expiriment with, please give me a sign.

    Thank you.
    Coders rule nr 1: Face ur bugz.. dont cage them with code, kill'em with ur cursor.

  2. #2
    It propably wouldn't look too bad if the delta (0..1) would be calculated from function ClosestPointOnLine for each segment. Even if it's just a temporary fix

Bookmarks

Posting Permissions

  • You may not post new threads
  • You may not post replies
  • You may not post attachments
  • You may not edit your posts
  •