js/Mobilizing/renderer/3D/three/physics/PhysicsEngine.js

import Component from "../../../../core/Component";
import Time from "../../../../time/Time";
import Vector3 from "../types/Vector3";

export default class PhysicsEngine extends Component {
    /**
    * @param {Object} params Parameters object, given by the constructor.
    * @param {Number} params.iterations iterations of the physic engine when constrains are calculated
    */
    constructor({
        iterations = 5,
    } = {}) {

        super(...arguments);

        this.iterations = iterations;

        this.joints = [];
        this.bodies = [];

        this.G = 6.6742 * Math.pow(10, -11); //gravitationnal constant
        this.ke = 8.99 * Math.pow(10, 9); //coulomb's constant
        this.relaxationCoeff = 1;

        this._time = new Time();
        this.chain(this._time);
    }

    /**
    * Set the number of iteration made by the physic engine
    * @param {Number} val
    */
    setIterations(val) {
        this.iterations = val;
    }

    /**
    * Initialization method
    */
    setup() {
        super.setup();
        this._time.on();
    }

    update() {
        if (this.active) {
            let dt = this._time.delta;
            //
            if (dt === 0) {
                return;
            }

            if (dt > 1) {
                dt = 1;
            }

            //apply forces / acc / vel / pos
            for (let i = 0; i < this.bodies.length; ++i) {
                const body = this.bodies[i];
                body.forces = new Vector3(0, 0, 0);
            }

            //springs
            for (let j = 0; j < this.joints.length; ++j) {
                const joint = this.joints[j];

                if (!joint.enabled) {
                    continue; //ignore this joint
                }

                if (joint.type === "spring") {
                    const p1 = joint.body1.position;
                    const p2 = joint.body2.position;
                    const x = p2.x - p1.x;
                    const y = p2.y - p1.y;
                    const z = p2.z - p1.z;

                    const distance = Math.sqrt(x * x + y * y + z * z);

                    //compute spring force
                    const force = (distance - joint.distance) * joint.k;
                    joint.body1.forces.x += x * force;
                    joint.body1.forces.y += y * force;
                    joint.body1.forces.z += z * force;

                    joint.body2.forces.x -= x * force;
                    joint.body2.forces.y -= y * force;
                    joint.body2.forces.z -= z * force;

                    //damping force
                    const sx = joint.body2.velocity.x - joint.body1.velocity.x;
                    const sy = joint.body2.velocity.y - joint.body1.velocity.y;
                    const sz = joint.body2.velocity.z - joint.body1.velocity.z;

                    joint.body1.forces.x += sx * joint.damping;
                    joint.body1.forces.y += sy * joint.damping;
                    joint.body1.forces.z += sz * joint.damping;

                    joint.body2.forces.x -= sx * joint.damping;
                    joint.body2.forces.y -= sy * joint.damping;
                    joint.body2.forces.z -= sz * joint.damping;
                }
                else if (joint.type === "gravity") {
                    const p1 = joint.body1.position;
                    const p2 = joint.body2.position;
                    let x = p2.x - p1.x;
                    let y = p2.y - p1.y;
                    let z = p2.z - p1.z;

                    const distance = Math.sqrt(x * x + y * y + z * z);

                    //normalize
                    x /= distance;
                    y /= distance;
                    z /= distance;

                    //compute gravity force
                    const mass = joint.body1.mass + joint.body2.mass;
                    const force = this.G * mass / (distance * distance);

                    joint.body1.forces.x += x * force;
                    joint.body1.forces.y += y * force;
                    joint.body1.forces.z += z * force;

                    joint.body2.forces.x -= x * force;
                    joint.body2.forces.y -= y * force;
                    joint.body2.forces.z -= z * force;
                }
                else if (joint.type === "electrostatics") {
                    const p1 = joint.body1.position;
                    const p2 = joint.body2.position;
                    let x = p2.x - p1.x;
                    let y = p2.y - p1.y;
                    let z = p2.z - p1.z;

                    const distance = Math.sqrt(x * x + y * y + z * z);

                    //normalize
                    x /= distance;
                    y /= distance;
                    z /= distance;

                    //compute electrostatics force
                    const force = this.ke * joint.charge1 * joint.charge2 / (distance * distance);
                    joint.body1.forces.x -= x * force;
                    joint.body1.forces.y -= y * force;
                    joint.body1.forces.z -= z * force;

                    joint.body2.forces.x += x * force;
                    joint.body2.forces.y += y * force;
                    joint.body2.forces.z += z * force;
                }
            }

            for (let i = 0; i < this.bodies.length; ++i) {
                const body = this.bodies[i];

                //quasi Verlet
                if (body.speed !== undefined) {
                    body.velocity = new Vector3(body.speed.x, body.speed.y, body.speed.z);
                    body.speed = undefined;
                }
                else {
                    body.velocity = new Vector3(body.position.x - body.previousposition.x, body.position.y - body.previousposition.y, body.position.z - body.previousposition.z);

                    body.velocity.x /= dt;
                    body.velocity.y /= dt;
                    body.velocity.z /= dt;
                }

                body.previousposition = new Vector3(body.position.x, body.position.y, body.position.z);

                //apply gravity (@FIXME, not accurate !!!)
                if (this.globalgravity !== undefined) {
                    body.forces.x += this.globalgravity.x * body.mass;
                    body.forces.y += this.globalgravity.y * body.mass;
                    body.forces.z += this.globalgravity.z * body.mass;
                }

                if (this.globalwind !== undefined) {
                    body.forces.x += this.globalwind.x;
                    body.forces.y += this.globalwind.y;
                    body.forces.z += this.globalwind.z;
                }

                //acc
                const acc = new Vector3(0, 0, 0);
                acc.x = body.forces.x / body.mass;
                acc.y = body.forces.y / body.mass;
                acc.z = body.forces.z / body.mass;
                //console.log("body acc", acc);

                //speed
                body.velocity.x += acc.x * dt;
                body.velocity.y += acc.y * dt;
                body.velocity.z += acc.z * dt;
                //pos
                body.position.x += body.velocity.x * dt;
                body.position.y += body.velocity.y * dt;
                body.position.z += body.velocity.z * dt;

                //console.log("body position", body.position);
            }

            //satisfy constraints
            for (let i = 0; i < this.iterations; ++i) {
                for (let j = 0; j < this.joints.length; ++j) {
                    const joint = this.joints[j];

                    if (!joint.enabled) {
                        continue; //ignore this joint
                    }

                    if (joint.type === "stick" || joint.type === "string") {
                        const p1 = joint.body1.position;
                        const p2 = joint.body2.position;
                        const x = p2.x - p1.x;
                        const y = p2.y - p1.y;
                        const z = p2.z - p1.z;
                        const distance = Math.sqrt(x * x + y * y + z * z);

                        //satisfy distance constraint
                        const err = (distance - joint.distance) / distance * 1;
                        //console.log("err=" + err);

                        if (joint.type === "stick" || (joint.type === "string" && distance > joint.distance)) {
                            let err1 = err / 2;
                            let err2 = err / 2;

                            if (joint.body1.fixed) {
                                err1 = 0;
                                err2 = err;
                            }
                            else if (joint.body2.fixed) {
                                err1 = err;
                                err2 = 0;
                            }

                            p1.x = p1.x + x * err1 * this.relaxationCoeff;
                            p2.x = p2.x - x * err2 * this.relaxationCoeff;
                            p1.y = p1.y + y * err1 * this.relaxationCoeff;
                            p2.y = p2.y - y * err2 * this.relaxationCoeff;
                            p1.z = p1.z + z * err1 * this.relaxationCoeff;
                            p2.z = p2.z - z * err2 * this.relaxationCoeff;
                        }
                    }
                    else if (joint.type === "fixed") {
                        //console.log("fixed = " , joint.position);
                        joint.body1.position = new Vector3(joint.position.x, joint.position.y, joint.position.z);
                        joint.body1.velocity = new Vector3(0, 0, 0);
                    }
                }
            }

            //apply new positions
            for (let i = 0; i < this.bodies.length; ++i) {
                const body = this.bodies[i];

                if (body.object !== undefined) {
                    //console.log("new pos=" , body.position);
                    body.object.transform.setLocalPosition(body.position);
                }
            }
        }
    }

