2d Primitive collision detection

BlitzMax Forums/BlitzMax Programming/2d Primitive collision detection

matibee

(Posted 2009) [#1]

(Hope it's not too big a post.. heck I've seen entire 3d engines posted.. maybe it should go to code archives, but surely it wants reviewing first.. it's late.. I'm rambling)

As the title states really. This is something I've done before in C++ a looong time ago.

It shows how to use different primitives (circles, rects and polygons) to represent objects and how to check for collisions against each other. Collisions may be penetrations or overlaps it'll detect either. There's some other useful 2d stuff in here too, like point-to-line distance, line-to-line intersection, point-in-polygon etc.

Obviously there's some performance benefits from using simpler objects; Rect vs. Rect is the fastest, Circle vs. Circle is next, but the code is there for all eventualities.

You will have to install the Mauft 2D Vector mod from http://mauft.com/open-source/blitzmax.

Questions? Comments?

'###############################################################
'
'	Some basic collision detection code using primitives
'	such as lines, rects, circles and polygons (concave
'	or convex).
'
'	Most math adapted from P.Bourkes geometry pages:
'	<a href="http://local.wasp.uwa.edu.au/~pbourke/geometry/" target="_blank">http://local.wasp.uwa.edu.au/~pbourke/geometry/</a>
'
'
'	In the sample press space to switch between mouse driven
'	primitives and see it change color on collision.
'	Care has been taken to correctly handle all intersection
'	and overlap conditions.
'
'	Matt Bennett Jul 09.  
'
'	All code is public domain, use as you wish.
'

SuperStrict 

'
' requires the Mauft Vector Mod from
' <a href="http://mauft.com/open-source/blitzmax" target="_blank">http://mauft.com/open-source/blitzmax</a>
'
Import Mauft.Vector


' Collision types

'------------------------------------------------------------------------------
Type collisionCircle
	Field center:TVector
	Field radius:Float
	
	Function Create:collisionCircle ( x:Float, y:Float, r:Float )
		Local t:collisionCircle = New collisionCircle
		t.center = VectorCreate( x, y )
		t.radius = r
		Return t
	End Function
	
	Method AbsPosition( iX:Int, iY:Int )
		center._x = iX
		center._y = iY
	End Method 
	
	Method debugDraw()
		DrawOval( center._x - radius, center._y - radius, radius * 2, radius * 2 )
	End Method
End Type
'------------------------------------------------------------------------------
Type collisionRect
	Field p1:TVector
	Field p2:TVector
	Field position:TVector
	
	Function Create:collisionRect ( upperLeft:TVector, lowerRight:TVector )
		Local t:collisionRect = New collisionRect
		t.position = New TVector
		t.p1 = VectorClone( upperLeft )
		t.p2 = VectorClone( lowerRight )
		Return t
	End Function
	
	Method debugDraw()
		DrawLine( p1._x, p1._y, p2._x, p1._y )
		DrawLine( p2._x, p1._y, p2._x, p2._y )
		DrawLine( p2._x, p2._y, p1._x, p2._y )
		DrawLine( p1._x, p2._y, p1._x, p1._y )
	End Method
	
	Method AbsPosition( iX:Int, iY:Int )
		Local vMove:TVector = VectorCreate( iX - position._x, iY - position._y )
		position.AddVector( vMove )
		p1.AddVector( vMove )
		p2.AddVector( vMove )
	End Method
	
	Method pointInside:Int ( p:TVector )
		Return ( p._x >= p1._x And p._x <= p2._x And p._y >= p1._y And p._y <= p2._y )
	End Method
	
	Method lineIntersect:Int ( v1:TVector, v2:TVector )
		If ( lineLineIntersection( v1, v2, VectorCreate( p2._x, p1._y ), VectorCreate( p1._x, p1._y ) ) )
			Return True
		Else If lineLineIntersection( v1, v2, VectorCreate( p2._x, p2._y ), VectorCreate( p2._x, p1._y ) ) 
			Return True 
		Else If lineLineIntersection( v1, v2, VectorCreate( p1._x, p2._y ), VectorCreate( p2._x, p2._y ) ) 
			Return True 
		Else If lineLineIntersection( v1, v2, VectorCreate( p1._x, p1._y ), VectorCreate( p1._x, p2._y ) ) 
			Return True
		End If
		Return False			
	End Method 
End Type
'------------------------------------------------------------------------------
Type collisionPoly
	
	Const CPOLY_MAX_VERTICES:Int = 32
	
	Field vertices:TVector[ CPOLY_MAX_VERTICES ]
	Field vertexCount:Int
	Field position:TVector
	
	Function Create:collisionPoly ( p1:TVector )
		Local t:collisionPoly = New collisionPoly
		t.position = New TVector
		t.vertices[0] = VectorClone( p1 )
		t.vertexCount = 1
		Return t
	End Function
	
	Method AddVertex( p1:TVector )
		Assert( vertexCount < CPOLY_MAX_VERTICES ) ' ran out of space!
		vertices[ vertexCount ] = VectorClone( p1 )
		vertexCount :+ 1
	End Method 
	
	Method pointInside:Int ( p:TVector )
		Local counter:Int
		Local p1:TVector = VectorClone( vertices[0] )
		Local xinters:Double
		For Local i:Int = 1 To vertexCount
			Local p2:TVector = VectorClone( vertices[ i Mod vertexCount ] )
			If ( p._y > Min( p1._y, p2._y ) )
				If ( p._y <= Max( p1._y, p2._y ) )
					If ( p._x <= Max( p1._x, p2._x ) )
						If ( p1._y <> p2._y ) 
							xinters = (p._y-p1._y)*(p2._x-p1._x)/(p2._y-p1._y)+p1._x
					          If (p1._x = p2._x Or p._x <= xinters)
              						counter :+ 1
							End If 
						End If
					End If
				End If
			End If
			p1 = VectorClone( p2 )
		Next
		
		Return counter Mod 2
	End Method
	
	Method lineIntersect:Int ( v1:TVector, v2:TVector )
		For Local t:Int = 0 To vertexCount - 1
			If ( lineLineIntersection( v1, v2, vertices[t], vertices[ (t + 1) Mod vertexCount ] ) )
				Return True
			End If
		Next		
	End Method 
	
	Method debugDraw()
		For Local t:Int = 0 To vertexCount - 1
			Local tt:Int = (t + 1) Mod vertexCount
			DrawLine( vertices[t]._x, vertices[t]._y, vertices[tt]._x, vertices[tt]._y )
		Next
	End Method
	
	Method AbsPosition( iX:Int, iY:Int )
		Local vMove:TVector = VectorCreate( iX - position._x, iY - position._y )
		position.AddVector( vMove )
		For Local t:Int = 0 To vertexCount - 1
			vertices[ t ].AddVector( vMove )
		Next
	End Method
	
End Type
'##############################################################################
' Utility functions

'------------------------------------------------------------------------------
' Distance between two 2d points (pythagoras' theorem)
'
Function pointPointDistance:Float ( v1:TVector, v2:TVector )
	Return Sqr( ((v2._x - v1._x) * (v2._x - v1._x)) + ((v2._y - v1._y) * (v2._y - v1._y)) )
