Distance of a point to a spline / Binary Search

BlitzMax Forums/BlitzMax Programming/Distance of a point to a spline / Binary Search

beanage

(Posted 2010) [#1]

Hey,

this is a solution I had a hard time to find myself, so I thought you might be interested.

My Key Problem was, that I had to figure out the distance of an arbitrary point (aka arbitrary distance) to a cubic spline segment (not the whole function!). I then stumbled across all sorts of issues I never had dealt with before, like unsolvable fifth order polynomials. You aint get taught at highschool that there are unsolvable equations... frustration!

Then there was all the trouble with the Newton-Raphson iteration. At that point I was first reminded, that a spline is an indiscrete function going on beyound the zero and one interpolation values.. also there was trouble with getting a good initial guess for the newton raphson iteration. And finally, it didnt work out for arbitrary distances..

So I developed a pretty much brute force binary search "workaround". The performance is not stellar - ~~about 0.3 ms~~ [0.03 ms actually :)] on my desktop machine for a single search [with e-3 precision] - but good enough for now. And after all it works - which makes it better than any other solution I tried regardless of performance.

IF YOU HAVE A MORE ANALYTICAL SOLUTION, PLEASE SHARE IT WITH ME!!

The beanage.bsearch Module, which has been developed only for this problem, will be required for the test to run.
Screenshot:

SuperStrict

Import beanage.BSearch

