@c-frame/physx/src/physics.js

Physics for A-Frame using Nvidia PhysX

// This is a modification of the physics/PhysX libraries
//   created by Lee Stemkoski
//   from the VARTISTE project @ https://vartiste.xyz/ 
//   by Zachary Capalbo https://github.com/zach-capalbo/vartiste
// with the goal of creating a simplified standalone codebase.
// Further performance modifications by Diarmid Mackenzie.

// original documentation: https://vartiste.xyz/docs.html#physics.js

// Came via: https://github.com/stemkoski/A-Frame-Examples/blob/66f05fe5cf89879996f1f6a4c0475ce475e8796a/js/physics.js
// and then via: https://github.com/diarmidmackenzie/christmas-scene/blob/a94ae7e7167937f10d34df8429fb71641e343bb1/lib/physics.js
// ======================================================================

let PHYSX = require('./physx.release.js');

// patching in Pool functions
var poolSize = 0

function sysPool(name, type) {
    if (this.system._pool[name]) return this.system._pool[name]
    this.system._pool[name] = new type()
    // console.log("SysPooling", type.name)
    return this.system._pool[name]
}

function pool(name, type) {
    if (this._pool[name]) return this._pool[name]
    this._pool[name] = new type()
    // console.log("Pooling", type.name)
    return this._pool[name]
}

class Pool {
  static init(where, {useSystem = false} = {}) {
    if (useSystem)
    {
      if (!where.system) {
        console.error("No system for system pool", where.attrName)
      }
      if (!where.system._pool) where.system._pool = {};

      where.pool = sysPool;
    }
    else
    {
      where._pool = {}
      where.pool = pool;
    }
  }
}

// ==================================================================================================

// patching in required Util functions from VARTISTE

Util = {}

Pool.init(Util);

// Copies `matrix` into `obj`'s (a `THREE.Object3D`) `matrix`, and decomposes
// it to `obj`'s position, rotation, and scale
Util.applyMatrix = function(matrix, obj) {
  obj.matrix.copy(matrix)
  matrix.decompose(obj.position, obj.rotation, obj.scale)
}

Util.traverseCondition = function(obj3D, condition, fn) 
{
  if (!condition(obj3D)) return;

  fn(obj3D)
  for (let c of obj3D.children)
  {
    this.traverseCondition(c, condition, fn)
  }
}

Util.positionObject3DAtTarget = function(obj, target, {scale, transformOffset, transformRoot} = {}) 
{
  if (typeof transformRoot === 'undefined') transformRoot = obj.parent

  target.updateWorldMatrix()
  let destMat = this.pool('dest', THREE.Matrix4)
  destMat.copy(target.matrixWorld)

  if (transformOffset) {
    let transformMat = this.pool('transformMat', THREE.Matrix4)
    transformMat.makeTranslation(transformOffset.x, transformOffset.y, transformOffset.z)
    destMat.multiply(transformMat)
  }

  if (scale) {
    let scaleVect = this.pool('scale', THREE.Vector3)
    scaleVect.setFromMatrixScale(destMat)
    scaleVect.set(scale.x / scaleVect.x, scale.y / scaleVect.y, scale.z / scaleVect.z)
    destMat.scale(scaleVect)
  }

  let invMat = this.pool('inv', THREE.Matrix4)

  transformRoot.updateWorldMatrix()
  invMat.copy(transformRoot.matrixWorld).invert()
  destMat.premultiply(invMat)

  Util.applyMatrix(destMat, obj)
}

// untested functions

// Executes function `fn` when `entity` has finished loading, or immediately
// if it has already loaded. `entity` may be a single `a-entity` element, or
// an array of `a-entity` elements. If `fn` is not provided, it will return a
// `Promise` that will resolve when `entity` is loaded (or immediately if
// `entity` is already loaded).
Util.whenLoaded = function(entity, fn) {
  if (Array.isArray(entity) && fn) return whenLoadedAll(entity, fn)
  if (Array.isArray(entity)) return awaitLoadingAll(entity)
  if (fn) return whenLoadedSingle(entity, fn)
  return awaitLoadingSingle(entity)
}

function whenLoadedSingle(entity, fn) {
  if (entity.hasLoaded)
  {
    fn()
  }
  else
  {
    entity.addEventListener('loaded', fn)
  }
}

function whenLoadedAll(entities, fn) {
  let allLoaded = entities.map(() => false)
  for (let i = 0; i < entities.length; ++i)
  {
    let ii = i
    let entity = entities[ii]
    whenLoadedSingle(entity, () => {
      allLoaded[ii] = true
      if (allLoaded.every(t => t)) fn()
    })
  }
}

function awaitLoadingSingle(entity) {
  return new Promise((r, e) => whenLoadedSingle(entity, r))
}

async function awaitLoadingAll(entities) {
  for (let entity of entities)
  {
    await awaitLoadingSingle(entity)
  }
}

Util.whenComponentInitialized = function(el, component, fn) {
  if (el && el.components[component] && el.components[component].initialized) {
    return Promise.resolve(fn ? fn() : undefined)
  }

  return new Promise((r, e) => {
    if (el && el.components[component] && el.components[component].initialized) {
      return Promise.resolve(fn ? fn() : undefined)
    }

    let listener = (e) => {
      if (e.detail.name === component) {
        el.removeEventListener('componentinitialized', listener);
        if (fn) fn();
        r();
      }
    };
    el.addEventListener('componentinitialized', listener)
  })
}

// ========================================================================================

// Extra utility functions for dealing with PhysX

