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