    /**
    * addBody
    * @param {Vector3} position
    * @param {Number} mass
    * @param {Mesh} object the Mesh to attach this physics body to
    * @return {Body} the added physic body
    */
    addBody(position, mass, object) {
        const body = {};
        body.position = new Vector3(position.x, position.y, position.z);
        //console.log("body position : ", body.position);
        body.previousposition = new Vector3(position.x, position.y, position.z);
        body.mass = mass;
        body.object = object;
        body.velocity = new Vector3(0, 0, 0);
        this.bodies.push(body);
        return body;
    }

    /**
    * addFixedJoint
    * @param {Mesh} body
    * @param {Vector3} position
    * @return {Body} the added joint body
    */
    addFixedJoint(body, position) {
        const joint = {};
        joint.body1 = body;
        joint.body1.fixed = true;
        joint.type = "fixed";
        joint.position = new Vector3(position.x, position.y, position.z);
        joint.enabled = true;
        this.joints.push(joint);
        return joint;
    }

    /**
    * addStickJoint
    * @param {Mesh} body1
    * @param {Mesh} body2
    * @param {Number} distance
    * @return {Body} the added joint body
    */
    addStickJoint(body1, body2, distance) {
        const joint = {};
        joint.body1 = body1;
        joint.body2 = body2;
        joint.type = "stick";
        joint.distance = distance;
        joint.enabled = true;
        this.joints.push(joint);
        return joint;
    }