const PhysXUtil = {
    // Gets the world position transform of the given object3D in PhysX format
    object3DPhysXTransform: (() => {
      let pos = new THREE.Vector3();
      let quat = new THREE.Quaternion();
      return function (obj) {
        obj.getWorldPosition(pos);
        obj.getWorldQuaternion(quat);

        return {
          translation: {
            x: pos.x,
            y: pos.y,
            z: pos.z,
          },
          rotation: {
            w: quat.w, // PhysX uses WXYZ quaternions,
            x: quat.x,
            y: quat.y,
            z: quat.z,
          },
        }
      }
    })(),

  // Converts a THREE.Matrix4 into a PhysX transform
  matrixToTransform: (() => {
      let pos = new THREE.Vector3();
      let quat = new THREE.Quaternion();
      let scale = new THREE.Vector3();
      let scaleInv = new THREE.Matrix4();
      let mat2 = new THREE.Matrix4();
      return function (matrix) {
        matrix.decompose(pos, quat, scale);

        return {
          translation: {
            x: pos.x,
            y: pos.y,
            z: pos.z,
          },
          rotation: {
            w: quat.w, // PhysX uses WXYZ quaternions,
            x: quat.x,
            y: quat.y,
            z: quat.z,
          },
        }
      }
    })(),

  // Converts an arry of layer numbers to an integer bitmask
  layersToMask: (() => {
    let layers = new THREE.Layers();
    return function(layerArray) {
      layers.disableAll();
      for (let layer of layerArray)
      {
          layers.enable(parseInt(layer));
      }
      return layers.mask;
    };
  })(),

  axisArrayToEnums: function(axes) {
    let enumAxes = []
    for (let axis of axes)
    {
      if (axis === 'swing') {
        enumAxes.push(PhysX.PxD6Axis.eSWING1)
        enumAxes.push(PhysX.PxD6Axis.eSWING2)
        continue
      }
      let enumKey = `e${axis.toUpperCase()}`
      if (!(enumKey in PhysX.PxD6Axis))
      {
        console.warn(`Unknown axis ${axis} (PxD6Axis::${enumKey})`)
      }
      enumAxes.push(PhysX.PxD6Axis[enumKey])
    }
    return enumAxes;
  }
};

let PhysX

