wikipedia: https://en.wikipedia.org/wiki/Quadtree
The first number of the QuadTree() constructor are the number of nodes stored in a quad before it will be divided in four more quads !
Joshy
file: "QuadTree.bi"
Code: Select all
#pragma once
#ifdef DEBUG
#define dprint(msg) : open err for output as #99 : print #99,msg : close #99
#else
#define dprint(msg) :
#endif
' !!! you can store and query any stuff in a QuadTree !!!
' the minimum specs are it must have a 2D position (x,y)
' so you can replace tThing with your own particle, pathnode, rigidbody, game object, sprite ... etc.
#ifndef tThing
type tThing
declare constructor
declare constructor(byval px as single=0, _
byval py as single=0)
as single x=any,y=any
end type
constructor tThing
end constructor
constructor tThing(byval px as single, _
byval py as single)
x=px : y = py
end constructor
#endif
type tQuadTree
declare destructor
'declare constructor
declare constructor (byval capacity as integer, _
byval px as single, byval py as single, _
byval pw as single, byval ph as single, _
byval bSquare as boolean = false)
declare function Insert(byref node as const tThing, _
byval bUnique as boolean=false) as boolean
' point
declare function Contains(byval px as single, _
byval py as single) as boolean
' circle
declare function Intersect(byval px as single, _
byval py as single, _
byval pr as single) as boolean
' rectangle
declare function Intersect(byval px as single, _
byval py as single, _
byval pw as single, _
byval ph as single) as boolean
' get nearest thing to a point (x,y)
declare function Query(byval px as single, _
byval py as single, _
byref nearest as tThing ,_
byref distance as single) as boolean
' get nearest thing inside a circle (x,y,radius)
declare function Query(byval px as single, _
byval py as single, _
byval pr as single, _
byref nearest as tThing ,_
byref distance as single) as boolean
' get nearest thing inside a rectangle (x,y,w,h)
declare function Query(byval px as single, _
byval py as single, _
byval pw as single, _
byval ph as single, _
byref nearest as tThing,_
byref distance as single) as boolean
' collect all things inside a circle (x,y,radius)
declare function Query(byval px as single, _
byval py as single, _
byval pr as single, _, _
collection() as tThing) as boolean
' collect all things inside a rectangle (x,y,w,h)
declare function Query(byval px as single, _
byval py as single, _
byval pw as single, _
byval ph as single, _
collection() as tThing) as boolean
' for debuging
declare sub draw (byval colour as ulong = RGB(255,255,255) )
declare sub drawNodes(byval colour as ulong = RGB(255,255,255) )
declare function IsOk() as boolean
private:
declare function getNearest(byval px as single, _
byval py as single, _
collection() as tThing, _
byref nearest as tThing ,_
byref distance as single) as boolean
declare function Divide() as boolean
declare function IsDivided() as boolean
declare function IsLeaf() as boolean
' rectangular boundary (x,y,w,h)
as single bx,by,bw,bh
as integer capacity
as boolean IsSquare
as tThing nodes(any)
as tQuadTree ptr quads(3)
end type
destructor tQuadTree
'if ubound(this.nodes)>-1 then erase this.nodes
if this.IsDivided()=false then return
for i as integer = 0 to 3
delete this.quads(i)
next
end destructor
constructor tQuadTree(byval capacity as integer, _
byval px as single, byval py as single, _
byval pw as single, byval ph as single, _
byval bSquare as boolean)
dprint("request tQuadTree(" & capacity & "," & px & "," & py & "," & pw & "," & ph & "," & bSquare & ")")
this.capacity = capacity
this.bx = px
this.by = py
this.IsSquare = bSquare
' be sure the dimension of the boundary is >= power of 2 (e.g. 2,4,8,16,64 ...)
dim as integer iw=1 : while iw<pw:iw shl=1 : wend
dim as integer ih=1 : while ih<ph:ih shl=1 : wend
if this.IsSquare then
if iw>ih then
this.bw = iw
this.bh = iw
else
this.bw = ih
this.bh = ih
endif
else
this.bw = iw
this.bh = ih
endif
end constructor
' is point (x,y) inside this quad ?
function tQuadTree . contains(byval x as single, _
byval y as single) as boolean
if x<this.bx then return false
if y<this.by then return false
if x>this.bx+this.bw then return false
if y>this.by+this.bh then return false
return true
end function
' does a circle (x,y,r) overlap this quad ?
function tQuadTree . intersect(byval x as single, _
byval y as single, _
byval r as single) as boolean
if x+r < this.bx then return false
if y+r < this.by then return false
if x-r > this.bx+this.bw then return false
if y-r > this.by+this.bh then return false
return true
end function
' does a rectangle (x,y,w,h) overlap this quad ?
function tQuadTree . intersect(byval x as single, _
byval y as single, _
byval w as single, _
byval h as single) as boolean
if x+w < this.bx then return false
if y+h < this.by then return false
if x > this.bx+this.bw then return false
if y > this.by+this.bh then return false
return true
end function
function tQuadTree . IsDivided() as boolean
return iif(this.quads(0)=0,false,true)
end function
function tQuadTree . IsLeaf() as boolean
return iif(ubound(this.nodes)<0,false,true)
end function
function tQuadTree . IsOk() as boolean
dim as boolean bL = this.IsLeaf()
dim as boolean bC = this.IsDivided()
if bL = bC then
if bL then
dprint("fatal error: this quad is leaf and has childs !")
beep : sleep : return false
endif
dprint("warning: this quad is empty !")
return true
endif
' no childs we are done ...
if bC=false then return true
' test all childs also
for i as integer = 0 to 3
if this.quads(i)->IsOk()=false then return false
next
return true
end function
function tQuadTree . Divide() as boolean
' can we split this quad by half ?
if this.bw<2 orelse this.bh<2 then
dprint("tQuadTree.Divide() warning: to small")
'sleep 1000
return false
endif
' split this quad in new quads
dim as integer w2=bw,h2=bh : w2 shr=1 : h2 shr=1
this.quads(0) = new tQuadTree(this.capacity, bx ,by ,w2,h2, this.IsSquare)
this.quads(1) = new tQuadTree(this.capacity, bx+w2,by ,w2,h2, this.IsSquare)
this.quads(2) = new tQuadTree(this.capacity, bx ,by+h2,w2,h2, this.IsSquare)
this.quads(3) = new tQuadTree(this.capacity, bx+w2,by+h2,w2,h2, this.IsSquare)
return true
end function
function tQuadTree . Insert(byref node as const tThing, _
byval bUnique as boolean) as boolean
' if this quad rectangle are not the parent of the node ignore it
if this.contains(node.x,node.y) = false then return false
' is it a leave and has free storage
if this.IsDivided()=false andalso ubound(this.nodes)+1 < this.capacity then
' ignore same things
if this.IsLeaf() andalso bUnique=true then
for i as integer = 0 to ubound(this.nodes)
if this.nodes(i).x=node.x andalso _
this.nodes(i).y=node.y then return false
next
endif
' store the node
redim preserve this.nodes(ubound(this.nodes)+1)
this.nodes(ubound(this.nodes))=node
return true
else
' create four new quads and store it in one of it
if this.IsDivided() = false then
' if the quad are too small to divide insert the node in this quad
if this.Divide() = false then
redim preserve this.nodes(ubound(this.nodes)+1)
this.nodes(ubound(this.nodes))=node
return true
else
' NOTE: this quad becomes a leaf !
' it means we have to move any stored nodes in the new quads
if this.IsLeaf() then
for q as integer = 0 to 3
for i as integer = 0 to ubound(this.nodes)
this.quads(q)->insert(this.nodes(i), bUnique)
next
next
' free the nodes this make it as a leafe
erase this.nodes
endif
endif
endif
' if we are here store the new node in one of the child quads
for q as integer = 0 to 3
if this.quads(q)->insert(node, bUnique) then return true
next
endif
' !!! this should never happen !!!
dprint("fatal error: impossible to add node ! ...")
beep : sleep : end 1
return false
end function
' private: get nearest node to x,y from a collection of nodes
function tQuadTree . getNearest(byval x as single, _
byval y as single, _
collection() as tThing, _
byref nearest as tThing, _
byref distance as single) as boolean
dim as single dx=any,dy=any,l2=any,n2=any
dim as integer index=any,i=any,n=any
n = ubound(collection) : if n<0 then return false
' get the nearest node to x,y
index = -1 : n2 = 2^31 ' any huge number
for i = 0 to n
dx = x-collection(i).x : dy = y-collection(i).y
l2 = dx*dx + dy*dy : if l2<n2 then index = i : n2 = l2
next
' should never happen
if index = -1 then return false
nearest = collection(index)
' any squared distance ?
distance = iif(n2,sqr(n2),0)
return true
end function
' try to find the nearest node to X,Y
' and store it in found and fill distance (BYREF)
function tQuadTree . Query(byval x as single, _
byval y as single, _
byref found as tThing,_
byref distance as single) as boolean
dim as tThing near(any)
' if this quad rectangle are not the parent ignore it
if this.contains(x,y)=false then return false
' collect all nodes in this quad
if this.Query(this.bx,this.by,this.bw,this.bh,near())=false then return false
return this.getNearest(x,y,near(),found,distance)
end function
' try to find the nearest node inside a circle (x,y,radius)
' and store it in found and fill distance (BYREF)
function tQuadTree . Query(byval x as single, byval y as single, byval r as single, _
byref found as tThing,_
byref distance as single) as boolean
dim as tThing collection(any)
' if this quad isn't a part of the circle we are done
if this.intersect(x,y,r)=false then return false
' collect all nodes in this circle
if this.Query(x,y,r,collection())=false then return false
' get the nearest node to the center of the circle
return this.getNearest(x,y,collection(),found,distance)
end function
' try to find the nearest node inside a rectangle (x,y,w,h)
' and store it in found and fill the argument distance (BYREF)
function tQuadTree . Query(byval x as single, _
byval y as single, _
byval w as single, _
byval h as single, _
byref found as tThing,_
byref distance as single) as boolean
dim as tThing collection(any)
' if this quad isn't a part of the rectangle we are done
if this.intersect(x,y,w,h)=false then return false
' collect all nodes in this rectangle
if this.Query(x,y,w,h,collection())=false then return false
' get the nearest node to the center of the rectangle
return this.getNearest(x+w/2,y+h/2,collection(),found,distance)
end function
' collect all nodes which are inside a circle (x,y,radius)
function tQuadTree . Query(byval x as single, _
byval y as single, _
byval r as single, _
found() as tThing) as boolean
dim as boolean bFound = false
' if this quad isn't a part of the circle we are done
if this.intersect(x,y,r)=false then return bFound
' collect all nodes which are inside the circle
if this.IsDivided() = false andalso ubound(this.nodes)>-1 then
dim as single r2 = r*r
for i as integer = 0 to ubound(this.nodes)
dim as single xd=this.nodes(i).x-x
dim as single yd=this.nodes(i).y-y
dim as single l2=xd*xd + yd*yd
if l2<r2 then
redim preserve found(ubound(found)+1)
found(ubound(found))=this.nodes(i)
bFound=true
endif
next
endif
' query the child quads or ignore it
if this.IsDivided() = false then return bFound
for q as integer = 0 to 3
if this.quads(q)->Query(x,y,r, found()) then bFound=true
next
return bFound
end function
' collect all nodes which are inside a rectangle (x,y,w,h)
function tQuadTree . Query(byval x as single, _
byval y as single, _
byval w as single, _
byval h as single, _
found() as tThing) as boolean
dim as boolean bFound
' if this quad isn't a part of the rectangle we are done
' if the search rectangle doesn't overlap this quad ignore it
if this.intersect(x,y,w,h)=false then return bFound
' collect all nodes which are inside the rectangle
if this.IsDivided() = false andalso ubound(this.nodes)>-1 then
for i as integer = 0 to ubound(this.nodes)
if this.nodes(i).x<x then continue for
if this.nodes(i).y<y then continue for
if this.nodes(i).x>x+w then continue for
if this.nodes(i).y>y+h then continue for
redim preserve found(ubound(found)+1)
found(ubound(found))=this.nodes(i)
bFound = true
next
endif
' query the child quads or ignore it
if this.IsDivided() = false then return bFound
for q as integer = 0 to 3
if this.quads(q)->Query(x,y,w,h,found()) then bFound=true
next
return bFound
end function
sub tQuadTree . draw (byval colour as ulong)
if ubound(this.nodes)>-1 andalso this.IsDivided()=false then
line (bx,by)-step(bw-1,bh-1),colour,B
for i as integer = 0 to ubound(this.nodes)
pset(this.nodes(i).x,this.nodes(i).y),colour
next
return ' we are done
endif
' is it a complete empty quad
if this.IsDivided()=false then
line (bx,by)-step(bw-1,bh-1),rgb(255,255,255),B
return ' we are done
endif
this.quads(0)->draw(rgb(255,0,0))
this.quads(1)->draw(rgb(0,255,0))
this.quads(2)->draw(rgb(0,0,255))
this.quads(3)->draw(rgb(255,255,0))
end sub
sub tQuadTree . drawNodes(byval colour as ulong)
if ubound(this.nodes)>-1 andalso this.IsDivided()=false then
for i as integer = 0 to ubound(this.nodes)
pset(this.nodes(i).x,this.nodes(i).y),colour
next
endif
' draw the child quads or ignore it
if this.IsDivided()=false then return
this.quads(0)->drawNodes(colour)
this.quads(1)->drawNodes(colour)
this.quads(2)->drawNodes(colour)
this.quads(3)->drawNodes(colour)
end sub