cannon-es-control/src/world/Narrowphase.ts

A lightweight 3D physics engine written in JavaScript with control system tools

import { Shape } from '../shapes/Shape'
import { Vec3 } from '../math/Vec3'
import { Transform } from '../math/Transform'
import { Quaternion } from '../math/Quaternion'
import { Body } from '../objects/Body'
import { AABB } from '../collision/AABB'
import { Ray } from '../collision/Ray'
import { Vec3Pool } from '../utils/Vec3Pool'
import { ContactEquation } from '../equations/ContactEquation'
import { FrictionEquation } from '../equations/FrictionEquation'
import type { Box } from '../shapes/Box'
import type { Sphere } from '../shapes/Sphere'
import type { ConvexPolyhedron, ConvexPolyhedronContactPoint } from '../shapes/ConvexPolyhedron'
import type { Particle } from '../shapes/Particle'
import type { Plane } from '../shapes/Plane'
import type { Trimesh } from '../shapes/Trimesh'
import type { Heightfield } from '../shapes/Heightfield'
import { Cylinder } from '../shapes/Cylinder'
import type { ContactMaterial } from '../material/ContactMaterial'
import type { World } from '../world/World'

// Naming rule: based of the order in SHAPE_TYPES,
// the first part of the method is formed by the
// shape type that comes before, in the second part
// there is the shape type that comes after in the SHAPE_TYPES list
export const COLLISION_TYPES = {
  sphereSphere: Shape.types.SPHERE as 1,
  spherePlane: (Shape.types.SPHERE | Shape.types.PLANE) as 3,
  boxBox: (Shape.types.BOX | Shape.types.BOX) as 4,
  sphereBox: (Shape.types.SPHERE | Shape.types.BOX) as 5,
  planeBox: (Shape.types.PLANE | Shape.types.BOX) as 6,
  convexConvex: Shape.types.CONVEXPOLYHEDRON as 16,
  sphereConvex: (Shape.types.SPHERE | Shape.types.CONVEXPOLYHEDRON) as 17,
  planeConvex: (Shape.types.PLANE | Shape.types.CONVEXPOLYHEDRON) as 18,
  boxConvex: (Shape.types.BOX | Shape.types.CONVEXPOLYHEDRON) as 20,
  sphereHeightfield: (Shape.types.SPHERE | Shape.types.HEIGHTFIELD) as 33,
  boxHeightfield: (Shape.types.BOX | Shape.types.HEIGHTFIELD) as 36,
  convexHeightfield: (Shape.types.CONVEXPOLYHEDRON | Shape.types.HEIGHTFIELD) as 48,
  sphereParticle: (Shape.types.PARTICLE | Shape.types.SPHERE) as 65,
  planeParticle: (Shape.types.PLANE | Shape.types.PARTICLE) as 66,
  boxParticle: (Shape.types.BOX | Shape.types.PARTICLE) as 68,
  convexParticle: (Shape.types.PARTICLE | Shape.types.CONVEXPOLYHEDRON) as 80,
  cylinderCylinder: Shape.types.CYLINDER as 128,
  sphereCylinder: (Shape.types.SPHERE | Shape.types.CYLINDER) as 129,
  planeCylinder: (Shape.types.PLANE | Shape.types.CYLINDER) as 130,
  boxCylinder: (Shape.types.BOX | Shape.types.CYLINDER) as 132,
  convexCylinder: (Shape.types.CONVEXPOLYHEDRON | Shape.types.CYLINDER) as 144,
  heightfieldCylinder: (Shape.types.HEIGHTFIELD | Shape.types.CYLINDER) as 160,
  particleCylinder: (Shape.types.PARTICLE | Shape.types.CYLINDER) as 192,
  sphereTrimesh: (Shape.types.SPHERE | Shape.types.TRIMESH) as 257,
  planeTrimesh: (Shape.types.PLANE | Shape.types.TRIMESH) as 258,
}

export type CollisionType = typeof COLLISION_TYPES[keyof typeof COLLISION_TYPES]

/**
 * Helper class for the World. Generates ContactEquations.
 * @todo Sphere-ConvexPolyhedron contacts
 * @todo Contact reduction
 * @todo should move methods to prototype
 */
export class Narrowphase {
  /**
   * Internal storage of pooled contact points.
   */
  contactPointPool: ContactEquation[]
  frictionEquationPool: FrictionEquation[]
  result: ContactEquation[]
  frictionResult: FrictionEquation[]
  /**
   * Pooled vectors.
   */
  v3pool: Vec3Pool
  world: World
  currentContactMaterial: ContactMaterial
  enableFrictionReduction: boolean

  get [COLLISION_TYPES.sphereSphere]() {
    return this.sphereSphere
  }
  get [COLLISION_TYPES.spherePlane]() {
    return this.spherePlane
  }
  get [COLLISION_TYPES.boxBox]() {
    return this.boxBox
  }
  get [COLLISION_TYPES.sphereBox]() {
    return this.sphereBox
  }
  get [COLLISION_TYPES.planeBox]() {
    return this.planeBox
  }
  get [COLLISION_TYPES.convexConvex]() {
    return this.convexConvex
  }
  get [COLLISION_TYPES.sphereConvex]() {
    return this.sphereConvex
  }
  get [COLLISION_TYPES.planeConvex]() {
    return this.planeConvex
  }
  get [COLLISION_TYPES.boxConvex]() {
    return this.boxConvex
  }
  get [COLLISION_TYPES.sphereHeightfield]() {
    return this.sphereHeightfield
  }
  get [COLLISION_TYPES.boxHeightfield]() {
    return this.boxHeightfield
  }
  get [COLLISION_TYPES.convexHeightfield]() {
    return this.convexHeightfield
  }
  get [COLLISION_TYPES.sphereParticle]() {
    return this.sphereParticle
  }
  get [COLLISION_TYPES.planeParticle]() {
    return this.planeParticle
  }
  get [COLLISION_TYPES.boxParticle]() {
    return this.boxParticle
  }
  get [COLLISION_TYPES.convexParticle]() {
    return this.convexParticle
  }
  get [COLLISION_TYPES.cylinderCylinder]() {
    return this.convexConvex
  }
  get [COLLISION_TYPES.sphereCylinder]() {
    return this.sphereConvex
  }
  get [COLLISION_TYPES.planeCylinder]() {
    return this.planeConvex
  }
  get [COLLISION_TYPES.boxCylinder]() {
    return this.boxConvex
  }
  get [COLLISION_TYPES.convexCylinder]() {
    return this.convexConvex
  }
  get [COLLISION_TYPES.heightfieldCylinder]() {
    return this.heightfieldCylinder
  }
  get [COLLISION_TYPES.particleCylinder]() {
    return this.particleCylinder
  }
  get [COLLISION_TYPES.sphereTrimesh]() {
    return this.sphereTrimesh
  }
  get [COLLISION_TYPES.planeTrimesh]() {
    return this.planeTrimesh
  }
  // get [COLLISION_TYPES.convexTrimesh]() {
  //   return this.convexTrimesh
  // }