// Implements the a physics system using an emscripten compiled PhysX engine.
//
//
// If `autoLoad` is `true`, or when you call `startPhysX`, the `physx` system will
// automatically load and initialize the physics system with reasonable defaults
// and a ground plane. All you have to do is add [`physx-body`](#physx-body) to
// the bodies that you want to be part of the simulation. The system will take
// try to take care of things like collision meshes, position updates, etc
// automatically.  The simplest physics scene looks something like:
//
//```
// <a-scene physx="autoLoad: true">
//  <a-assets><a-asset-item id="#mymodel" src="..."></a-asset-item></a-assets>
//
//  <a-box physx-body="type: static" color="green" position="0 0 -3"></a-box>
//  <a-sphere physx-body="type: dynamic" position="0.4 2 -3" color="blue"></a-sphere>
//  <a-entity physx-body="type: dynamic" position="0 5 -3" gltf-model="#mymodel"></a-entity>
// </a-scene>
//```
//
// If you want a little more control over how things behave, you can set the
// [`physx-material`](#physx-material) component on the objects in your
// simulation, or use [`physx-joint`s](#physx-joint),
// [`physx-constraint`s](#physx-constraint) and [`physx-driver`s](#physx-driver)
// to add some complexity to your scene.
//
// If you need more low-level control, the PhysX bindings are exposed through
// the `PhysX` property of the system. So for instance, if you wanted to make
// use of the [`PxCapsuleGeometry`](https://gameworksdocs.nvidia.com/PhysX/4.1/documentation/physxapi/files/classPxCapsuleGeometry.html)
// in your own component, you would call:
//
//```
//    let myGeometry = new this.el.sceneEl.PhysX.PxCapsuleGeometry(1.0, 2.0)
//```
//
// The system uses [my fork](https://github.com/zach-capalbo/PhysX) of PhysX, built using the [Docker Wrapper](https://github.com/ashconnell/physx-js). To see what's exposed to JavaScript, see [PxWebBindings.cpp](https://github.com/zach-capalbo/PhysX/blob/emscripten_wip/physx/source/physxwebbindings/src/PxWebBindings.cpp)
//
// For a complete example of how to use this, you can see the
// [aframe-vartiste-toolkit Physics
// Playground](https://glitch.com/edit/#!/fascinated-hip-period?path=index.html)
//
// It is also helpful to refer to the [NVIDIA PhysX
// documentation](https://nvidiagameworks.github.io/PhysX/4.1/documentation/physxguide/Index.html)
AFRAME.registerSystem('physx', {
  schema: {
    // Amount of time to wait after loading before starting the physics. Can be
    // useful if there is still some things loading or initializing elsewhere in
    // the scene
    delay: {default: 5000},

    // Throttle for running the physics simulation. On complex scenes, you can
    // increase this to avoid dropping video frames
    throttle: {default: 10},

    // If true, the PhysX will automatically be loaded and started. If false,
    // you will have to call `startPhysX()` manually to load and start the
    // physics engine
    autoLoad: {default: false},

    // Simulation speed multiplier. Increase or decrease to speed up or slow
    // down simulation time
    speed: {default: 1.0},

    // URL for the PhysX WASM bundle.
    wasmUrl: {default: "../../wasm/physx.release.wasm"},

    // If true, sets up a default scene with a ground plane and bounding
    // cylinder.
    useDefaultScene: {default: true},

    // NYI
    wrapBounds: {default: false},

    // Which collision layers the ground belongs to
    groundCollisionLayers: {default: [2]},

    // Which collision layers will collide with the ground
    groundCollisionMask: {default: [1,2,3,4]},

    // Global gravity vector
    gravity: {type: 'vec3', default: {x: 0, y: -9.8, z: 0}},

    // Whether to output stats, and how to output them.  One or more of "console", "events", "panel"
    stats: {type: 'array', default: []}
  },
  init() {
    this.PhysXUtil = PhysXUtil;

    // for logging.
    this.cumTimeEngine = 0;
    this.cumTimeWrapper = 0;
    this.tickCounter = 0;


    this.objects = new Map();
    this.shapeMap = new Map();
    this.jointMap = new Map();
    this.boundaryShapes = new Set();
    this.worldHelper = new THREE.Object3D();
    this.el.object3D.add(this.worldHelper);
    this.tock = AFRAME.utils.throttleTick(this.tock, this.data.throttle, this)
    this.collisionObject = {thisShape: null, otherShape:null, points: [], impulses: [], otherComponent: null};

    let defaultTarget = document.createElement('a-entity')
    this.el.append(defaultTarget)
    this.defaultTarget = defaultTarget

    this.initializePhysX = new Promise((r, e) => {
      this.fulfillPhysXPromise = r;
    })

    this.initStats()

    this.el.addEventListener('inspectortoggle', (e) => {
      console.log("Inspector toggle", e)
      if (e.detail === true)
      {
          this.running = false
      }
    })
  },

  initStats() {
    // Data used for performance monitoring.
    this.statsToConsole = this.data.stats.includes("console")
    this.statsToEvents = this.data.stats.includes("events")
    this.statsToPanel = this.data.stats.includes("panel")

    this.bodyTypeToStatsPropertyMap = {
      "static": "staticBodies",
      "dynamic": "dynamicBodies",
      "kinematic": "kinematicBodies",
    }

    if (this.statsToConsole || this.statsToEvents || this.statsToPanel) {
      this.trackPerf = true;
      this.tickCounter = 0;
      this.statsTickData = {};
      this.statsBodyData = {};

      const scene = this.el.sceneEl;
      scene.setAttribute("stats-collector", `inEvent: physics-tick-data;
                                             properties: engine, after, total;
                                             outputFrequency: 100;
                                             outEvent: physics-tick-summary;
                                             outputs: percentile__50, percentile__90, max`);
    }

    if (this.statsToPanel) {
      const scene = this.el.sceneEl;
      const space = "&nbsp&nbsp&nbsp"
    
      scene.setAttribute("stats-panel", "")
      scene.setAttribute("stats-group__bodies", `label: Physics Bodies`)
      scene.setAttribute("stats-row__b1", `group: bodies;
                                           event:physics-body-data;
                                           properties: staticBodies;
                                           label: Static`)
      scene.setAttribute("stats-row__b2", `group: bodies;
                                           event:physics-body-data;
                                           properties: dynamicBodies;
                                           label: Dynamic`)
      scene.setAttribute("stats-row__b3", `group: bodies;
                                           event:physics-body-data;
                                           properties: kinematicBodies;
                                           label: Kinematic`)
      scene.setAttribute("stats-group__tick", `label: Physics Ticks: Median${space}90th%${space}99th%`)
      scene.setAttribute("stats-row__1", `group: tick; 
                                          event:physics-tick-summary; 
                                          properties: engine.percentile__50, 
                                                      engine.percentile__90, 
                                                      engine.max;
                                          label: Engine`)
      scene.setAttribute("stats-row__2", `group: tick;
                                          event:physics-tick-summary;
                                          properties: after.percentile__50, 
                                                      after.percentile__90, 
                                                      after.max; 
                                          label: After`)

      scene.setAttribute("stats-row__3", `group: tick;
                                          event:physics-tick-summary;
                                          properties: total.percentile__50, 
                                                      total.percentile__90, 
                                                      total.max;
                                          label: Total`)
    }
  },
  findWasm() {
    return this.data.wasmUrl;
  },
  // Loads PhysX and starts the simulation
  async startPhysX() {
    this.running = true;
    let self = this;
    let resolveInitialized;
    let initialized = new Promise((r, e) => resolveInitialized = r)
    let instance = PHYSX({
        locateFile() {
            return self.findWasm()
        },
        onRuntimeInitialized() {
          resolveInitialized();
        }
      });
    if (instance instanceof Promise) instance = await instance;
    this.PhysX = instance;
    PhysX = instance;
    await initialized;
    self.startPhysXScene()
    self.physXInitialized = true
    self.fulfillPhysXPromise()
    self.el.emit('physx-started', {})
  },
  startPhysXScene() {
    console.info("Starting PhysX scene")
    const foundation = PhysX.PxCreateFoundation(
      PhysX.PX_PHYSICS_VERSION,
      new PhysX.PxDefaultAllocator(),
      new PhysX.PxDefaultErrorCallback()
    );
    this.foundation = foundation
    const physxSimulationCallbackInstance = PhysX.PxSimulationEventCallback.implement({
      onContactBegin: (shape0, shape1, points, impulses) => {
        let c0 = this.shapeMap.get(shape0.$$.ptr)
        let c1 = this.shapeMap.get(shape1.$$.ptr)

        if (c1 === c0) return;

        if (c0 && c0.data.emitCollisionEvents) {
          this.collisionObject.thisShape = shape0
          this.collisionObject.otherShape = shape1
          this.collisionObject.points = points
          this.collisionObject.impulses = impulses
          this.collisionObject.otherComponent = c1
          c0.el.emit('contactbegin', this.collisionObject)
        }

        if (c1 && c1.data.emitCollisionEvents) {
          this.collisionObject.thisShape = shape1
          this.collisionObject.otherShape = shape0
          this.collisionObject.points = points
          this.collisionObject.impulses = impulses
          this.collisionObject.otherComponent = c0
          c1.el.emit('contactbegin', this.collisionObject)
        }
      },
      onContactEnd: (shape0, shape1) => {
          let c0 = this.shapeMap.get(shape0.$$.ptr)
          let c1 = this.shapeMap.get(shape1.$$.ptr)

          if (c1 === c0) return;

        if (c0 && c0.data.emitCollisionEvents) {
          this.collisionObject.thisShape = shape0
          this.collisionObject.otherShape = shape1
          this.collisionObject.points = null
          this.collisionObject.impulses = null
          this.collisionObject.otherComponent = c1
          c0.el.emit('contactend', this.collisionObject)
        }

        if (c1 && c1.data.emitCollisionEvents) {
          this.collisionObject.thisShape = shape1
          this.collisionObject.otherShape = shape0
          this.collisionObject.points = null
          this.collisionObject.impulses = null
          this.collisionObject.otherComponent = c0
          c1.el.emit('contactend', this.collisionObject)
        }
      },
      onContactPersist: () => {},
      onTriggerBegin: () => {},
      onTriggerEnd: () => {},
      onConstraintBreak: (joint) => {
        let component = this.jointMap.get(joint.$$.ptr);

        if (!component) return;

        component.el.emit('constraintbreak', {})
      },
    });
    let tolerance = new PhysX.PxTolerancesScale();
    // tolerance.length /= 10;
    // console.log("Tolerances", tolerance.length, tolerance.speed);
    this.physics = PhysX.PxCreatePhysics(
      PhysX.PX_PHYSICS_VERSION,
      foundation,
      tolerance,
      false,
      null
    )
    PhysX.PxInitExtensions(this.physics, null);

    this.cooking = PhysX.PxCreateCooking(
      PhysX.PX_PHYSICS_VERSION,
      foundation,
      new PhysX.PxCookingParams(tolerance)
    )

    const sceneDesc = PhysX.getDefaultSceneDesc(
      this.physics.getTolerancesScale(),
      0,
      physxSimulationCallbackInstance
    )
    this.scene = this.physics.createScene(sceneDesc)

    this.setupDefaultEnvironment()
  },
  setupDefaultEnvironment() {
    this.defaultActorFlags = new PhysX.PxShapeFlags(
      PhysX.PxShapeFlag.eSCENE_QUERY_SHAPE.value |
        PhysX.PxShapeFlag.eSIMULATION_SHAPE.value
    )
    this.defaultFilterData = new PhysX.PxFilterData(PhysXUtil.layersToMask(this.data.groundCollisionLayers), PhysXUtil.layersToMask(this.data.groundCollisionMask), 0, 0);

    this.scene.setGravity(this.data.gravity)

    if (this.data.useDefaultScene)
    {
      this.createGroundPlane()
      this.createBoundingCylinder()
    }


    this.defaultTarget.setAttribute('physx-body', 'type', 'static')

  },
  createGroundPlane() {
    let geometry = new PhysX.PxPlaneGeometry();
    // let geometry = new PhysX.PxBoxGeometry(10, 1, 10);
    let material = this.physics.createMaterial(0.8, 0.8, 0.1);

    const shape = this.physics.createShape(geometry, material, false, this.defaultActorFlags)
    shape.setQueryFilterData(this.defaultFilterData)
    shape.setSimulationFilterData(this.defaultFilterData)
        const transform = {
      translation: {
        x: 0,
        y: 0,
        z: -5,
      },
      rotation: {
        w: 0.707107, // PhysX uses WXYZ quaternions,
        x: 0,
        y: 0,
        z: 0.707107,
      },
    }
    let body = this.physics.createRigidStatic(transform)
    body.attachShape(shape)
    this.scene.addActor(body, null)
    this.ground = body
    this.rigidBody = body
  },
  createBoundingCylinder() {
    const numPlanes = 16
    let geometry = new PhysX.PxPlaneGeometry();
    let material = this.physics.createMaterial(0.1, 0.1, 0.8);
    let spherical = new THREE.Spherical();
    spherical.radius = 30;
    let quat = new THREE.Quaternion();
    let pos = new THREE.Vector3;
    let euler = new THREE.Euler();

    for (let i = 0; i < numPlanes; ++i)
    {
      spherical.theta = i * 2.0 * Math.PI / numPlanes;
      pos.setFromSphericalCoords(spherical.radius, spherical.theta, spherical.phi)
      pos.x = - pos.y
      pos.y = 0;
      euler.set(0, spherical.theta, 0);
      quat.setFromEuler(euler)

      const shape = this.physics.createShape(geometry, material, false, this.defaultActorFlags)
      shape.setQueryFilterData(this.defaultFilterData)
      shape.setSimulationFilterData(this.defaultFilterData)
      const transform = {
        translation: {
          x: pos.x,
          y: pos.y,
          z: pos.z,
        },
        rotation: {
          w: quat.w, // PhysX uses WXYZ quaternions,
          x: quat.x,
          y: quat.y,
          z: quat.z,
        },
      }
      this.boundaryShapes.add(shape.$$.ptr)
      let body = this.physics.createRigidStatic(transform)
      body.attachShape(shape)
      this.scene.addActor(body, null)
    }
  },
  async registerComponentBody(component, {type}) {
    await this.initializePhysX;

    // const shape = this.physics.createShape(geometry, material, false, flags)
    const transform = PhysXUtil.object3DPhysXTransform(component.el.object3D);

    let body
    if (type === 'dynamic' || type === 'kinematic')
    {
      body = this.physics.createRigidDynamic(transform)

      // body.setRigidBodyFlag(PhysX.PxRigidBodyFlag.eENABLE_CCD, true);
      // body.setMaxContactImpulse(1e2);
    }
    else
    {
      body = this.physics.createRigidStatic(transform)
    }

    let attemptToUseDensity = true;
    let seenAnyDensity = false;
    let densities = new PhysX.VectorPxReal()
    for (let shape of component.createShapes(this.physics, this.defaultActorFlags))
    {
      body.attachShape(shape)

      if (isFinite(shape.density))
      {
        seenAnyDensity = true
        densities.push_back(shape.density)
      }
      else
      {
        attemptToUseDensity = false

        if (seenAnyDensity)
        {
          console.warn("Densities not set for all shapes. Will use total mass instead.", component.el)
        }
      }
    }
    if (type === 'dynamic' || type === 'kinematic') {
      if (attemptToUseDensity && seenAnyDensity)
      {
        console.log("Setting density vector", densities)
        body.updateMassAndInertia(densities)
      }
      else {
        body.setMassAndUpdateInertia(component.data.mass)
      }
    }
    densities.delete()
    this.scene.addActor(body, null)
    this.objects.set(component.el.object3D, body)
    component.rigidBody = body
  },
  registerShape(shape, component) {
    this.shapeMap.set(shape.$$.ptr, component);
  },
  registerJoint(joint, component) {
    this.jointMap.set(joint.$$.ptr, component);
  },
  removeBody(component) {
    let body = component.rigidBody
    this.objects.delete(component.el.object3D)
    body.release()
  },
  tock(t, dt) {
    if (t < this.data.delay) return
    if (!this.physXInitialized && this.data.autoLoad && !this.running) this.startPhysX()
    if (!this.physXInitialized) return
    if (!this.running) return

    const engineStartTime = performance.now();

    this.scene.simulate(THREE.MathUtils.clamp(dt * this.data.speed / 1000, 0, 0.03 * this.data.speed), true)
    //this.scene.simulate(0.02, true) // (experiment with fixed interval)
    this.scene.fetchResults(true)

    const engineEndTime = performance.now();

    for (let [obj, body] of this.objects)
    {
        // no updates needed for static objects.
        if (obj.el.components['physx-body'].data.type === 'static') continue;

        const transform = body.getGlobalPose()
        this.worldHelper.position.copy(transform.translation);
        this.worldHelper.quaternion.copy(transform.rotation);
        obj.getWorldScale(this.worldHelper.scale)
        Util.positionObject3DAtTarget(obj, this.worldHelper);
    }

    if (this.trackPerf) {
      const afterEndTime = performance.now();

      this.statsTickData.engine = engineEndTime - engineStartTime
      this.statsTickData.after = afterEndTime - engineEndTime
      this.statsTickData.total = afterEndTime - engineStartTime
      this.el.emit("physics-tick-data", this.statsTickData)

      this.tickCounter++;

      if (this.tickCounter === 100) {

        this.countBodies()

        if (this.statsToConsole) {
          console.log("Physics tick stats:", this.statsData)
        }

        if (this.statsToEvents || this.statsToPanel) {
          this.el.emit("physics-body-data", this.statsBodyData)
        }

        this.tickCounter = 0;
      }
    }
  },

  countBodies() {

    // Aditional statistics beyond simple body counts should be possible.
    // They could be accessed via PxScene::getSimulationStatistics()
    // https://nvidiagameworks.github.io/PhysX/4.1/documentation/physxguide/Manual/Statistics.html
    // https://nvidiagameworks.github.io/PhysX/4.1/documentation/physxapi/files/classPxSimulationStatistics.html
    // However this part of the API is not yet exposed in the
    // WASM PhysX build we are using
    // See: https://github.com/zach-capalbo/PhysX/blob/emscripten_wip/physx/source/physxwebbindings/src/PxWebBindings.cpp
    
    const statsData = this.statsBodyData
    statsData.staticBodies = 0
    statsData.kinematicBodies = 0
    statsData.dynamicBodies = 0

    this.objects.forEach((pxBody, object3D) => {
      const el = object3D.el
      const type = el.components['physx-body'].data.type
      const property = this.bodyTypeToStatsPropertyMap[type]
      statsData[property]++
    })
  },
})

