From 169990cb9f98e7335d42ba17a01e1b9b73a53c5f 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 | 16 +++++--- src/collision/DynamicTree.ts | 75 ++++++++++++++++++++++++++---------- src/dynamics/Body.ts | 16 +++++--- src/dynamics/Fixture.ts | 2 +- 5 files changed, 77 insertions(+), 34 deletions(-) diff --git a/src/Settings.ts b/src/Settings.ts index eb5406f2..8be03d2a 100644 --- a/src/Settings.ts +++ b/src/Settings.ts @@ -53,7 +53,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 186e5d2c..6ff4c35d 100644 --- a/src/collision/BroadPhase.ts +++ b/src/collision/BroadPhase.ts @@ -188,8 +188,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; } @@ -202,8 +202,8 @@ export class BroadPhase { this.m_tree.query(fatAABB, this.queryCallback); } - // Try to keep the tree balanced. - // this.m_tree.rebalance(4); + // Reset move buffer + this.m_moveBuffer.length = 0; } queryCallback = (proxyId: number): boolean => { @@ -212,11 +212,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 42b55c1d..ad259cfa 100644 --- a/src/collision/DynamicTree.ts +++ b/src/collision/DynamicTree.ts @@ -48,6 +48,8 @@ export class TreeNode { /** 0: leaf, -1: free node */ height: number = -1; + moved: boolean = false; + constructor(id?: number) { this.id = id; } @@ -104,6 +106,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. * @@ -122,7 +136,8 @@ export class DynamicTree { node.parent = null; node.child1 = null; node.child2 = null; - node.height = -1; + node.height = 0; + node.moved = false; this.m_nodes[node.id] = node; return node; } @@ -152,6 +167,7 @@ export class DynamicTree { node.userData = userData; node.height = 0; + node.moved = true; this.insertLeaf(node); @@ -176,48 +192,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: Vec2): boolean { + moveProxy(id: number, aabb: AABB, displacement: Vec2): 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; } diff --git a/src/dynamics/Body.ts b/src/dynamics/Body.ts index c0226b66..215b93ce 100644 --- a/src/dynamics/Body.ts +++ b/src/dynamics/Body.ts @@ -590,13 +590,19 @@ export class Body { * Update fixtures in broad-phase. */ synchronizeFixtures(): void { - const xf = Transform.identity(); + const broadPhase = this.m_world.m_broadPhase; - this.m_sweep.getTransform(xf, 0); + if (this.m_awakeFlag) { + const xf = Transform.identity(); + 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); + 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 2115d42b..e0911db5 100644 --- a/src/dynamics/Fixture.ts +++ b/src/dynamics/Fixture.ts @@ -404,7 +404,7 @@ export class Fixture { proxy.aabb.combine(aabb1, aabb2); - const displacement = Vec2.sub(xf2.p, xf1.p); + const displacement = Vec2.sub(aabb2.getCenter(), aabb1.getCenter()); broadPhase.moveProxy(proxy.proxyId, proxy.aabb, displacement); }