  constructor(world: World) {
    this.contactPointPool = []
    this.frictionEquationPool = []
    this.result = []
    this.frictionResult = []
    this.v3pool = new Vec3Pool()
    this.world = world
    this.currentContactMaterial = world.defaultContactMaterial
    this.enableFrictionReduction = false
  }

  /**
   * Make a contact object, by using the internal pool or creating a new one.
   */
  createContactEquation(
    bi: Body,
    bj: Body,
    si: Shape,
    sj: Shape,
    overrideShapeA?: Shape | null,
    overrideShapeB?: Shape | null
  ): ContactEquation {
    let c
    if (this.contactPointPool.length) {
      c = this.contactPointPool.pop()!
      c.bi = bi
      c.bj = bj
    } else {
      c = new ContactEquation(bi, bj)
    }

    c.enabled = bi.collisionResponse && bj.collisionResponse && si.collisionResponse && sj.collisionResponse

    const cm = this.currentContactMaterial

    c.restitution = cm.restitution

    c.setSpookParams(cm.contactEquationStiffness, cm.contactEquationRelaxation, this.world.dt)

    const matA = si.material || bi.material
    const matB = sj.material || bj.material
    if (matA && matB && matA.restitution >= 0 && matB.restitution >= 0) {
      c.restitution = matA.restitution * matB.restitution
    }

    c.si = overrideShapeA || si
    c.sj = overrideShapeB || sj

    return c
  }

  createFrictionEquationsFromContact(contactEquation: ContactEquation, outArray: FrictionEquation[]): boolean {
    const bodyA = contactEquation.bi
    const bodyB = contactEquation.bj
    const shapeA = contactEquation.si!
    const shapeB = contactEquation.sj!

    const world = this.world
    const cm = this.currentContactMaterial

    // If friction or restitution were specified in the material, use them
    let friction = cm.friction
    const matA = shapeA.material || bodyA.material
    const matB = shapeB.material || bodyB.material
    if (matA && matB && matA.friction >= 0 && matB.friction >= 0) {
      friction = matA.friction * matB.friction
    }

    if (friction > 0) {
      // Create 2 tangent equations
      const mug = friction * world.gravity.length()
      let reducedMass = bodyA.invMass + bodyB.invMass
      if (reducedMass > 0) {
        reducedMass = 1 / reducedMass
      }
      const pool = this.frictionEquationPool
      const c1 = pool.length ? pool.pop()! : new FrictionEquation(bodyA, bodyB, mug * reducedMass)
      const c2 = pool.length ? pool.pop()! : new FrictionEquation(bodyA, bodyB, mug * reducedMass)

      c1.bi = c2.bi = bodyA
      c1.bj = c2.bj = bodyB
      c1.minForce = c2.minForce = -mug * reducedMass
      c1.maxForce = c2.maxForce = mug * reducedMass

      // Copy over the relative vectors
      c1.ri.copy(contactEquation.ri)
      c1.rj.copy(contactEquation.rj)
      c2.ri.copy(contactEquation.ri)
      c2.rj.copy(contactEquation.rj)

      // Construct tangents
      contactEquation.ni.tangents(c1.t, c2.t)

      // Set spook params
      c1.setSpookParams(cm.frictionEquationStiffness, cm.frictionEquationRelaxation, world.dt)
      c2.setSpookParams(cm.frictionEquationStiffness, cm.frictionEquationRelaxation, world.dt)

      c1.enabled = c2.enabled = contactEquation.enabled

      outArray.push(c1, c2)

      return true
    }

    return false
  }

  /**
   * Take the average N latest contact point on the plane.
   */
  createFrictionFromAverage(numContacts: number): void {
    // The last contactEquation
    let c = this.result[this.result.length - 1]

    // Create the result: two "average" friction equations
    if (!this.createFrictionEquationsFromContact(c, this.frictionResult) || numContacts === 1) {
      return
    }

    const f1 = this.frictionResult[this.frictionResult.length - 2]
    const f2 = this.frictionResult[this.frictionResult.length - 1]

    averageNormal.setZero()
    averageContactPointA.setZero()
    averageContactPointB.setZero()

    const bodyA = c.bi
    const bodyB = c.bj
    for (let i = 0; i !== numContacts; i++) {
      c = this.result[this.result.length - 1 - i]
      if (c.bi !== bodyA) {
        averageNormal.vadd(c.ni, averageNormal)
        averageContactPointA.vadd(c.ri, averageContactPointA)
        averageContactPointB.vadd(c.rj, averageContactPointB)
      } else {
        averageNormal.vsub(c.ni, averageNormal)
        averageContactPointA.vadd(c.rj, averageContactPointA)
        averageContactPointB.vadd(c.ri, averageContactPointB)
      }
    }

    const invNumContacts = 1 / numContacts
    averageContactPointA.scale(invNumContacts, f1.ri)
    averageContactPointB.scale(invNumContacts, f1.rj)
    f2.ri.copy(f1.ri) // Should be the same
    f2.rj.copy(f1.rj)
    averageNormal.normalize()
    averageNormal.tangents(f1.t, f2.t)
    // return eq;
  }