End Function

'------------------------------------------------------------------------------
' Closest distance between a point (p) and the line (v1 -> v2)
'
Function pointDistanceToLine:Float ( p:TVector, v1:TVector, v2:TVector )
	 ' get the vector of the line direction
	Local vLine:TVector = VectorClone( v2 )
	vLine.SubtractVector( v1 )
	
	' get the closest point on the plane of the lane
	Local u:Float = ((p._x - v1._x) * ( v2._x - v1._x ) + ( p._y - v1._y ) * ( v2._y - v1._y ) ) / vLine.DotProduct( vLine )
	
	' If the closest point on the plane is also on the line, u will be between 0.0 (the start) and (1.0) of the line
	If ( u < 0 )
		Return pointPointDistance( p, v1 ) ' closest point is the start point
	Else If ( u > 1.0 )
		Return pointPointDistance( p, v2 )	' closest point is the end point
	End If
	
	' closest point is on the line, find the actual intersection point
	Local vecInters:TVector = VectorClone( v1 )
	vecInters._x :+ vLine._x * u
	vecInters._y :+ vLine._y * u
	Return pointPointDistance( vecInters, p ) ' return the distance
End Function

'------------------------------------------------------------------------------
' Intersection point of line segments (p1 -> p2) and (p3 -> p4)
'
'	See <a href="http://local.wasp.uwa.edu.au/~pbourke/geometry/lineline2d/" target="_blank">http://local.wasp.uwa.edu.au/~pbourke/geometry/lineline2d/</a>
'
Function lineLineIntersection:Int ( p1:TVector, p2:TVector, p3:TVector, p4:TVector )
	Local deno:Float = (p4._y - p3._y) * (p2._x - p1._x) - (p4._x - p3._x) * (p2._y - p1._y)
	Local numa:Float = (p4._x - p3._x) * (p1._y - p3._y) - (p4._y - p3._y) * (p1._x - p3._x)
	Local numb:Float = (p2._x - p1._x) * (p1._y - p3._y) - (p2._y - p1._y) * (p1._x - p3._x)
	
	' I check for abitrary closeness to zero, instead of "= 0.0"
	If ( deno < 0.01 And deno > -0.01 )
		If ( ( numa < 0.01 And numa > -0.01 ) And ( numb < 0.01 And numb > -0.01 ) )
			Return True ' lines are coincident
		End If
		Return False ' lines are parallel, but not coincident
	End If
	
	Local ua:Float = numa / deno
	Local ub:Float = numb / deno
	
	If ( ua >= 0 And ua <= 1.0 And ub >= 0 And ub <= 1.0 )
		Return True
	End If
	
	Return False
End Function 

'##############################################################################
'	Primitive Vs Primitive collision functions

'------------------------------------------------------------------------------
' Check collision between a circle and a Polygon
'
Function circlePolyIntersection:Int ( circle:collisionCircle, poly:collisionPoly )
	' Is the circle center inside the polygon??
	If poly.pointInside( circle.center )
		Return True
	End If
	
	' loop through the polygon lines to see if the circle is touching any of them
	For Local t:Int = 0 To poly.vertexCount - 1
		
		Local v:TVector = VectorClone( poly.vertices[t] )
		v.SubtractVector( circle.center )
		If ( v.GetLength() < circle.Radius )
			Return True
		End If
		
		Local tt:Int = (t + 1) Mod poly.vertexCount
		Local dist:Float = pointDistanceToLine( circle.center, poly.vertices[t], poly.vertices[tt] )
		If ( dist >= 0 And dist < circle.Radius )
			Return True
		End If
	Next
End Function

'------------------------------------------------------------------------------
' Check collision between two circles
'
Function circleCircleIntersection:Int ( c1:collisionCircle, c2:collisionCircle )
	Return pointPointDistance( c1.center, c2.center ) <= c1.radius + c2.radius
End Function

'------------------------------------------------------------------------------
' Check collision between two polygons
'
Function polyPolyIntersection:Int ( poly1:collisionPoly, poly2:collisionPoly )
	For Local t:Int = 0 To poly1.vertexCount - 1
		If ( poly2.pointInside( poly1.vertices[ t ] ) )
			Return True
		End If
	Next
	For Local t:Int = 0 To poly2.vertexCount - 1
		If ( poly1.pointInside( poly2.vertices[ t ] ) )
			Return True
		End If
	Next
	For Local t:Int = 0 To poly1.vertexCount - 1
		If ( poly2.lineIntersect( poly1.vertices[t], poly1.vertices[ (t + 1) Mod poly1.vertexCount ] ) )
			Return True
		End If
	Next 
	Return False 	
