@needle-tools/engine 2.35.5-pre → 2.37.0-pre

package/src/engine/engine_physics.ts

@@ -1,81 +1,62 @@
-import * as CANNON from 'cannon-es'
-import * as THREE from 'three'
+import { BasicDepthPacking, Box3, BufferAttribute, BufferGeometry, Camera, Intersection, Layers, LineBasicMaterial, LineSegments, Matrix4, Mesh, NormalAnimationBlendMode, Object3D, Quaternion, Ray, Raycaster, Sphere, Vector2, Vector3 } from 'three'
 import { Context } from './engine_setup';
-import cannonDebugger from 'cannon-es-debugger'
-import * as utils from "./engine_utils"
-import * as threeutils from "./engine_three_utils"
+import { getParam } from "./engine_utils"
+import { getWorldPosition, getWorldQuaternion, getWorldScale, setWorldPosition, setWorldPositionXYZ, setWorldQuaternionXYZW } from "./engine_three_utils"
 import {
-    IComponent as Component,
-    IGameObject as GameObject,
+    IComponent,
+    IGameObject,
     ICollider,
-    IRigidbody as Rigidbody, $physicsKey,
-    Collision, CannonCollision,
+    IRigidbody,
+    Collision,
     ICollisionContext,
-    IComponent
-}
-    from './engine_types';
-import { Shape } from 'cannon-es';
+    ContactPoint,
+    Vec3
+} from './engine_types';
 import { InstancingUtil } from './engine_instancing';
 import { foreachComponent } from './engine_gameobject';
-import { getComponentInChildren } from './engine_components';
-
 
-const debugPhysics = utils.getParam("debugphysics");
-const debugCollisions = utils.getParam("debugcollisions");
+import RAPIER, { ActiveEvents, Collider, ColliderDesc, EventQueue, RigidBody, TempContactManifold, World } from '@dimforge/rapier3d-compat';
+import { CollisionDetectionMode, RigidbodyConstraints } from '../engine/engine_physics.types';
+import { showBalloonWarning } from './debug/debug';
+export type Rapier = typeof RAPIER;
 
-export class BodyOptions {
-    mass: number = 1;
-    kinematic: boolean = false;
-    physicsEvents: boolean = false;
-    drag: number = 0;
-    angularDrag: number = 0.05;
-    sleepThreshold: number = .01;
-}
 
+const debugPhysics = getParam("debugphysics");
+const debugColliderPlacement = getParam("debugphysicscolliders");
+const debugCollisions = getParam("debugcollisions");
 