// Controls physics properties for individual shapes or rigid bodies. You can
// set this either on an entity with the `phyx-body` component, or on a shape or
// model contained in an entity with the `physx-body` component. If it's set on
// a `physx-body`, it will be the default material for all shapes in that body.
// If it's set on an element containing geometry or a model, it will be the
// material used for that shape only.
//
// For instance, in the following scene fragment:
//```
// <a-entity id="bodyA" physx-body physx-material="staticFriction: 0.5">
//   <a-box id="shape1" physx-material="staticFriction: 1.0"></a-box>
//   <a-sphere id="shape2"></a-sphere>
// </a-entity>
// <a-cone id="bodyB" physx-body></a-cone>
//```
//
// `shape1`, which is part of the `bodyA` rigid body, will have static friction
// of 1.0, since it has a material set on it. `shape2`, which is also part of
// the `bodyA` rigid body, will have a static friction of 0.5, since that is
// the body default. `bodyB` will have the component default of 0.2, since it is
// a separate body.
AFRAME.registerComponent('physx-material', {
  schema: {
    // Static friction
    staticFriction: {default: 0.2},
    // Dynamic friction
    dynamicFriction: {default: 0.2},
    // Restitution, or "bounciness"
    restitution: {default: 0.2},

    // Density for the shape. If densities are specified for _all_ shapes in a
    // rigid body, then the rigid body's mass properties will be automatically
    // calculated based on the different densities. However, if density
    // information is not specified for every shape, then the mass defined in
    // the overarching [`physx-body`](#physx-body) will be used instead.
    density: {type: 'number', default: NaN},

    // Which collision layers this shape is present on
    collisionLayers: {default: [1], type: 'array'},
    // Array containing all layers that this shape should collide with
    collidesWithLayers: {default: [1,2,3,4], type: 'array'},

    // If `collisionGroup` is greater than 0, this shape will *not* collide with
    // any other shape with the same `collisionGroup` value
    collisionGroup: {default: 0},

    // If >= 0, this will set the PhysX contact offset, indicating how far away
    // from the shape simulation contact events should begin.
    contactOffset: {default: -1.0},

    // If >= 0, this will set the PhysX rest offset
    restOffset: {default: -1.0},
  }
})

