@needle-tools/engine 2.36.0-pre → 2.38.0-pre

package/src/engine/engine_physics.ts

@@ -1,57 +1,37 @@
-import { Body, Box, Constraint, Vec3, World, Sphere as PhysicsSphere, Quaternion as PhysicsQuaternion } from 'cannon-es'
-import { Box3, Camera, Intersection, Layers, Matrix4, Mesh, Object3D, Quaternion, Ray, Raycaster, Sphere, Vector2, Vector3 } 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 { getParam } from "./engine_utils"
-import { getWorldPosition, getWorldQuaternion, setWorldPositionXYZ, setWorldQuaternionXYZW } from "./engine_three_utils"
+import { getWorldPosition, getWorldQuaternion, getWorldScale, setWorldPositionXYZ, setWorldQuaternionXYZW } from "./engine_three_utils"
 import {
-    IComponent as Component,
-    IGameObject as GameObject,
+    IComponent,
     ICollider,
-    IRigidbody as Rigidbody,
-    $physicsKey,
-    Collision, 
-    CannonCollision,
-    ICollisionContext
+    IRigidbody,
+    Collision,
+    ContactPoint
 } from './engine_types';
-import { Shape } from 'cannon-es';
 import { InstancingUtil } from './engine_instancing';
 import { foreachComponent } from './engine_gameobject';
 
+import RAPIER, { ActiveEvents, Collider, ColliderDesc, EventQueue, RigidBody, RigidBodyType, World } from '@dimforge/rapier3d-compat';
+import { CollisionDetectionMode } from '../engine/engine_physics.types';
+export type Rapier = typeof RAPIER;
+
 
 const debugPhysics = getParam("debugphysics");
+const debugColliderPlacement = getParam("debugphysicscolliders");
 const debugCollisions = getParam("debugcollisions");
 
