@needle-tools/engine 2.24.0-pre

package/src/engine/engine_physics.ts

@@ -0,0 +1,652 @@
+import * as CANNON from 'cannon-es'
+import * as THREE from 'three'
+import { Context } from './engine_setup';
+import cannonDebugger from 'cannon-es-debugger'
+import { Rigidbody } from '../engine-components/Rigidbody';
+import * as utils from "./engine_utils"
+import * as threeutils from "./engine_three_utils"
+import { InstancingUtil } from '../engine-components/Renderer';
+import { GameObject } from '../engine-components/Component';
+import { Shape } from 'cannon-es';
+
+const debugPhysics = utils.getParam("debugphysics");
+const debugCollisions = utils.getParam("debugcollisions");
+
+export interface ICollisionEventReceiver {
+    onCollision(data: CollisionData);
+}
+
+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: 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;
+    }
+}
+
+export class CollisionData {
+    obj: THREE.Object3D | null;
+    contact: CANNON.ContactEquation;
+    target: THREE.Object3D | null;
+    type: string;
+
+    constructor(contact: CANNON.ContactEquation, type: string) {
+        this.contact = contact;
+        this.type = type;
+        this.obj = null;
+        this.target = null;
+    }
+}
+class CannonCollision {
+    body: CANNON.Body;
+    contact: CANNON.ContactEquation;
+    target: CANNON.Body;
+    type: string;
+    constructor(body: CANNON.Body, contact: CANNON.ContactEquation, target: CANNON.Body, type: string) {
+        this.body = body;
+        this.contact = contact;
+        this.target = target;
+        this.type = type;
+    }
+}
+
+export class RaycastOptions {
+    ray: THREE.Ray | undefined = undefined;
+    screenPoint: THREE.Vector2 | undefined = undefined;
+    raycaster: THREE.Raycaster | undefined = undefined;
+    results: Array<THREE.Intersection> | undefined = undefined;
+    targets: Array<THREE.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;
+
+    screenPointFromOffset(ox: number, oy: number) {
+        if (this.screenPoint === undefined) this.screenPoint = new THREE.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 (lm)
+            lm.mask = mask;
+        else this.layerMask = mask;
+    }
+
+    public static AllLayers = 0xFFFFFFFF;
+}
+
+export class SphereIntersection implements THREE.Intersection {
+    distance: number;
+    point: THREE.Vector3;
+    object: THREE.Object3D;
+    constructor(object: THREE.Object3D, distance: number, point: THREE.Vector3) {
+        this.object = object;
+        this.distance = distance;
+        this.point = point;
+    }
+}
+
+export class Physics {
+
+    // raycasting
+
+    private readonly raycaster: THREE.Raycaster = new THREE.Raycaster();
+    private readonly defaultRaycastOptions: RaycastOptions = new RaycastOptions();
+    private readonly targetBuffer: Array<THREE.Object3D> = new Array<THREE.Object3D>(1);
+    private readonly defaultThresholds = {
+        Mesh: {},
+        Line: { threshold: 0 },
+        LOD: {},
+        Points: { threshold: 0 },
+        Sprite: {}
+    }
+
+
+    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> {
+        this.sphereResults.length = 0;
+        if (!this.context.scene) return this.sphereResults;
+        const sphere = new THREE.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);
+        }
+        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;
+            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;
+            }
+        }
+        else if (obj.children) {
+            for (const ch of obj.children) {
+                const i = this.onSphereOverlap(ch, sp, mask);
+                if (i) return i;
+            }
+        }
+        return null;
+    }
+
+    public raycastFromRay(ray: THREE.Ray, options: RaycastOptions | null = null): Array<THREE.Intersection> {
+        const opts = options ?? this.defaultRaycastOptions;
+        opts.ray = ray;
+        return this.raycast(opts);
+    }
+
+    public raycast(options: RaycastOptions | null = null): Array<THREE.Intersection> {
+        if (!options) options = this.defaultRaycastOptions;
+        const mp = options.screenPoint ?? this.context.input.mousePositionRC;
+        const rc = options.raycaster ?? this.raycaster;
+        rc.near = options.minDistance ?? 0;
+        rc.far = options.maxDistance ?? Infinity;
+        rc.params = this.defaultThresholds;
+        if (options.lineThreshold)
+            rc.params.Line = { threshold: options.lineThreshold };
+        else rc.params.Line = { threshold: 0 };
+        if (options.ray) {
+            rc.ray.copy(options.ray);
+        }
+        else {
+            const cam = this.context.mainCamera;
+            if (!cam) {
+                console.error("Can not perform raycast - no main camera found");
+                if (this.defaultRaycastOptions.results) this.defaultRaycastOptions.results.length = 0;
+                return this.defaultRaycastOptions.results ?? [];
+            }
+            rc.setFromCamera(mp, cam);
+        }
+        let targets = options.targets;
+        if (!targets) {
+            targets = this.targetBuffer;
+            targets[0] = this.context.scene;
+        }
+        let results = options.results;
+        if (!results) {
+            if (!this.defaultRaycastOptions.results)
+                this.defaultRaycastOptions.results = new Array<THREE.Intersection>();
+            results = this.defaultRaycastOptions.results;
+        }
+
+        // layermask
+        // https://github.com/mrdoob/three.js/blob/master/src/core/Layers.js
+        if (options.layerMask !== undefined) {
+            if (options.layerMask instanceof THREE.Layers)
+                rc.layers.mask = options.layerMask.mask;
+            else
+                rc.layers.mask = options.layerMask;
+        }
+        else {
+            rc.layers.enableAll();
+            rc.layers.disable(2);
+        }
+
+        // console.log(rc)
+        // console.log(targets);
+
+        // shoot
+        results.length = 0;
+        rc.intersectObjects(targets, options.recursive, results);
+
+        const ignorelist = options.ignore;
+        if (ignorelist !== undefined && ignorelist.length > 0) {
+            results = results.filter(r => !ignorelist.includes(r.object));
+        }
+        return results;
+    }
+
+
+
+
+    // physics simulation
+
+    get isUpdating() : boolean { return this._isUpdatingPhysicsWorld; }
+
+    private _isUpdatingPhysicsWorld : boolean = false;
+
+    private context: Context;
+
+    private world: CANNON.World = new CANNON.World();
+    private objects: Array<PhysicsObject> = [];
+
+    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);
+        }
+    }
+
+    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);
+    }
+
+    public setGravity(vec: THREE.Vector3) {
+        this.world.gravity.set(vec.x, vec.y, vec.z);
+    }
+
+    public multiplyGravity(vec: THREE.Vector3) {
+        this.world.gravity.x *= vec.x;
+        this.world.gravity.y *= vec.y;
+        this.world.gravity.z *= vec.z;
+    }
+
+    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;
+            }
+        }
+        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;
+            }
+        }
+    }
+
+    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;
+            }
+        }
+    }
+
+    // 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;
+
+        this.world.addBody(body);
+        const po = new PhysicsObject(obj, body);
+        po._hasRigidbody = true;
+        this.objects.push(po);
+
+        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);
+
+        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;
+
+        center = center.clone();
+        center.multiply(scale);
+
+        const body = this.addShape(obj, shape, center, rb);
+        if (body !== null) {
+            if (this.isAlreadyRegistered(body)) return shape;
+            const po = new PhysicsObject(obj, body);
+            this.objects.push(po);
+        }
+        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 body = this.addShape(obj, shape, center, rb);
+        if (body !== null) {
+            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>>();
+
+        // 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 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);
+    }
+
+    private isAlreadyRegistered(body: CANNON.Body): boolean {
+        for (const obj of this.objects) {
+            if (obj.body === body) return true;
+        }
+        return false;
+    }
+
+    private addShape(obj: THREE.Object3D, shape: CANNON.Shape, center: THREE.Vector3, rb: Rigidbody | null): CANNON.Body | null {
+
+        let body: CANNON.Body | null = null;
+
+        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;
+        }
+        else {
+            // console.log("has no rb", obj);
+            body = this.internalCreateBody(obj, null);
+            body.type = CANNON.Body.KINEMATIC;
+            this.world.addBody(body);
+        }
+
+        if (body) {
+            center.x *= -1;
+
+            const wr = threeutils.getWorldQuaternion(obj);
+            // get rotation difference
+
+            const wp = threeutils.getWorldPosition(obj);
+            wp.add(center);
+
+            const r = new THREE.Quaternion(body.quaternion.x, body.quaternion.y, body.quaternion.z, body.quaternion.w);
+            wr.multiply(r.invert());
+
+            // wp.applyQuaternion(wr);
+
+            const bp = body.position;
+            const pos = new CANNON.Vec3(wp.x - bp.x, wp.y - bp.y, wp.z - bp.z);
+            const rot = new CANNON.Quaternion(wr.x, wr.y, wr.z, wr.w);
+            body.addShape(shape, pos, rot);
+            body.updateMassProperties();
+        }
+        return body;
+    }
+
+    // private _lastStepTime: number | undefined = 0;
+
+    public step(deltaTime: number) {
+        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);
+        }
+    }
+
+    private temp : THREE.Vector3 = new THREE.Vector3();
+    private tempQuat : THREE.Quaternion = new THREE.Quaternion();
+
+    public postStep() {
+        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;
+
+            body.sleepTick(this.context.time.time);
+
+            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);
+                continue;
+            }
+
+
+
+            // 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);
+
+                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);
+                // }
+            }
+        }
+        this._isUpdatingPhysicsWorld = false;
+    }
+
+    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;
+    }
+
+    // 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;
+    // }
+
+    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);
+    }
+
+    private unregisterCollisionEvents(obj: PhysicsObject) {
+        if (!obj.collisonCallback) return;
+        if (!obj.body) return;
+        obj.body.removeEventListener("collide", obj.collisonCallback);
+    }
+
+    private raiseCollisionEvents(obj: THREE.Object3D, event: CannonCollision) {
+
+        const resolved = new CollisionData(event.contact, event.type);
+
+        let addedData = 0;
+        const objs = this.objects;
+        function requestData() {
+            if (addedData > 0) return;
+            for (let i = 0; i < objs.length; i++) {
+                if (addedData == 2) return;
+                const entry = objs[i];
+                if (entry.body == event.body) {
+                    resolved.obj = entry.obj;
+                    ++addedData;
+                }
+                else if (entry.body == event.target) {
+                    resolved.target = entry.obj;
+                    ++addedData;
+                }
+            }
+        }
+
+        if (debugCollisions)
+            console.log("collision between", event.contact.bi, event.contact.bj, obj, event);
+            
+        for (let i = 0; i < obj.userData?.components?.length; i++) {
+            const component = obj.userData.components[i];
+            if (component.onCollision) {
+                requestData();
+                component.onCollision(resolved);
+            }
+        }
+    }
+
+}
+
+