Ammo Debug Drawer Interface Implementation

AmmoDebugDrawer.ts

/* ADOPTED FROM
https://github.com/InfiniteLee/ammo-debug-drawer
*/

import Ammo from "ammo.js"

export const DefaultBufferSize = 3 * 1000000

export const AmmoDebugConstants = {
  NoDebug: 0,
  DrawWireframe: 1,
  DrawAabb: 2,
  DrawFeaturesText: 4,
  DrawContactPoints: 8,
  NoDeactivation: 16,
  NoHelpText: 32,
  DrawText: 64,
  ProfileTimings: 128,
  EnableSatComparison: 256,
  DisableBulletLCP: 512,
  EnableCCD: 1024,
  DrawConstraints: 1 << 11, //2048
  DrawConstraintLimits: 1 << 12, //4096
  FastWireframe: 1 << 13, //8192
  DrawNormals: 1 << 14, //16384
  MAX_DEBUG_DRAW_MODE: 0xffffffff
};

const setXYZ = function(array: number[] | Float32Array, index: number, x: number, y: number, z: number) {
  index *= 3;
  array[index + 0] = x;
  array[index + 1] = y;
  array[index + 2] = z;
}

/**
 * An implementation of the btIDebugDraw interface in Ammo.js, for debug rendering of Ammo shapes
*/
export class AmmoDebugDrawer implements Ammo.btIDebugDraw {
  world: Ammo.btDiscreteDynamicsWorld
  debugDrawer: Ammo.DebugDrawer
  indexArray: Uint32Array
  verticesArray: Float32Array
  colorsArray: Float32Array
  debugDrawMode: number
  index: number
  enabled: boolean
  warnedOnce: boolean

  constructor(
    indexArray: Uint32Array,
    verticesArray: Float32Array,
    colorsArray: Float32Array,
    world: Ammo.btDiscreteDynamicsWorld,
    options: {
      debugDrawMode: number
    } = {
      debugDrawMode: AmmoDebugConstants.DrawWireframe
    }
  ) {

    this.world = world
    //@ts-ignore
    this.debugDrawer = new window.Ammo.DebugDrawer()
    this.verticesArray = verticesArray
    this.colorsArray = colorsArray
    this.indexArray = indexArray

    this.debugDrawMode = options.debugDrawMode || AmmoDebugConstants.DrawWireframe

    this.index = 0
    if (this.indexArray) {
      Atomics.store(this.indexArray, 0, this.index)
    }

    this.enabled = false

    this.drawLine = this.drawLine.bind(this)
    this.drawContactPoint = this.drawContactPoint.bind(this)
    this.reportErrorWarning = this.reportErrorWarning.bind(this)
    this.draw3dText = this.draw3dText.bind(this)
    this.setDebugMode = this.setDebugMode.bind(this)
    this.getDebugMode = this.getDebugMode.bind(this)
    this.enable = this.enable.bind(this)
    this.disable = this.disable.bind(this)
    //@ts-ignore
    this.update = this.update.bind(this)

    //@ts-ignore
    this.debugDrawer.drawLine = this.drawLine
    //@ts-ignore
    this.debugDrawer.drawContactPoint = this.drawContactPoint.bind(this)
    this.debugDrawer.reportErrorWarning = this.reportErrorWarning.bind(this)
    this.debugDrawer.draw3dText = this.draw3dText.bind(this)
    this.debugDrawer.setDebugMode = this.setDebugMode.bind(this)
    this.debugDrawer.getDebugMode = this.getDebugMode.bind(this)
    //@ts-ignore
    this.debugDrawer.enable = this.enable.bind(this)
    //@ts-ignore
    this.debugDrawer.disable = this.disable.bind(this)
    //@ts-ignore
    this.debugDrawer.update = this.update.bind(this)
    //@ts-ignore
    this.world.setDebugDrawer(this.debugDrawer)
  }

  enable() {
    this.enabled = true
  }

  disable() {
    this.enabled = false
  }

  update()  {
    if (!this.enabled) {
      return
    }
  
    if (this.indexArray) {
      if (Atomics.load(this.indexArray, 0) === 0) {
        this.index = 0
        this.world.debugDrawWorld()
        Atomics.store(this.indexArray, 0, this.index)
      }
    } else {
      this.index = 0
      this.world.debugDrawWorld()
    }
  }

