func sortEntries(p gmath.Vec3, entries []entry) ([]entry, []float64) {
sorted := make([]entry, len(entries))
dists := make([]float64, len(entries))
for i := 0; i < len(entries); i++ {
sorted[i] = entries[i]
bounds := entries[i].bounds
dists[i] = p.Sub(bounds.Min).LengthSq()
}
sort.Sort(entrySlice{sorted, dists, p})
return sorted, dists
}
func (t *Tree) nearestNeighbors(k int, p gmath.Vec3, n *node, dists []float64, nearest []gfx.Spatial) ([]gfx.Spatial, []float64) {
if n.leaf {
for _, e := range n.entries {
dist := p.Sub(e.bounds.Min).Length()
dists, nearest = insertNearest(k, dists, nearest, dist, e.obj)
}
} else {
branches, branchDists := sortEntries(p, n.entries)
branches = pruneEntries(p, branches, branchDists)
for _, e := range branches {
nearest, dists = t.nearestNeighbors(k, p, e.child, dists, nearest)
}
}
return nearest, dists
}
func pruneEntries(p gmath.Vec3, entries []entry, minDists []float64) []entry {
minMinMaxDist := math.MaxFloat64
for i := range entries {
minMaxDist := p.Sub(entries[i].bounds.Max).LengthSq()
if minMaxDist < minMinMaxDist {
minMinMaxDist = minMaxDist
}
}
// remove all entries with minDist > minMinMaxDist
pruned := []entry{}
for i := range entries {
if minDists[i] <= minMinMaxDist {
pruned = append(pruned, entries[i])
}
}
return pruned
}
func (t *Tree) nearestNeighbor(p gmath.Vec3, n *node, d float64, nearest gfx.Spatial) (gfx.Spatial, float64) {
if n.leaf {
for _, e := range n.entries {
dist := p.Sub(e.bounds.Min).Length()
if dist < d {
d = dist
nearest = e.obj
}
}
} else {
branches, dists := sortEntries(p, n.entries)
branches = pruneEntries(p, branches, dists)
for _, e := range branches {
subNearest, dist := t.nearestNeighbor(p, e.child, d, nearest)
if dist < d {
d = dist
nearest = subNearest
}
}
}
return nearest, d
}
func LineVerts(start, end math.Vec3, width float64, result []gfx.Vec3) []gfx.Vec3 {
hw := math.Vec3Zero.AddScalar(width).DivScalar(2.0)
start = start.Sub(hw)
end = end.Add(hw)
return CubeVerts(start, end, result)
}
// Closest returns the closest point on the plane p nearest to the point q.
//
// Implemented as described in:
// Real-Time Collision Detection, 5.1.1 "Closest Point on Plane to Point".
func (p Plane3) Closest(q math.Vec3) math.Vec3 {
t := p.Normal.Dot(q) - p.Pos
return q.Sub(t).Mul(p.Normal)
}
