dronedemo/physics.js

var gameState = require('./gamestate');

const PHYSICS_FPS = 30;
var GRAVITY = 9.81; // m/s^2
GRAVITY/=3;
const GRAVITY_MS = GRAVITY / 1000; // m/ms^2
const HOVER_PROP_SPEED = 3;
const MIN_PROP_SPEED = -3;
const MAX_PROP_SPEED = 10;
const DRONE_THRUST_PER_PROP_SPEED = GRAVITY_MS / HOVER_PROP_SPEED;
const PROP_CTRL_MS = 9000; // Time to go from min to max by holding down the control
const PROP_CHANGE_PER_MS = (MAX_PROP_SPEED - MIN_PROP_SPEED) / PROP_CTRL_MS;
const PHYSICS_MS = Math.round(1000 / PHYSICS_FPS);
const JUMP_VELOCITY = 1; // m/s
const DRONE_SENSITIVITY = 0.01; // rad/s/s
const DRONE_SENSITIVITY_MS = DRONE_SENSITIVITY/1000; // rad/s/ms
const DRONE_STABILITY_FACTOR = 2/3;
var BABYLON = null;
import('@babylonjs/core').then(i=>BABYLON=i);

// Update game state at approximately 30 FPS
setInterval(updateGameState, PHYSICS_MS);

function updateGameState() {
    var deltaT = PHYSICS_MS;
    gameState.timestamp = Date.now();

    // Apply control physics
    Object.values(gameState.clients).forEach(client => applyControlPhysics(client, deltaT));
    
    // Apply physics to all models
    Object.values(gameState.models).forEach(model => {
        if (model.type === 'human') applyHumanPhysics(model, deltaT);
        if (model.type === 'drone') applyDronePhysics(model, deltaT);
    });

}

function applyControlPhysics(client, deltaT) {
    const model = gameState.models[client.controllingId];
    //console.log('control model',model);
    if (!model) return;
    if (model.type === 'human') applyHumanControlPhysics(client, model, deltaT);
    if (model.type === 'drone') applyDroneControlPhysics(client, model, deltaT);
}

function applyHumanPhysics(model, deltaT) {
    // Apply gravity
    model.dz -= GRAVITY_MS * deltaT;

    // Apply movement
    model.x += model.dx * deltaT;
    model.y += model.dy * deltaT;
    model.z += model.dz * deltaT;


    // Apply walk slowing
    var backthrust = 0.1*GRAVITY_MS;
    var velocity = Math.sqrt(model.dx*model.dx+model.dy*model.dy+model.dz*model.dz); 
    var nx = model.dx/velocity;
    var ny = model.dy/velocity;
    var nz = model.dz/velocity;
    var backx = -nx*deltaT*backthrust;
    var backy = -ny*deltaT*backthrust;
    var backz = -nz*deltaT*backthrust;
    if(backx>model.dx) {
     model.dx=0;
     model.dy=0;
     model.dz=0;
    }
    else {
     model.dx=backx;
     model.dy=backy;
     model.dz=backz;
    }

    // Prevent going below ground
    if (model.z < 1.8) {
        model.z = 1.8;
        model.dz = 0;
    }
}

function applyDronePhysics(model, deltaT) {
    // Apply gravity
    //console.log('pre physics',model);

    // Apply thrust
    if(BABYLON) {
     model.dz -= GRAVITY_MS * deltaT;
     const thrust = model.propSpeed * DRONE_THRUST_PER_PROP_SPEED;
     const thrustVector = new BABYLON.Vector3(0, thrust, 0);
     const rotationMatrix = BABYLON.Matrix.RotationYawPitchRoll(model.yaw, model.pitch, model.roll);
     const rotatedThrustVector = BABYLON.Vector3.TransformNormal(thrustVector, rotationMatrix);
     //console.log('prop vector',rotatedThrustVector);

     model.dx += rotatedThrustVector.x * deltaT;
     model.dy += rotatedThrustVector.z * deltaT;
     model.dz += rotatedThrustVector.y * deltaT;
     //console.log({gravity:GRAVITY_MS*deltaT,thrust:rotatedThrustVector.y*deltaT});
    }

    // Apply movement
    model.x += model.dx * deltaT;
    model.y += model.dy * deltaT;
    model.z += model.dz * deltaT;
    model.pitch += model.dpitch * deltaT;
    model.yaw += model.dyaw * deltaT;
    model.roll += model.droll * deltaT;
    //console.log({dronedz:model.dz});

    // Apply drag (air resistance)
    //const dragFactor = 0.1 * deltaT;
    //model.dx *= (1 - dragFactor);
    //model.dy *= (1 - dragFactor);
    //model.dz *= (1 - dragFactor);
    //model.dpitch *= (1 - dragFactor);
    //model.dyaw *= (1 - dragFactor);
    //model.droll *= (1 - dragFactor);
    //Apply linear drag
    var velocity = Math.sqrt(model.dx*model.dx+model.dy*model.dy+model.dz*model.dz);
    if(velocity) {
     var drag_coef=500;
     var dragForce = drag_coef*velocity*velocity;
     var dragForce_ms = dragForce/1000;
     var nx = model.dx/velocity;
     var ny = model.dy/velocity;
     var nz = model.dz/velocity;
     model.dx-=nx*dragForce_ms*deltaT;
     model.dy-=ny*dragForce_ms*deltaT;
     model.dz-=nz*dragForce_ms*deltaT;
     //console.log({velocity,drag_coef,dragForce,nx,ny,nz,deltaT});
    }
    //Apply rotational drag
    const rotationSpeed = DRONE_STABILITY_FACTOR * DRONE_SENSITIVITY_MS * deltaT;
    if(model.droll>0) { 
     model.droll-=rotationSpeed;
     if(model.droll<0) model.droll=0;
    }
    else if(model.droll<0) { 
     model.droll+=rotationSpeed;
     if(model.droll>0) model.droll=0;
    }
    if(model.dpitch>0) { 
     model.dpitch-=rotationSpeed;
     if(model.dpitch<0) model.dpitch=0;
    }
    else if(model.dpitch<0) { 
     model.dpitch+=rotationSpeed;
     if(model.dpitch>0) model.dpitch=0;
    }
    if(model.dyaw>0) { 
     model.dyaw-=rotationSpeed;
     if(model.dyaw<0) model.dyaw=0;
    }
    else if(model.dyaw<0) { 
     model.dyaw+=rotationSpeed;
     if(model.dyaw>0) model.dyaw=0;
    }
    // Apply prop speed centering
    if(model.propSpeed>HOVER_PROP_SPEED) {
     model.propSpeed-=PROP_CHANGE_PER_MS*DRONE_STABILITY_FACTOR*deltaT;
     if(model.propSpeed<HOVER_PROP_SPEED) model.propSpeed=HOVER_PROP_SPEED;
    }
    else if(model.propSpeed<HOVER_PROP_SPEED) {
     model.propSpeed+=PROP_CHANGE_PER_MS*DRONE_STABILITY_FACTOR*deltaT;
     if(model.propSpeed>HOVER_PROP_SPEED) model.propSpeed=HOVER_PROP_SPEED;
    }
     
    // Prevent going below ground
    if (model.z < 0) {
       //console.log('Hit ground',{z:model.z,dz:model.dz});
        model.z = 0;
        model.dz = 0;
    }
    //console.log('post physics',model);
}