Type TBezier
	
	Global cspline:TBezier
	Global px:Double
	Global py:Double
	
	Field _a:Double
	Field _b:Double
	Field _c:Double
	Field _d:Double
	Field _e:Double
	Field _f:Double
	Field _g:Double
	Field _h:Double
	Field _x1:Double
	Field _y1:Double
	Field _x2:Double
	Field _y2:Double
	Field _px:Double
	Field _py:Double
	Field _qx:Double
	Field _qy:Double
	
	'-----------------------
	'Binary Search Callbacks
	'-----------------------
	
	Function _bsearchEvaluator:Byte( this_:Double, that_:Double )
		Local x_:Double, y_:Double
		
		cspline.GetCoords this_, x_, y_
		Local this_distance_squared_:Double = (x_- px)^2 + (y_- py)^2
		
		cspline.GetCoords that_, x_, y_
		Return this_distance_squared_ <= ( (x_- px)^2 + (y_- py)^2 )'* cspline.GetCurvature( that_ ) * cspline.GetCurvature( this_ )
	End Function
	
	Function _0_up:Double[]( suggestion_:Double[] )
		If IsNan( suggestion_[1] )
			suggestion_[0] = .0 'return initial guess if no suggestion
		Else
			suggestion_[0]:+ .05
		
		End If
		cspline.HighlightCoordsAt suggestion_[0]
		Return suggestion_
	End Function
	
	Function _0_5_down:Double[]( suggestion_:Double[] )
		If IsNan( suggestion_[1] )
			suggestion_[0] = .5 'return initial guess if no suggestion
		Else
			suggestion_[0]:- .05
		
		End If
		cspline.HighlightCoordsAt suggestion_[0]
		Return suggestion_
	End Function

	Function _0_5_up:Double[]( suggestion_:Double[] )
		If IsNan( suggestion_[1] )
			suggestion_[0] = .5 'return initial guess if no suggestion
		Else
			suggestion_[0]:+ .05
		
		End If
		cspline.HighlightCoordsAt suggestion_[0]
		Return suggestion_
	End Function

	Function _1_down:Double[]( suggestion_:Double[] )
		If IsNan( suggestion_[1] )
			suggestion_[0] = 1.0 'return initial guess if no suggestion
		Else
			suggestion_[0]:- .05
		
		End If
		cspline.HighlightCoordsAt suggestion_[0]
		Return suggestion_
	End Function
	
	'------------
	'Spline Stuff
	'------------
	
	Method _updateParameters()
		_a = _x2 - 3.0*_qx + 3.0*_px - _x1
		_b = 3.0*_qx - 6.0*_px + 3.0*_x1
		_c = 3.0*_px - 3.0*_x1
		_d = _x1
		_e = _y2 - 3.0*_qy + 3.0*_py - _y1
		_f = 3.0*_qy - 6.0*_py + 3.0*_y1
		_g = 3.0*_py - 3.0*_y1
		_h = _y1
	End Method
	
	Method HighlightCoordsAt( t_:Double )
		Local x_:Double, y_:Double
		
		GetCoords t_, x_, y_
		DrawOval x_- 3, y_- 3, 6, 6
	End Method
		
	Method SetCoords( x1_:Double, y1_:Double, px_:Double, py_:Double, x2_:Double, y2_:Double, qx_:Double, qy_:Double )
		_x1 = x1_
		_y1 = y1_
		_x2 = x2_
		_y2 = y2_
		_px = px_
		_py = py_
		_qx = qx_
		_qy = qy_
		_updateParameters
	End Method
	
	Method DrawTangent( s_:Double )
		Local x_:Double, y_:Double
		
		GetCoords s_, x_, y_
		PlotLine x_, y_, GetSlope( s_ )
	End Method

	Method GetCoords( s_:Double, out_x_:Double Var, out_y_:Double Var )
		out_x_ = ( ( _a*s_ + _b )*s_ + _c )*s_ + _d
		out_y_ = ( ( _e*s_ + _f )*s_ + _g )*s_ + _h
	End Method
	
	Method GetSlope:Double( s_:Double )
		Return ( ( 3.0*_e*s_ + 2.0*_f )*s_ + _g )/( ( 3.0*_a*s_ + 2.0*_b )*s_ + _c )
	End Method
	
	Method GetCurvature:Double( s_:Double )
		 Return ( 6*_e*s_ + 2*_f )/( 3*_a*s_^2 + 2*_b*s_ + _c ) - ( 3*_e*s_^2 + 2*_f*s_ + _g )/( 3*_a*s_^2 + 2*_b*s_ + _c )^2 * ( 6*_a*s_ + 2*_b )
	End Method
	
	Method Draw( steps_:Int )
		Local x0_:Double, y0_:Double
		Local x1_:Double, y1_:Double
		
		GetCoords Null, x0_, y0_
		For Local i_:Int = 1 To steps_
			GetCoords Double i_/ Double steps_, x1_, y1_
			DrawLine x0_, y0_, x1_, y1_
			x0_ = x1_
			y0_ = y1_
		Next
		SetAlpha .2
		DrawLine _x1, _y1, _x2, _y2
		PlotLine _x1, _y1, ( _y1- _py )/( _x1- _px )
		PlotLine _x1, _y1, -( _x1- _px )/( _y1- _py )
		PlotLine _x2, _y2, ( _y2- _qy )/( _x2- _qx )
		PlotLine _x2, _y2, -( _x2- _qx )/( _y2- _qy )
		SetAlpha 1
	End Method
	
	Method GetInterpolationClosestToPoint:Double( px_:Double, py_:Double, out_iterations_:Int Ptr=Null, guess_:Double=.0 )
		'==============
		'BSEARCH METHOD
		'==============
		cspline = Self
		px = px_
		py = py_
		Local test_:BSearch = BSearch.Create( .0, 1.0, _bsearchEvaluator, .001, [ _0_up, _0_5_down, _0_5_up, _1_down ] )

		test_.Search
		guess_ = test_.GetResult()
		DrawText "t = ["+String(test_.info_interval[0])[..4]+".."+String(test_.info_interval[1])[..4]+"]", 10, 10
		DrawText "j = "+test_.info_best_probe_index, 10, 40
		
		If out_iterations_ Then out_iterations_[0] = test_.info_num_cycles
		Return guess_
	End Method
	
	Method Interact( mx_:Int, my_:Int, tol_:Double )
		Local dmx_:Int = MouseXSpeed()
		Local dmy_:Int = MouseYSpeed()
		
		If MouseDown(1)
			If PointDistance( mx_, my_, _x1, _y1 )<= tol_
				_x1:+ dmx_
				_y1:+ dmy_
				_updateParameters
			
			ElseIf PointDistance( mx_, my_, _x2, _y2 )<= tol_
				_x2:+ dmx_
				_y2:+ dmy_
				_updateParameters
	
			ElseIf PointDistance( mx_, my_, _px, _py )<= tol_
				_px:+ dmx_
				_py:+ dmy_
				_updateParameters
	
			ElseIf PointDistance( mx_, my_, _qx, _qy )<= tol_
				_qx:+ dmx_
				_qy:+ dmy_
				_updateParameters
	
			End If
		End If
	End Method

End Type

Function PlotLine( x_:Double, y_:Double, m_:Double )
	DrawLine 0, m_*-x_ + y_, 800, m_*( 800- x_ ) + y_
End Function

Function InterpolateLine( ax_:Double, ay_:Double, bx_:Double, by_:Double, t_:Double, out_x_:Double Var, out_y_:Double var )
	out_x_ = ( bx_- ax_ )* t_ + ax_
	out_y_ = ( by_- ay_ )* t_ + ay_
End Function

Function PointDistance:Double( px_:Double, py_:Double, qx_:Double, qy_:Double )
	Return Sqr( ( px_- qx_ )^2 + ( py_- qy_ )^2 )
End Function

Function DoubleToNAN( ptr_:Byte Ptr )
	ptr_[0] = $00
	ptr_[1] = $00
	ptr_[2] = $00
	ptr_[3] = $00
	ptr_[4] = $00
	ptr_[5] = $00
	ptr_[6] = $F8
	ptr_[7] = $7F
End Function

