Find all AABBs the point within

server/internal/bvh/bvh.go

@@ -14,6 +14,11 @@ func (v Vec2) Min(other Vec2) Vec2 { return Vec2{math.Min(v[0], other[0]), math.
 
 type AABB2 struct{ Upper, Lower Vec2 }
 
+func (aabb AABB2) WithIn(point Vec2) bool {
+	return aabb.Lower[0] < point[0] && point[0] < aabb.Upper[0] &&
+		aabb.Lower[1] < point[1] && point[1] < aabb.Upper[1]
+}
+
 func (aabb AABB2) Union(other AABB2) AABB2 {
 	return AABB2{
 		Upper: aabb.Upper.Max(other.Upper),
@@ -28,19 +33,21 @@ func (aabb AABB2) Surface() float64 {
 
 type Node2 struct {
 	box      AABB2
+	Value    interface{}
 	parent   *Node2
 	children [2]*Node2
 	isLeaf   bool
 }
 
 func (n *Node2) findAnotherChild(not *Node2) *Node2 {
-	if v := n.children[0]; v != nil && v != not {
-		return v
+	if n.isLeaf {
+		return nil
+	} else if n.children[0] == not {
+		return n.children[1]
+	} else if n.children[1] == not {
+		return n.children[0]
 	}
-	if v := n.children[1]; v != nil && v != not {
-		return v
-	}
-	return nil
+	panic("unreachable, please make sure the 'not' is the n's child")
 }
 
 type Tree2 struct {
@@ -115,6 +122,22 @@ func (t *Tree2) Insert(leaf AABB2) (n *Node2) {
 	return
 }
 
+func (t *Tree2) Find(point Vec2, f func(*Node2)) {
+	t.root.lookup(point, f)
+}
+
+func (n *Node2) lookup(point Vec2, f func(node2 *Node2)) {
+	if n == nil || !n.box.WithIn(point) {
+		return
+	}
+	if n.isLeaf {
+		f(n)
+	} else {
+		n.children[0].lookup(point, f)
+		n.children[1].lookup(point, f)
+	}
+}
+
 type searchHeap []searchItem
 type searchItem struct {
 	pointer       *Node2
@@ -122,10 +145,8 @@ type searchItem struct {
 	inheritedCost float64
 }
 
-func (h searchHeap) Len() int { return len(h) }
-func (h searchHeap) Less(i, j int) bool {
-	return h[i].pointer.box.Surface() < h[j].pointer.box.Surface()
-}
+func (h searchHeap) Len() int            { return len(h) }
+func (h searchHeap) Less(i, j int) bool  { return h[i].inheritedCost < h[j].inheritedCost }
 func (h searchHeap) Swap(i, j int)       { h[i], h[j] = h[j], h[i] }
 func (h *searchHeap) Push(x interface{}) { *h = append(*h, x.(searchItem)) }
 func (h *searchHeap) Pop() interface{} {

server/internal/bvh/bvh_test.go

@@ -17,6 +17,7 @@ func TestTree2_Insert(t *testing.T) {
 		{Upper: Vec2{102, 1}, Lower: Vec2{101, 0}},
 		{Upper: Vec2{111, 1}, Lower: Vec2{110, 0}},
 		{Upper: Vec2{112, 1}, Lower: Vec2{111, 0}},
+		{Upper: Vec2{1, 1}, Lower: Vec2{-1, -1}},
 	}
 	var bvh Tree2
 	for _, aabb := range aabbs {
@@ -26,6 +27,9 @@ func TestTree2_Insert(t *testing.T) {
 		toString(&sb, bvh.root)
 		t.Log(sb.String())
 	}
+	bvh.Find(Vec2{0.5, 0.5}, func(n *Node2) {
+		t.Logf("find! %v", n.box)
+	})
 }
 
 func TestTree2_Insert2_notLinkedTable(t *testing.T) {
@@ -83,11 +87,12 @@ func BenchmarkTree2_Insert(b *testing.B) {
 	const size = 25
 	// generate test cases
 	aabbs := make([]AABB2, b.N)
+	poses := make([]Vec2, b.N)
 	for i := range aabbs {
-		pos := Vec2{rand.Float64() * 1e4, rand.Float64() * 1e4}
+		poses[i] = Vec2{rand.Float64() * 1e4, rand.Float64() * 1e4}
 		aabbs[i] = AABB2{
-			Upper: Vec2{pos[0] + size, pos[0] + size},
-			Lower: Vec2{pos[0] - size, pos[0] - size},
+			Upper: Vec2{poses[i][0] + size, poses[i][0] + size},
+			Lower: Vec2{poses[i][0] - size, poses[i][0] - size},
 		}
 	}
 	b.ResetTimer()
@@ -96,4 +101,7 @@ func BenchmarkTree2_Insert(b *testing.B) {
 	for _, v := range aabbs {
 		bvh.Insert(v)
 	}
+	for _, v := range poses {
+		bvh.Find(v, func(n *Node2) {})
+	}
 }