  //@ts-ignore
  drawLine(from: number, to: number, color: number) {
    //@ts-ignore
    const heap: Float32Array = window.Ammo.HEAPF32
    const r = heap[(color + 0) / 4]
    const g = heap[(color + 4) / 4]
    const b = heap[(color + 8) / 4]
  
    const fromX = heap[(from + 0) / 4]
    const fromY = heap[(from + 4) / 4]
    const fromZ = heap[(from + 8) / 4]
    setXYZ(this.verticesArray, this.index, fromX, fromY, fromZ)
    setXYZ(this.colorsArray, this.index++, r, g, b)
  
    const toX = heap[(to + 0) / 4]
    const toY = heap[(to + 4) / 4]
    const toZ = heap[(to + 8) / 4]
    setXYZ(this.verticesArray, this.index, toX, toY, toZ)
    setXYZ(this.colorsArray, this.index++, r, g, b)
  }

  //TODO: figure out how to make lifeTime work
  //@ts-ignore
  drawContactPoint(
    pointOnB: number, normalOnB: number, distance: number, _lifeTime: number, color: number
  ) {
    //@ts-ignore
    const heap: Float32Array = window.Ammo.HEAPF32
    const r = heap[(color + 0) / 4]
    const g = heap[(color + 4) / 4]
    const b = heap[(color + 8) / 4]
  
    const x = heap[(pointOnB + 0) / 4]
    const y = heap[(pointOnB + 4) / 4]
    const z = heap[(pointOnB + 8) / 4]
    setXYZ(this.verticesArray, this.index, x, y, z)
    setXYZ(this.colorsArray, this.index++, r, g, b)
  
    const dx = heap[(normalOnB + 0) / 4] * distance
    const dy = heap[(normalOnB + 4) / 4] * distance
    const dz = heap[(normalOnB + 8) / 4] * distance
    setXYZ(this.verticesArray, this.index, x + dx, y + dy, z + dz)
    setXYZ(this.colorsArray, this.index++, r, g, b)
  }

  reportErrorWarning(warningString: string) {
    if (Ammo.hasOwnProperty("UTF8ToString")) {
      //@ts-ignore
      console.warn(window.Ammo.UTF8ToString(warningString))
    } else if (!this.warnedOnce) {
      this.warnedOnce = true
      console.warn("Cannot print warningString, please export UTF8ToString from Ammo.js in make.py")
    }
  }

  draw3dText = function(_location: Ammo.btVector3, textString: string) {
    //TODO
    console.warn("TODO: draw3dText");
  }

  setDebugMode(debugMode: number) {
    this.debugDrawMode = debugMode
  }

  getDebugMode() {
    return this.debugDrawMode
  }
}

BabylonAmmoDebugDrawer.ts

import { Color4, Mesh, Particle, PointsCloudSystem, Scene, Vector3 } from "@babylonjs/core"
import Ammo from "ammo.js"

import { AmmoDebugDrawer } from './AmmoDebugDrawer.ts'

export class BabylonAmmoDebugDrawer extends AmmoDebugDrawer {
  pcs: PointsCloudSystem
  _pcsMesh: Mesh
  points: Vector3[]
  scene: Scene
  seenPoints: { [key: string]: number }
  pointPairs: number[][]
  _drawCount: number // useful info 
  _pcsCount: number  // for development
  _pointSize: number
  _color1: Color4
  _color2: Color4
  _pointsPerLine: number
  _alternate: number
  _pointsDrawn: number
  _updateAfter: number
  _updateTimer: number | null

  constructor(
    indexArray: Uint32Array,
    verticesArray: Float32Array,
    colorsArray: Float32Array,
    world: Ammo.btDiscreteDynamicsWorld,
    scene: Scene,
    options: {
      debugDrawMode: number
    } = {
      debugDrawMode: AmmoDebugConstants.DrawWireframe
    }
  ) {
    super(
      indexArray,
      verticesArray,
      colorsArray,
      world,
      options,
    )

    this.scene = scene
    this.seenPoints = {}
    this.pointPairs = []
    this._drawCount = 0
    this._pointSize = 2
    this._color1 = new Color4(1, 0, 0, 0.5)
    this._color2 = new Color4(0, 0, 1, 0.5)
    this._alternate = 0
    this._pointsDrawn = 0
    this._pointsPerLine = 2
    this._pcsCount = 0
    this._updateAfter = 0
  }

  update() {
    // reset everything
    this.seenPoints = {}
    this.pointPairs = []
    super.update()
  }

