aUCBLogo Demos and Tests / test_bsp_tree
be test_bsp_tree
clearScreen
WindowMode
setPC HSBA 0 0 0 0.5
(rerandom 14)
; t1=randtri
; t2=randtri
; t3=randtri
comment[
N=5
t=array N
for [i 1 N]
[ t.i=triangle
randxy+circ i N
randxy+circ i N
randxy+circ i N
]
]
;comment[
imax=100
t=array imax*2
dr=100
for [i 1 imax]
[ p1=(list 1.2*cos i/imax*330 sin i/imax*330)*(300-dr)
p2=(list 1.2*cos (i+1)/imax*330 sin (i+1)/imax*330)*(300-dr)
p3=(list 1.2*cos (i+1/2)/imax*330 sin (i+1/2)/imax*330)*300
p4=(list 1.2*cos (i+1+1/2)/imax*330 sin (i+1+1/2)/imax*330)*300
t.(i*2-1)=triangle p1 p2 p3
t.(i*2)=triangle p3 p2 p4
]
;]
; pr t1
; pr t2
; pr t3
(rerandom int TimeMilli)
tree1=bsp_tree [0 0][400 300] (tolist t) 0 [] 0
updateGraph
; clearScreen
tree1'draw
;mousecheck stop
p=randxy
w=random 360
v=(list cos w sin w)*5
running=true
PenDown
bsp_tree::findpoly::lastNode=[]
while [running]
[ setPos p
p=p+v
if p.1 <-400 [v.1=-v.1]
if p.2 <-300 [v.2=-v.2]
if p.1 > 400 [v.1=-v.1]
if p.2 > 300 [v.2=-v.2]
; ifelse bsp_tree::findpoly::lastNode !=[]
; [ t=(bsp_tree::findpoly::lastNode'findpoly p)
; ][ t=(tree1'findpoly p)
; ]
t=(tree1'findpoly p)
if t !=[]
[ p=p-v
comment[
d=v
repeat 2
[ p=p-d+d/2
t2=(tree1'findpoly p)
ifelse t2 !=[]
[ t=t2
][ p=p-d
]
d=d/2
]
]
maxi=0
if (max t's1 t's4)> maxi [maxi=max t's1 t's4 k=1]
if (max t's2 t's5)> maxi [maxi=max t's2 t's5 k=2]
if (max t's3 t's6)> maxi [maxi=max t's3 t's6 k=3]
if k==1 [edge=t'p1-t'p2]
if k==2 [edge=t'p2-t'p3]
if k==3 [edge=t'p3-t'p1]
ifelse (Norm edge) > 1e-10
[ edge=edge/Norm edge
][ edge=v/Norm v
]
projection=edge*(0+edge*v)
ortho=v-projection
v=projection-ortho
p=p+v
]
if key? [running=false]
]
be mousecheck
while [not key?]
[ p=list MouseX MouseY
setPos p
t=(tree1'findpoly p)
if t != [] [t'draw]
dispatchMessages
waitms 20
]
end
end
be go
running=true
i=0
clearText
while [running]
[ print i
(rerandom i)
clearScreen
tt=triangle randxy randxy randxy
(rerandom int TimeMilli)
tree1=bsp_tree [0 0][400 300] (list tt) 0 0
updateGraph
ch=readchar
if ch==char 27 [running=false]
if ch=="- [i=i-2]
if ch==char wxk_space [i=i+2]
]
end
to randx
output (random 600)-300
end
to randy
output (random 500)-250
end
to randxy
output list randx randy
end
be randtri
w=random 360
a=(list cos w sin w)*(random 400)
w=w-random 170
b=(list cos w sin w)*(random 400)
w=w-random 170
c=(list cos w sin w)*(random 400)
output triangle a b c
end
to randcol
setPenColor HSBA random 360 1 1 0.3
end
to circ i N
output (list cos i/N*360 sin i/N*360)*180
end
be triangle p1 p2 p3
local [w1 w2 w3 s1 s2 s3 s4 s5 s6]
;show p1
;show p2
;show p3
w1=dotnorm p1-p2 p1-p3
w2=dotnorm p2-p3 p2-p1
w3=dotnorm p3-p1 p3-p2
draw
be draw
randcol
drawshape
end
be highlight
setPenColor 0 ;HSBA 60 0.5 1 1
drawshape
setPos (p1+p2+p3)/3
PenDown
circle ((norm p2-p1)+(norm p3-p2)+(norm p1-p3))/6
PenUp
updateGraph
end
be drawshape
p0=Pos
h0=Heading
PenUp
setPos p3
PenDown
Polygon
[ setPos p1
setPos p2
setPos p3
]
; arrow p1
; arrow p2
; arrow p3
PenUp
setPos p0
setHeading h0
end
be inside? p
s1=dotnorm p1-p p1-p2
s2=dotnorm p2-p p2-p3
s3=dotnorm p3-p p3-p1
s4=dotnorm p1-p p1-p3
s5=dotnorm p2-p p2-p1
s6=dotnorm p3-p p3-p2
; if s1>w1 and2 s2>w2 and2 s3>w3 [(pr s1 s2 s3)]
; if s1>w1 and2 s4>w1 [randcol setPos p1 pd arrow p2 pu]
; if s2>w2 and2 s5>w2 [randcol setPos p2 pd arrow p3 pu]
; if s3>w3 and2 s6>w3 [randcol setPos p3 pd arrow p1 pu]
if s1>w1 and2 s2>w2 and2 s3>w3 and2 s4>w1 and2 s5>w2 and2 s6>w3 [output true]
output false
end
end
be dotnorm a b
if ((norm a) > 1e-12) and2 ((norm b) > 1e-12)
[ output (0+a*b)/(Norm a)/(Norm b)
]
output 0
end
to alpha a b
if ((norm a) > 1e-12) and2 ((norm b) > 1e-12)
[ h=(0+a*b)/((Norm a)*(Norm b))
if h > 1 [output 0]
if h <-1 [output 180]
output arccos h
]
output 0
end
be arrow p
p0=Pos
setHeading towards p
d=Norm p-p0
forward d
right 170
a=min 20 d
forward a back a
left 170*2
forward a back a
end
be bsp_tree center size polys axis parent depth
local [sub1 sub2 l inside in1 in2
p1 p2 p3 t t1 t2 t3 t4 x y v1 v2 v3]
sub1=[]
sub2=[]
if (norm size) < 10 or2 depth > 8 [stop]
if axis==0 [x=1 y=2]
if axis==1 [x=2 y=1]
setPos xy center.x center.y
setFloodColor HSBA (depth+random 2)/10*360 1 1 0.05
; fillRect -xy size.x size.y xy size.x size.y
;pause
mindepth=2
l=polys
inside=[]
in1=[]
in2=[]
while [l != []]
[ t=first l
l=butFirst l
v1=t'p1
v2=t'p2
v3=t'p3
ifelse (t'p1).x >= center.x
[ ifelse (t'p2).x >= center.x
[ ifelse (t'p3).x >= center.x
[ ifelse depth>mindepth
[ inside=fput t inside
][ in1=fput t in1
]
][ partition v2 v1 v3
]
][ ifelse (t'p3).x >= center.x
[ partition v1 v3 v2
][ partition v2 v3 v1
]
]
][ ifelse (t'p2).x <= center.x
[ ifelse (t'p3).x <= center.x
[ ifelse depth>mindepth
[ inside=fput t inside
][ in2=fput t in2
]
][ partition v2 v1 v3
]
][ ifelse (t'p3).x <= center.x
[ partition v3 v1 v2
][ partition v2 v3 v1
]
]
]
]
polys=inside
; erase [[][l inside p1 p2 p3 t t1 t2 t3 t4 v1 v2 v3]]
if in1 != [] [
sub1=bsp_tree
xy center.x+size.x/2 center.y
xy size.x/2 size.y
in1
modulo axis+1 2
this
depth+1
; erase [[][in1]]
]
if in2 != [] [
sub2=bsp_tree
xy center.x-size.x/2 center.y
xy size.x/2 size.y
in2
modulo axis+1 2
this
depth+1
; erase [[][in2]]
]
; center=xy center.x center.y
be xy x y
if axis==0 [output list x y]
if axis==1 [output list y x]
end
be xygt x y
output x>y
; if axis==0 [output x > y]
; if axis==1 [output y > x]
end
be partition v1 v2 v3
d2=(v1.x-v3.x)
ifelse (abs d2) < 1e-12
[ stop
][ p2=xy center.x v1.y-(v1.x-center.x)/d2*(v1.y-v3.y)
]
d3=(v2.x-v3.x)
ifelse (abs d3) < 1e-12
[ stop
][ p3=xy center.x v2.y-(v2.x-center.x)/d3*(v2.y-v3.y)
]
p1=xy v1.x v1.y
p1_=xy v3.x v3.y
a1=alpha p2-p1 p3-p1
a1_=alpha p2-p1_ p3-p1_
if a1_ < a1 [p1=p1_]
t1=triangle p1 p2 p3
ifelse (xygt p1.x center.x) or2 (xygt p2.x center.x) or2 (xygt p3.x center.x)
[ in1=fput t1 in1
][ in2=fput t1 in2
]
t2=triangle v1 p2 p3
ifelse (xygt v1.x center.x) or2 (xygt p2.x center.x) or2 (xygt p3.x center.x)
[ in1=fput t2 in1
][ in2=fput t2 in2
]
t3=triangle v2 p1 p3
ifelse (xygt v2.x center.x) or2 (xygt p1.x center.x) or2 (xygt p3.x center.x)
[ in1=fput t3 in1
][ in2=fput t3 in2
]
t4=triangle v3 p2 p3
ifelse (xygt v3.x center.x) or2 (xygt p2.x center.x) or2 (xygt p3.x center.x)
[ in1=fput t4 in1
][ in2=fput t4 in2
]
;t4'highlight
end
be findpoly p [axis 0]
local [result l t]
l=polys
while [l != []]
[ t=first l
l=butFirst l
if (t'inside? p)
[
; pr "Dong!
lastNode=parent
output t
]
]
if axis==0 [x=1 y=2]
if axis==1 [x=2 y=1]
if sub1 !=[] and2 (p.x >= center.x)
[ result=(sub1'findpoly p modulo axis+1 2)
if result != [] [output result]
]
if sub2 !=[] and2 (p.x <= center.x)
[ result=(sub2'findpoly p modulo axis+1 2)
if result != [] [output result]
]
output []
end
be draw
local [l t]
l=polys
while [l !=[]]
[ t=first l
l=butFirst l
t'draw
]
if sub1 !=[] [sub1'draw]
if sub2 !=[] [sub2'draw]
end
end