Merge pull request #347 from Thorium-Sim/autopilot-rotation

Add rotation autopilot to rotate a ship towards a destination

server/package.json

@@ -39,6 +39,7 @@
     "fastify-websocket": "^4.0.0",
     "less": "^4.1.2",
     "luxon": "^2.2.0",
+    "node-pid-controller": "^1.0.1",
     "postcss": "^8.4.5",
     "postcss-less": "^5.0.0",
     "tailwindcss": "^3.0.5",

server/src/components/autopilot.ts

@@ -0,0 +1,32 @@
+import Controller from "node-pid-controller";
+import {Coordinates} from "../utils/unitTypes";
+import {Component} from "./utils";
+
+export class AutopilotComponent extends Component {
+  static id: "autopilot" = "autopilot";
+  static serialize(component: Omit<AutopilotComponent, "init">) {
+    const {
+      yawController,
+      pitchController,
+      rollController,
+      impulseController,
+      warpController,
+      ...data
+    } = component;
+    return data;
+  }
+  /** The desired coordinates of the ship in the current stage. If desiredSolarSystemId is null, then it's interstellar coordinates */
+  desiredCoordinates?: Coordinates<number>;
+  /** Desired interstellar system. For when we are traveling from one system to another. */
+  desiredSolarSystemId?: number | null;
+  /** Whether the rotation autopilot is on. */
+  rotationAutopilot: boolean = true;
+  /** Whether the forward movement autopilot is on. */
+  forwardAutopilot: boolean = true;
+
+  yawController?: Controller;
+  pitchController?: Controller;
+  rollController?: Controller;
+  impulseController?: Controller;
+  warpController?: Controller;
+}

server/src/components/list.ts

@@ -24,3 +24,4 @@ export * from "./solarSystem/isPlanet";
 export * from "./satellite";
 export * from "./temperature";
 export * from "./population";
+export * from "./autopilot";

server/src/systems/AutoRotateSystem.ts

@@ -0,0 +1,145 @@
+import {Quaternion, Vector3, Matrix4} from "three";
+import Controller from "node-pid-controller";
+import {Entity, System} from "../utils/ecs";
+import {autopilotGetCoordinates} from "../utils/autopilotGetCoordinates";
+
+let positionVec = new Vector3();
+let rotationQuat = new Quaternion();
+let desiredDestination = new Vector3();
+let desiredRotationQuat = new Quaternion();
+let up = new Vector3(0, 1, 0);
+let matrix = new Matrix4();
+const rotationMatrix = new Matrix4().makeRotationY(-Math.PI);
+
+const C_PROPORTION = 1;
+const C_INTEGRAL = 0;
+const C_DERIVATIVE = 0.8;
+
+const getYawPitchRoll = (quat: Quaternion) => {
+  const yaw =
+    Math.atan2(
+      2 * quat.y * quat.w - 2 * quat.x * quat.z,
+      1 - 2 * quat.y * quat.y - 2 * quat.z * quat.z
+    ) +
+    Math.PI * 4;
+  const pitch =
+    Math.atan2(
+      2 * quat.x * quat.w - 2 * quat.y * quat.z,
+      1 - 2 * quat.x * quat.x - 2 * quat.z * quat.z
+    ) +
+    Math.PI * 4;
+  const roll =
+    Math.asin(2 * quat.x * quat.y + 2 * quat.z * quat.w) + Math.PI * 4;
+  return [yaw, pitch, roll];
+};
+
+function getClosestAngle(current: number, target: number) {
+  if (Math.abs(target - (current + Math.PI * 2)) < Math.abs(target - current)) {
+    return current + Math.PI * 2;
+  }
+  return current;
+}
+export class AutoRotateSystem extends System {
+  test(entity: Entity) {
+    return !!(
+      entity.components.isShip &&
+      entity.components.rotation &&
+      entity.components.autopilot
+    );
+  }
+
+  update(entity: Entity, elapsed: number) {
+    const {position, rotation, autopilot} = entity.components;
+    if (
+      !position ||
+      !rotation ||
+      !autopilot?.rotationAutopilot ||
+      !autopilot?.desiredCoordinates
+    ) {
+      return;
+    }
+
+    // Get the current system the ship is in and the autopilot desired system
+    autopilotGetCoordinates(
+      this.ecs.entities,
+      entity,
+      desiredDestination,
+      positionVec
+    );
+
+    const distance = positionVec.distanceTo(desiredDestination);
+    rotationQuat.set(rotation.x, rotation.y, rotation.z, rotation.w);
+    up.set(0, 1, 0).applyQuaternion(rotationQuat);
+
+    matrix.lookAt(positionVec, desiredDestination, up).multiply(rotationMatrix);
+    // Use the thrusters to adjust the rotation of the ship to point towards the desired destination.
+    // First, determine the angle to the destination.
+    desiredRotationQuat.setFromRotationMatrix(matrix);
+
+    const desiredAngles = getYawPitchRoll(desiredRotationQuat);
+    const currentAngles = getYawPitchRoll(rotationQuat);
+
+    // Initialize the controllers, if necessary
+    if (!(autopilot.yawController instanceof Controller)) {
+      autopilot.yawController = new Controller(
+        C_PROPORTION,
+        C_INTEGRAL,
+        C_DERIVATIVE,
+        1000 / 60 / 1000
+      );
+    }
+    if (!(autopilot.pitchController instanceof Controller)) {
+      autopilot.pitchController = new Controller(
+        C_PROPORTION,
+        C_INTEGRAL,
+        C_DERIVATIVE,
+        1000 / 60 / 1000
+      );
+    }
+    if (!(autopilot.rollController instanceof Controller)) {
+      autopilot.rollController = new Controller(
+        C_PROPORTION,
+        C_INTEGRAL,
+        C_DERIVATIVE,
+        1000 / 60 / 1000
+      );
+    }
+
+    // Set the target of the controllers
+    autopilot.yawController.target = desiredAngles[0];
+    autopilot.pitchController.target = desiredAngles[1];
+    autopilot.rollController.target = desiredAngles[2];
+
+    // Update controllers with the current rotation
+    let yawCorrection = autopilot.yawController.update(
+      getClosestAngle(currentAngles[0], desiredAngles[0])
+    );
+    let pitchCorrection = autopilot.pitchController.update(
+      getClosestAngle(currentAngles[1], desiredAngles[1])
+    );
+    let rollCorrection = autopilot.rollController.update(
+      getClosestAngle(currentAngles[2], desiredAngles[2])
+    );
+
+    if (distance < 1) {
+      yawCorrection = 0;
+      pitchCorrection = 0;
+      rollCorrection = 0;
+    }
+    // Set the thruster acceleration to those values
+    const thrusters = this.ecs.entities.find(
+      sysEntity =>
+        sysEntity.components.isThrusters &&
+        entity.components.shipSystems?.shipSystemIds.includes(sysEntity.id)
+    );
+    if (thrusters?.components?.isThrusters) {
+      thrusters.updateComponent("isThrusters", {
+        rotationDelta: {
+          x: Math.min(Math.max(pitchCorrection, -1), 1),
+          y: Math.min(Math.max(yawCorrection, -1), 1),
+          z: Math.min(Math.max(rollCorrection, -1), 1),
+        },
+      });
+    }
+  }
+}