  /**
   * Generate all contacts between a list of body pairs
   * @param p1 Array of body indices
   * @param p2 Array of body indices
   * @param result Array to store generated contacts
   * @param oldcontacts Optional. Array of reusable contact objects
   */
  getContacts(
    p1: Body[],
    p2: Body[],
    world: World,
    result: ContactEquation[],
    oldcontacts: ContactEquation[],
    frictionResult: FrictionEquation[],
    frictionPool: FrictionEquation[]
  ): void {
    // Save old contact objects
    this.contactPointPool = oldcontacts
    this.frictionEquationPool = frictionPool
    this.result = result
    this.frictionResult = frictionResult

    const qi = tmpQuat1
    const qj = tmpQuat2
    const xi = tmpVec1
    const xj = tmpVec2

    for (let k = 0, N = p1.length; k !== N; k++) {
      // Get current collision bodies
      const bi = p1[k]

      const bj = p2[k]

      // Get contact material
      let bodyContactMaterial = null
      if (bi.material && bj.material) {
        bodyContactMaterial = world.getContactMaterial(bi.material, bj.material) || null
      }

      const justTest =
        (bi.type & Body.KINEMATIC && bj.type & Body.STATIC) ||
        (bi.type & Body.STATIC && bj.type & Body.KINEMATIC) ||
        (bi.type & Body.KINEMATIC && bj.type & Body.KINEMATIC)

      for (let i = 0; i < bi.shapes.length; i++) {
        bi.quaternion.mult(bi.shapeOrientations[i], qi)
        bi.quaternion.vmult(bi.shapeOffsets[i], xi)
        xi.vadd(bi.position, xi)
        const si = bi.shapes[i]

        for (let j = 0; j < bj.shapes.length; j++) {
          // Compute world transform of shapes
          bj.quaternion.mult(bj.shapeOrientations[j], qj)
          bj.quaternion.vmult(bj.shapeOffsets[j], xj)
          xj.vadd(bj.position, xj)
          const sj = bj.shapes[j]

          if (!(si.collisionFilterMask & sj.collisionFilterGroup && sj.collisionFilterMask & si.collisionFilterGroup)) {
            continue
          }

          if (xi.distanceTo(xj) > si.boundingSphereRadius + sj.boundingSphereRadius) {
            continue
          }

          // Get collision material
          let shapeContactMaterial = null
          if (si.material && sj.material) {
            shapeContactMaterial = world.getContactMaterial(si.material, sj.material) || null
          }

          this.currentContactMaterial = shapeContactMaterial || bodyContactMaterial || world.defaultContactMaterial

          // Get contacts
          const resolverIndex = (si.type | sj.type) as CollisionType
          const resolver = this[resolverIndex]
          if (resolver) {
            let retval = false

            // TO DO: investigate why sphereParticle and convexParticle
            // resolvers expect si and sj shapes to be in reverse order
            // (i.e. larger integer value type first instead of smaller first)
            if (si.type < sj.type) {
              retval = (resolver as any).call(this, si, sj, xi, xj, qi, qj, bi, bj, si, sj, justTest)
            } else {
              retval = (resolver as any).call(this, sj, si, xj, xi, qj, qi, bj, bi, si, sj, justTest)
            }

            if (retval && justTest) {
              // Register overlap
              world.shapeOverlapKeeper.set(si.id, sj.id)
              world.bodyOverlapKeeper.set(bi.id, bj.id)
            }
          }
        }
      }
    }
  }

  sphereSphere(
    si: Sphere,
    sj: Sphere,
    xi: Vec3,
    xj: Vec3,
    qi: Quaternion,
    qj: Quaternion,
    bi: Body,
    bj: Body,
    rsi?: Shape | null,
    rsj?: Shape | null,
    justTest?: boolean
  ): boolean | void {
    if (justTest) {
      return xi.distanceSquared(xj) < (si.radius + sj.radius) ** 2
    }

    // We will have only one contact in this case
    const contactEq = this.createContactEquation(bi, bj, si, sj, rsi, rsj)

    // Contact normal
    xj.vsub(xi, contactEq.ni)
    contactEq.ni.normalize()

    // Contact point locations
    contactEq.ri.copy(contactEq.ni)
    contactEq.rj.copy(contactEq.ni)
    contactEq.ri.scale(si.radius, contactEq.ri)
    contactEq.rj.scale(-sj.radius, contactEq.rj)

    contactEq.ri.vadd(xi, contactEq.ri)
    contactEq.ri.vsub(bi.position, contactEq.ri)

    contactEq.rj.vadd(xj, contactEq.rj)
    contactEq.rj.vsub(bj.position, contactEq.rj)

    this.result.push(contactEq)

    this.createFrictionEquationsFromContact(contactEq, this.frictionResult)
  }

  spherePlane(
    si: Sphere,
    sj: Plane,
    xi: Vec3,
    xj: Vec3,
    qi: Quaternion,
    qj: Quaternion,
    bi: Body,
    bj: Body,
    rsi?: Shape | null,
    rsj?: Shape | null,
    justTest?: boolean
  ): true | void {
    // We will have one contact in this case
    const r = this.createContactEquation(bi, bj, si, sj, rsi, rsj)

    // Contact normal
    r.ni.set(0, 0, 1)
    qj.vmult(r.ni, r.ni)
    r.ni.negate(r.ni) // body i is the sphere, flip normal
    r.ni.normalize() // Needed?

    // Vector from sphere center to contact point
    r.ni.scale(si.radius, r.ri)

    // Project down sphere on plane
    xi.vsub(xj, point_on_plane_to_sphere)
    r.ni.scale(r.ni.dot(point_on_plane_to_sphere), plane_to_sphere_ortho)
    point_on_plane_to_sphere.vsub(plane_to_sphere_ortho, r.rj) // The sphere position projected to plane

    if (-point_on_plane_to_sphere.dot(r.ni) <= si.radius) {
      if (justTest) {
        return true
      }

      // Make it relative to the body
      const ri = r.ri
      const rj = r.rj
      ri.vadd(xi, ri)
      ri.vsub(bi.position, ri)
      rj.vadd(xj, rj)
      rj.vsub(bj.position, rj)

      this.result.push(r)
      this.createFrictionEquationsFromContact(r, this.frictionResult)
    }
  }

  boxBox(
    si: Box,
    sj: Box,
    xi: Vec3,
    xj: Vec3,
    qi: Quaternion,
    qj: Quaternion,
    bi: Body,
    bj: Body,
    rsi?: Shape | null,
    rsj?: Shape | null,
    justTest?: boolean
  ): true | void {
    si.convexPolyhedronRepresentation.material = si.material
    sj.convexPolyhedronRepresentation.material = sj.material
    si.convexPolyhedronRepresentation.collisionResponse = si.collisionResponse
    sj.convexPolyhedronRepresentation.collisionResponse = sj.collisionResponse
    return this.convexConvex(
      si.convexPolyhedronRepresentation,
      sj.convexPolyhedronRepresentation,
      xi,
      xj,
      qi,
      qj,
      bi,
      bj,
      si,
      sj,
      justTest
    )
  }

