diff --git a/server/internal/bvh/bvh.go b/server/internal/bvh/bvh.go new file mode 100644 index 0000000..c282b07 --- /dev/null +++ b/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 +} diff --git a/server/internal/bvh/bvh_test.go b/server/internal/bvh/bvh_test.go new file mode 100644 index 0000000..26e631a --- /dev/null +++ b/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) + } +}