physicsManager.ts

import { AmmoJSPlugin, PhysicsImpostor, Vector3 } from "babylonjs"
import { Actor, RigidBody } from "./actors"
import { Assert } from "./assert"
import { IdGenerator } from "./idGenerator"
import type { Table } from "./table"
import { AmmoUtils } from "./utils"
import type { btAmmo, btManifoldPoint, btRigidBody, btWorld } from './utils/ammo'

/*
 This class manages manifolds from bullet which are a data structure that is created and destroyed
 when two rigid bodies come in contact with each other. There are multiple manifolds for each collision.
 Consider a box landing on a ground, there's 4 intersection points which are the 4 corners. The contact
 created event will fire 4 times, and destroy is called when the box is picked up. collision occurs on
 the first event, and collision ends on the last event.

 It's also worth noting that it's possible manifolds are destroyed after the actor is removed, therefore
 we must check for the actor being disposed when destroy manifold is called to avoid calling functions
 on actors that have already been disposed of.

 You may be wondering why we so aggressively attempt to re-register bullet bodies from their imposters.
 It's because babylon reconstructs the body if you make any changes without telling you, so the information
 in rigidBody.getUserIndex() we attached to the body will be lost without being carried over to the next body.
 To solve this we merely late resolve, and pre update resolve any impostors until this can be fixed in babylon.
*/
export class PhysicsManager {
    table: Table
    plugin: AmmoJSPlugin
    world: btWorld
    ammo: btAmmo

    actorIdToImpostor = new Map<string, PhysicsImpostor>()
    actorIdToRegisterId = new Map<string, number>()
    collisionIdToActors = new Map<string, [Actor, Actor]>()
    collisionIdToManifoldCount = new Map<string, number>()
    manifoldIds = new IdGenerator()
    manifoldIdToActors = new Map<number, [Actor, Actor]>()
    registerId = new IdGenerator()
    registerIdToActor = new Map<number, Actor>()
    isCollidingThisFrame = new Set<Actor>()

    constructor(table: Table, plugin: AmmoJSPlugin) {
        this.table = table
        this.plugin = plugin
        this.world = plugin.world
        this.ammo = AmmoUtils.getAmmo()

        this.world.setContactProcessedCallback(this.ammo.addFunction(this.contactProcessedCallbackPtr, this.ammo.CONTACT_PROCESSED_CALLBACK_SIGNATURE))
        this.world.setContactDestroyedCallback(this.ammo.addFunction(this.contactDestroyedCallbackPtr, this.ammo.CONTACT_DESTROYED_CALLBACK_SIGNATURE))
    }

    prePhysicsUpdate(deltaTime: number): void {
        this.resolveUnknownImpostors(false)
    }

    get physicsObjectCount(): number  {
        return this.actorIdToImpostor.size
    }

    postPhysicsUpdate(deltaTime: number): void {
        for(const [actorA, actorB] of this.collisionIdToActors.values()) {
            const impostorA = this.actorIdToImpostor.get(actorA.actorId)
            const impostorB = this.actorIdToImpostor.get(actorB.actorId)

            Assert.IsNotNull(impostorA)
            Assert.IsNotNull(impostorB)

            if (actorA instanceof RigidBody && !this.isCollidingThisFrame.has(actorA) && !actorA.isDisposed)
                actorA.onCollisionStay(impostorB, actorB)

            if (actorB instanceof RigidBody && !this.isCollidingThisFrame.has(actorB) && !actorB.isDisposed)
                actorB.onCollisionStay(impostorA, actorA)
        }

        this.isCollidingThisFrame.clear()
    }

    registerActor(actor: Actor, impostor: PhysicsImpostor): boolean {
        const body = impostor.physicsBody as btRigidBody

        if (body.getUserIndex() === 0) {
            const oldId = this.actorIdToRegisterId.get(actor.actorId)

            if (oldId !== undefined)
                this.registerIdToActor.delete(oldId)

            const id = this.registerId.next()
            body.setUserIndex(id)
            this.registerIdToActor.set(id, actor)
            this.actorIdToRegisterId.set(actor.actorId, id)
            this.actorIdToImpostor.set(actor.actorId, impostor)
            AmmoUtils.enableCollisionEvents(impostor)

            return oldId !== undefined
        }

        return false
    }

    removeActor(actor: Actor): void {
        const oldId = this.actorIdToRegisterId.get(actor.actorId)

        if (oldId !== undefined)
            this.registerIdToActor.delete(oldId)

        this.actorIdToImpostor.delete(actor.actorId)
        this.actorIdToRegisterId.delete(actor.actorId)
    }