  sphereBox(
    si: Sphere,
    sj: Box,
    xi: Vec3,
    xj: Vec3,
    qi: Quaternion,
    qj: Quaternion,
    bi: Body,
    bj: Body,
    rsi?: Shape | null,
    rsj?: Shape | null,
    justTest?: boolean
  ): true | void {
    const v3pool = this.v3pool

    // we refer to the box as body j
    const sides = sphereBox_sides
    xi.vsub(xj, box_to_sphere)
    sj.getSideNormals(sides, qj)
    const R = si.radius
    const penetrating_sides = []

    // Check side (plane) intersections
    let found = false

    // Store the resulting side penetration info
    const side_ns = sphereBox_side_ns
    const side_ns1 = sphereBox_side_ns1
    const side_ns2 = sphereBox_side_ns2
    let side_h: number | null = null
    let side_penetrations = 0
    let side_dot1 = 0
    let side_dot2 = 0
    let side_distance = null
    for (let idx = 0, nsides = sides.length; idx !== nsides && found === false; idx++) {
      // Get the plane side normal (ns)
      const ns = sphereBox_ns
      ns.copy(sides[idx])

      const h = ns.length()
      ns.normalize()

      // The normal/distance dot product tells which side of the plane we are
      const dot = box_to_sphere.dot(ns)

      if (dot < h + R && dot > 0) {
        // Intersects plane. Now check the other two dimensions
        const ns1 = sphereBox_ns1
        const ns2 = sphereBox_ns2
        ns1.copy(sides[(idx + 1) % 3])
        ns2.copy(sides[(idx + 2) % 3])
        const h1 = ns1.length()
        const h2 = ns2.length()
        ns1.normalize()
        ns2.normalize()
        const dot1 = box_to_sphere.dot(ns1)
        const dot2 = box_to_sphere.dot(ns2)
        if (dot1 < h1 && dot1 > -h1 && dot2 < h2 && dot2 > -h2) {
          const dist = Math.abs(dot - h - R)
          if (side_distance === null || dist < side_distance) {
            side_distance = dist
            side_dot1 = dot1
            side_dot2 = dot2
            side_h = h
            side_ns.copy(ns)
            side_ns1.copy(ns1)
            side_ns2.copy(ns2)
            side_penetrations++

            if (justTest) {
              return true
            }
          }
        }
      }
    }
    if (side_penetrations) {
      found = true
      const r = this.createContactEquation(bi, bj, si, sj, rsi, rsj)
      side_ns.scale(-R, r.ri) // Sphere r
      r.ni.copy(side_ns)
      r.ni.negate(r.ni) // Normal should be out of sphere
      side_ns.scale(side_h!, side_ns)
      side_ns1.scale(side_dot1, side_ns1)
      side_ns.vadd(side_ns1, side_ns)
      side_ns2.scale(side_dot2, side_ns2)
      side_ns.vadd(side_ns2, r.rj)

      // Make relative to bodies
      r.ri.vadd(xi, r.ri)
      r.ri.vsub(bi.position, r.ri)
      r.rj.vadd(xj, r.rj)
      r.rj.vsub(bj.position, r.rj)

      this.result.push(r)
      this.createFrictionEquationsFromContact(r, this.frictionResult)
    }

    // Check corners
    let rj = v3pool.get()
    const sphere_to_corner = sphereBox_sphere_to_corner
    for (let j = 0; j !== 2 && !found; j++) {
      for (let k = 0; k !== 2 && !found; k++) {
        for (let l = 0; l !== 2 && !found; l++) {
          rj.set(0, 0, 0)
          if (j) {
            rj.vadd(sides[0], rj)
          } else {
            rj.vsub(sides[0], rj)
          }
          if (k) {
            rj.vadd(sides[1], rj)
          } else {
            rj.vsub(sides[1], rj)
          }
          if (l) {
            rj.vadd(sides[2], rj)
          } else {
            rj.vsub(sides[2], rj)
          }

          // World position of corner
          xj.vadd(rj, sphere_to_corner)
          sphere_to_corner.vsub(xi, sphere_to_corner)

          if (sphere_to_corner.lengthSquared() < R * R) {
            if (justTest) {
              return true
            }
            found = true
            const r = this.createContactEquation(bi, bj, si, sj, rsi, rsj)
            r.ri.copy(sphere_to_corner)
            r.ri.normalize()
            r.ni.copy(r.ri)
            r.ri.scale(R, r.ri)
            r.rj.copy(rj)

            // Make relative to bodies
            r.ri.vadd(xi, r.ri)
            r.ri.vsub(bi.position, r.ri)
            r.rj.vadd(xj, r.rj)
            r.rj.vsub(bj.position, r.rj)

            this.result.push(r)
            this.createFrictionEquationsFromContact(r, this.frictionResult)
          }
        }
      }
    }
    v3pool.release(rj)
    rj = null

    // Check edges
    const edgeTangent = v3pool.get()
    const edgeCenter = v3pool.get()
    const r = v3pool.get() // r = edge center to sphere center
    const orthogonal = v3pool.get()
    const dist = v3pool.get()
    const Nsides = sides.length
    for (let j = 0; j !== Nsides && !found; j++) {
      for (let k = 0; k !== Nsides && !found; k++) {
        if (j % 3 !== k % 3) {
          // Get edge tangent
          sides[k].cross(sides[j], edgeTangent)
          edgeTangent.normalize()
          sides[j].vadd(sides[k], edgeCenter)
          r.copy(xi)
          r.vsub(edgeCenter, r)
          r.vsub(xj, r)
          const orthonorm = r.dot(edgeTangent) // distance from edge center to sphere center in the tangent direction
          edgeTangent.scale(orthonorm, orthogonal) // Vector from edge center to sphere center in the tangent direction

          // Find the third side orthogonal to this one
          let l = 0
          while (l === j % 3 || l === k % 3) {
            l++
          }

          // vec from edge center to sphere projected to the plane orthogonal to the edge tangent
          dist.copy(xi)
          dist.vsub(orthogonal, dist)
          dist.vsub(edgeCenter, dist)
          dist.vsub(xj, dist)

          // Distances in tangent direction and distance in the plane orthogonal to it
          const tdist = Math.abs(orthonorm)
          const ndist = dist.length()

          if (tdist < sides[l].length() && ndist < R) {
            if (justTest) {
              return true
            }
            found = true
            const res = this.createContactEquation(bi, bj, si, sj, rsi, rsj)
            edgeCenter.vadd(orthogonal, res.rj) // box rj
            res.rj.copy(res.rj)
            dist.negate(res.ni)
            res.ni.normalize()

            res.ri.copy(res.rj)
            res.ri.vadd(xj, res.ri)
            res.ri.vsub(xi, res.ri)
            res.ri.normalize()
            res.ri.scale(R, res.ri)

            // Make relative to bodies
            res.ri.vadd(xi, res.ri)
            res.ri.vsub(bi.position, res.ri)
            res.rj.vadd(xj, res.rj)
            res.rj.vsub(bj.position, res.rj)

            this.result.push(res)
            this.createFrictionEquationsFromContact(res, this.frictionResult)
          }
        }
      }
    }
    v3pool.release(edgeTangent, edgeCenter, r, orthogonal, dist)
  }