function applyHumanControlPhysics(client, model, deltaT) {
    //console.log('applyHumanControlPhysics');
    var walkthrust_ms = 0.3*GRAVITY_MS
    var radpersec = Math.PI;
    var radperms = radpersec/1000;


    if (client.controlState['w']) {
        model.dx += Math.sin(model.yaw)*walkthrust_ms*deltaT;
        model.dy += Math.cos(model.yaw)*walkthrust_ms*deltaT;
    }
    if (client.controlState['s']) {
        model.dx -= Math.sin(model.yaw)*walkthrust_ms*deltaT;
        model.dy -= Math.cos(model.yaw)*walkthrust_ms*deltaT;
    }
    if (client.controlState['a']) {
        model.dx -= Math.cos(model.yaw)*walkthrust_ms*deltaT;
        model.dy += Math.sin(model.yaw)*walkthrust_ms*deltaT;
    }
    if (client.controlState['d']) {
        model.dx += Math.cos(model.yaw)*walkthrust_ms*deltaT;
        model.dy -= Math.sin(model.yaw)*walkthrust_ms*deltaT;
    }
    if (client.controlState['ArrowLeft']) model.yaw -= radperms*deltaT;
    if (client.controlState['ArrowRight']) model.yaw += radperms*deltaT;
    if (client.controlState['ArrowUp']) model.pitch -= radperms*deltaT;
    if (client.controlState['ArrowDown']) model.pitch += radperms*deltaT;
    if (client.controlState['j'] && Math.abs(model.z-1.8)<0.1) {
        model.dz = JUMP_VELOCITY;
    }
}

function applyDroneControlPhysics(client, model, deltaT) {
    const rotationSpeed = DRONE_SENSITIVITY_MS * deltaT; // 1 rad/s

    if (client.controlState['d']) model.droll -= rotationSpeed;
    if (client.controlState['a']) model.droll += rotationSpeed;
    if (client.controlState['s']) model.dpitch -= rotationSpeed;
    if (client.controlState['w']) model.dpitch += rotationSpeed;
    if (client.controlState['q']) model.dyaw -= rotationSpeed;
    if (client.controlState['e']) model.dyaw += rotationSpeed;
    if (client.controlState['ArrowLeft']) model.dyaw -= rotationSpeed;
    if (client.controlState['ArrowRight']) model.dyaw += rotationSpeed;
    if (client.controlState['f']) {
        model.roll = model.pitch = 0;
        model.droll = model.dpitch = model.dyaw = 0;
    }
    if (client.controlState['g']) {
        model.dx = model.dy = model.dz = 0;
        model.roll = model.pitch = 0;
        model.droll = model.dpitch = model.dyaw = 0;
    }
    if (client.controlState['ArrowUp']) {
        model.propSpeed += PROP_CHANGE_PER_MS * deltaT;
        if (model.propSpeed > MAX_PROP_SPEED) model.propSpeed = MAX_PROP_SPEED;
    }
    if (client.controlState['ArrowDown']) {
        model.propSpeed -= PROP_CHANGE_PER_MS * deltaT;
        if (model.propSpeed < MIN_PROP_SPEED) model.propSpeed = MIN_PROP_SPEED;
    }
}

module.exports =  {updateGameState };