-export class BodyOptions {
-    mass: number = 1;
-    kinematic: boolean = false;
-    physicsEvents: boolean = false;
-    drag: number = 0;
-    angularDrag: number = 0.05;
-    sleepThreshold: number = .01;
-}
-
-
-// TODO: refactor to return some kind of handle for adding/removing
-class PhysicsObject {
-    obj: Object3D;
-    parent: Object3D | null;
-    body: Body | null;
-    shapes: Array<Shape> = [];
-    collisonCallback: Function | null = null;
-
-    _hasRigidbody: boolean = false;
-    _didSleepLastStep: boolean = false;
 
-    constructor(obj: Object3D, body: 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");
+const $colliderRigidbody = Symbol("rigidbody");
+// const $removed = Symbol("removed");
+
 export class RaycastOptions {
     ray: Ray | undefined = undefined;
     cam: Camera | undefined | null = undefined;
@@ -225,484 +205,578 @@ export class Physics {
 
     // physics simulation
 
-    get isUpdating(): boolean { return this._isUpdatingPhysicsWorld; }
+    private _tempPosition: Vector3 = new Vector3();
+    private _tempQuaternion: Quaternion = new Quaternion();
+    private _tempScale: Vector3 = new Vector3();
+    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: World = new World();
-    private objects: Array<PhysicsObject> = [];
 
-    private tempPosition: Vector3 = new Vector3();
-    private tempQuaternion: Quaternion = new Quaternion();
+    // 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>();
+
 
     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: Body, mesh: Mesh, _shape: 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);
+    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 addPostStepListener(listener: (evt) => void) {
-        this.world.addEventListener("postStep", listener);
+    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 addConstraint(constraint: Constraint) {
-        this.world.addConstraint(constraint);
+    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 setGravity(vec: Vector3) {
-        this.world.gravity.set(vec.x, vec.y, vec.z);
+    addCapsuleCollider(collider: ICollider, center: Vector3, height: number, radius: number) {
+        const obj = collider.gameObject;
+        const scale = getWorldScale(obj, this._tempPosition);
+        if(debugPhysics) console.log("capsule scale", scale, height, radius);
+        const desc = ColliderDesc.capsule(height * .5 * scale.y - radius, radius * scale.x);
+        this.createCollider(collider, desc, center);
     }
 
-    public multiplyGravity(vec: Vector3) {
-        this.world.gravity.x *= vec.x;
-        this.world.gravity.y *= vec.y;
-        this.world.gravity.z *= vec.z;
-    }
+    addMeshCollider(collider: ICollider, mesh: Mesh, convex: boolean) {
+        const geo = mesh.geometry;
+        if (!geo) {
+            if (debugPhysics) console.warn("Missing mesh geometry", mesh.name);
+            return;
+        }
 
-    public addBody(go: GameObject, body: Body) {
-        for (let i = 0; i < this.objects.length; i++) {
-            const reg = this.objects[i];
-            if (reg.obj === go) {
-                reg._hasRigidbody = true;
-                break;
+        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;
         }
-        // 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: 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;
-            }
+        const desc = convex ? ColliderDesc.convexMesh(positions) : ColliderDesc.trimesh(positions, indices);
+        if (desc) {
+            this.createCollider(collider, desc);
+            // col.setTranslationWrtParent(new Vector3(0,2,0));
+
         }
     }
 
-    public removeShape(obj: Object3D, shape: Shape) {
-        for (const reg of this.objects) {
-            if (reg.obj === obj) {
-                if (reg.body) {
-                    reg.body.removeShape(shape);
-                    reg.body.updateMassProperties();
-                }
-                return;
-            }
+    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);
+
+        matrix.decompose(this._tempPosition, this._tempQuaternion, this._tempScale);
+        getWorldScale(collider.gameObject, this._tempScale);
+        if (center)
+            this._tempPosition.add(center).multiply(this._tempScale);
+        desc.setTranslation(this._tempPosition.x, this._tempPosition.y, this._tempPosition.z);
+        desc.setRotation(this._tempQuaternion);
+
+        desc.setSensor(collider.isTrigger);
+
+        // 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);
         }
-    }
 
-    // TODO: make it work with rigibody in parent
-    public createBody(obj: Object3D, settings: BodyOptions): Body {
-        const body = this.internalCreateBody(obj, null);
-        if (settings.mass)
-            body.mass = settings.mass;
-        if (settings.kinematic)
-            body.type = Body.KINEMATIC;
-        else body.type = Body.DYNAMIC;
-        if (settings.drag)
-            body.linearDamping = settings.drag;
-        if (settings.angularDrag)
-            body.angularDamping = settings.angularDrag;
-        if (settings.sleepThreshold)
-            body.sleepSpeedLimit = settings.sleepThreshold;
+        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);
+
+        }
+        else {
 
-        if (body.shapes.length > 0)
-            this.world.addBody(body);
-        const po = new PhysicsObject(obj, body);
-        po._hasRigidbody = true;
-        this.objects.push(po);
+            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;
 
-        if (debugPhysics) {
-            console.log("created new body", obj.name, body, body.sleepState, this.world.gravity);
         }
 
-        if (settings.physicsEvents)
-            this.registerCollisionEvents(po);
+        collider[$colliderRigidbody] = rigidBody;
 
-        return body;
+        return { rigidBody: rigidBody, useExplicitMassProperties: useExplicitMassProperties };
     }
 
-    public addBoxCollider(obj: Object3D, trigger: boolean, center: Vector3, size: Vector3, rb: Rigidbody | null): Shape {
-
-        const scale = this.tempPosition;
-        obj.getWorldScale(scale);
+    removeBody(obj: IComponent) {
+        const body = obj[$bodyKey];
+        obj[$bodyKey] = null;
+        if (body && this.world) {
+            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);
 
-        const pos = new Vec3(
-            .5 * scale.x * size.x,
-            .5 * scale.y * size.y,
-            .5 * scale.z * size.z
-        );
-        const shape = new Box(pos);
-        shape.collisionResponse = !trigger;
+                if (body instanceof Collider) {
+                    const collider = body as Collider;
+                    this.world?.removeCollider(collider, true);
 
-        center = center.clone();
-        center.multiply(scale);
+                    // remove the rigidbody if it doesnt have colliders anymore
+                    const rb = collider.parent();
+                    if (rb && rb.numColliders() <= 0) {
+                        this.world?.removeRigidBody(rb);
+                    }
+                }
+                else if (body instanceof RigidBody) {
+                    // TODO: running this code below causes a crash in rapier
+                    // const rb = body as RigidBody;
+                    // console.log("colliders", rb.numColliders())
+                    // for (let i = 0; i < rb.numColliders(); i++) {
+                    //     const col = rb.collider(i);
+                    //     this.world?.removeCollider(col, true);
+                    // }
+                    // console.log("colliders", rb.numColliders(), rb)
+                    // console.log(rb.handle, rb.userData);
+                    // if (rb.userData === undefined)
+                    //     this.world?.removeRigidBody(rb);
+                }
 
-        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);
+                // check if we need to remove the rigidbody too
+                // const col = obj as ICollider;
+                // if (col.isCollider && col.attachedRigidbody) {
+                //     const rb = col.attachedRigidbody[$bodyKey] as RigidBody;
+                //     if (rb && rb.numColliders() <= 0) {
+                //         // this.world?.removeRigidBody(rb);
+                //     }
+                // }
+            }
         }
-        return shape;
     }
 
-    public addSphereCollider(obj: Object3D, center: Vector3, radius: number, rb: Rigidbody | null): Shape {
-        const scale = this.tempPosition;
-        obj.getWorldScale(scale);
-
-        const factor = Math.max(scale.x, scale.y, scale.z);
-        const shape = new PhysicsSphere(radius * factor);
-        // shape.collisionResponse = !trigger;
-
-        center = center.clone();
-        center.multiply(scale);
+    updateBody(comp: ICollider | IRigidbody, translation: boolean, rotation: boolean) {
+        if (comp.destroyed || !comp.gameObject) return;
+        if (!translation && !rotation) return;
 
-        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 ((comp as ICollider).isCollider === true) {
+            // const collider = comp as ICollider;
+            console.warn("TODO: implement updating collider position");
+        }
+        else {
+            const rigidbody = comp as IRigidbody;
+            const body = rigidbody[$bodyKey];
+            if (body) {
+                this.syncPhysicsBody(rigidbody.gameObject, body, translation, rotation);
+            }
         }
-        return shape;
     }
 
-    public addMeshCollider(_obj: Object3D) {
-        // see https://github.com/schteppe/js/blob/master/demos/bunny.html
-        if (debugPhysics)
-            console.warn("TODO mesh collider not yet supported")
-        // const geometry: 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>>();
-
-        // console.log(geometry);
-
-        // 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 Vector3(verts[i0 * 3], verts[i0 * 3 + 1], verts[i0 * 3 + 2]);
-        //     const v1 = new Vector3(verts[i1 * 3], verts[i1 * 3 + 1], verts[i1 * 3 + 2]);
-        //     const v2 = new Vector3(verts[i2 * 3], verts[i2 * 3 + 1], verts[i2 * 3 + 2]);
-        //     const face = [v0, v1, v2];
-        //     faces.push(face);
-        // }
-        // const convex = new ConvexBufferGeometry(faces);
-
-        // var shape = new ConvexPolyhedron({ verts, faces });
-        // this.addShape(obj, shape);
+    updateProperties(rigidbody: IRigidbody) {
+        const physicsBody = rigidbody[$bodyKey]
+        if (physicsBody) {
+            this.internalUpdateProperties(rigidbody, physicsBody);
+        }
     }
 
-    private isAlreadyRegistered(body: Body): boolean {
-        for (const obj of this.objects) {
-            if (obj.body === body) return true;
-        }
-        return false;
+    internal_getRigidbody(rb: IRigidbody): RigidBody | null {
+        return rb[$bodyKey] as RigidBody;
     }
 
-    private readonly tempMat1: Matrix4 = new Matrix4();
-    private readonly tempMat2: Matrix4 = new Matrix4();
+    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);
 
-    private addShape(obj: Object3D, shape: Shape, center: Vector3, rb: Rigidbody | null): Body | null {
+        // 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;
+        //     }
+        // }
 
-        let body: Body | null = null;
+        // lock rotations
+        rigidbody.setEnabledRotations(!rb.lockRotationX, !rb.lockRotationY, !rb.lockRotationZ, true);
+        rigidbody.setEnabledTranslations(!rb.lockPositionX, !rb.lockPositionY, !rb.lockPositionZ, true);
 
-        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 (rb.isKinematic) {
+            rigidbody.setBodyType(RAPIER.RigidBodyType.KinematicPositionBased);
         }
         else {
-            // console.log("has no rb", obj);
-            body = this.internalCreateBody(obj, null);
-            body.type = 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 = getWorldPosition(obj);
-                const bp = body.position;
-                wp.x -= bp.x;
-                wp.y -= bp.y;
-                wp.z -= bp.z;
-
-                wr = getWorldQuaternion(obj);
-                const r = new Quaternion(body.quaternion.x, body.quaternion.y, body.quaternion.z, body.quaternion.w);
-                wr.multiply(r.invert());
-            }
-            // get rotation difference
-
-            wp.add(center);
-
-
-
-
-            // 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 Vector3());
-            //     wp.copy(this.tempPosition);
-            // }
-
-            // wp.applyQuaternion(wr);
-
-            const pos = new Vec3(wp.x, wp.y, wp.z);
-            const rot = new PhysicsQuaternion(wr.x, wr.y, wr.z, wr.w);
-            body.addShape(shape, pos, rot);
-            body.updateMassProperties();
-            this.world.addBody(body);
+            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: Vector3 = new Vector3();
-    private tempQuat: Quaternion = new 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;
-            const obj = entry.obj;
+        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 === Body.SLEEPING) {
-                    console.log("BODY SLEEPING", body);
-                }
-                else if (entry._didSleepLastStep && body.sleepState !== Body.SLEEPING) {
-                    console.log("BODY WOKE UP", body);
-                }
-            }
-            entry._didSleepLastStep = body.sleepState === Body.SLEEPING;
-            // if(body.sleepState === Body.SLEEPING) {
-            //     console.log("SLEEP", body.name);
-            // }
-            // if (body.type == Body.KINEMATIC) continue;
-
-            if (body.type === Body.KINEMATIC) {
-                const wp = getWorldPosition(obj, this.temp);
-                body.position.set(wp.x, wp.y, wp.z);
-                const rot = 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;
             }
 
-            if ((isNaN(body.position.x) || isNaN(body.position.y) || isNaN(body.position.z))) {
-                console.error("body position is NaN on", obj.name, "this usually means some colliders are overlapping", body.previousPosition, obj.position);
-                this.world.removeBody(body);
-                continue;
+
+            // sync
+            const pos = body.translation();
+            const rot = body.rotation();
+            // make sure to keep the collider offset
+            const center = obj["center"] as Vector3;
+            if (center && center.isVector3) {
+                this._tempQuaternion.set(rot.x, rot.y, rot.z, rot.w);
+                const offset = this._tempPosition.copy(center).applyQuaternion(this._tempQuaternion);
+                const scale = getWorldScale(obj.gameObject);
+                offset.multiply(scale);
+                pos.x -= offset.x;
+                pos.y -= offset.y;
+                pos.z -= offset.z;
             }
+            setWorldPositionXYZ(obj.gameObject, pos.x, pos.y, pos.z);
+            setWorldQuaternionXYZW(obj.gameObject, rot.x, rot.y, rot.z, rot.w);
+        }
+    }
 
+    private syncPhysicsBody(obj: Object3D, body: RigidBody, translation: boolean, rotation: boolean) {
 
-            // 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) {
+        // const bodyType = body.bodyType();
+        // const previous = physicsBody.translation();
+        // const vel = physicsBody.linvel();
 
-                setWorldQuaternionXYZW(obj,
-                    body.quaternion.x, body.quaternion.y, body.quaternion.z, body.quaternion.w
-                );
+        const worldPosition = getWorldPosition(obj, this._tempPosition);
+        const worldQuaternion = getWorldQuaternion(obj, this._tempQuaternion);
+        const type = body.bodyType();
+        switch (type) {
+            case RigidBodyType.Fixed:
+            case RigidBodyType.KinematicPositionBased:
+            case RigidBodyType.KinematicVelocityBased:
+                if (translation)
+                    body.setNextKinematicTranslation(worldPosition);
+                if (rotation)
+                    body.setNextKinematicRotation(worldQuaternion);
+                break;
+            default:
+                if (translation)
+                    body.setTranslation(worldPosition, false);
+                if (rotation)
+                    body.setRotation(worldQuaternion, false);
+                break;
 
-                const p = body.position;
-                setWorldPositionXYZ(obj, p.x, p.y, p.z);
+        }
+        body.wakeUp();
+        // physicsBody.setBodyType(RAPIER.RigidBodyType.Fixed);
+        // physicsBody.setLinvel(vel, false);
+
+        // 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);
 
-                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);
-                // }
+        // body.setBodyType(bodyType);
+    }
+
+    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: Object3D, shape: Shape | undefined | null): Body {
 
-        const body = new Body();
-        body["_owner"] = obj;
-        body["_name"] = obj.name;
-        obj.getWorldPosition(this.tempPosition);
-        const pos = this.tempPosition;
-        body.position = new Vec3(pos.x, pos.y, pos.z);
+}
 
-        const quat = this.tempQuaternion;
-        obj.getWorldQuaternion(quat);
-        body.quaternion = new PhysicsQuaternion(quat.x, quat.y, quat.z, quat.w);
 
-        body.type = Body.KINEMATIC;
-        if (shape) {
-            body.addShape(shape);
-            body.updateMassProperties();
-        }
-        return body;
-    }
 