server/src/systems/__test__/AutoRotateSystem.test.ts

@@ -0,0 +1,74 @@
+import {createMockDataContext} from "server/src/utils/createMockDataContext";
+import {ECS, Entity} from "server/src/utils/ecs";
+import {AutoRotateSystem} from "../AutoRotateSystem";
+import {RotationSystem} from "../RotationSystem";
+import {ThrusterSystem} from "../ThrusterSystem";
+
+describe("AutoRotateSystem", () => {
+  let ecs: ECS;
+  let thrustersSystem: ThrusterSystem;
+  let rotationSystem: RotationSystem;
+  let autoRotateSystem: AutoRotateSystem;
+  beforeEach(() => {
+    const mockDataContext = createMockDataContext();
+    ecs = new ECS(mockDataContext.server);
+    thrustersSystem = new ThrusterSystem();
+    rotationSystem = new RotationSystem();
+    autoRotateSystem = new AutoRotateSystem();
+  });
+  it("should properly update an entity with the system destination component", async () => {
+    const thrusters = new Entity(null, {
+      isThrusters: {},
+      isShipSystem: {type: "thrusters"},
+    });
+    const dampening = new Entity(null, {
+      isInertialDampeners: {},
+      isShipSystem: {type: "inertialDampeners"},
+    });
+    const ship = new Entity(100, {
+      isShip: {},
+      mass: {mass: 2000},
+      position: {},
+      velocity: {},
+      rotation: {},
+      rotationVelocity: {},
+      autopilot: {},
+      shipSystems: {shipSystemIds: [thrusters.id, dampening.id]},
+    });
+
+    ecs.addSystem(autoRotateSystem);
+    ecs.addSystem(thrustersSystem);
+    ecs.addSystem(rotationSystem);
+    ecs.addEntity(thrusters);
+    ecs.addEntity(dampening);
+    ecs.addEntity(ship);
+    if (!ship.components.velocity)
+      throw new Error("Ship has no velocity component");
+    if (!ship.components.rotation)
+      throw new Error("Ship has no rotation component");
+    ship.updateComponent("autopilot", {
+      desiredCoordinates: {x: 0, y: 100, z: 100},
+    });
+    ecs.update(16);
+    expect(ship.components.rotation).toMatchInlineSnapshot(`
+      RotationComponent {
+        "w": 0.9999999992,
+        "x": -0.00003999999998933334,
+        "y": 0,
+        "z": 0,
+      }
+    `);
+    for (let i = 0; i < 60 * 50; i++) {
+      ecs.update(16);
+    }
+
+    expect(ship.components.rotation).toMatchInlineSnapshot(`
+      RotationComponent {
+        "w": 0.9238795355100139,
+        "x": -0.3826834251255424,
+        "y": 0,
+        "z": 0,
+      }
+    `);
+  });
+});

server/src/systems/index.ts

@@ -2,14 +2,28 @@
  * Since the order of system execution matters, we need to import all
  * of the systems and re-export them in an array
  */
+import {AutoRotateSystem} from "./AutoRotateSystem";
 import {DataStreamSystem} from "./DataStreamSystem";
+import {EngineVelocityPosition} from "./EngineVelocityPosition";
+import {EngineVelocitySystem} from "./EngineVelocitySystem";
 import {ImpulseSystem} from "./ImpulseSystem";
+import {PositionVelocitySystem} from "./PositionVelocitySystem";
 import {RandomMovementSystem} from "./RandomMovementSystem";
+import {RotationSystem} from "./RotationSystem";
+import {ThrusterSystem} from "./ThrusterSystem";
 import {TimerSystem} from "./TimerSystem";
+import {WarpSystem} from "./WarpSystem";
 
 const systems = [
   TimerSystem,
+  AutoRotateSystem,
+  ThrusterSystem,
   ImpulseSystem,
+  WarpSystem,
+  RotationSystem,
+  EngineVelocitySystem,
+  EngineVelocityPosition,
+  PositionVelocitySystem,
   RandomMovementSystem,
   DataStreamSystem,
 ];