Function DoubleToInf( ptr_:Byte Ptr )
	ptr_[0] = $00
	ptr_[1] = $00
	ptr_[2] = $00
	ptr_[3] = $00
	ptr_[4] = $00
	ptr_[5] = $00
	ptr_[6] = $F0
	ptr_[7] = $7F
End Function

Function GuessDistanceToBezier( px_:Double, py_:Double, spline_:TBezier )
	SetColor 255, 0, 0
	DrawLine spline_._x1, spline_._y1, spline_._px, spline_._py
	DrawLine spline_._x2, spline_._y2, spline_._qx, spline_._qy
	DrawLine spline_._qx, spline_._qy, spline_._px, spline_._py
	Local i_:Int
	Local guess_:Double = spline_.GetInterpolationClosestToPoint( px_, py_, Int Ptr VarPtr i_, .0 )
	Local x_:Double
	Local y_:Double
	
	spline_.GetCoords guess_, x_, y_
	SetColor 0, 0, 255
	DrawLine x_, y_, px_, py_
	SetColor 255, 0, 0
	SetAlpha .5
	spline_.DrawTangent guess_
	SetAlpha 1
	SetColor 255, 255, 255
	DrawText "t = "+String( guess_ )[..5], 10, 20
	DrawText "i = "+i_, 10, 30
End Function

Graphics 800, 600
SetBlend ALPHABLEND

Local spline:TBezier = New TBezier
spline.SetCoords 100, 200, 200, 400, 600, 400, 700, 200

Repeat
	Cls
	Local x_:Double, y_:Double
	
	spline.GetCoords .5, x_, y_
	DrawOval x_- 2, y_- 2, 4, 4
	spline.GetCoords .25, x_, y_
	DrawOval x_- 2, y_- 2, 4, 4
	spline.GetCoords .75, x_, y_
	DrawOval x_- 2, y_- 2, 4, 4
	
	DrawOval spline._x1- 5, spline._y1- 5, 10, 10
	DrawOval spline._x2- 5, spline._y2- 5, 10, 10
	DrawOval spline._px- 5, spline._py- 5, 10, 10
	DrawOval spline._qx- 5, spline._qy- 5, 10, 10
	DrawOval x_- 2, y_- 2, 4, 4
	spline.Interact MouseX(), MouseY(), 5
	GuessDistanceToBezier MouseX(), MouseY(), spline
	spline.Draw 100
	Flip False
Until KeyHit( KEY_ESCAPE ) Or AppTerminate()

ImaginaryHuman

(Posted 2010) [#2]

Wow, your grasp of all those formulas and stuff is impressive. I have no idea how it all works.

I do know how to generate a cubic bezier pretty fast, though. See my fairly simple tutorial here: http://gurumeditations.wordpress.com/articles/calculating-and-drawing-bezier-curves-with-blitzmax/

It can be used to find any point along the curve pretty quickly... ie if you want the mid-point of the curve you just find the mid point of the lines between the control handles, make some new lines, find the midpoints of those, make some new lines, then find the final midpoint. I am sure that is faster than 0.3ms.

Maybe if mine is faster you can adapt it to do a binary search much more quickly.

Also I wonder if you can get a quick estimate of which portion of the curve the intersection will likely be in, based on the orientation of the line, or some bounding boxes or something. A 3-point quadratic bezier would be easier to deal with since it can't loop back on itself. Maybe for a cubic you can test each `half` of the spline separately and then dismiss the unlikely half? You could create two bounding triangles using the control points and the `mid point` of the curve, then see if the line intersects either triangle and proceed from there?

beanage

(Posted 2010) [#3]

Hey IH, I actually hoped you'd show up here :) Your tut on gurumeditations really helped me getting into splines..

Wow, your grasp of all those formulas and stuff is impressive.

Thanks, but Matlab did most of the hard work tho :)

A 3-point quadratic bezier would be easier to deal with

Yea. Totally agree with you on that one - not only would it tremendously simplify determinig the binary search interval, you could propably even use an analytic solution, since no 5th order polynomial would be to solve. But falling back to quadratic beziers just because of this procedural issue was (and is?) a little beyound my honor.. i guess.. i may change my mind on that :)

Maybe for a cubic you can test each `half` of the spline separately and then dismiss the unlikely half?

Well.. thats what binary search is all about, isnt it?

You could create two bounding triangles using the control points and the `mid point` of the curve, then see if the line intersects either triangle and proceed from there?

See if what line intersects?

The problem with all these guessing methods is, that there is such a big - if not infinite - number of them. And with growing complexity, each one tends to have a growing bunch of border cases, that are pretty hard to deal with, and finally make a simple-seeming approximation method more complicated than your current mathematical one.

One last thing I want to try tho- that is, converting the cubic spline into two quadratics and back. The biggest problem with cubics is also theire biggest strength - namely the complexity of shapes you can achieve with them, which is partly why I'm so addicted to them ;)