-    // private findObject(obj: 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;
-    // }
+/** responsible of processing collision events for the component system */
+class PhysicsCollisionHandler {
 
-    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);
-    }
+    readonly world: World;
+    readonly eventQueue: EventQueue;
 
-    private unregisterCollisionEvents(obj: PhysicsObject) {
-        if (!obj.collisonCallback) return;
-        if (!obj.body) return;
-        obj.body.removeEventListener("collide", obj.collisonCallback);
+    constructor(world: World, eventQueue: EventQueue) {
+        this.world = world;
+        this.eventQueue = eventQueue;
     }
 
-    private onBeginContact(_) {
-        // this is called after the object collide event so we dont really need it
-        // console.log("START");
+    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];
+            if (debugCollisions)
+                console.log("EVT", colliderComponent1.name, colliderComponent2.name, started, col1, col2);
+            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);
+                }
+            }
+        });
     }
 
-    private readonly collisionContext: ICollisionContext = new CollisionContext();
+    update() {
+        this.onHandleCollisionStay();
+    }
 
-    private raiseCollisionEvents(obj: 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);
-        });
+    private onCollisionStarted(self: ICollider, selfBody: Collider, other: ICollider, otherBody: Collider) {
+        let collision: Collision | null = null;
 
-        // 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);
+        // 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 });
+                    }
+                });
             });
         }
     }
 
-    private onEndContact(args: { bodyA: Body, bodyB: Body }) {
-        // if(args.bodyB.sleepState !== Body.AWAKE) return;
-        // console.log("END", BODY_SLEEP_STATES, args.bodyB.sleepState);
-        const obj1 = args.bodyA[$physicsKey];
-        const obj2 = args.bodyB[$physicsKey];
-        // console.log(obj2);
-
-        foreachComponent(obj2, (c: Component) => {
-            c.__internalHandleExitCollisionEvent(obj1, false);
-        });
-
-        // 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);
-                });
+    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.otherCollider;
+                c.onTriggerStay(arg);
+            }
+        }
     }
 
-}
-
-export interface IColliderProvider {
-    getCollider(obj: Object3D): ICollider;
-}
-
-let colliderProvider: IColliderProvider | null = null;
-export function registerColliderProvider(prov: IColliderProvider) {
-    colliderProvider = prov;
-}
-
-class CollisionContext implements ICollisionContext {
-
-    getCollider(obj: Object3D<Event>): ICollider {
-        return colliderProvider!.getCollider(obj);
+    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);
+                }
+            }
+        }
     }
-
 }
-