End Function

'------------------------------------------------------------------------------
' Check collision between a rect and a polygon
'
Function rectPolyIntersection:Int ( rect:collisionRect, poly:collisionPoly )
	If ( poly.pointInside( rect.p1 ) )
		Return True
	Else If ( poly.pointInside( rect.p2 ) )
		Return True
	Else If ( poly.PointInside( VectorCreate( rect.p1._x, rect.p2._y ) ) )
		Return True
	Else If ( poly.PointInside( VectorCreate( rect.p2._x, rect.p1._y ) ) )
		Return True
	End If 
	If ( poly.lineIntersect( VectorCreate( rect.p1._x, rect.p1._y ), VectorCreate( rect.p2._x, rect.p1._y ) ) )
		Return True
	Else If ( poly.lineIntersect( VectorCreate( rect.p2._x, rect.p1._y ), VectorCreate( rect.p2._x, rect.p2._y ) ) )
		Return True
	Else If ( poly.lineIntersect( VectorCreate( rect.p2._x, rect.p2._y ), VectorCreate( rect.p1._x, rect.p2._y ) ) )
		Return True
	Else If ( poly.lineIntersect( VectorCreate( rect.p1._x, rect.p2._y ), VectorCreate( rect.p1._x, rect.p1._y ) ) )
		Return True
	End If 
	Return False 
End Function

'------------------------------------------------------------------------------
' Check collision between a rect and a circle
'
Function rectCircleIntersection:Int ( rect:collisionRect, circle:collisionCircle )
	If rect.pointInside( circle.center )
		Return True 
	End If

	Local dist:Float = pointDistanceToLine( circle.center, rect.p1, VectorCreate( rect.p2._x, rect.p1._y ) )
	If ( dist <= circle.Radius )
		Return True 
	End If 
	dist = pointDistanceToLine( circle.center, rect.p2, VectorCreate( rect.p1._x, rect.p2._y ) )
	If ( dist <= circle.Radius )
		Return True 
	End If 
	dist = pointDistanceToLine( circle.center, rect.p1, VectorCreate( rect.p1._x, rect.p2._y ) )
	If ( dist <= circle.Radius )
		Return True 
	End If 
	dist = pointDistanceToLine( circle.center, rect.p2, VectorCreate( rect.p2._x, rect.p1._y ) )
	If ( dist <= circle.Radius )
		Return True 
	End If 

	Return False
End Function

'------------------------------------------------------------------------------
' Check collisions between two rects
'
Function rectRectIntersection:Int ( rect1:collisionRect, rect2:collisionRect )
	Return rect1.p1._x <= rect2.p2._x And rect1.p2._x >= rect2.p1._x And ..
			rect1.p1._y <= rect2.p2._y And rect1.p2._y >= rect2.p1._y	
End Function


'##############################################################################
' Example

Graphics 800, 600

' Create some user controllable primitives..
Local myCircle:collisionCircle = collisionCircle.Create( 0, 0, 15 )
Local myRect:collisionRect = collisionRect.Create( VectorCreate( 0, 0 ), VectorCreate( 15, 75 ) )

' A polygon is a multiple creation step..
' at least one AddVertex is required (which makes a single line),
' two makes a triangle etc.
Local myPoly:collisionPoly = collisionPoly.Create( VectorCreate( 0, 0 ) )
myPoly.AddVertex( VectorCreate( 250, 30 ) )
myPoly.AddVertex( VectorCreate( 230, 45 ) )

' which primitive user is controlling
Local primitive:Int = 0

' Create some static scene primitives
Local sceneCircle:collisionCircle = collisionCircle.Create( 600, 200, 40 )
Local sceneRect:collisionRect = collisionRect.Create( VectorCreate( 40, 500 ), VectorCreate( 300, 520 ) )
Local scenePoly:collisionPoly = collisionPoly.Create( VectorCreate( 200, 200 ) )
scenePoly.AddVertex( VectorCreate( 600, 300 ) )
scenePoly.AddVertex( VectorCreate( 200, 400 ) )
scenePoly.AddVertex( VectorCreate( 275, 300 ) )

' And loop...
While ( Not ( KeyDown( KEY_ESCAPE ) ) )
	
	Cls

	Select primitive
	Case 0
		myCircle.AbsPosition( MouseX(), MouseY() )
		If ( circlePolyIntersection( myCircle, scenePoly ) Or ..
			circleCircleIntersection( myCircle, sceneCircle ) Or ..
			rectCircleIntersection( sceneRect, myCircle ) )
			SetColor 255,0,0
		End If
		myCircle.debugDraw()
	Case 1 
		myPoly.AbsPosition( MouseX(), MouseY() )
		If ( polyPolyIntersection( myPoly, scenePoly ) Or ..
			circlePolyIntersection( sceneCircle, myPoly ) Or ..
			rectPolyIntersection( sceneRect, myPoly ) )
			SetColor 255,0,0
		End If
		myPoly.debugDraw()
	Case 2
		myRect.AbsPosition( MouseX(), MouseY() )
		If ( rectPolyIntersection( myRect, scenePoly ) Or ..
			rectCircleIntersection( myRect, sceneCircle ) Or ..
			rectRectIntersection( myRect, sceneRect ) ) 
			SetColor 255,0,0
		End If
		myRect.debugDraw()
	End Select
	
	SetColor 255,255,255
	scenePoly.debugDraw()
	sceneCircle.debugDraw() 
	sceneRect.debugDraw()
	
	If ( KeyHit( KEY_SPACE ) )
		primitive :+ 1
		primitive :Mod 3
	End If
	
	Flip
	