  planeBox(
    si: Plane,
    sj: Box,
    xi: Vec3,
    xj: Vec3,
    qi: Quaternion,
    qj: Quaternion,
    bi: Body,
    bj: Body,
    rsi?: Shape | null,
    rsj?: Shape | null,
    justTest?: boolean
  ): true | void {
    sj.convexPolyhedronRepresentation.material = sj.material
    sj.convexPolyhedronRepresentation.collisionResponse = sj.collisionResponse
    sj.convexPolyhedronRepresentation.id = sj.id
    return this.planeConvex(si, sj.convexPolyhedronRepresentation, xi, xj, qi, qj, bi, bj, si, sj, justTest)
  }

  convexConvex(
    si: ConvexPolyhedron,
    sj: ConvexPolyhedron,
    xi: Vec3,
    xj: Vec3,
    qi: Quaternion,
    qj: Quaternion,
    bi: Body,
    bj: Body,
    rsi?: Shape | null,
    rsj?: Shape | null,
    justTest?: boolean,
    faceListA?: number[] | null,
    faceListB?: number[] | null
  ): true | void {
    const sepAxis = convexConvex_sepAxis

    if (xi.distanceTo(xj) > si.boundingSphereRadius + sj.boundingSphereRadius) {
      return
    }

    if (si.findSeparatingAxis(sj, xi, qi, xj, qj, sepAxis, faceListA, faceListB)) {
      const res: ConvexPolyhedronContactPoint[] = []
      const q = convexConvex_q
      si.clipAgainstHull(xi, qi, sj, xj, qj, sepAxis, -100, 100, res)
      let numContacts = 0
      for (let j = 0; j !== res.length; j++) {
        if (justTest) {
          return true
        }
        const r = this.createContactEquation(bi, bj, si, sj, rsi, rsj)
        const ri = r.ri
        const rj = r.rj
        sepAxis.negate(r.ni)
        res[j].normal.negate(q)
        q.scale(res[j].depth, q)
        res[j].point.vadd(q, ri)
        rj.copy(res[j].point)

        // Contact points are in world coordinates. Transform back to relative
        ri.vsub(xi, ri)
        rj.vsub(xj, rj)

        // Make relative to bodies
        ri.vadd(xi, ri)
        ri.vsub(bi.position, ri)
        rj.vadd(xj, rj)
        rj.vsub(bj.position, rj)

        this.result.push(r)
        numContacts++
        if (!this.enableFrictionReduction) {
          this.createFrictionEquationsFromContact(r, this.frictionResult)
        }
      }
      if (this.enableFrictionReduction && numContacts) {
        this.createFrictionFromAverage(numContacts)
      }
    }
  }

  sphereConvex(
    si: Sphere,
    sj: ConvexPolyhedron,
    xi: Vec3,
    xj: Vec3,
    qi: Quaternion,
    qj: Quaternion,
    bi: Body,
    bj: Body,
    rsi?: Shape | null,
    rsj?: Shape | null,
    justTest?: boolean
  ): true | void {
    const v3pool = this.v3pool
    xi.vsub(xj, convex_to_sphere)
    const normals = sj.faceNormals
    const faces = sj.faces
    const verts = sj.vertices
    const R = si.radius
    const penetrating_sides = []

    // if(convex_to_sphere.lengthSquared() > si.boundingSphereRadius + sj.boundingSphereRadius){
    //     return;
    // }
    let found = false

    // Check corners
    for (let i = 0; i !== verts.length; i++) {
      const v = verts[i]

      // World position of corner
      const worldCorner = sphereConvex_worldCorner
      qj.vmult(v, worldCorner)
      xj.vadd(worldCorner, worldCorner)
      const sphere_to_corner = sphereConvex_sphereToCorner
      worldCorner.vsub(xi, sphere_to_corner)
      if (sphere_to_corner.lengthSquared() < R * R) {
        if (justTest) {
          return true
        }
        found = true
        const r = this.createContactEquation(bi, bj, si, sj, rsi, rsj)
        r.ri.copy(sphere_to_corner)
        r.ri.normalize()
        r.ni.copy(r.ri)
        r.ri.scale(R, r.ri)
        worldCorner.vsub(xj, r.rj)

        // Should be relative to the body.
        r.ri.vadd(xi, r.ri)
        r.ri.vsub(bi.position, r.ri)

        // Should be relative to the body.
        r.rj.vadd(xj, r.rj)
        r.rj.vsub(bj.position, r.rj)

        this.result.push(r)
        this.createFrictionEquationsFromContact(r, this.frictionResult)
        return
      }
    }

    // Check side (plane) intersections
    for (let i = 0, nfaces = faces.length; i !== nfaces && found === false; i++) {
      const normal = normals[i]
      const face = faces[i]

      // Get world-transformed normal of the face
      const worldNormal = sphereConvex_worldNormal
      qj.vmult(normal, worldNormal)

      // Get a world vertex from the face
      const worldPoint = sphereConvex_worldPoint
      qj.vmult(verts[face[0]], worldPoint)
      worldPoint.vadd(xj, worldPoint)

      // Get a point on the sphere, closest to the face normal
      const worldSpherePointClosestToPlane = sphereConvex_worldSpherePointClosestToPlane
      worldNormal.scale(-R, worldSpherePointClosestToPlane)
      xi.vadd(worldSpherePointClosestToPlane, worldSpherePointClosestToPlane)

      // Vector from a face point to the closest point on the sphere
      const penetrationVec = sphereConvex_penetrationVec
      worldSpherePointClosestToPlane.vsub(worldPoint, penetrationVec)

      // The penetration. Negative value means overlap.
      const penetration = penetrationVec.dot(worldNormal)

      const worldPointToSphere = sphereConvex_sphereToWorldPoint
      xi.vsub(worldPoint, worldPointToSphere)

      if (penetration < 0 && worldPointToSphere.dot(worldNormal) > 0) {
        // Intersects plane. Now check if the sphere is inside the face polygon
        const faceVerts = [] // Face vertices, in world coords
        for (let j = 0, Nverts = face.length; j !== Nverts; j++) {
          const worldVertex = v3pool.get()
          qj.vmult(verts[face[j]], worldVertex)
          xj.vadd(worldVertex, worldVertex)
          faceVerts.push(worldVertex)
        }

        if (pointInPolygon(faceVerts, worldNormal, xi)) {
          // Is the sphere center in the face polygon?
          if (justTest) {
            return true
          }
          found = true
          const r = this.createContactEquation(bi, bj, si, sj, rsi, rsj)

          worldNormal.scale(-R, r.ri) // Contact offset, from sphere center to contact
          worldNormal.negate(r.ni) // Normal pointing out of sphere

          const penetrationVec2 = v3pool.get()
          worldNormal.scale(-penetration, penetrationVec2)
          const penetrationSpherePoint = v3pool.get()
          worldNormal.scale(-R, penetrationSpherePoint)

          //xi.vsub(xj).vadd(penetrationSpherePoint).vadd(penetrationVec2 , r.rj);
          xi.vsub(xj, r.rj)
          r.rj.vadd(penetrationSpherePoint, r.rj)
          r.rj.vadd(penetrationVec2, r.rj)

          // Should be relative to the body.
          r.rj.vadd(xj, r.rj)
          r.rj.vsub(bj.position, r.rj)

          // Should be relative to the body.
          r.ri.vadd(xi, r.ri)
          r.ri.vsub(bi.position, r.ri)

          v3pool.release(penetrationVec2)
          v3pool.release(penetrationSpherePoint)

          this.result.push(r)
          this.createFrictionEquationsFromContact(r, this.frictionResult)

          // Release world vertices
          for (let j = 0, Nfaceverts = faceVerts.length; j !== Nfaceverts; j++) {
            v3pool.release(faceVerts[j])
          }

          return // We only expect *one* face contact
        } else {
          // Edge?
          for (let j = 0; j !== face.length; j++) {
            // Get two world transformed vertices
            const v1 = v3pool.get()
            const v2 = v3pool.get()
            qj.vmult(verts[face[(j + 1) % face.length]], v1)
            qj.vmult(verts[face[(j + 2) % face.length]], v2)
            xj.vadd(v1, v1)
            xj.vadd(v2, v2)

            // Construct edge vector
            const edge = sphereConvex_edge
            v2.vsub(v1, edge)

            // Construct the same vector, but normalized
            const edgeUnit = sphereConvex_edgeUnit
            edge.unit(edgeUnit)

            // p is xi projected onto the edge
            const p = v3pool.get()
            const v1_to_xi = v3pool.get()
            xi.vsub(v1, v1_to_xi)
            const dot = v1_to_xi.dot(edgeUnit)
            edgeUnit.scale(dot, p)
            p.vadd(v1, p)

            // Compute a vector from p to the center of the sphere
            const xi_to_p = v3pool.get()
            p.vsub(xi, xi_to_p)

            // Collision if the edge-sphere distance is less than the radius
            // AND if p is in between v1 and v2
            if (dot > 0 && dot * dot < edge.lengthSquared() && xi_to_p.lengthSquared() < R * R) {
              // Collision if the edge-sphere distance is less than the radius
              // Edge contact!
              if (justTest) {
                return true
              }
              const r = this.createContactEquation(bi, bj, si, sj, rsi, rsj)
              p.vsub(xj, r.rj)

              p.vsub(xi, r.ni)
              r.ni.normalize()

              r.ni.scale(R, r.ri)

              // Should be relative to the body.
              r.rj.vadd(xj, r.rj)
              r.rj.vsub(bj.position, r.rj)

              // Should be relative to the body.
              r.ri.vadd(xi, r.ri)
              r.ri.vsub(bi.position, r.ri)

              this.result.push(r)
              this.createFrictionEquationsFromContact(r, this.frictionResult)

              // Release world vertices
              for (let j = 0, Nfaceverts = faceVerts.length; j !== Nfaceverts; j++) {
                v3pool.release(faceVerts[j])
              }

              v3pool.release(v1)
              v3pool.release(v2)
              v3pool.release(p)
              v3pool.release(xi_to_p)
              v3pool.release(v1_to_xi)

              return
            }

            v3pool.release(v1)
            v3pool.release(v2)
            v3pool.release(p)
            v3pool.release(xi_to_p)
            v3pool.release(v1_to_xi)
          }
        }

        // Release world vertices
        for (let j = 0, Nfaceverts = faceVerts.length; j !== Nfaceverts; j++) {
          v3pool.release(faceVerts[j])
        }
      }
    }
  }