  drawLine(from: number, to: number, color: number) {
    super.drawLine(from, to, color)
    this._drawCount += 1
    const seenPoints = this.seenPoints
    //@ts-ignore
    const heap: Float32Array = window.Ammo.HEAPF32

    const fromX = heap[(from + 0) / 4]
    const fromY = heap[(from + 4) / 4]
    const fromZ = heap[(from + 8) / 4]

    const toX = heap[(to + 0) / 4]
    const toY = heap[(to + 4) / 4]
    const toZ = heap[(to + 8) / 4]


    if (seenPoints[`${fromX}-${fromY}-${fromZ}`] === undefined) {
      this.pointPairs.push([fromX, fromY, fromZ, toX, toY, toZ])
      seenPoints[`${fromX}-${fromY}-${fromZ}`] = this.pointPairs.length - 1
    } else if (seenPoints[`${toX}-${toY}-${toZ}`] === undefined) {
      const ind = seenPoints[`${fromX}-${fromY}-${fromZ}`]
      this.pointPairs[ind] = [fromX, fromY, fromZ, toX, toY, toZ]
    }
  }

  drawPhysWorld() {
    if (this._pcsMesh) {
      this._pcsMesh.dispose()
    }

    this.pcs = new PointsCloudSystem(
      `pcs-world-view-${this._pcsCount}`, this._pointSize, this.scene
    )
    this._pcsCount += 1

    const numPoints = this._pointsPerLine

    for (let i = 0; i < this.pointPairs.length; i++) {
      const fromX = this.pointPairs[i][0]
      const fromY = this.pointPairs[i][1]
      const fromZ = this.pointPairs[i][2]

      const toX = this.pointPairs[i][3]
      const toY = this.pointPairs[i][4]
      const toZ = this.pointPairs[i][5]
  
      let dX = (toX - fromX) / numPoints
      let dY = (toY - fromY) / numPoints
      let dZ = (toZ - fromZ) / numPoints

      if (numPoints === 1) {
        /* take average instead */
        dX *= 0.5
        dY *= 0.5
        dZ *= 0.5
      }

      const myFunc = (particle: Particle, _i: number, s: number) => {
        this._pointsDrawn += 1
        let usedColor = this._color1

        if (this._alternate && (this._pointsDrawn % (this._alternate + 1)) === 0) {
          usedColor = this._color2 //@ts-ignore
        }
        const pos = new Vector3(
          dX * s + fromX,
          dY * s + fromY,
          dZ * s + fromZ,
        )
        particle.position = pos
        particle.color = usedColor
      }

      this.pcs.addPoints(numPoints, myFunc)
    }
    
    return this.pcs.buildMeshAsync().then((mesh) => {
      this._pcsMesh = mesh
      this._pcsMesh.hasVertexAlpha = true
      this._pcsMesh.isPickable = false
      return mesh
    })
  }

  setRenderPointSize(size: number) {
    this._pointSize = size
  }

  setColor(color: Color4) {
    this._color1 = color.clone()
  }

  setSecondaryColor(color: Color4) {
    this._color2 = color.clone()
  }


  setAlternate(switchEveryNPoints: number) {
    this._alternate = switchEveryNPoints
  }

  setInterpolationDensity(pointsPerLine: number) {
    this._pointsPerLine = pointsPerLine
  }

  /* period in miliseconds */
  setUpdatePeriod(updateAfter: number) {
    this._updateAfter = updateAfter

    if (this._updateTimer) {
      window.clearInterval(this._updateTimer)
    }

    if (updateAfter === 0) {
      return
    }

    this._updateTimer = window.setInterval(() => {
      this.update()
      this.drawPhysWorld()
    }, this._updateAfter)
  }
}

Example.js

/* Run this in your brower tab servering your project where you've setup your physics scene */
var DefaultBufferSize = 3 * 1000000
var debugVertices = new Float32Array(DefaultBufferSize)
var debugColors = new Float32Array(DefaultBufferSize)
var physicsWorld = ammojsPlugin.world

var debugDrawer = new BabylonAmmoDebugDrawer(null, debugVertices, debugColors, physicsWorld, scene);
debugDrawer.enable()
debugDrawer.update()
debugDrawer.setAlternate(1)
debugDrawer.setColor(new BABYLON.Color4(0, 0, 1, 0.5))
debugDrawer.setSecondaryColor(new BABYLON.Color4(0, 1, 0, 0.5))
debugDrawer.setInterpolationDensity(5)
debugDrawer.setRenderPointSize(3)
debugDrawer.setUpdatePeriod(3 * 1000)
debugDrawer.drawPhysWorld().then((mesh) => {
  console.log('All done rendring physics world.')
})

Link_to_ammo_js_build.txt

https://ammojs.s3.amazonaws.com/builds.zip