From f699e1c47d6ebc4db93fef82811b28b10cb17718 Mon Sep 17 00:00:00 2001 From: Oliver Zell Date: Sat, 5 Aug 2023 15:23:35 +0200 Subject: [PATCH] perf: optimize broadphase pair management --- src/Settings.ts | 2 +- src/collision/BroadPhase.ts | 26 ++++++++++--- src/collision/DynamicTree.ts | 74 ++++++++++++++++++++++++++---------- src/dynamics/Body.ts | 15 ++++++-- src/dynamics/Fixture.ts | 2 +- 5 files changed, 87 insertions(+), 32 deletions(-) diff --git a/src/Settings.ts b/src/Settings.ts index 093ffdc6..c10aa61e 100644 --- a/src/Settings.ts +++ b/src/Settings.ts @@ -57,7 +57,7 @@ export class Settings { * future position based on the current displacement. This is a dimensionless * multiplier. */ - static aabbMultiplier: number = 2.0; + static aabbMultiplier: number = 4.0; /** * A small length used as a collision and constraint tolerance. Usually it is diff --git a/src/collision/BroadPhase.ts b/src/collision/BroadPhase.ts index e6e46dcb..42708504 100644 --- a/src/collision/BroadPhase.ts +++ b/src/collision/BroadPhase.ts @@ -184,8 +184,8 @@ export class BroadPhase { this.m_callback = addPairCallback; // Perform tree queries for all moving proxies. - while (this.m_moveBuffer.length > 0) { - this.m_queryProxyId = this.m_moveBuffer.pop(); + for (let i = 0; i < this.m_moveBuffer.length; ++i) { + this.m_queryProxyId = this.m_moveBuffer[i]; if (this.m_queryProxyId === null) { continue; } @@ -198,8 +198,18 @@ export class BroadPhase { this.m_tree.query(fatAABB, this.queryCallback); } - // Try to keep the tree balanced. - // this.m_tree.rebalance(4); + // Clear move flags + for (let i = 0; i < this.m_moveBuffer.length; ++i) { + this.m_queryProxyId = this.m_moveBuffer[i]; + if (this.m_queryProxyId === null) { + continue; + } + + this.m_tree.clearMoved(this.m_queryProxyId); + } + + // Reset move buffer + this.m_moveBuffer.length = 0; } queryCallback = (proxyId: number): boolean => { @@ -208,11 +218,15 @@ export class BroadPhase { return true; } + const moved = this.m_tree.wasMoved(proxyId); + if (moved && proxyId > this.m_queryProxyId) { + // Both proxies are moving. Avoid duplicate pairs. + return true; + } + const proxyIdA = Math.min(proxyId, this.m_queryProxyId); const proxyIdB = Math.max(proxyId, this.m_queryProxyId); - // TODO: Skip any duplicate pairs. - const userDataA = this.m_tree.getUserData(proxyIdA); const userDataB = this.m_tree.getUserData(proxyIdB); diff --git a/src/collision/DynamicTree.ts b/src/collision/DynamicTree.ts index d620045c..7f98c115 100644 --- a/src/collision/DynamicTree.ts +++ b/src/collision/DynamicTree.ts @@ -47,6 +47,8 @@ export class TreeNode { /** 0: leaf, -1: free node */ height: number = -1; + moved: boolean = false; + constructor(id?: number) { this.id = id; } @@ -110,6 +112,18 @@ export class DynamicTree { return node.userData; } + wasMoved(proxyId: number): boolean { + const node = this.m_nodes[proxyId]; + _ASSERT && console.assert(!!node); + return node.moved; + } + + clearMoved(proxyId: number): void { + const node = this.m_nodes[proxyId]; + _ASSERT && console.assert(!!node); + node.moved = false; + } + /** * Get the fat AABB for a node id. * @@ -152,6 +166,7 @@ export class DynamicTree { node.userData = userData; node.height = 0; + node.moved = true; this.insertLeaf(node); @@ -176,48 +191,65 @@ export class DynamicTree { * fattened AABB, then the proxy is removed from the tree and re-inserted. * Otherwise the function returns immediately. * - * @param d Displacement + * @param displacement Displacement * * @return true if the proxy was re-inserted. */ - moveProxy(id: number, aabb: AABB, d: Vec2Value): boolean { + moveProxy(id: number, aabb: AABB, displacement: Vec2Value): boolean { _ASSERT && console.assert(AABB.isValid(aabb)); - _ASSERT && console.assert(!d || Vec2.isValid(d)); + _ASSERT && console.assert(!displacement || Vec2.isValid(displacement)); const node = this.m_nodes[id]; _ASSERT && console.assert(!!node); _ASSERT && console.assert(node.isLeaf()); - if (node.aabb.contains(aabb)) { - return false; - } - - this.removeLeaf(node); - - node.aabb.set(aabb); - // Extend AABB. - aabb = node.aabb; - AABB.extend(aabb, Settings.aabbExtension); + const fatAABB = new AABB() + fatAABB.set(aabb); + AABB.extend(fatAABB, Settings.aabbExtension); - // Predict AABB displacement. + // Predict AABB movement // const d = Vec2.mul(Settings.aabbMultiplier, displacement); - if (d.x < 0.0) { - aabb.lowerBound.x += d.x * Settings.aabbMultiplier; + if (displacement.x < 0.0) { + fatAABB.lowerBound.x += displacement.x * Settings.aabbMultiplier; } else { - aabb.upperBound.x += d.x * Settings.aabbMultiplier; + fatAABB.upperBound.x += displacement.x * Settings.aabbMultiplier; } - if (d.y < 0.0) { - aabb.lowerBound.y += d.y * Settings.aabbMultiplier; + if (displacement.y < 0.0) { + fatAABB.lowerBound.y += displacement.y * Settings.aabbMultiplier; } else { - aabb.upperBound.y += d.y * Settings.aabbMultiplier; + fatAABB.upperBound.y += displacement.y * Settings.aabbMultiplier; + } + + const treeAABB = node.aabb; + if (treeAABB.contains(aabb)) { + // 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. + const hugeAABB = new AABB(); + hugeAABB.set(fatAABB); + AABB.extend(hugeAABB, 4.0 * Settings.aabbExtension); + + if (hugeAABB.contains(treeAABB)) { + // 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 } + this.removeLeaf(node); + + node.aabb = fatAABB; + this.insertLeaf(node); + node.moved = true; + return true; } @@ -285,6 +317,7 @@ export class DynamicTree { newParent.userData = null; newParent.aabb.combine(leafAABB, sibling.aabb); newParent.height = sibling.height + 1; + newParent.moved = false; if (oldParent != null) { // The sibling was not the root. @@ -693,6 +726,7 @@ export class DynamicTree { parent.height = 1 + Math.max(child1.height, child2.height); parent.aabb.combine(child1.aabb, child2.aabb); parent.parent = null; + parent.moved = false; child1.parent = parent; child2.parent = parent; diff --git a/src/dynamics/Body.ts b/src/dynamics/Body.ts index c1ba7818..71766991 100644 --- a/src/dynamics/Body.ts +++ b/src/dynamics/Body.ts @@ -595,11 +595,18 @@ export class Body { * Update fixtures in broad-phase. */ synchronizeFixtures(): void { - this.m_sweep.getTransform(xf, 0); - const broadPhase = this.m_world.m_broadPhase; - for (let f = this.m_fixtureList; f; f = f.m_next) { - f.synchronize(broadPhase, xf, this.m_xf); + + if (this.m_awakeFlag) { + this.m_sweep.getTransform(xf, 0); + + for (let f = this.m_fixtureList; f; f = f.m_next) { + f.synchronize(broadPhase, xf, this.m_xf); + } + } else { + for (let f = this.m_fixtureList; f; f = f.m_next) { + f.synchronize(broadPhase, this.m_xf, this.m_xf); + } } } diff --git a/src/dynamics/Fixture.ts b/src/dynamics/Fixture.ts index 9e1da02f..0ac7415e 100644 --- a/src/dynamics/Fixture.ts +++ b/src/dynamics/Fixture.ts @@ -405,7 +405,7 @@ export class Fixture { proxy.aabb.combine(synchronize_aabb1, synchronize_aabb2); - matrix.diffVec2(displacement, xf2.p, xf1.p); + matrix.diffVec2(displacement, synchronize_aabb2.getCenter(), synchronize_aabb1.getCenter()); broadPhase.moveProxy(proxy.proxyId, proxy.aabb, displacement); }