  planeConvex(
    planeShape: Plane,
    convexShape: ConvexPolyhedron,
    planePosition: Vec3,
    convexPosition: Vec3,
    planeQuat: Quaternion,
    convexQuat: Quaternion,
    planeBody: Body,
    convexBody: Body,
    si?: Shape,
    sj?: Shape,
    justTest?: boolean
  ): true | void {
    // Simply return the points behind the plane.
    const worldVertex = planeConvex_v

    const worldNormal = planeConvex_normal
    worldNormal.set(0, 0, 1)
    planeQuat.vmult(worldNormal, worldNormal) // Turn normal according to plane orientation

    let numContacts = 0
    const relpos = planeConvex_relpos
    for (let i = 0; i !== convexShape.vertices.length; i++) {
      // Get world convex vertex
      worldVertex.copy(convexShape.vertices[i])
      convexQuat.vmult(worldVertex, worldVertex)
      convexPosition.vadd(worldVertex, worldVertex)
      worldVertex.vsub(planePosition, relpos)

      const dot = worldNormal.dot(relpos)
      if (dot <= 0.0) {
        if (justTest) {
          return true
        }

        const r = this.createContactEquation(planeBody, convexBody, planeShape, convexShape, si, sj)

        // Get vertex position projected on plane
        const projected = planeConvex_projected
        worldNormal.scale(worldNormal.dot(relpos), projected)
        worldVertex.vsub(projected, projected)
        projected.vsub(planePosition, r.ri) // From plane to vertex projected on plane

        r.ni.copy(worldNormal) // Contact normal is the plane normal out from plane

        // rj is now just the vector from the convex center to the vertex
        worldVertex.vsub(convexPosition, r.rj)

        // Make it relative to the body
        r.ri.vadd(planePosition, r.ri)
        r.ri.vsub(planeBody.position, r.ri)
        r.rj.vadd(convexPosition, r.rj)
        r.rj.vsub(convexBody.position, r.rj)

        this.result.push(r)
        numContacts++
        if (!this.enableFrictionReduction) {
          this.createFrictionEquationsFromContact(r, this.frictionResult)
        }
      }
    }

    if (this.enableFrictionReduction && numContacts) {
      this.createFrictionFromAverage(numContacts)
    }
  }

  boxConvex(
    si: Box,
    sj: ConvexPolyhedron,
    xi: Vec3,
    xj: Vec3,
    qi: Quaternion,
    qj: Quaternion,
    bi: Body,
    bj: Body,
    rsi?: Shape | null,
    rsj?: Shape | null,
    justTest?: boolean
  ): true | void {
    si.convexPolyhedronRepresentation.material = si.material
    si.convexPolyhedronRepresentation.collisionResponse = si.collisionResponse
    return this.convexConvex(si.convexPolyhedronRepresentation, sj, xi, xj, qi, qj, bi, bj, si, sj, justTest)
  }