End While

Arowx

(Posted 2009) [#2]

Cool, right now let's see how fast I can get AWOD to play now!

matibee

(Posted 2009) [#3]

:) Let us know!

FWIW, use rects for the bullets.

I've been thinking of some advances too... if collisionCircle and collisionPoly both contain a collisionRect (defined as their bounding rect) a quick and easy first-option would be to see if their bounding rects overlap (can't believe I missed that one).

This would also allow me to make a dynamic quad-tree of all objects using their bounding rects.

Nate the Great

(Posted 2009) [#4]

man this would have saved me some trouble... I just had to write something similar for my physics engine and it just hurts my brain... good job though

matibee

(Posted 2009) [#5]

Nate, full 2d physics is maybe where I'll take this if I have the time and energy.

I've just recoded it so circle and poly objects derive from the rect base type and I'm about to try a dynamic quadtree with objects spacially sorted using their bounding rects.

More demo's to follow :)

matibee

(Posted 2009) [#6]

Update - A thousand moving primitives with quadtree culling :)

I'll leave the original intact so you can see the progress.

Potential colliders are shown in blue, actual colliders in red. You can toggle bounding boxes[B], movement [M] (this avoids rebuilding the tree every frame so watch the fps difference), and the tree itself [T]. Space bar selects various primitive types.

'###############################################################
'
'	Some basic collision detection code using primitives
'	such as lines, rects, circles and polygons (concave
'	or convex).
'
'	Most math adapted from P.Bourkes geometry pages:
'	<a href="http://local.wasp.uwa.edu.au/~pbourke/geometry/" target="_blank">http://local.wasp.uwa.edu.au/~pbourke/geometry/</a>
'
'
'	In the sample press space to switch between mouse driven
'	primitives and see it change color on collision.
'	Care has been taken to correctly handle all intersection
'	and overlap conditions.
'
'	Matt Bennett Jul 09.  
'
'	All code is public domain, use as you wish.
'

SuperStrict 

'
' requires the Mauft Vector Mod from
' <a href="http://mauft.com/open-source/blitzmax" target="_blank">http://mauft.com/open-source/blitzmax</a>
'
Import Mauft.Vector

Global gSHOW_BOUNDING_RECTS:Int = 1


'------------------------------------------------------------------------------
' Collision types
'------------------------------------------------------------------------------
Type collisionRect
	Field p1:TVector
	Field p2:TVector
	Field position:TVector
	
	Field collisionType:Int
	
	
	Function Create:collisionRect ( upperLeft:TVector, lowerRight:TVector )
		Local t:collisionRect = New collisionRect
		t.position = New TVector
		t.p1 = VectorClone( upperLeft )
		t.p2 = VectorClone( lowerRight )
		t.position = VectorClone( upperLeft )
		Return t
	End Function
	
	Method debugDraw()
		DrawLine( p1._x, p1._y, p2._x, p1._y )
		DrawLine( p2._x, p1._y, p2._x, p2._y )
		DrawLine( p2._x, p2._y, p1._x, p2._y )
		DrawLine( p1._x, p2._y, p1._x, p1._y )
	End Method
	
	Method Move( fX:Int, fY:Int )
		p1._x :+ fX
		p1._y :+ fY
		p2._x :+ fX
		p2._y :+ fY
		position._x :+ fX
		position._y :+ fY
	End Method
	
	Method VectorMove( v:TVector )
		Move( v._x, v._y )
	End Method 
	
	Method AbsPosition( iX:Int, iY:Int )
		Move( iX - position._x, iY - position._y )
	End Method
	
	Method pointInside:Int ( p:TVector )
		Return ( p._x >= p1._x And p._x <= p2._x And p._y >= p1._y And p._y <= p2._y )
	End Method
	
	Method lineIntersect:Int ( v1:TVector, v2:TVector )
		If ( lineLineIntersection( v1, v2, VectorCreate( p2._x, p1._y ), VectorCreate( p1._x, p1._y ) ) )
			Return True
		Else If lineLineIntersection( v1, v2, VectorCreate( p2._x, p2._y ), VectorCreate( p2._x, p1._y ) ) 
			Return True 
		Else If lineLineIntersection( v1, v2, VectorCreate( p1._x, p2._y ), VectorCreate( p2._x, p2._y ) ) 
			Return True 
		Else If lineLineIntersection( v1, v2, VectorCreate( p1._x, p1._y ), VectorCreate( p1._x, p2._y ) ) 
			Return True
		End If
		Return False			
	End Method 
End Type
'------------------------------------------------------------------------------
Type collisionCircle Extends collisionRect
	Field center:TVector
	Field radius:Float
	
	Function CreateCircle:collisionCircle ( x:Float, y:Float, r:Float )
		Local t:collisionCircle = New collisionCircle
		t.center = VectorCreate( x, y )
		t.radius = r
		t.p1 = VectorCreate( x - r, y - r )
		t.p2 = VectorCreate( x + r, y + r )
		t.position = VectorClone( t.p1 )
		t.collisionType = 1
		Return t
	End Function
	
	Method Move( fX:Int, fY:Int )
		center._x :+ fX
		center._y :+ fY
		Super.Move( fX, fY )
	End Method
	
	Method VectorMove( v:TVector )
		Move( v._x, v._y )
	End Method 
	
	Method AbsPosition( iX:Int, iY:Int )
		center._x = iX
		center._y = iY
		Super.AbsPosition( iX - radius, iY - radius )
	End Method 
	
	Method debugDraw()
		DrawOval( center._x - radius, center._y - radius, radius * 2, radius * 2 )
		If ( gSHOW_BOUNDING_RECTS )
			Super.debugDraw()
		End If 
	End Method
End Type
'------------------------------------------------------------------------------
Type collisionPoly Extends collisionRect
	
	Const CPOLY_MAX_VERTICES:Int = 32
	
	Field vertices:TVector[ CPOLY_MAX_VERTICES ]
	Field vertexCount:Int
	
	Function CreatePoly:collisionPoly ( p1:TVector )
		Local t:collisionPoly = New collisionPoly
		t.position = New TVector
		t.vertices[0] = VectorClone( p1 )
		t.vertexCount = 1
		t.p1 = VectorClone( p1 )
		t.p2 = VectorClone( p1 )
		t.position = VectorClone( p1 )
		t.collisionType = 2
		Return t
	End Function
	
	Method Move( fX:Int, fY:Int )
		For Local t:Int = 0 To vertexCount - 1
			vertices[t]._x :+ fX
			vertices[t]._y :+ fY
		Next 
		Super.Move( fX, fY )
	End Method
	
	Method VectorMove( v:TVector )
		Move( v._x, v._y )
	End Method 
	
	Method AddVertex( p:TVector )
		Assert( vertexCount < CPOLY_MAX_VERTICES ) ' ran out of space!
		vertices[ vertexCount ] = VectorClone( p )
		vertexCount :+ 1
		p1._x = Min ( p1._x, p._x )
		p1._y = Min ( p1._y, p._y )
		p2._x = Max ( p2._x, p._x )
		p2._y = Max ( p2._y, p._y )
		position._x = Min ( position._x, p._x )
		position._y = Min ( position._y, p._y )
	End Method 
	
	Method pointInside:Int ( p:TVector )
		Local counter:Int
		Local p1:TVector = VectorClone( vertices[0] )
		Local xinters:Double
		For Local i:Int = 1 To vertexCount
			Local p2:TVector = VectorClone( vertices[ i Mod vertexCount ] )
			If ( p._y > Min( p1._y, p2._y ) )
				If ( p._y <= Max( p1._y, p2._y ) )
					If ( p._x <= Max( p1._x, p2._x ) )
						If ( p1._y <> p2._y ) 
							xinters = (p._y-p1._y)*(p2._x-p1._x)/(p2._y-p1._y)+p1._x
					          If (p1._x = p2._x Or p._x <= xinters)
              						counter :+ 1
							End If 
						End If
					End If
				End If
			End If
			p1 = VectorClone( p2 )
		Next
		
		Return counter Mod 2
	End Method
	
	Method lineIntersect:Int ( v1:TVector, v2:TVector )
		For Local t:Int = 0 To vertexCount - 1
			If ( lineLineIntersection( v1, v2, vertices[t], vertices[ (t + 1) Mod vertexCount ] ) )
				Return True
			End If
		Next		
	End Method 
	
	Method debugDraw()
		For Local t:Int = 0 To vertexCount - 1
			Local tt:Int = (t + 1) Mod vertexCount
			DrawLine( vertices[t]._x, vertices[t]._y, vertices[tt]._x, vertices[tt]._y )
		Next
		If ( gSHOW_BOUNDING_RECTS )
			Super.debugDraw()
		End If 
	End Method
	
	Method AbsPosition( iX:Int, iY:Int )
		Local vMove:TVector = VectorCreate( iX - position._x, iY - position._y )
		For Local t:Int = 0 To vertexCount - 1
			vertices[ t ].AddVector( vMove )
		Next
		Super.Move( vMove._x, vMove._y )
	End Method
	
End Type
'##############################################################################
' Utility functions

'------------------------------------------------------------------------------
' Distance between two 2d points (pythagoras' theorem)
'
Function pointPointDistance:Float ( v1:TVector, v2:TVector )
	Return Sqr( ((v2._x - v1._x) * (v2._x - v1._x)) + ((v2._y - v1._y) * (v2._y - v1._y)) )
End Function

'------------------------------------------------------------------------------
' Closest distance between a point (p) and the line (v1 -> v2)
'
Function pointDistanceToLine:Float ( p:TVector, v1:TVector, v2:TVector )
	 ' get the vector of the line direction
	Local vLine:TVector = VectorClone( v2 )
	vLine.SubtractVector( v1 )
	
	' get the closest point on the plane of the lane
	Local u:Float = ((p._x - v1._x) * ( v2._x - v1._x ) + ( p._y - v1._y ) * ( v2._y - v1._y ) ) / vLine.DotProduct( vLine )
	
	' If the closest point on the plane is also on the line, u will be between 0.0 (the start) and (1.0) of the line
	If ( u < 0 )
		Return pointPointDistance( p, v1 ) ' closest point is the start point
	Else If ( u > 1.0 )
		Return pointPointDistance( p, v2 )	' closest point is the end point
	End If
	
	' closest point is on the line, find the actual intersection point
	Local vecInters:TVector = VectorClone( v1 )
	vecInters._x :+ vLine._x * u
	vecInters._y :+ vLine._y * u
	Return pointPointDistance( vecInters, p ) ' return the distance
End Function

'------------------------------------------------------------------------------
' Intersection point of line segments (p1 -> p2) and (p3 -> p4)
'
'	See <a href="http://local.wasp.uwa.edu.au/~pbourke/geometry/lineline2d/" target="_blank">http://local.wasp.uwa.edu.au/~pbourke/geometry/lineline2d/</a>
'
Function lineLineIntersection:Int ( p1:TVector, p2:TVector, p3:TVector, p4:TVector )
	Local deno:Float = (p4._y - p3._y) * (p2._x - p1._x) - (p4._x - p3._x) * (p2._y - p1._y)
	Local numa:Float = (p4._x - p3._x) * (p1._y - p3._y) - (p4._y - p3._y) * (p1._x - p3._x)
	Local numb:Float = (p2._x - p1._x) * (p1._y - p3._y) - (p2._y - p1._y) * (p1._x - p3._x)
	
	' I check for abitrary closeness to zero, instead of "= 0.0"
	If ( deno < 0.01 And deno > -0.01 )
		If ( ( numa < 0.01 And numa > -0.01 ) And ( numb < 0.01 And numb > -0.01 ) )
			Return True ' lines are coincident
		End If
		Return False ' lines are parallel, but not coincident
	End If
	
	Local ua:Float = numa / deno
	Local ub:Float = numb / deno
	
	If ( ua >= 0 And ua <= 1.0 And ub >= 0 And ub <= 1.0 )
		Return True
	End If
	
	Return False
End Function 

'##############################################################################
'	Primitive Vs Primitive collision functions

'------------------------------------------------------------------------------
' Check collision between a circle and a Polygon
'
Function circlePolyIntersection:Int ( circle:collisionCircle, poly:collisionPoly )
	If ( Not rectRectIntersection( circle, poly ) )
		Return False
	End If
	
	' Is the circle center inside the polygon??
	If poly.pointInside( circle.center )
		Return True
	End If
	
	' loop through the polygon lines to see if the circle is touching any of them
	For Local t:Int = 0 To poly.vertexCount - 1
		
		Local v:TVector = VectorClone( poly.vertices[t] )
		v.SubtractVector( circle.center )
		If ( v.GetLength() < circle.Radius )
			Return True
		End If
		
		Local tt:Int = (t + 1) Mod poly.vertexCount
		Local dist:Float = pointDistanceToLine( circle.center, poly.vertices[t], poly.vertices[tt] )
		If ( dist >= 0 And dist < circle.Radius )
			Return True
		End If
	Next
End Function

'------------------------------------------------------------------------------
' Check collision between two circles
'
Function circleCircleIntersection:Int ( c1:collisionCircle, c2:collisionCircle )
	Return pointPointDistance( c1.center, c2.center ) <= c1.radius + c2.radius
End Function

'------------------------------------------------------------------------------
' Check collision between two polygons
'
Function polyPolyIntersection:Int ( poly1:collisionPoly, poly2:collisionPoly )
	If ( Not rectRectIntersection( poly1, poly2 ) )
		Return False
	End If

	For Local t:Int = 0 To poly1.vertexCount - 1
		If ( poly2.pointInside( poly1.vertices[ t ] ) )
			Return True
		End If
	Next
	For Local t:Int = 0 To poly2.vertexCount - 1
		If ( poly1.pointInside( poly2.vertices[ t ] ) )
			Return True
		End If
	Next
	For Local t:Int = 0 To poly1.vertexCount - 1
		If ( poly2.lineIntersect( poly1.vertices[t], poly1.vertices[ (t + 1) Mod poly1.vertexCount ] ) )
			Return True
		End If
	Next 
	Return False 	
End Function

'------------------------------------------------------------------------------
' Check collision between a rect and a polygon
'
Function rectPolyIntersection:Int ( rect:collisionRect, poly:collisionPoly )
	If ( Not rectRectIntersection( rect, poly ) )
		Return False
	End If

	If ( poly.pointInside( rect.p1 ) )
		Return True
	Else If ( poly.pointInside( rect.p2 ) )
		Return True
	Else If ( poly.PointInside( VectorCreate( rect.p1._x, rect.p2._y ) ) )
		Return True
	Else If ( poly.PointInside( VectorCreate( rect.p2._x, rect.p1._y ) ) )
		Return True
	End If 
	If ( poly.lineIntersect( VectorCreate( rect.p1._x, rect.p1._y ), VectorCreate( rect.p2._x, rect.p1._y ) ) )
		Return True
	Else If ( poly.lineIntersect( VectorCreate( rect.p2._x, rect.p1._y ), VectorCreate( rect.p2._x, rect.p2._y ) ) )
		Return True
	Else If ( poly.lineIntersect( VectorCreate( rect.p2._x, rect.p2._y ), VectorCreate( rect.p1._x, rect.p2._y ) ) )
		Return True
	Else If ( poly.lineIntersect( VectorCreate( rect.p1._x, rect.p2._y ), VectorCreate( rect.p1._x, rect.p1._y ) ) )
		Return True
	End If 
	Return False 
End Function

'------------------------------------------------------------------------------
' Check collision between a rect and a circle
'
Function rectCircleIntersection:Int ( rect:collisionRect, circle:collisionCircle )
	If ( Not rectRectIntersection( rect, circle ) )
		Return False
	End If
	
	If rect.pointInside( circle.center )
		Return True 
	End If

	Local dist:Float = pointDistanceToLine( circle.center, rect.p1, VectorCreate( rect.p2._x, rect.p1._y ) )
	If ( dist <= circle.Radius )
		Return True 
	End If 
	dist = pointDistanceToLine( circle.center, rect.p2, VectorCreate( rect.p1._x, rect.p2._y ) )
	If ( dist <= circle.Radius )
		Return True 
	End If 
	dist = pointDistanceToLine( circle.center, rect.p1, VectorCreate( rect.p1._x, rect.p2._y ) )
	If ( dist <= circle.Radius )
		Return True 
	End If 
	dist = pointDistanceToLine( circle.center, rect.p2, VectorCreate( rect.p2._x, rect.p1._y ) )
	If ( dist <= circle.Radius )
		Return True 
	End If 

	Return False
End Function

'------------------------------------------------------------------------------
' Check collisions between two rects
'
Function rectRectIntersection:Int ( rect1:collisionRect, rect2:collisionRect )
	Return rect1.p1._x <= rect2.p2._x And rect1.p2._x >= rect2.p1._x And ..
			rect1.p1._y <= rect2.p2._y And rect1.p2._y >= rect2.p1._y	
End Function

'##############################################################################
' Quadtree for sorting collision rects
'------------------------------------------------------------------------------
Type QuadTree
	Const QT_MAX_PER_NODE:Int = 6
	Const QT_MIN_NODE_SIZE:Float = 20.0:Float
	
	Field bounds:collisionRect
	
	Field children:QuadTree[4]
	Field objectList:TList
	
	Function CreateTree:QuadTree ( objects:TList )
		Local tree:QuadTree = New QuadTree
		tree.bounds = collisionRect.Create( VectorCreate( 9999.0:Float, 9999.0:Float ), ..
										VectorCreate( -9999.0:Float, -9999.0:Float ) )
		
		tree.objectList:TList = objects
		
		' find the area of this tree
		For Local t:collisionRect = EachIn objects
			tree.bounds.p1._x = Min( t.p1._x, tree.bounds.p1._x )
			tree.bounds.p1._y = Min( t.p1._y, tree.bounds.p1._y )
			tree.bounds.p2._x = Max( t.p2._x, tree.bounds.p2._x )
			tree.bounds.p2._y = Max( t.p2._y, tree.bounds.p2._y )
		Next 
		
		tree._BuildChildren()
		Return tree
	End Function
	
	Method _BuildChildren()
		Local width:Float = bounds.p2._x - bounds.p1._x
		Local height:Float = bounds.p2._y - bounds.p1._y
			
		If ( objectList.count() > QT_MAX_PER_NODE And ..
			(width >  QT_MIN_NODE_SIZE ) And (height >  QT_MIN_NODE_SIZE ) )
				
			Local xHalf:Float = width / 2.0:Float
			Local yHalf:Float = height / 2.0:Float
			
			children[0] = CreateNode( Self, bounds.p1._x, bounds.p1._y, bounds.p1._x + xHalf, bounds.p1._y + yHalf )
			children[1] = CreateNode( Self, bounds.p1._x + xHalf, bounds.p1._y, bounds.p2._x, bounds.p1._y + yHalf )
			children[2] = CreateNode( Self, bounds.p1._x, bounds.p1._y + yHalf, bounds.p1._x + xHalf, bounds.p2._y )
			children[3] = CreateNode( Self, bounds.p1._x + xHalf, bounds.p1._y + yHalf, bounds.p2._x, bounds.p2._y )
		End if
	End method

	Method CreateNode:QuadTree ( parent:QuadTree, minX:Float, minY:Float, maxX:Float, maxY:Float )
		Local node:QuadTree = New QuadTree
		node.bounds = collisionRect.Create( VectorCreate( minX, minY ), VectorCreate( maxX, maxY ) )
		node.objectList = New TList 
		For Local t:collisionRect = EachIn parent.objectList
			If ( rectRectIntersection( t, node.bounds ) )
				node.objectList.AddLast( t )
			End if
		next
		node._BuildChildren()
		Return node		
	End method
	
	Method debugDraw()
		If ( children[0] )
			For Local t:Int = 0 To 3
				If ( children[t] )
					children[t].debugDraw()
				End If
			next
		else
			bounds.debugDraw()
		End if
	End Method
	
	Method getPotentialColliders( myList:TList, region:collisionRect )
		If ( rectRectIntersection( bounds, region ) )
			If ( children[0] )
				For Local t:Int = 0 To 3
					children[t].GetPotentialColliders( myList, region )
				next
			else
				For Local t:collisionRect = EachIn objectList
					myList.AddLast( t )
				next
			End if
		End If 								
	End method
	
End Type



'##############################################################################
' Example

Graphics 800, 600

' Create some user controllable primitives..
Local myCircle:collisionCircle = collisionCircle.CreateCircle( 0, 0, 15 )
Local myRect:collisionRect = collisionRect.Create( VectorCreate( 0, 0 ), VectorCreate( 15, 75 ) )

' A polygon is a multiple creation step..
' at least one AddVertex is required (which makes a single line),
' two makes a triangle etc.
Local myPoly:collisionPoly = collisionPoly.CreatePoly( VectorCreate( 0, 0 ) )
myPoly.AddVertex( VectorCreate( 250, 30 ) )
myPoly.AddVertex( VectorCreate( 230, 45 ) )

' which primitive user is controlling
Local primitive:Int = 0

' Create some static scene primitives
Local sceneCircle:collisionCircle = collisionCircle.CreateCircle( 600, 200, 40 )
Local sceneRect:collisionRect = collisionRect.Create( VectorCreate( 40, 500 ), VectorCreate( 300, 520 ) )
Local scenePoly:collisionPoly = collisionPoly.CreatePoly( VectorCreate( 200, 200 ) )
scenePoly.AddVertex( VectorCreate( 600, 300 ) )
scenePoly.AddVertex( VectorCreate( 200, 400 ) )
scenePoly.AddVertex( VectorCreate( 275, 300 ) )

SeedRnd( MilliSecs() )

' Create hundreds of objects for the quadtree
Local direction:Int = 0
Local dir:Float[] = [ -1.0, 1.0 ]

Local colObjects:TList = New TList
Local colObjMove:TList = New TList 
For Local t:Int = 0 To 999
	Local x:Int = Rand( 0, 790 )
	Local y:Int = Rand( 0, 590 )
	Local colType:Int = Rand ( 0, 3 )
	Select colType
	Case 0
		colObjects.AddLast( collisionRect.Create( VectorCreate( x, y ), VectorCreate( x + 10, y + 10 ) ) )
	Case 1
		colObjects.AddLast( collisionCircle.CreateCircle( x, y, 5.0 ) )
	Case 2
		Local cPoly:collisionPoly = collisionPoly.CreatePoly( VectorCreate( x, y ) )
		cPoly.AddVertex( VectorCreate( x + 3, y + 5 ) )
		cPoly.AddVertex( VectorCreate( x - 3, y + 5 ) )
		colObjects.AddLast( cPoly )
	End Select 
	Local velX:Float = Rnd( 0.6, 2.5 )
	Local velY:Float = Rnd( 0.6, 2.5 )
	direction = Not direction
	colObjMove.AddLast( VectorCreate( velX * dir[ direction ], velY * dir[ direction ]) )
next

' our quadtree
Local qdTree:QuadTree = QuadTree.CreateTree( colObjects )		
' our list of potential colliders we'll retreive from the tree
Local potentialColliders:TList = New TList 

' some configs
Local showingTree:Int = 1
Local movingObjects:Int = 1
Local fps:Int
Local fpsCount:Int
Local fpsMS:Int
Local lastTime:Int = MilliSecs()

' And loop...
While ( Not ( KeyDown( KEY_ESCAPE ) ) )
	
	potentialColliders.Clear()
	
	If ( movingObjects )
		' move all the tiny collision rects..
		Local l:TLink = colObjMove.FirstLink()
		For Local c:collisionRect = EachIn colObjects
			c.VectorMove( TVector( l.Value() ) )
			If ( c.position._x > 800 )
				c.Move( -810, 0 )
			End If
			If ( c.position._x < -10 )
				c.Move( 810, 0 )
			End If
			If ( c.position._y > 600 )
				c.Move( 0, -610 )
			End If
			If ( c.position._y < -10 )
				c.Move( 0, 610 )
			End If
			l = l.NextLink()
		Next
	
		qdTree = QuadTree.CreateTree( colObjects )	
	End If 
	
	Cls
	SetColor 255,255,255

	Select primitive
	Case 0
		qdTree.GetPotentialColliders( potentialColliders, myCircle )
		myCircle.AbsPosition( MouseX(), MouseY() )
		If ( circlePolyIntersection( myCircle, scenePoly ) Or ..
			circleCircleIntersection( myCircle, sceneCircle ) Or ..
			rectCircleIntersection( sceneRect, myCircle ) )
			SetColor 255,0,0
		End If
		myCircle.debugDraw()
	Case 1 
		qdTree.GetPotentialColliders( potentialColliders, myPoly )
		myPoly.AbsPosition( MouseX(), MouseY() )
		If ( polyPolyIntersection( myPoly, scenePoly ) Or ..
			circlePolyIntersection( sceneCircle, myPoly ) Or ..
			rectPolyIntersection( sceneRect, myPoly ) )
			SetColor 255,0,0
		End If
		myPoly.debugDraw()
	Case 2
		qdTree.GetPotentialColliders( potentialColliders, myRect )
		myRect.AbsPosition( MouseX(), MouseY() )
		If ( rectPolyIntersection( myRect, scenePoly ) Or ..
			rectCircleIntersection( myRect, sceneCircle ) Or ..
			rectRectIntersection( myRect, sceneRect ) ) 
			SetColor 255,0,0
		End If
		myRect.debugDraw()
	End Select
	
	SetColor 255,255,255
	For Local t:collisionRect = EachIn colObjects
		t.debugDraw()
	Next

	scenePoly.debugDraw()
	sceneCircle.debugDraw() 
	sceneRect.debugDraw()
	
	For Local c:collisionRect = EachIn potentialColliders
		SetColor 0,0,255
		Select primitive
		Case 0
			If ( rectCircleIntersection( c, myCircle ) )
				SetColor 255,0,0
			End if
		Case 1
			If ( rectPolyIntersection( c, myPoly ) )
				SetColor 255,0,0
			End if
		Case 2
			If ( rectRectIntersection( c, myRect ) )
				SetColor 255,0,0
			End if
		End select
		c.debugDraw()
	Next

	If ( showingTree )
		SetColor 100,100,100
		qdTree.debugDraw()
	End If 
	
	If ( KeyHit( KEY_SPACE ) )
		primitive :+ 1
		primitive :Mod 3
	End If
	
	If ( KeyHit ( KEY_B ) )
		gSHOW_BOUNDING_RECTS = Not gSHOW_BOUNDING_RECTS
	End If 	
	
	If ( KeyHit ( KEY_T ) )
		showingTree = Not showingTree
	End If 
	
	If ( KeyHit ( KEY_M ) )
		movingObjects = Not movingObjects
	End if

	SetColor 0,255,0
	DrawText( "SPACE: Different User Primitives.  T: Tree On/Off.  M: Movement On/Of.  B: Bounding Boxes On/Off", 0, 8 )
	DrawText( "FPS: " + fps, 0, 24 )
	Flip
	
	fpsMS :+ MilliSecs() - lasttime
	lasttime = MilliSecs()
	fpsCount :+ 1
	If ( fpsMS >= 1000 )
		fps = fpsCount
		fpsCount = 0
		fpsMS :- 1000
	End if
	
End While

Arowx

(Posted 2009) [#7]

Wow that is cool matibee!

It looks like your rebuilding the quadtree each time, do you need to do that?

matibee

(Posted 2009) [#8]

Thanks Merx.

It is necessary to rebuild the tree when the objects move. This is not the greatest example of the use of a quadtree because all the objects are limited to screen coords. There's so many objects, and the tree will stop subdividing when it's nodes are less than 20 units wide (See QT_MIN_NODE_SIZE) so the effect is that the tree always looks pretty similar.

If we allowed them to move over a much bigger 'world' we could even use the quadtree test to limit the potentially visible objects we needed to draw.

To see more of the QuadTree workings change the quadtree constants to

const QT_MAX_PER_NODE:Int = 1
Const QT_MIN_NODE_SIZE:Float = 5.0:Float

and limit the demo to just 10 objects (line 551). Keep pressing M and watch the tree shape. While it's paused you'll see the potential colliders are the ones in the same region (leaf node) of the tree.

Mauft

(Posted 2013) [#9]

The 2D vector module is now officially available here:

http://retrocade.net/post/138/blitzmax-vector-library

(If the author or anyone else can fix the link in the first post, I'd be grateful)