A BVH tree implement without rotations

server/internal/bvh/bvh.go

@@ -0,0 +1,137 @@
+package bvh
+
+import (
+	"container/heap"
+	"math"
+)
+
+type Vec2 [2]float64
+
+func (v Vec2) Add(other Vec2) Vec2 { return Vec2{v[0] + other[0], v[1] + other[1]} }
+func (v Vec2) Sub(other Vec2) Vec2 { return Vec2{v[0] - other[0], v[1] - other[1]} }
+func (v Vec2) Max(other Vec2) Vec2 { return Vec2{math.Max(v[0], other[0]), math.Max(v[1], other[1])} }
+func (v Vec2) Min(other Vec2) Vec2 { return Vec2{math.Min(v[0], other[0]), math.Min(v[1], other[1])} }
+
+type AABB2 struct{ Upper, Lower Vec2 }
+
+func (aabb AABB2) Union(other AABB2) AABB2 {
+	return AABB2{
+		Upper: aabb.Upper.Max(other.Upper),
+		Lower: aabb.Lower.Min(other.Lower),
+	}
+}
+
+func (aabb AABB2) Surface() float64 {
+	d := aabb.Upper.Sub(aabb.Lower)
+	return 2 * (d[0] + d[1])
+}
+
+type Node2 struct {
+	box      AABB2
+	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 v := n.children[1]; v != nil && v != not {
+		return v
+	}
+	return nil
+}
+
+type Tree2 struct {
+	root *Node2
+}
+
+func (t *Tree2) Insert(leaf AABB2) (n *Node2) {
+	n = &Node2{
+		box:      leaf,
+		parent:   nil,
+		children: [2]*Node2{},
+		isLeaf:   true,
+	}
+	if t.root == nil {
+		t.root = n
+		return
+	}
+
+	// Stage 1: find the best sibling for the new leaf
+	sibling := t.root
+	bestCost := t.root.box.Union(leaf).Surface()
+	parentTo := &t.root // the parent's children pointer which point to the sibling
+	queue := searchHeap{searchItem{pointer: t.root, parentTo: &t.root}}
+
+	leafCost := leaf.Surface()
+	for len(queue) > 0 {
+		p := heap.Pop(&queue).(searchItem)
+		// determine if node p has the best cost
+		mergeSurface := p.pointer.box.Union(leaf).Surface()
+		deltaCost := mergeSurface - p.pointer.box.Surface()
+		cost := p.inheritedCost + mergeSurface
+		if cost < bestCost {
+			bestCost = cost
+			sibling = p.pointer
+			parentTo = p.parentTo
+		}
+		// determine if it is worthwhile to explore the children of node p.
+		inheritedCost := p.inheritedCost + deltaCost // lower bound
+		if inheritedCost+leafCost < bestCost {
+			if p.pointer.children[0] != nil {
+				heap.Push(&queue, searchItem{
+					pointer:       p.pointer.children[0],
+					parentTo:      &p.pointer.children[0],
+					inheritedCost: inheritedCost,
+				})
+			}
+			if p.pointer.children[1] != nil {
+				heap.Push(&queue, searchItem{
+					pointer:       p.pointer.children[1],
+					parentTo:      &p.pointer.children[1],
+					inheritedCost: inheritedCost,
+				})
+			}
+		}
+	}
+
+	// Stage 2: create a new parent
+	*parentTo = &Node2{
+		box:      sibling.box.Union(leaf), // we will calculate in Stage3
+		parent:   sibling.parent,
+		children: [2]*Node2{sibling, n},
+		isLeaf:   false,
+	}
+	n.parent = *parentTo
+	sibling.parent = *parentTo
+
+	// Stage 3: walk back up the tree refitting AABBs
+	for p := *parentTo; p.parent != nil; p = p.parent {
+		p.box = p.children[0].box.Union(p.children[1].box)
+		//TODO: t.rotate(p)
+	}
+	return
+}
+
+type searchHeap []searchItem
+type searchItem struct {
+	pointer       *Node2
+	parentTo      **Node2
+	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) 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{} {
+	old := *h
+	n := len(old)
+	x := old[n-1]
+	*h = old[0 : n-1]
+	return x
+}

server/internal/bvh/bvh_test.go

@@ -0,0 +1,99 @@
+package bvh
+
+import (
+	"fmt"
+	"math/rand"
+	"strings"
+	"testing"
+)
+
+func TestTree2_Insert(t *testing.T) {
+	aabbs := []AABB2{
+		{Upper: Vec2{1, 1}, Lower: Vec2{0, 0}},
+		{Upper: Vec2{2, 1}, Lower: Vec2{1, 0}},
+		{Upper: Vec2{11, 1}, Lower: Vec2{10, 0}},
+		{Upper: Vec2{12, 1}, Lower: Vec2{11, 0}},
+		{Upper: Vec2{101, 1}, Lower: Vec2{100, 0}},
+		{Upper: Vec2{102, 1}, Lower: Vec2{101, 0}},
+		{Upper: Vec2{111, 1}, Lower: Vec2{110, 0}},
+		{Upper: Vec2{112, 1}, Lower: Vec2{111, 0}},
+	}
+	var bvh Tree2
+	for _, aabb := range aabbs {
+		bvh.Insert(aabb)
+		// visualize
+		var sb strings.Builder
+		toString(&sb, bvh.root)
+		t.Log(sb.String())
+	}
+}
+
+func TestTree2_Insert2_notLinkedTable(t *testing.T) {
+	const items = 1000
+	var bvh Tree2
+	for i := 0; i < items; i++ {
+		pos := Vec2{0, float64(i)}
+		bvh.Insert(AABB2{
+			Upper: Vec2{pos[0] + 1, pos[1] + 1},
+			Lower: Vec2{pos[0], pos[1]},
+		})
+	}
+	//calc depth
+	if depth := lookupDepth(bvh.root); depth > items/2 {
+		t.Errorf("the bvh is unbalanced: depth %d", depth)
+	} else {
+		t.Logf("the depth of bvh with %d element is %d", items, depth)
+	}
+}
+
+func toString(sb *strings.Builder, n *Node2) {
+	if n.isLeaf {
+		_, _ = fmt.Fprintf(sb, "(%v,%v)", n.box.Upper[1], n.box.Upper[0])
+		return
+	}
+	sb.WriteByte('{')
+	v1 := n.children[0]
+	if v1 != nil {
+		toString(sb, v1)
+	}
+	v2 := n.children[1]
+	if v2 != nil {
+		if v1 != nil {
+			sb.WriteString(", ")
+		}
+		toString(sb, v2)
+	}
+	sb.WriteByte('}')
+}
+
+func lookupDepth(n *Node2) int {
+	depth := 0
+	for _, child := range n.children {
+		if child != nil {
+			subdepth := lookupDepth(child)
+			if subdepth > depth {
+				depth = subdepth
+			}
+		}
+	}
+	return depth + 1 // add itself
+}
+
+func BenchmarkTree2_Insert(b *testing.B) {
+	const size = 25
+	// generate test cases
+	aabbs := make([]AABB2, b.N)
+	for i := range aabbs {
+		pos := 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},
+		}
+	}
+	b.ResetTimer()
+
+	var bvh Tree2
+	for _, v := range aabbs {
+		bvh.Insert(v)
+	}
+}