  sphereHeightfield(
    sphereShape: Sphere,
    hfShape: Heightfield,
    spherePos: Vec3,
    hfPos: Vec3,
    sphereQuat: Quaternion,
    hfQuat: Quaternion,
    sphereBody: Body,
    hfBody: Body,
    rsi?: Shape | null,
    rsj?: Shape | null,
    justTest?: boolean
  ): true | void {
    const data = hfShape.data
    const radius = sphereShape.radius
    const w = hfShape.elementSize
    const worldPillarOffset = sphereHeightfield_tmp2

    // Get sphere position to heightfield local!
    const localSpherePos = sphereHeightfield_tmp1
    Transform.pointToLocalFrame(hfPos, hfQuat, spherePos, localSpherePos)

    // Get the index of the data points to test against
    let iMinX = Math.floor((localSpherePos.x - radius) / w) - 1

    let iMaxX = Math.ceil((localSpherePos.x + radius) / w) + 1
    let iMinY = Math.floor((localSpherePos.y - radius) / w) - 1
    let iMaxY = Math.ceil((localSpherePos.y + radius) / w) + 1

    // Bail out if we are out of the terrain
    if (iMaxX < 0 || iMaxY < 0 || iMinX > data.length || iMinY > data[0].length) {
      return
    }

    // Clamp index to edges
    if (iMinX < 0) {
      iMinX = 0
    }
    if (iMaxX < 0) {
      iMaxX = 0
    }
    if (iMinY < 0) {
      iMinY = 0
    }
    if (iMaxY < 0) {
      iMaxY = 0
    }
    if (iMinX >= data.length) {
      iMinX = data.length - 1
    }
    if (iMaxX >= data.length) {
      iMaxX = data.length - 1
    }
    if (iMaxY >= data[0].length) {
      iMaxY = data[0].length - 1
    }
    if (iMinY >= data[0].length) {
      iMinY = data[0].length - 1
    }

    const minMax: number[] = []
    hfShape.getRectMinMax(iMinX, iMinY, iMaxX, iMaxY, minMax)
    const min = minMax[0]
    const max = minMax[1]

    // Bail out if we can't touch the bounding height box
    if (localSpherePos.z - radius > max || localSpherePos.z + radius < min) {
      return
    }

    const result = this.result
    for (let i = iMinX; i < iMaxX; i++) {
      for (let j = iMinY; j < iMaxY; j++) {
        const numContactsBefore = result.length

        let intersecting = false

        // Lower triangle
        hfShape.getConvexTrianglePillar(i, j, false)
        Transform.pointToWorldFrame(hfPos, hfQuat, hfShape.pillarOffset, worldPillarOffset)
        if (
          spherePos.distanceTo(worldPillarOffset) <
          hfShape.pillarConvex.boundingSphereRadius + sphereShape.boundingSphereRadius
        ) {
          intersecting = this.sphereConvex(
            sphereShape,
            hfShape.pillarConvex,
            spherePos,
            worldPillarOffset,
            sphereQuat,
            hfQuat,
            sphereBody,
            hfBody,
            sphereShape,
            hfShape,
            justTest
          ) as boolean
        }

        if (justTest && intersecting) {
          return true
        }

        // Upper triangle
        hfShape.getConvexTrianglePillar(i, j, true)
        Transform.pointToWorldFrame(hfPos, hfQuat, hfShape.pillarOffset, worldPillarOffset)
        if (
          spherePos.distanceTo(worldPillarOffset) <
          hfShape.pillarConvex.boundingSphereRadius + sphereShape.boundingSphereRadius
        ) {
          intersecting = this.sphereConvex(
            sphereShape,
            hfShape.pillarConvex,
            spherePos,
            worldPillarOffset,
            sphereQuat,
            hfQuat,
            sphereBody,
            hfBody,
            sphereShape,
            hfShape,
            justTest
          ) as boolean
        }

        if (justTest && intersecting) {
          return true
        }

        const numContacts = result.length - numContactsBefore

        if (numContacts > 2) {
          return
        }
        /*
          // Skip all but 1
          for (let k = 0; k < numContacts - 1; k++) {
              result.pop();
          }
        */
      }
    }
  }

  boxHeightfield(
    si: Box,
    sj: Heightfield,
    xi: Vec3,
    xj: Vec3,
    qi: Quaternion,
    qj: Quaternion,
    bi: Body,
    bj: Body,
    rsi?: Shape | null,
    rsj?: Shape | null,
    justTest?: boolean
  ): true | void {
    si.convexPolyhedronRepresentation.material = si.material
    si.convexPolyhedronRepresentation.collisionResponse = si.collisionResponse
    return this.convexHeightfield(si.convexPolyhedronRepresentation, sj, xi, xj, qi, qj, bi, bj, si, sj, justTest)
  }

  convexHeightfield(
    convexShape: ConvexPolyhedron,
    hfShape: Heightfield,
    convexPos: Vec3,
    hfPos: Vec3,
    convexQuat: Quaternion,
    hfQuat: Quaternion,
    convexBody: Body,
    hfBody: Body,
    rsi?: Shape | null,
    rsj?: Shape | null,
    justTest?: boolean
  ): true | void {
    const data = hfShape.data
    const w = hfShape.elementSize
    const radius = convexShape.boundingSphereRadius
    const worldPillarOffset = convexHeightfield_tmp2
    const faceList = convexHeightfield_faceList

    // Get sphere position to heightfield local!
    const localConvexPos = convexHeightfield_tmp1
    Transform.pointToLocalFrame(hfPos, hfQuat, convexPos, localConvexPos)

    // Get the index of the data points to test against
    let iMinX = Math.floor((localConvexPos.x - radius) / w) - 1

    let iMaxX = Math.ceil((localConvexPos.x + radius) / w) + 1
    let iMinY = Math.floor((localConvexPos.y - radius) / w) - 1
    let iMaxY = Math.ceil((localConvexPos.y + radius) / w) + 1

    // Bail out if we are out of the terrain
    if (iMaxX < 0 || iMaxY < 0 || iMinX > data.length || iMinY > data[0].length) {
      return
    }

    // Clamp index to edges
    if (iMinX < 0) {
      iMinX = 0
    }
    if (iMaxX < 0) {
      iMaxX = 0
    }
    if (iMinY < 0) {
      iMinY = 0
    }
    if (iMaxY < 0) {
      iMaxY = 0
    }
    if (iMinX >= data.length) {
      iMinX = data.length - 1
    }
    if (iMaxX >= data.length) {
      iMaxX = data.length - 1
    }
    if (iMaxY >= data[0].length) {
      iMaxY = data[0].length - 1
    }
    if (iMinY >= data[0].length) {
      iMinY = data[0].length - 1
    }

    const minMax: number[] = []
    hfShape.getRectMinMax(iMinX, iMinY, iMaxX, iMaxY, minMax)
    const min = minMax[0]
    const max = minMax[1]

    // Bail out if we're cant touch the bounding height box
    if (localConvexPos.z - radius > max || localConvexPos.z + radius < min) {
      return
    }

    for (let i = iMinX; i < iMaxX; i++) {
      for (let j = iMinY; j < iMaxY; j++) {
        let intersecting = false

        // Lower triangle
        hfShape.getConvexTrianglePillar(i, j, false)
        Transform.pointToWorldFrame(hfPos, hfQuat, hfShape.pillarOffset, worldPillarOffset)
        if (
          convexPos.distanceTo(worldPillarOffset) <
          hfShape.pillarConvex.boundingSphereRadius + convexShape.boundingSphereRadius
        ) {
          intersecting = this.convexConvex(
            convexShape,
            hfShape.pillarConvex,
            convexPos,
            worldPillarOffset,
            convexQuat,
            hfQuat,
            convexBody,
            hfBody,
            null,
            null,
            justTest,
            faceList,
            null
          ) as boolean
        }

        if (justTest && intersecting) {
          return true
        }

        // Upper triangle
        hfShape.getConvexTrianglePillar(i, j, true)
        Transform.pointToWorldFrame(hfPos, hfQuat, hfShape.pillarOffset, worldPillarOffset)
        if (
          convexPos.distanceTo(worldPillarOffset) <
          hfShape.pillarConvex.boundingSphereRadius + convexShape.boundingSphereRadius
        ) {
          intersecting = this.convexConvex(
            convexShape,
            hfShape.pillarConvex,
            convexPos,
            worldPillarOffset,
            convexQuat,
            hfQuat,
            convexBody,
            hfBody,
            null,
            null,
            justTest,
            faceList,
            null
          ) as boolean
        }

        if (justTest && intersecting) {
          return true
        }
      }
    }
  }