    /**
    * addSpringJoint
    * @param {Mesh} body1
    * @param {Mesh} body2
    * @param {Number} distance
    * @param {Number} k
    * @param {Number} damping
    * @return {Body} the added joint body
    */
    addSpringJoint(body1, body2, distance, k, damping) {
        const joint = {};
        joint.body1 = body1;
        joint.body2 = body2;
        joint.type = "spring";
        joint.distance = distance;
        joint.k = k;
        joint.damping = damping;
        joint.enabled = true;
        this.joints.push(joint);
        return joint;
    }

    /**
    * addStringJoint
    * @param {Mesh} body1
    * @param {Mesh} body2
    * @param {Number} distance
    * @return {Body} the added joint body
    */
    addStringJoint(body1, body2, distance) {
        const joint = {};
        joint.body1 = body1;
        joint.body2 = body2;
        joint.type = "string";
        joint.distance = distance;
        joint.enabled = true;
        this.joints.push(joint);
        return joint;
    }

    /**
    * addGravityJoint
    * @param {Mesh} body1
    * @param {Mesh} body2
    * @return {Body} the added joint body
    */
    addGravityJoint(body1, body2) {
        const joint = {};
        joint.body1 = body1;
        joint.body2 = body2;
        joint.type = "gravity";
        joint.enabled = true;
        this.joints.push(joint);
        return joint;
    }

    /**
    * addElectrostaticsJoint
    * @param {Mesh} body1
    * @param {Mesh} body2
    * @param {Number} charge1
    * @param {Number} charge2
    * @return {Body} the added joint body
    */
    addElectrostaticsJoint(body1, body2, charge1, charge2) {
        const joint = {};
        joint.body1 = body1;
        joint.body2 = body2;
        joint.type = "electrostatics";
        joint.charge1 = charge1;
        joint.charge2 = charge2;
        joint.enabled = true;
        this.joints.push(joint);
        return joint;
    }

    /**
    * setGlobalGravity
    * @param {Vector3} vector
    */
    setGlobalGravity(vector) {
        this.globalgravity = vector;
    }

    /**
    * setGlobalWind
    * @param {Vector3} vector
    */
    setGlobalWind(vector) {
        this.globalwind = vector;
    }
}