138 lines
3.4 KiB
Go
138 lines
3.4 KiB
Go
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
|
|
}
|