// Turns an entity into a PhysX rigid body. This is the main component for
// creating physics objects.
//
// **Types**
//
// There are 3 types of supported rigid bodies. The type can be set by using the
// `type` proeprty, but once initialized cannot be changed.
//
// - `dynamic` objects are objects that will have physics simulated on them. The
//   entity's world position, scale, and rotation will be used as the starting
//   condition for the simulation, however once the simulation starts the
//   entity's position and rotation will be replaced each frame with the results
//   of the simulation.
// - `static` objects are objects that cannot move. They cab be used to create
//   collidable objects for `dynamic` objects, or for anchor points for joints.
// - `kinematic` objects are objects that can be moved programmatically, but
//   will not be moved by the simulation. They can however, interact with and
//   collide with dynamic objects. Each frame, the entity's `object3D` will be
//   used to set the position and rotation for the simulation object.
//
// **Shapes**
//
// When the component is initialized, and on the `object3dset` event, all
// visible meshes that are descendents of this entity will have shapes created
// for them. Each individual mesh will have its own convex hull automatically
// generated for it. This means you can have reasonably accurate collision
// meshes both from building up shapes with a-frame geometry primitives, and
// from importing 3D models.
//
// Visible meshes can be excluded from this shape generation process by setting
// the `physx-no-collision` attribute on the corresponding `a-entity` element.
// Invisible meshes can be included into this shape generation process by
// settingt the `physx-hidden-collision` attribute on the corresponding
// `a-entity` element. This can be especially useful when using an external tool
// (like [Blender V-HACD](https://github.com/andyp123/blender_vhacd)) to create
// a low-poly convex collision mesh for a high-poly or concave mesh. This leads
// to this pattern for such cases:
//
// ```
//    <a-entity physx-body="type: dynamic">
//      <a-entity gltf-model="HighPolyOrConcaveURL.gltf" physx-no-collision=""></a-entity>
//      <a-entity gltf-model="LowPolyConvexURL.gltf" physx-hidden-collision="" visible="false"></a-entity>
//    </a-entity>
// ```
//
// Note, in such cases that if you are setting material properties on individual
// shapes, then the property should go on the collision mesh entity
//
// **Use with the [Manipulator](#manipulator) component**
//
// If a dynamic entity is grabbed by the [Manipulator](#manipulator) component,
// it will temporarily become a kinematic object. This means that collisions
// will no longer impede its movement, and it will track the manipulator
// exactly, (subject to any manipulator constraints, such as
// [`manipulator-weight`](#manipulator-weight)). If you would rather have the
// object remain dynamic, you will need to [redirect the grab](#redirect-grab)
// to a `physx-joint` instead, or even easier, use the
// [`dual-wieldable`](#dual-wieldable) component.
//
// As soon as the dynamic object is released, it will revert back to a dynamic
// object. Objects with the type `kinematic` will remain kinematic.
//
// Static objects should not be moved. If a static object can be the target of a
// manipulator grab (or any other kind of movement), it should be `kinematic`
// instead.
AFRAME.registerComponent('physx-body', {
  dependencies: ['physx-material'],
  schema: {
    // **[dynamic, static, kinematic]** Type of the rigid body to create
    type: {default: 'dynamic', oneOf: ['dynamic', 'static', 'kinematic']},

    // Total mass of the body
    mass: {default: 1.0},

    // If > 0, will set the rigid body's angular damping
    angularDamping: {default: 0.0},

    // If > 0, will set the rigid body's linear damping
    linearDamping: {default: 0.0},

    // If set to `true`, it will emit `contactbegin` and `contactend` events
    // when collisions occur
    emitCollisionEvents: {default: false},

    // If set to `true`, the object will receive extra attention by the
    // simulation engine (at a performance cost).
    highPrecision: {default: false},

    shapeOffset: {type: 'vec3', default: {x: 0, y: 0, z: 0}}
  },
  events: {
    stateadded: function(e) {
      if (e.detail === 'grabbed') {
        this.rigidBody.setRigidBodyFlag(PhysX.PxRigidBodyFlag.eKINEMATIC, true)
      }
    },
    stateremoved: function(e) {
      if (e.detail === 'grabbed') {
        if (this.floating) {
          this.rigidBody.setLinearVelocity({x: 0, y: 0, z: 0}, true)
        }
        if (this.data.type !== 'kinematic')
        {
          this.rigidBody.setRigidBodyFlag(PhysX.PxRigidBodyFlag.eKINEMATIC, false)
        }
      }
    },
    'bbuttonup': function(e) {
      this.toggleGravity()
    },
    componentchanged: function(e) {
      if (e.name === 'physx-material')
      {
        this.el.emit('object3dset', {})
      }
    },
    object3dset: function(e) {
      if (this.rigidBody) {
        for (let shape of this.shapes)
        {
            this.rigidBody.detachShape(shape, false)
        }

        let attemptToUseDensity = true;
        let seenAnyDensity = false;
        let densities = new PhysX.VectorPxReal()
        let component = this
        let type = this.data.type
        let body = this.rigidBody
        for (let shape of component.createShapes(this.system.physics, this.system.defaultActorFlags))
        {
          body.attachShape(shape)

          if (isFinite(shape.density))
          {
            seenAnyDensity = true
            densities.push_back(shape.density)
          }
          else
          {
            attemptToUseDensity = false

            if (seenAnyDensity)
            {
              console.warn("Densities not set for all shapes. Will use total mass instead.", component.el)
            }
          }
        }
        if (type === 'dynamic' || type === 'kinematic') {
          if (attemptToUseDensity && seenAnyDensity)
          {
            console.log("Setting density vector", densities)
            body.updateMassAndInertia(densities)
          }
          else {
            body.setMassAndUpdateInertia(component.data.mass)
          }
        }
      }
    },
    contactbegin: function(e) {
      // console.log("Collision", e.detail.points)
    }
  },
  init() {
    this.system = this.el.sceneEl.systems.physx
    this.physxRegisteredPromise = this.system.registerComponentBody(this, {type: this.data.type})
    this.el.setAttribute('grab-options', 'scalable', false)

    this.kinematicMove = this.kinematicMove.bind(this)
    if (this.el.sceneEl.systems['button-caster'])
    {
      this.el.sceneEl.systems['button-caster'].install(['bbutton'])
    }

    this.physxRegisteredPromise.then(() => this.update())
  },
  update(oldData) {
    if (!this.rigidBody) return;

    if (this.data.type === 'dynamic')
    {
      this.rigidBody.setAngularDamping(this.data.angularDamping)
      this.rigidBody.setLinearDamping(this.data.linearDamping)
      this.rigidBody.setRigidBodyFlag(PhysX.PxRigidBodyFlag.eKINEMATIC, false)
    }

    if (this.data.highPrecision)
    {
      if (this.data.type === 'dynamic') {
        this.rigidBody.setSolverIterationCounts(4, 2);
        this.rigidBody.setRigidBodyFlag(PhysX.PxRigidBodyFlag.eENABLE_CCD, true)
      }
      else if (this.data.type === 'kinematic') {
        this.rigidBody.setSolverIterationCounts(4, 2);
        this.rigidBody.setRigidBodyFlag(PhysX.PxRigidBodyFlag.eENABLE_SPECULATIVE_CCD, true);
      }
    }

    if (!oldData || this.data.mass !== oldData.mass) this.el.emit('object3dset', {})
  },
  remove() {
    if (!this.rigidBody) return;
    this.system.removeBody(this)
  },
  createGeometry(o) {
    if (o.el.hasAttribute('geometry'))
    {
      let geometry = o.el.getAttribute('geometry');
      switch(geometry.primitive)
      {
        case 'sphere':
          return new PhysX.PxSphereGeometry(geometry.radius * this.el.object3D.scale.x * 0.98)
        case 'box':
          return new PhysX.PxBoxGeometry(geometry.width / 2, geometry.height / 2, geometry.depth / 2)
        default:
          return this.createConvexMeshGeometry(o.el.getObject3D('mesh'));
      }
    }
  },
  createConvexMeshGeometry(mesh, rootAncestor) {
    let vectors = new PhysX.PxVec3Vector()

    let g = mesh.geometry.attributes.position
    if (!g) return;
    if (g.count < 3) return;
    if (g.itemSize != 3) return;
    let t = new THREE.Vector3;

    if (rootAncestor)
    {
      let matrix = new THREE.Matrix4();
      mesh.updateMatrix();
      matrix.copy(mesh.matrix)
      let ancestor = mesh.parent;
      while(ancestor && ancestor !== rootAncestor)
      {
          ancestor.updateMatrix();
          matrix.premultiply(ancestor.matrix);
          ancestor = ancestor.parent;
      }
      for (let i = 0; i < g.count; ++i) {
        t.fromBufferAttribute(g, i)
        t.applyMatrix4(matrix);
        vectors.push_back(Object.assign({}, t));
      }
    }
    else
    {
      for (let i = 0; i < g.count; ++i) {
        t.fromBufferAttribute(g, i)
        vectors.push_back(Object.assign({}, t));
      }
    }

    let worldScale = new THREE.Vector3;
    let worldBasis = (rootAncestor || mesh);
    worldBasis.updateMatrixWorld();
    worldBasis.getWorldScale(worldScale);
    let convexMesh = this.system.cooking.createConvexMesh(vectors, this.system.physics)
    return new PhysX.PxConvexMeshGeometry(convexMesh, new PhysX.PxMeshScale({x: worldScale.x, y: worldScale.y, z: worldScale.z}, {w: 1, x: 0, y: 0, z: 0}), new PhysX.PxConvexMeshGeometryFlags(PhysX.PxConvexMeshGeometryFlag.eTIGHT_BOUNDS.value))
  },
  createShape(physics, geometry, materialData)
  {
    let material = physics.createMaterial(materialData.staticFriction, materialData.dynamicFriction, materialData.restitution);
    let shape = physics.createShape(geometry, material, false, this.system.defaultActorFlags)
    shape.setQueryFilterData(new PhysX.PxFilterData(PhysXUtil.layersToMask(materialData.collisionLayers), PhysXUtil.layersToMask(materialData.collidesWithLayers), materialData.collisionGroup, 0))
    shape.setSimulationFilterData(new PhysX.PxFilterData(PhysXUtil.layersToMask(materialData.collisionLayers), PhysXUtil.layersToMask(materialData.collidesWithLayers), materialData.collisionGroup, 0))

    if (materialData.contactOffset >= 0.0)
    {
      shape.setContactOffset(materialData.contactOffset)
    }
    if (materialData.restOffset >= 0.0)
    {
      shape.setRestOffset(materialData.restOffset)
    }

    shape.density = materialData.density;
    this.system.registerShape(shape, this)

    return shape;
  },
  createShapes(physics) {
    if (this.el.hasAttribute('geometry'))
    {
      let geometry = this.createGeometry(this.el.object3D);
      if (!geometry) return;
      let materialData = this.el.components['physx-material'].data
      this.shapes = [this.createShape(physics, geometry, materialData)];

      return this.shapes;
    }

    let shapes = []
    Util.traverseCondition(this.el.object3D,
      o => {
        if (o.el && o.el.hasAttribute("physx-no-collision")) return false;
        if (o.el && !o.el.object3D.visible && !o.el.hasAttribute("physx-hidden-collision")) return false;
        if (!o.visible && o.el && !o.el.hasAttribute("physx-hidden-collision")) return false;
        if (o.userData && o.userData.vartisteUI) return false;
        return true
      },
      o => {
      if (o.geometry) {
        let geometry;
        if (false && o.el && o.el.hasAttribute('geometry'))
        {
          geometry = this.createGeometry(o);
        }
        else
        {
          geometry = this.createConvexMeshGeometry(o, this.el.object3D);
        }
        if (!geometry) {
          console.warn("Couldn't create geometry", o)
          return;
        }

        let material, materialData;
        if (o.el && o.el.hasAttribute('physx-material'))
        {
          materialData = o.el.getAttribute('physx-material')
        }
        else
        {
            materialData = this.el.components['physx-material'].data
        }
        let shape = this.createShape(physics, geometry, materialData)

        // shape.setLocalPose({translation: this.data.shapeOffset, rotation: {w: 1, x: 0, y: 0, z: 0}})

        shapes.push(shape)
      }
    });

    this.shapes = shapes

    return shapes
  },
  // Turns gravity on and off
  toggleGravity() {
    this.rigidBody.setActorFlag(PhysX.PxActorFlag.eDISABLE_GRAVITY, !this.floating)
    this.floating = !this.floating
  },
  resetBodyPose() {
    this.rigidBody.setGlobalPose(PhysXUtil.object3DPhysXTransform(this.el.object3D), true)
  },
  kinematicMove() {
    this.rigidBody.setKinematicTarget(PhysXUtil.object3DPhysXTransform(this.el.object3D))
  },
  tock(t, dt) {
    if (this.rigidBody && this.data.type === 'kinematic' && !this.setKinematic)
    {
      this.rigidBody.setRigidBodyFlag(PhysX.PxRigidBodyFlag.eKINEMATIC, true)
      this.setKinematic = true
    }
    if (this.rigidBody && (this.data.type === 'kinematic' || this.el.is("grabbed"))) {
      // this.el.object3D.scale.set(1,1,1)
      this.kinematicMove()
    }
  }
})