  sphereParticle(
    sj: Sphere,
    si: Particle,
    xj: Vec3,
    xi: Vec3,
    qj: Quaternion,
    qi: Quaternion,
    bj: Body,
    bi: Body,
    rsi?: Shape | null,
    rsj?: Shape | null,
    justTest?: boolean
  ): true | void {
    // The normal is the unit vector from sphere center to particle center
    const normal = particleSphere_normal
    normal.set(0, 0, 1)
    xi.vsub(xj, normal)
    const lengthSquared = normal.lengthSquared()

    if (lengthSquared <= sj.radius * sj.radius) {
      if (justTest) {
        return true
      }
      const r = this.createContactEquation(bi, bj, si, sj, rsi, rsj)
      normal.normalize()
      r.rj.copy(normal)
      r.rj.scale(sj.radius, r.rj)
      r.ni.copy(normal) // Contact normal
      r.ni.negate(r.ni)
      r.ri.set(0, 0, 0) // Center of particle
      this.result.push(r)
      this.createFrictionEquationsFromContact(r, this.frictionResult)
    }
  }

  planeParticle(
    sj: Plane,
    si: Particle,
    xj: Vec3,
    xi: Vec3,
    qj: Quaternion,
    qi: Quaternion,
    bj: Body,
    bi: Body,
    rsi?: Shape | null,
    rsj?: Shape | null,
    justTest?: boolean
  ): true | void {
    const normal = particlePlane_normal
    normal.set(0, 0, 1)
    bj.quaternion.vmult(normal, normal) // Turn normal according to plane orientation
    const relpos = particlePlane_relpos
    xi.vsub(bj.position, relpos)
    const dot = normal.dot(relpos)
    if (dot <= 0.0) {
      if (justTest) {
        return true
      }

      const r = this.createContactEquation(bi, bj, si, sj, rsi, rsj)
      r.ni.copy(normal) // Contact normal is the plane normal
      r.ni.negate(r.ni)
      r.ri.set(0, 0, 0) // Center of particle

      // Get particle position projected on plane
      const projected = particlePlane_projected
      normal.scale(normal.dot(xi), projected)
      xi.vsub(projected, projected)
      //projected.vadd(bj.position,projected);

      // rj is now the projected world position minus plane position
      r.rj.copy(projected)
      this.result.push(r)
      this.createFrictionEquationsFromContact(r, this.frictionResult)
    }
  }

  boxParticle(
    si: Box,
    sj: Particle,
    xi: Vec3,
    xj: Vec3,
    qi: Quaternion,
    qj: Quaternion,
    bi: Body,
    bj: Body,
    rsi?: Shape | null,
    rsj?: Shape | null,
    justTest?: boolean
  ): true | void {
    si.convexPolyhedronRepresentation.material = si.material
    si.convexPolyhedronRepresentation.collisionResponse = si.collisionResponse
    return this.convexParticle(si.convexPolyhedronRepresentation, sj, xi, xj, qi, qj, bi, bj, si, sj, justTest)
  }

  convexParticle(
    sj: ConvexPolyhedron,
    si: Particle,
    xj: Vec3,
    xi: Vec3,
    qj: Quaternion,
    qi: Quaternion,
    bj: Body,
    bi: Body,
    rsi?: Shape | null,
    rsj?: Shape | null,
    justTest?: boolean
  ): true | void {
    let penetratedFaceIndex = -1
    const penetratedFaceNormal = convexParticle_penetratedFaceNormal
    const worldPenetrationVec = convexParticle_worldPenetrationVec
    let minPenetration = null
    let numDetectedFaces = 0

    // Convert particle position xi to local coords in the convex
    const local = convexParticle_local
    local.copy(xi)
    local.vsub(xj, local) // Convert position to relative the convex origin
    qj.conjugate(cqj)
    cqj.vmult(local, local)

    if (sj.pointIsInside(local)) {
      if (sj.worldVerticesNeedsUpdate) {
        sj.computeWorldVertices(xj, qj)
      }
      if (sj.worldFaceNormalsNeedsUpdate) {
        sj.computeWorldFaceNormals(qj)
      }

      // For each world polygon in the polyhedra
      for (let i = 0, nfaces = sj.faces.length; i !== nfaces; i++) {
        // Construct world face vertices
        const verts = [sj.worldVertices[sj.faces[i][0]]]
        const normal = sj.worldFaceNormals[i]

        // Check how much the particle penetrates the polygon plane.
        xi.vsub(verts[0], convexParticle_vertexToParticle)
        const penetration = -normal.dot(convexParticle_vertexToParticle)
        if (minPenetration === null || Math.abs(penetration) < Math.abs(minPenetration)) {
          if (justTest) {
            return true
          }

          minPenetration = penetration
          penetratedFaceIndex = i
          penetratedFaceNormal.copy(normal)
          numDetectedFaces++
        }
      }

      if (penetratedFaceIndex !== -1) {
        // Setup contact
        const r = this.createContactEquation(bi, bj, si, sj, rsi, rsj)
        pe