    contactProcessedCallbackPtr = (manifoldPtr: unknown, sourcePtr: unknown, destPtr: unknown) => {
        const ammo = this.ammo

        const source = ammo.wrapPointer<btRigidBody>(sourcePtr, ammo.btRigidBody)
        const dest = ammo.wrapPointer<btRigidBody>(destPtr, ammo.btRigidBody)
        const manifold = ammo.wrapPointer<btManifoldPoint>(manifoldPtr, ammo.btManifoldPoint)

        var userPersistentData = manifold.get_m_userPersistentData();

        if (userPersistentData === 0) {
            // Assign and map a unique collision identifier with both bodies and handle begin events
            const manifoldId = this.manifoldIds.next();
            manifold.set_m_userPersistentData(manifoldId)

            let actorA = this.registerIdToActor.get(source.getUserIndex())
            let actorB = this.registerIdToActor.get(dest.getUserIndex())

            if(!actorA || !actorB) {
                this.resolveUnknownImpostors(true)
                actorA = this.registerIdToActor.get(source.getUserIndex())
                actorB = this.registerIdToActor.get(dest.getUserIndex())
            }

            Assert.IsNotNull(actorA)
            Assert.IsNotNull(actorB)

            this.manifoldIdToActors.set(manifoldId, [actorA, actorB])

            const collisionId = this.getCollisionId(actorA, actorB)
            let count = this.collisionIdToManifoldCount.get(collisionId) || 0
            this.collisionIdToManifoldCount.set(collisionId, ++count)

            if (count === 1) {
                this.collisionIdToActors.set(collisionId, [actorA, actorB])
                this.isCollidingThisFrame.add(actorA)
                this.isCollidingThisFrame.add(actorB)

                const impostorA = this.actorIdToImpostor.get(actorA.actorId)
                const impostorB = this.actorIdToImpostor.get(actorB.actorId)

                Assert.IsNotNull(impostorA)
                Assert.IsNotNull(impostorB)

                const velocityA = impostorA.getLinearVelocity() || Vector3.ZeroReadOnly
                const velocityB = impostorB.getLinearVelocity() || Vector3.ZeroReadOnly

                if (actorA instanceof RigidBody)
                    actorA.onCollisionStart(impostorB, actorB, velocityA, velocityA.length())

                if (actorB instanceof RigidBody)
                    actorB.onCollisionStart(impostorA, actorA, velocityB, velocityB.length())
            }
        }
    }

    contactDestroyedCallbackPtr = (userPersistentData: unknown) => {
        Assert.IsNumber(userPersistentData)

        const actors = this.manifoldIdToActors.get(userPersistentData)
        Assert.IsNotNull(actors, `Could not find actor with manifold id ${userPersistentData}`)

        this.manifoldIdToActors.delete(userPersistentData)

        const [actorA, actorB] = actors
        Assert.IsNotNull(actorA)
        Assert.IsNotNull(actorB)

        const collisionId = this.getCollisionId(actorA, actorB)
        let count = this.collisionIdToManifoldCount.get(collisionId)

        if(count === undefined || typeof count !== 'number' || count <= 0)
            throw new Error(`Physics Manifold count mismatch: ${count}`)

        this.collisionIdToManifoldCount.set(collisionId, --count)

        if (count > 0)
            return

        this.collisionIdToManifoldCount.delete(collisionId)
        this.collisionIdToActors.delete(collisionId)

        const impostorA = this.actorIdToImpostor.get(actorA.actorId)
        const impostorB = this.actorIdToImpostor.get(actorB.actorId)

        Assert.IsNotNull(impostorA)
        Assert.IsNotNull(impostorB)

        if (actorA instanceof RigidBody && !actorA.isDisposed)
            actorA.onCollisionEnd(impostorB, actorB)

        if (actorB instanceof RigidBody && !actorB.isDisposed)
            actorB.onCollisionEnd(impostorA, actorA)
    }

    cleanup() {
        this.world.setContactProcessedCallback(null)
        this.world.setContactDestroyedCallback(null)

        for (const actor of this.registerIdToActor.values())
            this.removeActor(actor)
    }

    private resolveUnknownImpostors(warn: boolean): void {
        for (const [actorId, impostor] of this.actorIdToImpostor.entries()) {
            const actor = this.table.getActorById(actorId)
            Assert.IsNotNull(actor, `Physics actor was not cleaned up from impostor map`)
            const resolved = this.registerActor(actor, impostor)

            if(resolved && warn)
                console.warn(`WARNING: Actor ${actorId}.${actor.actorClassId} resolved late, investigate why babylon constructed a new rigid body mid frame`)
        }
    }

    private getCollisionId(actorA: Actor, actorB: Actor): string {
        return [actorA.actorId, actorB.actorId].sort().join('_')
    }
}