-// TODO: refactor to return some kind of handle for adding/removing
-class PhysicsObject {
-    obj: THREE.Object3D;
-    parent: THREE.Object3D | null;
-    body: CANNON.Body | null;
-    shapes: Array<Shape> = [];
-    collisonCallback: Function | null = null;
 
-    _hasRigidbody: boolean = false;
-    _didSleepLastStep: boolean = false;
-
-    constructor(obj: THREE.Object3D, body: CANNON.Body | null) {
-        this.obj = obj;
-        this.parent = obj.parent;
-        this.body = body;
-        if (this.body)
-            this.body[$physicsKey] = obj;
-    }
+declare type PhysicsBody = {
+    translation(): { x: number, y: number, z: number }
+    rotation(): { x: number, y: number, z: number, w: number }
 }
 
+const $componentKey = Symbol("needle component");
+const $bodyKey = Symbol("physics body");
+
 export class RaycastOptions {
-    ray: THREE.Ray | undefined = undefined;
-    cam: THREE.Camera | undefined | null = undefined;
-    screenPoint: THREE.Vector2 | undefined = undefined;
-    raycaster: THREE.Raycaster | undefined = undefined;
-    results: Array<THREE.Intersection> | undefined = undefined;
-    targets: Array<THREE.Object3D> | undefined = undefined;
+    ray: Ray | undefined = undefined;
+    cam: Camera | undefined | null = undefined;
+    screenPoint: Vector2 | undefined = undefined;
+    raycaster: Raycaster | undefined = undefined;
+    results: Array<Intersection> | undefined = undefined;
+    targets: Array<Object3D> | undefined = undefined;
     recursive: boolean | undefined = true;
     minDistance: number | undefined = undefined;
     maxDistance: number | undefined = undefined;
     lineThreshold: number | undefined = undefined;
-    layerMask: THREE.Layers | number | undefined = undefined;
-    ignore: THREE.Object3D[] | undefined = undefined;
+    layerMask: Layers | number | undefined = undefined;
+    ignore: Object3D[] | undefined = undefined;
 
     screenPointFromOffset(ox: number, oy: number) {
-        if (this.screenPoint === undefined) this.screenPoint = new THREE.Vector2();
+        if (this.screenPoint === undefined) this.screenPoint = new Vector2();
         this.screenPoint.x = ox / window.innerWidth * 2 - 1;
         this.screenPoint.y = -(oy / window.innerHeight) * 2 + 1;
     }
 
     setMask(mask: number) {
-        if (!this.layerMask) this.layerMask = new THREE.Layers();
-        const lm = this.layerMask as THREE.Layers;
+        if (!this.layerMask) this.layerMask = new Layers();
+        const lm = this.layerMask as Layers;
         if (lm)
             lm.mask = mask;
         else this.layerMask = mask;
@@ -84,11 +65,11 @@ export class RaycastOptions {
     public static AllLayers = 0xFFFFFFFF;
 }
 
-export class SphereIntersection implements THREE.Intersection {
+export class SphereIntersection implements Intersection {
     distance: number;
-    point: THREE.Vector3;
-    object: THREE.Object3D;
-    constructor(object: THREE.Object3D, distance: number, point: THREE.Vector3) {
+    point: Vector3;
+    object: Object3D;
+    constructor(object: Object3D, distance: number, point: Vector3) {
         this.object = object;
         this.distance = distance;
         this.point = point;
@@ -99,9 +80,9 @@ export class Physics {
 
     // raycasting
 
-    private readonly raycaster: THREE.Raycaster = new THREE.Raycaster();
+    private readonly raycaster: Raycaster = new Raycaster();
     private readonly defaultRaycastOptions: RaycastOptions = new RaycastOptions();
-    private readonly targetBuffer: Array<THREE.Object3D> = new Array<THREE.Object3D>(1);
+    private readonly targetBuffer: Array<Object3D> = new Array<Object3D>(1);
     private readonly defaultThresholds = {
         Mesh: {},
         Line: { threshold: 0 },
@@ -111,60 +92,56 @@ export class Physics {
     }
 
 
-    private sphereResults: Array<THREE.Intersection> = new Array<THREE.Intersection>();
-    private sphereMask: THREE.Layers = new THREE.Layers();
-    public sphereOverlap(spherePos: THREE.Vector3, radius: number): Array<THREE.Intersection> {
+    private sphereResults: Array<Intersection> = new Array<Intersection>();
+    private sphereMask: Layers = new Layers();
+    public sphereOverlap(spherePos: Vector3, radius: number, traverseChildsAfterHit: boolean = true): Array<Intersection> {
         this.sphereResults.length = 0;
         if (!this.context.scene) return this.sphereResults;
-        const sphere = new THREE.Sphere(spherePos, radius);
+        const sphere = new Sphere(spherePos, radius);
         const mask = this.sphereMask;
         mask.enableAll();
         mask.disable(2);
-        // mask testing
-        // const dummy = new THREE.Layers();
-        // dummy.set(2);
-        // console.log(dummy.mask, mask.test(dummy))
         for (const ch of this.context.scene.children) {
-            const i = this.onSphereOverlap(ch, sphere, mask);
-            if (i) this.sphereResults.push(i);
+            this.onSphereOverlap(ch, sphere, mask, this.sphereResults, traverseChildsAfterHit);
         }
         return this.sphereResults.sort((a, b) => a.distance - b.distance);
     }
-    private tempBoundingBox: THREE.Box3 = new THREE.Box3();
-    private onSphereOverlap(obj: THREE.Object3D, sp: THREE.Sphere, mask: THREE.Layers): THREE.Intersection | null {
-        if (obj.type === "Mesh") {
-            if (!obj.layers.test(mask)) return null;
-            const mesh = obj as THREE.Mesh;
+    private tempBoundingBox: Box3 = new Box3();
+    private onSphereOverlap(obj: Object3D, sp: Sphere, mask: Layers, results: Array<Intersection>, traverseChildsAfterHit: boolean): void {
+        if (obj.type === "Mesh" && obj.layers.test(mask)) {
+            const mesh = obj as Mesh;
             const geo = mesh.geometry;
             if (!geo.boundingBox)
                 geo.computeBoundingBox();
-            if (!geo.boundingBox) return null;
-            if (mesh.matrixWorldNeedsUpdate) mesh.updateMatrixWorld();
-            const test = this.tempBoundingBox.copy(geo.boundingBox).applyMatrix4(mesh.matrixWorld);
-            if (sp.intersectsBox(test)) {
-                // console.log(obj, obj.layers.test(mask), obj.layers.mask, mask.mask);
-                const wp = threeutils.getWorldPosition(obj);
-                const dist = wp.distanceTo(sp.center);
-                const int = new SphereIntersection(obj, dist, sp.center.clone());
-                return int;
+            if (geo.boundingBox) {
+                if (mesh.matrixWorldNeedsUpdate) mesh.updateMatrixWorld();
+                const test = this.tempBoundingBox.copy(geo.boundingBox).applyMatrix4(mesh.matrixWorld);
+                if (sp.intersectsBox(test)) {
+                    // console.log(obj, obj.layers.test(mask), obj.layers.mask, mask.mask);
+                    const wp = getWorldPosition(obj);
+                    const dist = wp.distanceTo(sp.center);
+                    const int = new SphereIntersection(obj, dist, sp.center.clone());
+                    results.push(int);
+                    if (!traverseChildsAfterHit) return;
+                }
             }
         }
-        else if (obj.children) {
+        if (obj.children) {
             for (const ch of obj.children) {
-                const i = this.onSphereOverlap(ch, sp, mask);
-                if (i) return i;
+                const len = results.length;
+                this.onSphereOverlap(ch, sp, mask, results, traverseChildsAfterHit);
+                if (len != results.length && !traverseChildsAfterHit) return;
             }
         }
-        return null;
     }
 
-    public raycastFromRay(ray: THREE.Ray, options: RaycastOptions | null = null): Array<THREE.Intersection> {
+    public raycastFromRay(ray: Ray, options: RaycastOptions | null = null): Array<Intersection> {
         const opts = options ?? this.defaultRaycastOptions;
         opts.ray = ray;
         return this.raycast(opts);
     }
 
-    public raycast(options: RaycastOptions | null = null): Array<THREE.Intersection> {
+    public raycast(options: RaycastOptions | null = null): Array<Intersection> {
         if (!options) options = this.defaultRaycastOptions;
         const mp = options.screenPoint ?? this.context.input.mousePositionRC;
         const rc = options.raycaster ?? this.raycaster;
@@ -194,14 +171,14 @@ export class Physics {
         let results = options.results;
         if (!results) {
             if (!this.defaultRaycastOptions.results)
-                this.defaultRaycastOptions.results = new Array<THREE.Intersection>();
+                this.defaultRaycastOptions.results = new Array<Intersection>();
             results = this.defaultRaycastOptions.results;
         }
 
         // layermask
-        // https://github.com/mrdoob/three.js/blob/master/src/core/Layers.js
+        // https://github.com/mrdoob/js/blob/master/src/core/Layers.js
         if (options.layerMask !== undefined) {
-            if (options.layerMask instanceof THREE.Layers)
+            if (options.layerMask instanceof Layers)
                 rc.layers.mask = options.layerMask.mask;
             else
                 rc.layers.mask = options.layerMask;
@@ -230,479 +207,541 @@ export class Physics {
 
     // physics simulation
 
-    get isUpdating(): boolean { return this._isUpdatingPhysicsWorld; }
+    private _tempPosition: Vector3 = new Vector3();
+    private _tempQuaternion: Quaternion = new Quaternion();
+    private _tempMatrix: Matrix4 = new Matrix4();
+
+    private static _didLoadPhysicsEngine: boolean = false;
 
     private _isUpdatingPhysicsWorld: boolean = false;
+    get isUpdating(): boolean { return this._isUpdatingPhysicsWorld; }
+
 
     private context: Context;
+    private world?: World;
+    private _hasCreatedWorld: boolean = false;
+    private eventQueue?: EventQueue;
+    private collisionHandler?: PhysicsCollisionHandler;
+
 
-    private world: CANNON.World = new CANNON.World();
-    private objects: Array<PhysicsObject> = [];
+    // private rigidbodies: Array<IRigidbody | null> = [];
+    private objects: IComponent[] = [];
+    private bodies: PhysicsBody[] = [];
+    // private rigidbodiesLookup: Map<IRigidbody, RigidBody> = new Map<IRigidbody, RigidBody>();
+    // private kinematicColliders: Array<IComponent> = [];
+    // private rigidbodyLookup: Map<IRigidbody, IComponent[]> = new Map<IRigidbody, IComponent[]>();
+    // private objectLookup: Map<Object3D, IRigidbody> = new Map<Object3D, IRigidbody>();
 
-    private tempPosition: THREE.Vector3 = new THREE.Vector3();
-    private tempQuaternion: THREE.Quaternion = new THREE.Quaternion();
 
     constructor(context: Context) {
         this.context = context;
-        this.world.gravity.set(0, -9.82, 0);
-        if (debugPhysics) {
-            // https://www.npmjs.com/package/cannon-es-debugger
-            const opts = {};
-            opts["onInit"] = (_body: CANNON.Body, mesh: THREE.Mesh, _shape: CANNON.Shape) => {
-                // ignore in raycast
-                mesh.layers.set(-1);
-            };
-            cannonDebugger(context.scene, this.world.bodies, opts);
-        }
-
-        this.world.addEventListener("beginContact", this.onBeginContact.bind(this));
-        this.world.addEventListener("endContact", this.onEndContact.bind(this))
-    }
-
-    public addPreStepListener(listener: (evt) => void) {
-        this.world.addEventListener("preStep", listener);
-    }
-
-    public addPostStepListener(listener: (evt) => void) {
-        this.world.addEventListener("postStep", listener);
     }
 
-    public addConstraint(constraint: CANNON.Constraint) {
-        this.world.addConstraint(constraint);
+    async createWorld() {
+        if (this._hasCreatedWorld) {
+            console.error("Invalid call to create physics world: world is already created");
+            return;
+        }
+        this._hasCreatedWorld = true;
+        if (!Physics._didLoadPhysicsEngine) {
+            await RAPIER.init().then(() => RAPIER)
+            Physics._didLoadPhysicsEngine = true;
+        }
+        const gravity = { x: 0.0, y: -9.81, z: 0.0 };
+        this.world = new World(gravity);
     }
 
-    public setGravity(vec: THREE.Vector3) {
-        this.world.gravity.set(vec.x, vec.y, vec.z);
+    addBoxCollider(collider: ICollider, center: Vector3, size: Vector3) {
+        const obj = collider.gameObject;
+        const scale = getWorldScale(obj, this._tempPosition).multiply(size);
+        scale.multiplyScalar(0.5);
+        const desc = ColliderDesc.cuboid(scale.x, scale.y, scale.z);
+        this.createCollider(collider, desc, center);
     }
 
-    public multiplyGravity(vec: THREE.Vector3) {
-        this.world.gravity.x *= vec.x;
-        this.world.gravity.y *= vec.y;
-        this.world.gravity.z *= vec.z;
+    addSphereCollider(collider: ICollider, center: Vector3, radius: number) {
+        const obj = collider.gameObject;
+        const scale = getWorldScale(obj, this._tempPosition).multiplyScalar(radius);
+        const desc = ColliderDesc.ball(scale.x);
+        this.createCollider(collider, desc, center);
     }
 
-    public addBody(go: GameObject, body: CANNON.Body) {
-        for (let i = 0; i < this.objects.length; i++) {
-            const reg = this.objects[i];
-            if (reg.obj === go) {
-                reg._hasRigidbody = true;
-                break;
-            }
-        }
-        // dont add the body before it has shapes
-        // otherwise things like forces appplied in the frame before the shapes exist will be zeroed out
-        if (body.shapes.length > 0)
-            this.world.addBody(body);
-    }
-
-    public removeBody(go: GameObject, body: CANNON.Body, removeCompletely: boolean = true) {
-        this.world.removeBody(body);
-        for (let i = 0; i < this.objects.length; i++) {
-            const reg = this.objects[i];
-            if (reg.obj === go) {
-                reg._hasRigidbody = false;
-                if (removeCompletely)
-                    this.objects.splice(i, 1);
-                break;
-            }
+    addMeshCollider(collider: ICollider, mesh: Mesh, convex: boolean) {
+        const geo = mesh.geometry;
+        if (!geo) {
+            if (debugPhysics) console.warn("Missing mesh geometry", mesh.name);
+            return;
         }
-    }
 
-    public removeShape(obj: THREE.Object3D, shape: CANNON.Shape) {
-        for (const reg of this.objects) {
-            if (reg.obj === obj) {
-                if (reg.body) {
-                    reg.body.removeShape(shape);
-                    reg.body.updateMassProperties();
-                }
-                return;
+        let positions = geo.getAttribute("position").array as Float32Array;
+        const indices = geo.index?.array as Uint32Array;
+
+        // console.log(geo.center())
+
+        // scaling seems not supported yet https://github.com/dimforge/rapier/issues/243
+        const scale = getWorldScale(mesh, this._tempPosition)
+        if (Math.abs(scale.x - 1) > 0.0001 || Math.abs(scale.y - 1) > 0.0001 || Math.abs(scale.z - 1) > 0.0001) {
+            console.warn("Your model is using scaled mesh colliders which is not optimal for performance", mesh.name, Object.assign({}, scale), mesh);
+            // showBalloonWarning("Your model is using scaled mesh colliders which is not optimal for performance: " + mesh.name + ", consider using unscaled objects");
+            const scaledPositions = new Float32Array(positions.length);
+            for (let i = 0; i < positions.length; i += 3) {
+                scaledPositions[i] = positions[i] * scale.x;
+                scaledPositions[i + 1] = positions[i + 1] * scale.y;
+                scaledPositions[i + 2] = positions[i + 2] * scale.z;
             }
+            positions = scaledPositions;
         }
-    }
-
-    // TODO: make it work with rigibody in parent
-    public createBody(obj: THREE.Object3D, settings: BodyOptions): CANNON.Body {
-        const body = this.internalCreateBody(obj, null);
-        if (settings.mass)
-            body.mass = settings.mass;
-        if (settings.kinematic)
-            body.type = CANNON.Body.KINEMATIC;
-        else body.type = CANNON.Body.DYNAMIC;
-        if (settings.drag)
-            body.linearDamping = settings.drag;
-        if (settings.angularDrag)
-            body.angularDamping = settings.angularDrag;
-        if (settings.sleepThreshold)
-            body.sleepSpeedLimit = settings.sleepThreshold;
 
-        if (body.shapes.length > 0)
-            this.world.addBody(body);
-        const po = new PhysicsObject(obj, body);
-        po._hasRigidbody = true;
-        this.objects.push(po);
+        const desc = convex ? ColliderDesc.convexMesh(positions) : ColliderDesc.trimesh(positions, indices);
+        if (desc) {
+            this.createCollider(collider, desc);
+            // col.setTranslationWrtParent(new Vector3(0,2,0));
 
-        if (debugPhysics) {
-            console.log("created new body", obj.name, body, body.sleepState, this.world.gravity);
         }
-
-        if (settings.physicsEvents)
-            this.registerCollisionEvents(po);
-
-        return body;
     }
 
-    public addBoxCollider(obj: THREE.Object3D, trigger: boolean, center: THREE.Vector3, size: THREE.Vector3, rb: Rigidbody | null): CANNON.Shape {
-
-        const scale = this.tempPosition;
-        obj.getWorldScale(scale);
+    private createCollider(collider: ICollider, desc: ColliderDesc, center?: Vector3) {
+        if (!this.world) throw new Error("Physics world not initialized");
+        const matrix = this._tempMatrix;
+        const {
+            rigidBody,
+            useExplicitMassProperties
+        } = this.getRigidbody(collider, this._tempMatrix);
 
-        const pos = new CANNON.Vec3(
-            .5 * scale.x * size.x,
-            .5 * scale.y * size.y,
-            .5 * scale.z * size.z
-        );
-        const shape = new CANNON.Box(pos);
-        shape.collisionResponse = !trigger;
+        matrix.decompose(this._tempPosition, this._tempQuaternion, new Vector3());
+        if (center)
+            this._tempPosition.add(center);
+        desc.setTranslation(this._tempPosition.x, this._tempPosition.y, this._tempPosition.z);
+        desc.setRotation(this._tempQuaternion);
 
-        center = center.clone();
-        center.multiply(scale);
+        desc.setSensor(collider.isTrigger);
 
-        const body = this.addShape(obj, shape, center, rb);
-        if (body !== null) {
-            this.world.addBody(body);
-            if (this.isAlreadyRegistered(body)) return shape;
-            const po = new PhysicsObject(obj, body);
-            this.objects.push(po);
+        // if we want to use explicit mass properties, we need to set the collider density to 0
+        // otherwise rapier will compute the mass properties based on the collider shape and density
+        // https://rapier.rs/docs/user_guides/javascript/rigid_bodies#mass-properties
+        if (useExplicitMassProperties) {
+            // desc.setDensity(0);
         }
-        return shape;
-    }
-
-    public addSphereCollider(obj: THREE.Object3D, center: THREE.Vector3, radius: number, rb: Rigidbody | null): CANNON.Shape {
-        const scale = this.tempPosition;
-        obj.getWorldScale(scale);
 
-        const factor = Math.max(scale.x, scale.y, scale.z);
-        const shape = new CANNON.Sphere(radius * factor);
-        // shape.collisionResponse = !trigger;
-
-        center = center.clone();
-        center.multiply(scale);
+        const col = this.world.createCollider(desc, rigidBody);
+        col[$componentKey] = collider;
+        collider[$bodyKey] = col;
+        col.setActiveEvents(ActiveEvents.COLLISION_EVENTS);
+        this.objects.push(collider);
+        this.bodies.push(col);
+        return col;
+    }
+
+    private getRigidbody(collider: ICollider, _matrix: Matrix4): { rigidBody: RigidBody, useExplicitMassProperties: boolean } {
+
+        if (!this.world) throw new Error("Physics world not initialized");
+        let rigidBody: RigidBody | null = null;
+        let useExplicitMassProperties = false;
+
+        if (collider.attachedRigidbody) {
+
+            const rb = collider.attachedRigidbody;
+            rigidBody = rb[$bodyKey];
+            useExplicitMassProperties = true;
+            if (!rigidBody) {
+                const kinematic = rb.isKinematic && !debugColliderPlacement;
+                if (debugPhysics)
+                    console.log("Create rigidbody", kinematic);
+                const rigidBodyDesc = kinematic ? RAPIER.RigidBodyDesc.kinematicPositionBased() : RAPIER.RigidBodyDesc.dynamic();
+                const pos = getWorldPosition(collider.attachedRigidbody.gameObject);
+                rigidBodyDesc.setTranslation(pos.x, pos.y, pos.z);
+                rigidBodyDesc.setRotation(getWorldQuaternion(collider.attachedRigidbody.gameObject));
+                rigidBody = this.world.createRigidBody(rigidBodyDesc);
+                this.bodies.push(rigidBody);
+                this.objects.push(rb);
+            }
+            rigidBody[$componentKey] = rb;
+            rb[$bodyKey] = rigidBody;
+            this.internalUpdateProperties(rb, rigidBody);
+            this.getRigidbodyRelativeMatrix(collider.gameObject, rb.gameObject, _matrix);
 
-        const body = this.addShape(obj, shape, center, rb);
-        if (body !== null) {
-            this.world.addBody(body);
-            if (this.isAlreadyRegistered(body)) return shape;
-            const po = new PhysicsObject(obj, body);
-            this.objects.push(po);
         }
-        return shape;
-    }
-
-    public addMeshCollider(_obj: THREE.Object3D) {
-        // see https://github.com/schteppe/cannon.js/blob/master/demos/bunny.html
-        if (debugPhysics)
-            console.warn("TODO mesh collider not yet supported")
-        // const geometry: THREE.BufferGeometry = obj["geometry"];
-        // console.log(geometry);
-        // const size = geometry.boundingBox.max.clone();
-        // size.sub(geometry.boundingBox.min);
-        // console.log(size);
-        // this.addBoxCollider(obj, size);
-
-        // const verts = geometry.getAttribute("position").array;
-        // const faces = new Array<Array<number>>();
+        else {
 
-        // console.log(geometry);
+            const rigidBodyDesc = RAPIER.RigidBodyDesc.kinematicPositionBased();
+            const pos = getWorldPosition(collider.gameObject);
+            rigidBodyDesc.setTranslation(pos.x, pos.y, pos.z);
+            rigidBodyDesc.setRotation(getWorldQuaternion(collider.gameObject));
+            rigidBody = this.world.createRigidBody(rigidBodyDesc);
+            _matrix.identity();
+            rigidBody[$componentKey] = null;
 
-        // for (let i = 0; i < geometry.index.array.length; i += 3) {
-        //     const i0 = geometry.index.array[i];
-        //     const i1 = geometry.index.array[i + 1];
-        //     const i2 = geometry.index.array[i + 2];
-        //     const v0 = new THREE.Vector3(verts[i0 * 3], verts[i0 * 3 + 1], verts[i0 * 3 + 2]);
-        //     const v1 = new THREE.Vector3(verts[i1 * 3], verts[i1 * 3 + 1], verts[i1 * 3 + 2]);
-        //     const v2 = new THREE.Vector3(verts[i2 * 3], verts[i2 * 3 + 1], verts[i2 * 3 + 2]);
-        //     const face = [v0, v1, v2];
-        //     faces.push(face);
-        // }
-        // const convex = new THREE.ConvexBufferGeometry(faces);
+        }
 
-        // var shape = new CANNON.ConvexPolyhedron({ verts, faces });
-        // this.addShape(obj, shape);
+        return { rigidBody: rigidBody, useExplicitMassProperties: useExplicitMassProperties };
     }
 
-    private isAlreadyRegistered(body: CANNON.Body): boolean {
-        for (const obj of this.objects) {
-            if (obj.body === body) return true;
+    removeBody(obj: IComponent) {
+        const body = obj[$bodyKey];
+        obj[$bodyKey] = null;
+        if (body) {
+            const index = this.objects.findIndex(o => o === obj);
+            if (index >= 0) {
+                const body = this.bodies[index];
+                this.bodies.splice(index, 1);
+                this.objects.splice(index, 1);
+
+                if (body instanceof Collider) {
+                    this.world?.removeCollider(body as Collider, true);
+                }
+                else if (body instanceof RigidBody) {
+                    this.world?.removeRigidBody(body as RigidBody);
+                }
+
+                // check if we need to remove the rigidbody too
+                const col = obj as ICollider;
+                if (col.isCollider && col.attachedRigidbody) {
+                    const rb = col.attachedRigidbody[$bodyKey];
+                    if (rb && rb.numColliders() <= 0 && rb.world() === this.world) {
+                        this.world?.removeRigidBody(rb);
+                    }
+                }
+            }
         }
-        return false;
     }
 
-    private readonly tempMat1: THREE.Matrix4 = new THREE.Matrix4();
-    private readonly tempMat2: THREE.Matrix4 = new THREE.Matrix4();
-
-    private addShape(obj: THREE.Object3D, shape: CANNON.Shape, center: THREE.Vector3, rb: Rigidbody | null): CANNON.Body | null {
-
-        let body: CANNON.Body | null = null;
+    updateBody(comp: ICollider | IRigidbody, translation: boolean, rotation: boolean) {
+        if (comp.destroyed || !comp.gameObject) return;
+        if (!translation && !rotation) return;
 
-        if (rb) {
-            // if (debugPhysics)
-            //     console.log("get rb body", rb);
-            rb.initialize();
-            console.assert(rb.body ? true : false, "rigidbody didn't initialize / produce a physics body", rb);
-            body = rb.body;
+        if ((comp as ICollider).isCollider === true) {
+            // const collider = comp as ICollider;
+            console.warn("TODO: implement updating collider position");
         }
         else {
-            // console.log("has no rb", obj);
-            body = this.internalCreateBody(obj, null);
-            body.type = CANNON.Body.KINEMATIC;
-        }
-
-        if (body) {
-            // console.log(obj.name, obj.position, obj.rotation)
-
-            // the center is serialized from Unity so we need to move it into threejs space
-            // this should probably happen on export for colliders
-            center.x *= -1;
-
-            let wp = obj.position;
-            let wr = obj.quaternion;
-
-            // console.log(obj.name, wp)
-
-            if (rb && rb.gameObject !== obj) {
-                this.tempMat1.copy(obj.matrixWorld);
-                this.tempMat2.copy(rb.gameObject.matrixWorld).invert();
-                this.tempMat1.premultiply(this.tempMat2);
-                this.tempMat1.decompose(wp, wr, this.tempPosition);
-            }
-            else {
-                wp = threeutils.getWorldPosition(obj);
-                const bp = body.position;
-                wp.x -= bp.x;
-                wp.y -= bp.y;
-                wp.z -= bp.z;
-
-                wr = threeutils.getWorldQuaternion(obj);
-                const r = new THREE.Quaternion(body.quaternion.x, body.quaternion.y, body.quaternion.z, body.quaternion.w);
-                wr.multiply(r.invert());
+            const rigidbody = comp as IRigidbody;
+            const body = rigidbody[$bodyKey];
+            if (body) {
+                this.syncPhysicsBody(rigidbody.gameObject, body, translation, rotation);
             }
-            // get rotation difference
-
-            wp.add(center);
+        }
+    }
 
+    updateProperties(rigidbody: IRigidbody) {
+        const physicsBody = rigidbody[$bodyKey]
+        if (physicsBody) {
+            this.internalUpdateProperties(rigidbody, physicsBody);
+        }
+    }
 
+    internal_getRigidbody(rb: IRigidbody): RigidBody | null {
+        return rb[$bodyKey] as RigidBody;
+    }
 
+    private internalUpdateProperties(rb: IRigidbody, rigidbody: RigidBody) {
+        // continuous collision detection 
+        // https://rapier.rs/docs/user_guides/javascript/rigid_bodies#continuous-collision-detection
+        rigidbody.enableCcd(rb.collisionDetectionMode !== CollisionDetectionMode.Discrete);
+        rigidbody.setLinearDamping(rb.drag);
+        rigidbody.setAngularDamping(rb.angularDrag);
+        rigidbody.setGravityScale(rb.useGravity ? 1 : 0, true);
 
-            // if (rb) {
-            //     this.tempMat.setPosition(wp);
-            //     this.tempMat.makeRotationFromQuaternion(wr);
-            //     this.tempMat.multiplyMatrices(this.tempMat, rb?.gameObject.matrix);
-            //     this.tempMat.decompose(this.tempPosition, this.tempQuaternion, new THREE.Vector3());
-            //     wp.copy(this.tempPosition);
-            // }
+        // https://rapier.rs/docs/user_guides/javascript/rigid_bodies#mass-properties
+        // rigidbody.setAdditionalMass(rb.mass, true);
+        // for (let i = 0; i < rigidbody.numColliders(); i++) {
+        //     const collider = rigidbody.collider(i);
+        //     if (collider) {
+        //         collider.setMass(rb.mass);
+        //         // const density = rb.mass / collider.shape.computeMassProperties().mass;
+        //     }
+        // }
 
-            // wp.applyQuaternion(wr);
+        // lock rotations
+        rigidbody.setEnabledRotations(!rb.lockRotationX, !rb.lockRotationY, !rb.lockRotationZ, true);
+        rigidbody.setEnabledTranslations(!rb.lockPositionX, !rb.lockPositionY, !rb.lockPositionZ, true);
 
-            const pos = new CANNON.Vec3(wp.x, wp.y, wp.z);
-            const rot = new CANNON.Quaternion(wr.x, wr.y, wr.z, wr.w);
-            body.addShape(shape, pos, rot);
-            body.updateMassProperties();
-            this.world.addBody(body);
+        if (rb.isKinematic) {
+            rigidbody.setBodyType(RAPIER.RigidBodyType.KinematicPositionBased);
+        }
+        else {
+            rigidbody.setBodyType(RAPIER.RigidBodyType.Dynamic);
         }
-        return body;
     }
 
     // private _lastStepTime: number | undefined = 0;
+    private lines?: LineSegments;
 
-    public step(deltaTime: number) {
+    public step(_deltaTime?: number) {
+        if (!this.world) return;
         this._isUpdatingPhysicsWorld = true;
-        deltaTime = Math.min(deltaTime, 1 / 30);
-        this.world.step(deltaTime);
-        this._isUpdatingPhysicsWorld = false;
-        if (debugPhysics && this.context.time.frameCount % 60 === 0) {
-            // console.log("physics world has " + this.world.bodies.length + " bodies", this.world);
+        if (!this.eventQueue) {
+            this.eventQueue = new EventQueue(false);
         }
+        this.world.step(this.eventQueue);
+        this._isUpdatingPhysicsWorld = false;
+        this.updateDebugRendering(this.world);
     }
 
-    private temp: THREE.Vector3 = new THREE.Vector3();
-    private tempQuat: THREE.Quaternion = new THREE.Quaternion();
+    private updateDebugRendering(world: World) {
+        if (debugPhysics || debugColliderPlacement) {
+            if (!this.lines) {
+                let material = new LineBasicMaterial({
+                    color: 0xffffff,
+                    // vertexColors: THREE.VertexColors
+                });
+                let geometry = new BufferGeometry();
+                this.lines = new LineSegments(geometry, material);
+                this.context.scene.add(this.lines);
+            }
+            const buffers = world.debugRender();
+            this.lines.geometry.setAttribute('position', new BufferAttribute(buffers.vertices, 3));
+            this.lines.geometry.setAttribute('color', new BufferAttribute(buffers.colors, 4));
+        }
+    }
 
     public postStep() {
+        if (!this.world) return;
         this._isUpdatingPhysicsWorld = true;
-        for (let i = 0; i < this.objects.length; i++) {
-            const entry = this.objects[i];
-            const body = entry.body;
-            if (!body || !body.world) continue;
+        this.syncObjects();
+        this._isUpdatingPhysicsWorld = false;
 
-            body.sleepTick(this.context.time.time);
+        if (this.eventQueue && !this.collisionHandler) {
+            this.collisionHandler = new PhysicsCollisionHandler(this.world, this.eventQueue);
+        }
+        if (this.collisionHandler) {
+            this.collisionHandler.handleCollisionEvents();
+            this.collisionHandler.update();
+        }
+    }
 
-            if (debugPhysics) {
-                if (!entry._didSleepLastStep && body.sleepState === CANNON.Body.SLEEPING) {
-                    console.log("BODY SLEEPING", body);
-                }
-                else if (entry._didSleepLastStep && body.sleepState !== CANNON.Body.SLEEPING) {
-                    console.log("BODY WOKE UP", body);
-                }
-            }
-            entry._didSleepLastStep = body.sleepState === CANNON.Body.SLEEPING;
-            // if(body.sleepState === CANNON.Body.SLEEPING) {
-            //     console.log("SLEEP", body.name);
-            // }
-            // if (body.type == CANNON.Body.KINEMATIC) continue;
-            const obj = entry.obj;
-
-            if (body.type === CANNON.Body.KINEMATIC) {
-                const wp = threeutils.getWorldPosition(obj, this.temp);
-                body.position.set(wp.x, wp.y, wp.z);
-                const rot = threeutils.getWorldQuaternion(obj, this.tempQuat);
-                body.quaternion.set(rot.x, rot.y, rot.z, rot.w);
+    /** sync rendered objects with physics world (except for colliders without rigidbody) */
+    private syncObjects() {
+        if (debugColliderPlacement) return;
+        for (let i = 0; i < this.bodies.length; i++) {
+            const obj = this.objects[i];
+            const body = this.bodies[i] as Collider;
+
+            // if the collider is not attached to a rigidbody
+            // it means that its kinematic so we need to update its position
+            const col = (obj as ICollider);
+            if (col?.isCollider === true && !col.attachedRigidbody) {
+                const rigidbody = body.parent();
+                if (rigidbody)
+                    this.syncPhysicsBody(obj.gameObject, rigidbody, true, true);
                 continue;
             }
 
+            // sync
+            const pos = body.translation();
+            setWorldPositionXYZ(obj.gameObject, pos.x, pos.y, pos.z);
+            const rot = body.rotation();
+            setWorldQuaternionXYZW(obj.gameObject, rot.x, rot.y, rot.z, rot.w);
+        }
+    }
 
+    private syncPhysicsBody(obj: Object3D, body: RigidBody, translation: boolean, rotation: boolean) {
+
+        // const bodyType = body.bodyType();
+        // const previous = physicsBody.translation();
+        // const vel = physicsBody.linvel();
+
+        const worldPosition = getWorldPosition(obj, this._tempPosition);
+        const worldQuaternion = getWorldQuaternion(obj, this._tempQuaternion);
+        // physicsBody.setBodyType(RAPIER.RigidBodyType.Fixed);
+        if (translation)
+            body.setTranslation(worldPosition, false);
+        if (rotation)
+            body.setRotation(worldQuaternion, false);
+        // physicsBody.setLinvel(vel, false);
+        body.wakeUp();
+
+        // update velocity
+        // const pos = physicsBody.translation();
+        // pos.x -= previous.x;
+        // pos.y -= previous.y;
+        // pos.z -= previous.z;
+        // // threhold
+        // const t = 1;
+        // const canUpdateVelocity = Math.abs(pos.x) < t && Math.abs(pos.y) < t && Math.abs(pos.z) < t;
+        // if (canUpdateVelocity) {
+        //     const damping = 1 + this.context.time.deltaTime;
+        //     vel.x *= damping;
+        //     vel.y *= damping;
+        //     vel.z *= damping;
+        //     vel.x += pos.x;
+        //     vel.y += pos.y;
+        //     vel.z += pos.z;
+        //     console.log(vel);
+        //     physicsBody.setLinvel(vel, true);
+        // }
+        // else if(debugPhysics) console.warn("Movement exceeded threshold, not updating velocity", pos);
 
-            // when reparenting (e.g. attached to controller) I think it doesnt work with previous parent? need to test again, to tired now
-            if (entry.parent && obj.parent === entry.parent) {
-
-                threeutils.setWorldQuaternionXYZW(obj,
-                    body.quaternion.x, body.quaternion.y, body.quaternion.z, body.quaternion.w
-                );
-
-                const p = body.position;
-                threeutils.setWorldPositionXYZ(obj, p.x, p.y, p.z);
+        // body.setBodyType(bodyType);
+    }
 
-                if (body.velocity.length() > body.sleepSpeedLimit) {
-                    InstancingUtil.markDirty(obj);
-                }
-                // this.worldToLocal.x = body.position.x;
-                // this.worldToLocal.y = body.position.y;
-                // this.worldToLocal.z = body.position.z;
-                // const pos = entry.parent.worldToLocal(this.worldToLocal);
-                // obj.position.x = pos.x;
-                // obj.position.y = pos.y;
-                // obj.position.z = pos.z;
-
-                // if (entry.center) {
-                //     this.rotatedCenter.copy(entry.center);
-                //     const rot = this.tempQuaternion;
-                //     rot.copy(obj.quaternion);
-                //     // obj.getWorldQuaternion(this.tempQuaternion)
-                //     this.rotatedCenter.applyQuaternion(rot);
-                //     obj.getWorldScale(this.tempVector);
-                //     this.rotatedCenter.divide(this.tempVector);
-                //     obj.position.sub(this.rotatedCenter);
-                // }
+    private static _matricesBuffer: Matrix4[] = [];
+    private getRigidbodyRelativeMatrix(comp: Object3D, rigidbody: Object3D, mat: Matrix4, matrices?: Matrix4[]): Matrix4 {
+        // collect all matrices to the rigidbody and then build the rigidbody relative matrix
+        if (matrices === undefined) {
+            matrices = Physics._matricesBuffer;
+            matrices.length = 0;
+        }
+        if (comp === rigidbody) {
+            const scale = getWorldScale(comp, this._tempPosition);
+            mat.makeScale(scale.x, scale.y, scale.z);
+            for (let i = matrices.length - 1; i >= 0; i--) {
+                mat.multiply(matrices[i]);
             }
+            return mat;
         }
-        this._isUpdatingPhysicsWorld = false;
+        matrices.push(comp.matrix);
+        if (comp.parent) {
+            this.getRigidbodyRelativeMatrix(comp.parent, rigidbody, mat, matrices);
+        }
+        return mat;
     }
 
-    private internalCreateBody(obj: THREE.Object3D, shape: CANNON.Shape | undefined | null): CANNON.Body {
 
-        const body = new CANNON.Body();
-        body["_owner"] = obj;
-        body["_name"] = obj.name;
-        obj.getWorldPosition(this.tempPosition);
-        const pos = this.tempPosition;
-        body.position = new CANNON.Vec3(pos.x, pos.y, pos.z);
+}
 
-        const quat = this.tempQuaternion;
-        obj.getWorldQuaternion(quat);
-        body.quaternion = new CANNON.Quaternion(quat.x, quat.y, quat.z, quat.w);
 
-        body.type = CANNON.Body.KINEMATIC;
-        if (shape) {
-            body.addShape(shape);
-            body.updateMassProperties();
-        }
-        return body;
-    }
+export interface IColliderProvider {
+    getCollider(obj: Object3D): ICollider;
+}
 
-    // private findObject(obj: THREE.Object3D): PhysicsObject | null {
-    //     for (let i = 0; i < this.objects.length; i++) {
-    //         const entry = this.objects[i];
-    //         if (entry.obj == obj)
-    //             return entry;
-    //     }
-    //     return null;
-    // }
+let colliderProvider: IColliderProvider | null = null;
+export function registerColliderProvider(prov: IColliderProvider) {
+    colliderProvider = prov;
+}
 
-    private registerCollisionEvents(obj: PhysicsObject) {
-        if (obj.collisonCallback) this.unregisterCollisionEvents(obj);
-        if (!obj.body) return;
-        const evt = evt => this.raiseCollisionEvents(obj.obj, evt);
-        obj.collisonCallback = evt.bind(this);
-        obj.body.addEventListener("collide", obj.collisonCallback);
-    }
+class CollisionContext implements ICollisionContext {
 
-    private unregisterCollisionEvents(obj: PhysicsObject) {
-        if (!obj.collisonCallback) return;
-        if (!obj.body) return;
-        obj.body.removeEventListener("collide", obj.collisonCallback);
+    getCollider(obj: Object3D<Event>): ICollider {
+        return colliderProvider!.getCollider(obj);
     }
 
-    private onBeginContact(_) {
-        // this is called after the object collide event so we dont really need it
-        // console.log("START");
-    }
+}
 
-    private readonly collisionContext: ICollisionContext = new CollisionContext();
 
-    private raiseCollisionEvents(obj: THREE.Object3D, event: CannonCollision) {
-        const collision = new Collision(obj, event, this.collisionContext);
-        if (debugCollisions)
-            console.log("collision between", event.contact.bi, event.contact.bj, obj, event);
-        foreachComponent(obj, (c: Component) => {
-            c.__internalHandleCollision(collision, false);
-        });
 
-        // handle triggers
-        if (collision.collider && !collision.collider.attachedRigidbody && collision.collider.isTrigger) {
-            const collision2 = new Collision(collision.gameObject, event, this.collisionContext, true);
-            foreachComponent(collision.gameObject, (c: Component) => {
-                c.__internalHandleCollision(collision2, true);
-            });
-        }
-    }
 
-    private onEndContact(args: { bodyA: CANNON.Body, bodyB: CANNON.Body }) {
-        // if(args.bodyB.sleepState !== CANNON.Body.AWAKE) return;
-        // console.log("END", CANNON.BODY_SLEEP_STATES, args.bodyB.sleepState);
-        const obj1 = args.bodyA[$physicsKey];
-        const obj2 = args.bodyB[$physicsKey];
-        // console.log(obj2);
+/** responsible of processing collision events for the component system */
+class PhysicsCollisionHandler {
 
-        foreachComponent(obj2, (c: Component) => {
-            c.__internalHandleExitCollisionEvent(obj1, false);
-        });
+    readonly world: World;
+    readonly eventQueue: EventQueue;
 
-        // TODO: stop iterating when we found the collider
-        foreachComponent(obj1, c => {
-            const collider = c as ICollider;
-            if (collider.isCollider && !collider.attachedRigidbody && collider.isTrigger) {
-                foreachComponent(collider.gameObject, (c: Component) => {
-                    c.__internalHandleExitCollisionEvent(obj2, true);
-                });
+    constructor(world: World, eventQueue: EventQueue) {
+        this.world = world;
+        this.eventQueue = eventQueue;
+    }
+
+    private activeCollisions: Array<{ collider: ICollider, component: IComponent, collision: Collision }> = [];
+    private activeTriggers: Array<{ collider: ICollider, component: IComponent, otherCollider: ICollider }> = [];
+
+    handleCollisionEvents() {
+        if (!this.eventQueue) return;
+        if (!this.world) return;
+        this.eventQueue.drainCollisionEvents((handle1, handle2, started) => {
+            const col1 = this.world!.getCollider(handle1);
+            const col2 = this.world!.getCollider(handle2);
+            const colliderComponent1 = col1[$componentKey];
+            const colliderComponent2 = col2[$componentKey];
+            // console.log("EVT", colliderComponent1.name, colliderComponent2.name, started);
+            if (colliderComponent1 && colliderComponent2) {
+                if (started) {
+                    this.onCollisionStarted(colliderComponent1, col1, colliderComponent2, col2);
+                    this.onCollisionStarted(colliderComponent2, col2, colliderComponent1, col1);
+                }
+                else {
+                    this.onCollisionEnded(colliderComponent1, colliderComponent2);
+                    this.onCollisionEnded(colliderComponent2, colliderComponent1);
+                }
             }
         });
     }
 
-}
-
-export interface IColliderProvider {
-    getCollider(obj: THREE.Object3D): ICollider;
-}
+    update() {
+        this.onHandleCollisionStay();
+    }
 
-let colliderProvider: IColliderProvider | null = null;
-export function registerColliderProvider(prov: IColliderProvider) {
-    colliderProvider = prov;
-}
+    private onCollisionStarted(self: ICollider, selfBody: Collider, other: ICollider, otherBody: Collider) {
+        let collision: Collision | null = null;
 
-class CollisionContext implements ICollisionContext {
+        // if one is a trigger we dont get collisions but want to raise the trigger events
+        if (self.isTrigger || other.isTrigger) {
+            foreachComponent(self.gameObject, (c: IComponent) => {
+                if (c.onTriggerEnter) {
+                    c.onTriggerEnter(other);
+                }
+                this.activeTriggers.push({ collider: self, component: c, otherCollider: other });
+            });
+        }
+        else {
+            const object = self.gameObject;
+            // TODO: we dont respect the flip value here!
+            this.world.contactPair(selfBody, otherBody, (manifold, _flipped) => {
+                foreachComponent(object, (c: IComponent) => {
+                    if (c.onCollisionEnter) {
+                        if (!collision) {
+                            const contacts: Array<ContactPoint> = [];
+                            const normal = manifold.normal();
+                            for (let i = 0; i < manifold.numContacts(); i++) {
+                                const pt1 = manifold.localContactPoint1(i);
+                                const dist = manifold.contactDist(i);
+                                if (pt1) {
+                                    const contact = new ContactPoint(pt1, dist, normal);
+                                    contacts.push(contact);
+                                }
+                            }
+                            collision = new Collision(object, other, contacts);
+                        }
+                        c.onCollisionEnter.call(c, collision);
+                        this.activeCollisions.push({ collider: self, component: c, collision });
+                    }
+                });
+            });
+        }
+    }
 
-    getCollider(obj: THREE.Object3D<THREE.Event>): ICollider {
-        return colliderProvider!.getCollider(obj);
+    private onHandleCollisionStay() {
+        for (const active of this.activeCollisions) {
+            const c = active.component;
+            if (c.activeAndEnabled && c.onCollisionStay) {
+                const arg = active.collision;
+                c.onCollisionStay(arg);
+            }
+        }
+        for (const active of this.activeTriggers) {
+            const c = active.component;
+            if (c.activeAndEnabled && c.onTriggerStay) {
+                const arg = active.collider;
+                c.onTriggerStay(arg);
+            }
+        }
     }
 
+    private onCollisionEnded(self: ICollider, other: ICollider) {
+        for (let i = 0; i < this.activeCollisions.length; i++) {
+            const active = this.activeCollisions[i];
+            const collider = active.collider;
+            if (collider === self && active.collision.collider === other) {
+                const c = active.component;
+                this.activeCollisions.splice(i, 1);
+                i--;
+                if (c.activeAndEnabled && c.onCollisionExit) {
+                    const collision = active.collision;
+                    c.onCollisionExit(collision);
+                }
+            }
+        }
+        for (let i = 0; i < this.activeTriggers.length; i++) {
+            const active = this.activeTriggers[i];
+            const collider = active.collider;
+            if (collider === self && active.otherCollider === other) {
+                const c = active.component;
+                this.activeTriggers.splice(i, 1);
+                i--;
+                if (c.activeAndEnabled && c.onTriggerExit) {
+                    const collision = active.otherCollider;
+                    c.onTriggerExit(collision);
+                }
+            }
+        }
+    }
 }
-