// Creates a driver which exerts force to return the joint to the specified
// (currently only the initial) position with the given velocity
// characteristics.
//
// This can only be used on an entity with a `physx-joint` component. Currently
// only supports **D6** joint type. E.g.
//
//```
// <a-box physx-body>
//    <a-entity position="0.2 0.3 0.4" rotation="0 90 0"
//              physx-joint="type: D6; target: #other-body"
//              physx-joint-driver="axes: swing, twist; stiffness: 30; angularVelocity: 3 3 0">
//    </a-entity>
// </a-box>
//```
AFRAME.registerComponent('physx-joint-driver', {
  dependencies: ['physx-joint'],
  multiple: true,
  schema: {
    // Which axes the joint should operate on. Should be some combination of `x`, `y`, `z`, `twist`, `swing`
    axes: {type: 'array', default: []},

    // How stiff the drive should be
    stiffness: {default: 1.0},

    // Damping to apply to the drive
    damping: {default: 1.0},

    // Maximum amount of force used to get to the target position
    forceLimit: {default: 3.4028234663852885981170418348452e+38},

    // If true, will operate directly on body acceleration rather than on force
    useAcceleration: {default: true},

    // Target linear velocity relative to the joint
    linearVelocity: {type: 'vec3', default: {x: 0, y: 0, z: 0}},

    // Targget angular velocity relative to the joint
    angularVelocity: {type: 'vec3', default: {x: 0, y: 0, z: 0}},

    // If true, will automatically lock axes which are not being driven
    lockOtherAxes: {default: false},

    // If true SLERP rotation mode. If false, will use SWING mode.
    slerpRotation: {default: true},
  },
  events: {
    'physx-jointcreated': function(e) {
      this.setJointDriver()
    }
  },
  init() {
    this.el.setAttribute('phsyx-custom-constraint', "")
  },
  setJointDriver() {
    if (!this.enumAxes) this.update();
    if (this.el.components['physx-joint'].data.type !== 'D6') {
      console.warn("Only D6 joint drivers supported at the moment")
      return;
    }

    let PhysX = this.el.sceneEl.systems.physx.PhysX;
    this.joint = this.el.components['physx-joint'].joint

    if (this.data.lockOtherAxes)
    {
      this.joint.setMotion(PhysX.PxD6Axis.eX, PhysX.PxD6Motion.eLOCKED)
      this.joint.setMotion(PhysX.PxD6Axis.eY, PhysX.PxD6Motion.eLOCKED)
      this.joint.setMotion(PhysX.PxD6Axis.eZ, PhysX.PxD6Motion.eLOCKED)
      this.joint.setMotion(PhysX.PxD6Axis.eSWING1, PhysX.PxD6Motion.eLOCKED)
      this.joint.setMotion(PhysX.PxD6Axis.eSWING2, PhysX.PxD6Motion.eLOCKED)
      this.joint.setMotion(PhysX.PxD6Axis.eTWIST, PhysX.PxD6Motion.eLOCKED)
    }

    for (let enumKey of this.enumAxes)
    {
      this.joint.setMotion(enumKey, PhysX.PxD6Motion.eFREE)
    }

    let drive = new PhysX.PxD6JointDrive;
    drive.stiffness = this.data.stiffness;
    drive.damping = this.data.damping;
    drive.forceLimit = this.data.forceLimit;
    drive.setAccelerationFlag(this.data.useAcceleration);

    for (let axis of this.driveAxes)
    {
      this.joint.setDrive(axis, drive);
    }

    console.log("Setting joint driver", this.driveAxes, this.enumAxes)

    this.joint.setDrivePosition({translation: {x: 0, y: 0, z: 0}, rotation: {w: 1, x: 0, y: 0, z: 0}}, true)

    this.joint.setDriveVelocity(this.data.linearVelocity, this.data.angularVelocity, true);
  },
  update(oldData) {
    if (!PhysX) return;

    this.enumAxes = []
    for (let axis of this.data.axes)
    {
      if (axis === 'swing') {
        this.enumAxes.push(PhysX.PxD6Axis.eSWING1)
        this.enumAxes.push(PhysX.PxD6Axis.eSWING2)
        continue
      }
      let enumKey = `e${axis.toUpperCase()}`
      if (!(enumKey in PhysX.PxD6Axis))
      {
        console.warn(`Unknown axis ${axis} (PxD6Axis::${enumKey})`)
      }
      this.enumAxes.push(PhysX.PxD6Axis[enumKey])
    }

    this.driveAxes = []

    for (let axis of this.data.axes)
    {
      if (axis === 'swing') {
        if (this.data.slerpRotation)
        {
          this.driveAxes.push(PhysX.PxD6Drive.eSLERP)
        }
        else
        {
          this.driveAxes.push(PhysX.PxD6Drive.eSWING)
        }
        continue
      }

      if (axis === 'twist' && this.data.slerpRotation) {
        this.driveAxes.push(PhysX.PxD6Drive.eSLERP)
        continue;
      }

      let enumKey = `e${axis.toUpperCase()}`
      if (!(enumKey in PhysX.PxD6Drive))
      {
        console.warn(`Unknown axis ${axis} (PxD6Axis::${enumKey})`)
      }
      this.driveAxes.push(PhysX.PxD6Drive[enumKey])
    }
  }
})

