Wrote this for Relativity because:
- Chipmunk's Hash tables are not good for every size of query
- Chipmunk's spacial lookup functions are clunky
- Chipmunk's spacial lookups must be filtered by actual groups or layers
so for 1. using a quadtree fixes that
for 2., this returns an array of results instead of using a callback function
for 3., you attach a meta object to the data field of each body (I called this CollisionObj), which should have an 'index' field which decides which index the object will be placed it, or -1 if it should not be indexed at all
basically you call the update method on your QuadTree instance after calling chipmunk's step function, then you can run closest object, objects within radius, and objects within rectangle queries via methods on the QT instance
code:
#define QT_NODE_SIZE 4
#define QT_MAX_DEPTH 20
#define QT_MAX_INDEX 16
#define QT_BRANCH 1
#define QT_LEAF 2
type QTData
p as cpVect
d as any ptr
end type
type QTArray
declare constructor()
declare destructor()
declare sub push ( byval item as QTData )
length as integer
max as integer
items as QTData ptr
end type
#define QTResultSet QTArray
type QTNode
declare constructor ( byval ntype as integer, byval min as cpVect, byval max as cpVect )
declare destructor ()
ntype as integer
links(0 to 3) as QTNode ptr
pdata as QTArray
min as cpVect
max as cpVect
end type
type QuadTree
declare constructor()
declare destructor()
declare sub eraseAll ()
declare sub update ( byval space as cpSpace ptr )
declare function addObj ( byval node as QTNode ptr, byref item as QTData, byval level as integer = 0 ) as integer
declare sub eraseNode ( byval node as QTNode ptr )
declare function closestTo ( byval ind as integer, byval p as cpVect, byval range as double, byval exclude as any ptr = 0 ) as QTResultSet
declare function inRangeOf ( byval ind as integer, byval p as cpVect, byval range as double ) as QTResultSet
declare function inBox ( byval ind as integer, byval min as cpVect, byval max as cpVect ) as QTResultSet
declare sub boxQuery ( byval node as QTNode ptr, byval min as cpVect, byval max as cpVect, byref res as QTResultSet )
index(0 to QT_MAX_INDEX-1) as QTNode ptr
end type
constructor QuadTree ()
for i as integer = 0 to QT_MAX_INDEX-1
this.index(i) = 0
next i
end constructor
destructor QuadTree ()
this.eraseAll()
end destructor
sub QuadTree.eraseAll ()
for i as integer = 0 to QT_MAX_INDEX-1
if this.index(i) <> 0 then
this.eraseNode(this.index(i))
this.index(i) = 0
end if
next i
end sub
sub QuadTree.update ( byval space as cpSpace ptr )
this.eraseAll()
dim as cpArray ptr bodies = space->bodies
dim as cpVect min(0 to QT_MAX_INDEX-1)
dim as cpVect max(0 to QT_MAX_INDEX-1)
dim as QTArray items(0 to QT_MAX_INDEX-1)
for i as integer = 0 to bodies->num-1
dim as cpBody ptr body = bodies->arr[i]
dim as CollisionObj ptr co = body->data
if co <> 0 then
if co->index >= 0 and co->index < QT_MAX_INDEX then
dim as QTData item
item.p = body->p
item.d = co
if items(co->index).length = 0 then
min(co->index) = item.p
max(co->index) = item.p
else
if item.p.x < min(co->index).x then min(co->index).x = item.p.x
if item.p.x > max(co->index).x then max(co->index).x = item.p.x
if item.p.y < min(co->index).y then min(co->index).y = item.p.y
if item.p.y > max(co->index).y then max(co->index).y = item.p.y
end if
items(co->index).push(item)
end if
end if
next i
for i as integer = 0 to QT_MAX_INDEX-1
if items(i).length > 0 then
this.index(i) = new QTNode(QT_LEAF, min(i), max(i))
for j as integer = 0 to items(i).length-1
this.addObj(this.index(i), items(i).items[j])
next j
end if
next i
end sub
#define QT_P_IN_B(_p, _a, _b) (((_p).x >= (_a).x) and ((_p).y >= (_a).y) and ((_p).x <= (_b).x) and ((_p).y <= (_b).y))
#define QT_B_IN_B(_a1, _b1, _a2, _b2) (((_a1).x <= (_b2).x) and ((_a1).y <= (_b2).y) and ((_b1).x >= (_a2).x) and ((_b1).y >= (_a2).y))
function QuadTree.addObj ( byval node as QTNode ptr, byref item as QTData, byval level as integer = 0 ) as integer
if QT_P_IN_B(item.p, node->min, node->max) then
if node->ntype = QT_LEAF then
if node->pdata.length < QT_NODE_SIZE or level >= QT_MAX_DEPTH then
node->pdata.push(item)
else
dim as QTArray items = node->pdata
items.push(item)
*node = *(new QTNode(QT_BRANCH, node->min, node->max))
for i as integer = 0 to items.length - 1
this.addObj(node, items.items[i], level)
next i
end if
return 1
elseif node->ntype = QT_BRANCH then
if this.addObj(node->links(0), item, level+1) = 1 then return 1
if this.addObj(node->links(1), item, level+1) = 1 then return 1
if this.addObj(node->links(2), item, level+1) = 1 then return 1
if this.addObj(node->links(3), item, level+1) = 1 then return 1
end if
end if
return 0
end function
function QuadTree.closestTo ( byval ind as integer, byval p as cpVect, byval range as double, byval exclude as any ptr = 0 ) as QTResultSet
dim as QTResultSet r1, rout
if ind < 0 or ind >= QT_MAX_INDEX then return rout
if this.index(ind) = 0 then return rout
dim as cpVect min = cpv(p.x-range, p.y-range), max = cpv(p.x+range, p.y+range)
this.boxQuery(this.index(ind), min, max, r1)
dim as double range2 = range*range
dim as double bdist2 = 0
for i as integer = 0 to r1.length-1
dim as cpVect p2 = r1.items[i].p
dim as double dx = p2.x-p.x, dy = p2.y-p.y
dim as double dist2 = dx*dx+dy*dy
if exclude = r1.items[i].d then continue for
if rout.length = 0 then
bdist2 = dist2
rout.push(r1.items[i])
elseif dist2 <= bdist2 then
bdist2 = dist2
rout.items[0] = r1.items[i]
end if
next i
if bdist2 > range2 then rout.length = 0
return rout
end function
function QuadTree.inRangeOf ( byval ind as integer, byval p as cpVect, byval range as double ) as QTResultSet
dim as QTResultSet r1, rout
if ind < 0 or ind >= QT_MAX_INDEX then return rout
if this.index(ind) = 0 then return rout
dim as cpVect min = cpv(p.x-range, p.y-range), max = cpv(p.x+range, p.y+range)
this.boxQuery(this.index(ind), min, max, r1)
dim as double range2 = range*range
for i as integer = 0 to r1.length-1
dim as cpVect p2 = r1.items[i].p
dim as double dx = p2.x-p.x, dy = p2.y-p.y
if (dx*dx+dy*dy) <= range2 then rout.push(r1.items[i])
next i
return rout
end function
function QuadTree.inBox ( byval ind as integer, byval min as cpVect, byval max as cpVect ) as QTResultSet
dim as QTResultSet rout
if ind < 0 or ind >= QT_MAX_INDEX then return rout
if this.index(ind) = 0 then return rout
this.boxQuery(this.index(ind), min, max, rout)
return rout
end function
sub QuadTree.boxQuery ( byval node as QTNode ptr, byval min as cpVect, byval max as cpVect, byref res as QTResultSet )
if QT_B_IN_B(node->min, node->max, min, max) then
if node->ntype = QT_LEAF then
for i as integer = 0 to node->pdata.length-1
dim as QTData item = node->pdata.items[i]
if QT_P_IN_B(item.p, min, max) then
res.push(item)
end if
next i
elseif node->ntype = QT_BRANCH then
this.boxQuery(node->links(0), min, max, res)
this.boxQuery(node->links(1), min, max, res)
this.boxQuery(node->links(2), min, max, res)
this.boxQuery(node->links(3), min, max, res)
end if
end if
end sub
constructor QTArray ()
this.length = 0
this.max = 10
this.items = allocate(sizeof(QTData)*this.max)
end constructor
destructor QTArray ()
deallocate this.items
end destructor
sub QTArray.push(byval item as QTData)
if this.length >= this.max then
this.max *= 10
dim as QTData ptr newItems = allocate(sizeof(QTData)*this.max)
for i as integer = 0 to this.length-1
newItems[i] = this.items[i]
next i
deallocate this.items
this.items = newItems
end if
this.items[this.length] = item
this.length += 1
end sub
constructor QTNode ( byval ntype as integer, byval min as cpVect, byval max as cpVect )
this.ntype = ntype
this.min = min
this.max = max
if this.ntype = QT_LEAF then
elseif this.ntype = QT_BRANCH then
dim as double cx = (min.x+max.x)*0.5, cy = (min.y+max.y)*0.5
this.links(0) = new QTNode(QT_LEAF, min, cpv(cx, cy))
this.links(1) = new QTNode(QT_LEAF, cpv(cx, min.y), cpv(max.x, cy))
this.links(2) = new QTNode(QT_LEAF, cpv(min.x, cy), cpv(cx, max.y))
this.links(3) = new QTNode(QT_LEAF, cpv(cx, cy), max)
end if
end constructor
destructor QTNode ()
end destructor
sub QuadTree.eraseNode ( byval node as QTNode ptr )
if node->ntype = QT_BRANCH then
this.eraseNode(node->links(0))
this.eraseNode(node->links(1))
this.eraseNode(node->links(2))
this.eraseNode(node->links(3))
deallocate(node)
end if
end sub
Topic: Quadtree implementation for use with Chipmunk 2D (Read 3515 times)