Compute closest point on spline

[chronozphere]

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.

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.

[User137]

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