PDA

View Full Version : Compute closest point on spline



chronozphere
22-04-2011, 09:42 PM
Hey guys,

I want to implement an efficient way to do this. I translated this snippet (http://www.ogre3d.org/tikiwiki/Nearest+point+on+a+Spline&structure=Cookbook) to pascal. That snippet was based on this article (http://www.cs.uiowa.edu/~kearney/pubs/CurvesAndSufacesClosestPoint.pdf).



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
23-04-2011, 01:48 AM
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 :)