760 lines
18 KiB
Kotlin
760 lines
18 KiB
Kotlin
package ru.dbotthepony.kbox2d.collision
|
|
|
|
import ru.dbotthepony.kbox2d.api.*
|
|
import ru.dbotthepony.kvector.util2d.AABB
|
|
import ru.dbotthepony.kvector.vector.ndouble.Vector2d
|
|
import kotlin.math.absoluteValue
|
|
|
|
/**
|
|
* A node in the dynamic tree. The client does not interact with this directly.
|
|
*/
|
|
private data class TreeNode(
|
|
val tree: DynamicTree,
|
|
/// Enlarged AABB
|
|
var aabb: AABB? = null,
|
|
|
|
var child1: Int = 0,
|
|
var child2: Int = 0,
|
|
|
|
// leaf = 0, free node = -1
|
|
var height: Int = 0,
|
|
|
|
var moved: Boolean = false,
|
|
|
|
var userData: Any? = null,
|
|
) {
|
|
val isLeaf get() = child1 == b2_nullNode
|
|
|
|
// union
|
|
private var _union: Int = b2_nullNode
|
|
|
|
var parent by this::_union
|
|
var next by this::_union
|
|
}
|
|
|
|
const val b2_nullNode = -1
|
|
|
|
class DynamicTree : IProxieable, IMovable {
|
|
private var root: Int = b2_nullNode
|
|
|
|
private val nodeCapacity get() = nodes.size
|
|
private var nodeCount = 0
|
|
private var freeList = 0
|
|
private var insertionCount = 0
|
|
|
|
private var nodes = Array(16) { TreeNode(this) }
|
|
|
|
init {
|
|
// Build a linked list for the free list.
|
|
for (i in 0 until nodeCapacity - 1) {
|
|
nodes[i].next = i + 1
|
|
nodes[i].height = -1
|
|
}
|
|
|
|
nodes[nodeCapacity - 1].next = b2_nullNode
|
|
nodes[nodeCapacity - 1].height = -1
|
|
}
|
|
|
|
private fun allocateNode(): Int {
|
|
// Expand the node pool as needed.
|
|
if (freeList == b2_nullNode) {
|
|
check(nodeCount == nodeCapacity) { "$nodeCount != $nodeCapacity" }
|
|
|
|
// The free list is empty. Rebuild a bigger pool.
|
|
nodes = Array(nodeCapacity * 2) memcopy@{
|
|
if (it >= nodeCapacity)
|
|
return@memcopy TreeNode(this)
|
|
else
|
|
return@memcopy nodes[it]
|
|
}
|
|
|
|
// Build a linked list for the free list. The parent
|
|
// pointer becomes the "next" pointer.
|
|
for (i in nodeCount until nodeCapacity - 1) {
|
|
nodes[i].next = i + 1
|
|
nodes[i].height = -1
|
|
}
|
|
|
|
nodes[nodeCapacity - 1].next = b2_nullNode
|
|
nodes[nodeCapacity - 1].height = -1
|
|
|
|
freeList = nodeCount
|
|
}
|
|
|
|
// Peel a node off the free list.
|
|
val nodeId = freeList
|
|
freeList = nodes[nodeId].next
|
|
val node = nodes[nodeId]
|
|
|
|
node.parent = b2_nullNode
|
|
node.child1 = b2_nullNode
|
|
node.child2 = b2_nullNode
|
|
node.height = 0
|
|
node.userData = null
|
|
node.moved = false
|
|
|
|
nodeCount++
|
|
return nodeId
|
|
}
|
|
|
|
private fun freeNode(nodeId: Int) {
|
|
require(nodeId in 0 until nodeCapacity) { "$nodeId is out of range (0 to ${nodeCapacity - 1})" }
|
|
check(0 < nodeCount) { "We have $nodeCount nodes" }
|
|
|
|
val node = nodes[nodeId]
|
|
|
|
node.next = freeList
|
|
node.height = -1
|
|
// node.aabb = null
|
|
// node.userData = null
|
|
|
|
freeList = nodeId
|
|
nodeCount--
|
|
}
|
|
|
|
override fun createProxy(aabb: AABB, userData: Any?): Int {
|
|
val proxyId = allocateNode()
|
|
|
|
// Fatten the aabb.
|
|
val node = nodes[proxyId]
|
|
node.aabb = AABB(aabb.mins - R, aabb.maxs + R)
|
|
node.userData = userData
|
|
node.height = 0
|
|
node.moved = true
|
|
|
|
insertLeaf(proxyId)
|
|
|
|
return proxyId
|
|
}
|
|
|
|
override fun destroyProxy(proxyID: Int) {
|
|
check(nodes[proxyID].isLeaf) { "Can't chop whole branch" }
|
|
|
|
removeLeaf(proxyID)
|
|
freeNode(proxyID)
|
|
}
|
|
|
|
override fun moveProxy(proxyID: Int, aabb: AABB, displacement: Vector2d): Boolean {
|
|
check(nodes[proxyID].isLeaf) { "Can't move whole branch" }
|
|
|
|
// Extend AABB
|
|
val mins = (aabb.mins - R).toMutableVector()
|
|
val maxs = (aabb.maxs + R).toMutableVector()
|
|
|
|
// Predict AABB movement
|
|
val d = displacement * b2_aabbMultiplier
|
|
|
|
if (d.x < 0.0) {
|
|
mins.x += d.x
|
|
} else {
|
|
maxs.x += d.x
|
|
}
|
|
|
|
if (d.y < 0.0) {
|
|
mins.y += d.y
|
|
} else {
|
|
maxs.y += d.y
|
|
}
|
|
|
|
val fatAABB = AABB(mins.toVector(), maxs.toVector())
|
|
val treeAABB = checkNotNull(nodes[proxyID].aabb) { "Node at $proxyID has null AABB" }
|
|
|
|
if (treeAABB.contains(fatAABB)) {
|
|
// The tree AABB still contains the object, but it might be too large.
|
|
// Perhaps the object was moving fast but has since gone to sleep.
|
|
// The huge AABB is larger than the new fat AABB.
|
|
val hugeAABB = AABB(
|
|
fatAABB.mins - R * 4.0,
|
|
fatAABB.maxs + R * 4.0,
|
|
)
|
|
|
|
if (treeAABB.contains(hugeAABB)) {
|
|
// The tree AABB contains the object AABB and the tree AABB is
|
|
// not too large. No tree update needed.
|
|
return false
|
|
}
|
|
|
|
// Otherwise the tree AABB is huge and needs to be shrunk
|
|
}
|
|
|
|
removeLeaf(proxyID)
|
|
nodes[proxyID].aabb = fatAABB
|
|
insertLeaf(proxyID)
|
|
nodes[proxyID].moved = true
|
|
|
|
return true
|
|
}
|
|
|
|
override fun getUserData(proxyID: Int): Any? {
|
|
return nodes[proxyID].userData
|
|
}
|
|
|
|
fun wasMoved(proxyID: Int): Boolean {
|
|
return nodes[proxyID].moved
|
|
}
|
|
|
|
fun clearMoved(proxyID: Int) {
|
|
nodes[proxyID].moved = false
|
|
}
|
|
|
|
override fun getFatAABB(proxyID: Int): AABB {
|
|
return checkNotNull(nodes[proxyID].aabb) { "Tree node has null aabb. This can be either by a bug, or $proxyID is not a valid proxy" }
|
|
}
|
|
|
|
private fun insertLeaf(leaf: Int) {
|
|
insertionCount++
|
|
|
|
if (root == b2_nullNode) {
|
|
root = leaf
|
|
nodes[leaf].parent = b2_nullNode
|
|
return
|
|
}
|
|
|
|
// Find the best sibling for this node
|
|
val leafAABB = checkNotNull(nodes[leaf].aabb) { "Leaf at $leaf has null aabb" }
|
|
var index = root
|
|
|
|
while (!nodes[index].isLeaf) {
|
|
val child1 = nodes[index].child1
|
|
val child2 = nodes[index].child2
|
|
|
|
val area = checkNotNull(nodes[index].aabb) { "Node at $index has null aabb" }.perimeter
|
|
val combined = nodes[index].aabb?.combine(leafAABB) ?: throw ConcurrentModificationException()
|
|
|
|
val combinedArea = combined.perimeter
|
|
|
|
// Cost of creating a new parent for this node and the new leaf
|
|
val cost = 2.0 * combinedArea
|
|
|
|
// Minimum cost of pushing the leaf further down the tree
|
|
val inheritanceCost = 2.0 * (combinedArea - area)
|
|
|
|
// Cost of descending into child1
|
|
val cost1: Double
|
|
|
|
if (nodes[child1].isLeaf) {
|
|
val aabb = leafAABB.combine(checkNotNull(nodes[child1].aabb) { "Node at $child1 has null aabb" })
|
|
cost1 = aabb.perimeter + inheritanceCost
|
|
} else {
|
|
val aabb = leafAABB.combine(checkNotNull(nodes[child1].aabb) { "Node at $child1 has null aabb" })
|
|
val oldArea = nodes[child1].aabb?.perimeter ?: throw ConcurrentModificationException()
|
|
val newArea = aabb.perimeter
|
|
cost1 = (newArea - oldArea) + inheritanceCost
|
|
}
|
|
|
|
// Cost of descending into child2
|
|
val cost2: Double
|
|
|
|
if (nodes[child2].isLeaf) {
|
|
val aabb = leafAABB.combine(checkNotNull(nodes[child2].aabb) { "Node at $child2 has null aabb" })
|
|
cost2 = aabb.perimeter + inheritanceCost
|
|
} else {
|
|
val aabb = leafAABB.combine(checkNotNull(nodes[child2].aabb) { "Node at $child2 has null aabb" })
|
|
val oldArea = nodes[child2].aabb?.perimeter ?: throw ConcurrentModificationException()
|
|
val newArea = aabb.perimeter
|
|
cost2 = (newArea - oldArea) + inheritanceCost
|
|
}
|
|
|
|
// Descend according to the minimum cost.
|
|
if (cost < cost1 && cost < cost2) {
|
|
break
|
|
}
|
|
|
|
// Descend
|
|
if (cost1 < cost2) {
|
|
index = child1
|
|
} else {
|
|
index = child2
|
|
}
|
|
}
|
|
|
|
val sibling = index
|
|
|
|
// Create a new parent.
|
|
val oldParent = nodes[sibling].parent
|
|
val newParent = allocateNode()
|
|
|
|
nodes[newParent].parent = oldParent
|
|
nodes[newParent].userData = null
|
|
nodes[newParent].aabb = leafAABB.combine(checkNotNull(nodes[sibling].aabb) { "Node at $sibling has null aabb" })
|
|
nodes[newParent].height = nodes[sibling].height + 1
|
|
|
|
if (oldParent != b2_nullNode) {
|
|
// The sibling was not the root.
|
|
if (nodes[oldParent].child1 == sibling) {
|
|
nodes[oldParent].child1 = newParent
|
|
} else {
|
|
nodes[oldParent].child2 = newParent
|
|
}
|
|
|
|
nodes[newParent].child1 = sibling
|
|
nodes[newParent].child2 = leaf
|
|
|
|
nodes[sibling].parent = newParent
|
|
nodes[leaf].parent = newParent
|
|
} else {
|
|
// The sibling was the root.
|
|
nodes[newParent].child1 = sibling
|
|
nodes[newParent].child2 = leaf
|
|
|
|
nodes[sibling].parent = newParent
|
|
nodes[leaf].parent = newParent
|
|
root = newParent
|
|
}
|
|
|
|
// Walk back up the tree fixing heights and AABBs
|
|
index = nodes[leaf].parent
|
|
|
|
while (index != b2_nullNode) {
|
|
index = balance(index)
|
|
|
|
val child1 = nodes[index].child1
|
|
val child2 = nodes[index].child2
|
|
|
|
check(child1 != b2_nullNode) { "Node at $index is supposed to have child1, but it does not" }
|
|
check(child2 != b2_nullNode) { "Node at $index is supposed to have child2, but it does not" }
|
|
|
|
nodes[index].height = 1 + nodes[child1].height.coerceAtLeast(nodes[child2].height)
|
|
nodes[index].aabb =
|
|
checkNotNull(nodes[child1].aabb) { "Node at $child1 has null aabb" }
|
|
.combine(checkNotNull(nodes[child2].aabb) { "Node at $child2 has null aabb" })
|
|
|
|
index = nodes[index].parent
|
|
}
|
|
|
|
// validate()
|
|
}
|
|
|
|
private fun removeLeaf(leaf: Int) {
|
|
if (leaf == root) {
|
|
root = b2_nullNode
|
|
return
|
|
}
|
|
|
|
val parent = nodes[leaf].parent
|
|
val grandParent = nodes[parent].parent
|
|
val sibling: Int
|
|
|
|
if (nodes[parent].child1 == leaf) {
|
|
sibling = nodes[parent].child2
|
|
} else {
|
|
sibling = nodes[parent].child1
|
|
}
|
|
|
|
if (grandParent != b2_nullNode) {
|
|
// Destroy parent and connect sibling to grandParent.
|
|
if (nodes[grandParent].child1 == parent) {
|
|
nodes[grandParent].child1 = sibling
|
|
} else {
|
|
nodes[grandParent].child2 = sibling
|
|
}
|
|
|
|
nodes[sibling].parent = grandParent
|
|
freeNode(parent)
|
|
|
|
// Adjust ancestor bounds.
|
|
var index = grandParent
|
|
|
|
while (index != b2_nullNode) {
|
|
index = balance(index)
|
|
|
|
val child1 = nodes[index].child1
|
|
val child2 = nodes[index].child2
|
|
|
|
nodes[index].aabb =
|
|
checkNotNull(nodes[child1].aabb) { "Node at $child1 has null aabb" }
|
|
.combine(checkNotNull(nodes[child2].aabb) { "Node at $child2 has null aabb" })
|
|
|
|
nodes[index].height = 1 + nodes[child1].height.coerceAtLeast(nodes[child2].height)
|
|
|
|
index = nodes[index].parent
|
|
}
|
|
} else {
|
|
root = sibling
|
|
nodes[sibling].parent = b2_nullNode
|
|
freeNode(parent)
|
|
}
|
|
|
|
// validate()
|
|
}
|
|
|
|
/**
|
|
* Perform a left or right rotation if node A is imbalanced.
|
|
* Returns the new root index.
|
|
*/
|
|
private fun balance(iA: Int): Int {
|
|
require(iA != b2_nullNode) { "iA is a null node" }
|
|
|
|
val A = nodes[iA]
|
|
|
|
if (A.isLeaf || A.height < 2) {
|
|
return iA
|
|
}
|
|
|
|
val iB = A.child1
|
|
val iC = A.child2
|
|
|
|
val B = nodes[iB]
|
|
val C = nodes[iC]
|
|
|
|
val balance = C.height - B.height
|
|
|
|
// Rotate C up
|
|
if (balance > 1) {
|
|
val iF = C.child1
|
|
val iG = C.child2
|
|
|
|
val F = nodes[iF]
|
|
val G = nodes[iG]
|
|
|
|
// Swap A and C
|
|
C.child1 = iA
|
|
C.parent = A.parent
|
|
A.parent = iC
|
|
|
|
// A's old parent should point to C
|
|
if (C.parent != b2_nullNode) {
|
|
if (nodes[C.parent].child1 == iA) {
|
|
nodes[C.parent].child1 = iC
|
|
} else {
|
|
check(nodes[C.parent].child2 == iA) { "${nodes[C.parent].child2} != $iA" }
|
|
nodes[C.parent].child2 = iC
|
|
}
|
|
} else {
|
|
root = iC
|
|
}
|
|
|
|
// Rotate
|
|
if (F.height > G.height) {
|
|
C.child2 = iF
|
|
A.child2 = iG
|
|
G.parent = iA
|
|
A.aabb = checkNotNull(B.aabb) { "Node at $iB has null aabb" }.combine(checkNotNull(G.aabb) { "Node at $iG has null aabb" })
|
|
C.aabb = checkNotNull(A.aabb) { "Node at $iA has null aabb" }.combine(checkNotNull(F.aabb) { "Node at $iF has null aabb" })
|
|
|
|
A.height = 1 + b2Max(B.height, G.height)
|
|
C.height = 1 + b2Max(A.height, F.height)
|
|
} else {
|
|
C.child2 = iG
|
|
A.child2 = iF
|
|
F.parent = iA
|
|
|
|
A.aabb = checkNotNull(B.aabb) { "Node at $iB has null aabb" }.combine(checkNotNull(F.aabb) { "Node at $iF has null aabb" })
|
|
C.aabb = checkNotNull(A.aabb) { "Node at $iA has null aabb" }.combine(checkNotNull(G.aabb) { "Node at $iG has null aabb" })
|
|
|
|
A.height = 1 + b2Max(B.height, F.height)
|
|
C.height = 1 + b2Max(A.height, G.height)
|
|
}
|
|
|
|
return iC
|
|
}
|
|
|
|
// rotate B up
|
|
if (balance < -1) {
|
|
val iD = B.child1
|
|
val iE = B.child2
|
|
|
|
val D = nodes[iD]
|
|
val E = nodes[iE]
|
|
|
|
// Swap A and B
|
|
B.child1 = iA
|
|
B.parent = A.parent
|
|
A.parent = iB
|
|
|
|
// A's old parent should point to B
|
|
if (B.parent != b2_nullNode) {
|
|
if (nodes[B.parent].child1 == iA) {
|
|
nodes[B.parent].child1 = iB
|
|
} else {
|
|
check(nodes[B.parent].child2 == iA) { "${nodes[B.parent].child2} != $iA" }
|
|
nodes[B.parent].child2 = iB
|
|
}
|
|
} else {
|
|
root = iB
|
|
}
|
|
|
|
// Rotate
|
|
if (D.height > E.height) {
|
|
B.child2 = iD
|
|
A.child1 = iE
|
|
E.parent = iA
|
|
|
|
A.aabb = checkNotNull(C.aabb) { "Node at $iC has null aabb" }.combine(checkNotNull(E.aabb) { "Node at $iE has null aabb" })
|
|
B.aabb = checkNotNull(A.aabb) { "Node at $iA has null aabb" }.combine(checkNotNull(D.aabb) { "Node at $iD has null aabb" })
|
|
|
|
A.height = 1 + b2Max(C.height, E.height)
|
|
B.height = 1 + b2Max(A.height, D.height)
|
|
} else {
|
|
B.child2 = iE
|
|
A.child1 = iD
|
|
D.parent = iA
|
|
|
|
A.aabb = checkNotNull(C.aabb) { "Node at $iC has null aabb" }.combine(checkNotNull(D.aabb) { "Node at $iD has null aabb" })
|
|
B.aabb = checkNotNull(A.aabb) { "Node at $iA has null aabb" }.combine(checkNotNull(E.aabb) { "Node at $iE has null aabb" })
|
|
|
|
A.height = 1 + b2Max(C.height, D.height)
|
|
B.height = 1 + b2Max(A.height, E.height)
|
|
}
|
|
|
|
return iB
|
|
}
|
|
|
|
return iA
|
|
}
|
|
|
|
/**
|
|
* Compute the height of the binary tree in O(N) time. Should not be
|
|
* called often.
|
|
*/
|
|
val height: Int get() = if (root == b2_nullNode) 0 else nodes[root].height
|
|
|
|
/**
|
|
* Get the ratio of the sum of the node areas to the root area.
|
|
*/
|
|
val getAreaRatio: Double get() {
|
|
if (root == b2_nullNode)
|
|
return 0.0
|
|
|
|
val root = nodes[root]
|
|
val rootArea = checkNotNull(root.aabb) { "Node at ${this.root} has null aabb" }.perimeter
|
|
|
|
var totalArea = 0.0
|
|
|
|
for ((i, node) in nodes.withIndex())
|
|
if (node.height >= 0)
|
|
totalArea += checkNotNull(node.aabb) { "Node at $i has null aabb" }.perimeter
|
|
|
|
return totalArea / rootArea
|
|
}
|
|
|
|
private fun computeHeight(nodeId: Int): Int {
|
|
val node = nodes[nodeId]
|
|
|
|
if (node.isLeaf)
|
|
return 0
|
|
|
|
val height1 = computeHeight(node.child1)
|
|
val height2 = computeHeight(node.child2)
|
|
return 1 + height1.coerceAtLeast(height2)
|
|
}
|
|
|
|
private fun computeHeight() = computeHeight(root)
|
|
|
|
private fun validateStructure(index: Int) {
|
|
if (index == b2_nullNode)
|
|
return
|
|
|
|
if (index == root)
|
|
check(nodes[index].parent == b2_nullNode)
|
|
|
|
val node = nodes[index]
|
|
|
|
val child1 = node.child1
|
|
val child2 = node.child2
|
|
|
|
if (node.isLeaf) {
|
|
check(child1 == b2_nullNode)
|
|
check(child2 == b2_nullNode)
|
|
check(node.height == 0)
|
|
return
|
|
}
|
|
|
|
check(nodes[child1].parent == index)
|
|
check(nodes[child2].parent == index)
|
|
|
|
validateStructure(child1)
|
|
validateStructure(child2)
|
|
}
|
|
|
|
private fun validateMetrics(index: Int) {
|
|
if (index == b2_nullNode)
|
|
return
|
|
|
|
val node = nodes[index]
|
|
|
|
val child1 = node.child1
|
|
val child2 = node.child2
|
|
|
|
if (node.isLeaf) {
|
|
check(child1 == b2_nullNode)
|
|
check(child2 == b2_nullNode)
|
|
check(node.height == 0)
|
|
return
|
|
}
|
|
|
|
val height1 = nodes[child1].height
|
|
val height2 = nodes[child2].height
|
|
|
|
val height = 1 + height1.coerceAtLeast(height2)
|
|
check(node.height == height1)
|
|
|
|
val combined = nodes[child1].aabb!!.combine(nodes[child2].aabb!!)
|
|
check(combined == node.aabb)
|
|
|
|
validateMetrics(child1)
|
|
validateMetrics(child2)
|
|
}
|
|
|
|
/**
|
|
* Validate this tree. For testing.
|
|
*/
|
|
fun validate() {
|
|
validateStructure(root)
|
|
validateMetrics(root)
|
|
|
|
var freeCount = 0
|
|
var freeIndex = freeList
|
|
|
|
while (freeIndex != b2_nullNode) {
|
|
freeIndex = nodes[freeIndex].next
|
|
freeCount++
|
|
}
|
|
|
|
check(height == computeHeight()) { "Height $height does not match checked height ${computeHeight()}" }
|
|
check(nodeCount + freeCount == nodeCapacity)
|
|
}
|
|
|
|
/**
|
|
* Get the maximum balance of an node in the tree. The balance is the difference
|
|
* in height of the two children of a node.
|
|
*/
|
|
val maxBalance: Int get() {
|
|
var maxBalance = 0
|
|
|
|
for (node in nodes) {
|
|
if (node.height <= 1)
|
|
continue
|
|
|
|
check(!node.isLeaf)
|
|
|
|
val child1 = node.child1
|
|
val child2 = node.child2
|
|
val balance = (nodes[child2].height - nodes[child1].height).absoluteValue
|
|
maxBalance = balance.coerceAtLeast(maxBalance)
|
|
}
|
|
|
|
return maxBalance
|
|
}
|
|
|
|
/**
|
|
* Build an optimal tree. Very expensive. For testing.
|
|
*/
|
|
fun rebuildBottomUp() {
|
|
TODO("Not Yet Implemented")
|
|
}
|
|
|
|
override fun shiftOrigin(newOrigin: Vector2d) {
|
|
// Build array of leaves. Free the rest.
|
|
for (node in nodes) {
|
|
if (node.aabb != null) {
|
|
node.aabb = node.aabb?.minus(newOrigin) ?: throw ConcurrentModificationException()
|
|
}
|
|
}
|
|
}
|
|
|
|
override fun query(aabb: AABB, callback: ProxyQueryCallback): Boolean {
|
|
val stack = ArrayDeque<Int>(256)
|
|
stack.add(root)
|
|
|
|
while (stack.isNotEmpty()) {
|
|
val nodeId = stack.removeLast()
|
|
|
|
if (nodeId == b2_nullNode) {
|
|
continue
|
|
}
|
|
|
|
val node = nodes[nodeId]
|
|
val nodeAABB = checkNotNull(node.aabb) { "Tree node at $nodeId has null aabb" }
|
|
|
|
if (nodeAABB.intersectWeak(aabb)) {
|
|
if (node.isLeaf) {
|
|
if (!callback.invoke(nodeId, node.userData)) {
|
|
return true
|
|
}
|
|
} else {
|
|
stack.add(node.child1)
|
|
stack.add(node.child2)
|
|
}
|
|
}
|
|
}
|
|
|
|
return false
|
|
}
|
|
|
|
override fun rayCast(input: RayCastInput, callback: ProxyRayCastCallback) {
|
|
val p1 = input.p1
|
|
val p2 = input.p2
|
|
var r = p2 - p1
|
|
val diff = r
|
|
require(r.lengthSquared > 0.0) { "Start and end points match: $p1 $p2" }
|
|
r = r.normalized
|
|
|
|
// v is perpendicular to the segment.
|
|
val v = b2Cross(1.0, r)
|
|
val abs_v = v.absoluteValue
|
|
|
|
// Separating axis for segment (Gino, p80).
|
|
// |dot(v, p1 - c)| > dot(|v|, h)
|
|
|
|
var maxFraction = input.maxFraction
|
|
|
|
// Build a bounding box for the segment.
|
|
|
|
var t = p1 + diff * maxFraction
|
|
|
|
var segmentAABB = AABB(
|
|
mins = p1.coerceAtMost(t),
|
|
maxs = p1.coerceAtLeast(t)
|
|
)
|
|
|
|
val stack = ArrayDeque<Int>(256)
|
|
stack.add(root)
|
|
|
|
while (stack.isNotEmpty()) {
|
|
val nodeId = stack.removeLast()
|
|
|
|
if (nodeId == b2_nullNode) {
|
|
continue
|
|
}
|
|
|
|
val node = nodes[nodeId]
|
|
val nodeAABB = checkNotNull(node.aabb) { "Tree node at $nodeId has null aabb" }
|
|
|
|
if (!nodeAABB.intersectWeak(segmentAABB)) {
|
|
continue
|
|
}
|
|
|
|
// Separating axis for segment (Gino, p80).
|
|
// |dot(v, p1 - c)| > dot(|v|, h)
|
|
if (v.dot(p1 - nodeAABB.centre).absoluteValue > abs_v.dot(nodeAABB.extents)) {
|
|
continue
|
|
}
|
|
|
|
if (node.isLeaf) {
|
|
val value = callback.invoke(RayCastInput(p1, p2, maxFraction), nodeId, node.userData)
|
|
|
|
if (value == 0.0) {
|
|
// The client has terminated the ray cast.
|
|
return
|
|
} else if (value > 0.0) {
|
|
// Update segment bounding box.
|
|
maxFraction = value
|
|
t = p1 + diff * maxFraction
|
|
segmentAABB = AABB(
|
|
mins = p1.coerceAtMost(t),
|
|
maxs = p1.coerceAtLeast(t)
|
|
)
|
|
}
|
|
} else {
|
|
stack.add(node.child1)
|
|
stack.add(node.child2)
|
|
}
|
|
}
|
|
}
|
|
|
|
companion object {
|
|
private val R = Vector2d(b2_aabbExtension, b2_aabbExtension)
|
|
}
|
|
}
|