// See README.md for examples
AFRAME.registerComponent('physx-joint-constraint', {
  multiple: true,
  schema: {
    // Which axes are explicitly locked by this constraint and can't be moved at all.
    // Should be some combination of `x`, `y`, `z`, `twist`, `swing`
    lockedAxes: {type: 'array', default: []}, // for D6 joint type

    // Which axes are constrained by this constraint. These axes can be moved within the set limits.
    // Should be some combination of `x`, `y`, `z`, `twist`, `swing`
    constrainedAxes: {type: 'array', default: []}, // for D6 joint type

    // Which axes are explicitly freed by this constraint. These axes will not obey any limits set here.
    // Should be some combination of `x`, `y`, `z`, `twist`, `swing`
    freeAxes: {type: 'array', default: []}, // for D6 joint type

    // Limit on linear movement. Only affects `x`, `y`, and `z` axes.
    // First vector component is the minimum allowed position
    linearLimit: {type: 'vec2'}, // for D6 and Prismatic joint type

    // Limit on angular movement in degrees. Example: `-110 80` to move between -110 and 80 degrees
    angularLimit: {type: 'vec2'}, // for Revolute joint type

    // Two angles in degrees specifying a cone in which the joint is allowed to swing, like
    // a pendulum.
    limitCone: {type: 'vec2'}, // for D6 joint type

    // Minimum and maximum angles in degrees that the joint is allowed to twist
    twistLimit: {type: 'vec2'}, // for D6 joint type

    // Spring damping for soft constraints
    damping: {default: 0.0},
    // Spring restitution for soft constraints
    restitution: {default: 0.0},
    // If greater than 0, will make this joint a soft constraint, and use a spring force model
    stiffness: {default: 0.0},
  },
  events: {
    'physx-jointcreated': function(e) {
      this.setJointConstraint()
    }
  },
  init() {
    this.propsInitialized = false;
    this.el.setAttribute('phsyx-custom-constraint', "")
  },
  setJointConstraint() {
    const jointType = this.el.components['physx-joint'].data.type;
    if (jointType !== 'D6' && jointType !== 'Revolute' && jointType !== 'Prismatic') {
      console.warn("Only D6, Revolute and Prismatic joint constraints supported at the moment")
      return;
    }

    if (!this.propsInitialized) this.update();

    const joint = this.el.components['physx-joint'].joint;

    if (jointType === 'Revolute') {
      // https://nvidiagameworks.github.io/PhysX/4.1/documentation/physxapi/files/classPxJointAngularLimitPair.html
      const spring = new PhysX.PxSpring(this.data.stiffness, this.data.damping);
      const limitPair = new PhysX.PxJointAngularLimitPair(
        -THREE.MathUtils.degToRad(this.data.angularLimit.y),
        -THREE.MathUtils.degToRad(this.data.angularLimit.x),
        spring)
      limitPair.restitution = this.data.restitution;
      joint.setLimit(limitPair);
      joint.setRevoluteJointFlag(PhysX.PxRevoluteJointFlag.eLIMIT_ENABLED, true);
    }

    if (jointType === 'Prismatic') {
      const spring = new PhysX.PxS