[pathmat] rotatecommand needs to be absolute

src/common/game-types.ts

@@ -30,7 +30,7 @@ export function oppositeSide(side: Side) {
 }
 
 export function getStartHeading(side: Side) {
-    return side === Side.WHITE ? 90 * DEGREE : 90 * DEGREE;
+    return side === Side.WHITE ? 90 * DEGREE : 270 * DEGREE;
 }
 export class Piece {
     constructor(

src/server/api/game-manager.ts

@@ -113,7 +113,7 @@ export class HumanGameManager extends GameManager {
             this.chess.makeMove(message.move);
 
             console.log("running executor");
-            console.log(command);
+            console.dir(command, { depth: null });
             await executor.execute(command);
             console.log("executor done");
 

src/server/command/move-command.ts

@@ -112,7 +112,10 @@ export class DriveCommand
         );
         robotManager.updateRobot(
             this.robotId,
-            new GridIndices(Math.floor(robot.position.x), Math.floor(robot.position.y)),
+            new GridIndices(
+                Math.floor(robot.position.x),
+                Math.floor(robot.position.y),
+            ),
         );
         return robot.sendDrivePacket(this.tileDistance);
     }
@@ -147,10 +150,12 @@ export class RelativeMoveCommand
 {
     public async execute(): Promise<void> {
         const robot = robotManager.getRobot(this.robotId);
-        robot.position = this.position.add(this.position);
         robotManager.updateRobot(
             this.robotId,
-            new GridIndices(Math.floor(robot.position.x), Math.floor(robot.position.y)),
+            new GridIndices(
+                Math.floor(robot.position.x + this.position.x),
+                Math.floor(robot.position.y + this.position.y),
+            ),
         );
         return robot.relativeMove(this.position);
     }
@@ -166,6 +171,13 @@ export class RelativeMoveCommand
 export class AbsoluteMoveCommand extends MoveCommand {
     public async execute(): Promise<void> {
         const robot = robotManager.getRobot(this.robotId);
+        robotManager.updateRobot(
+            this.robotId,
+            new GridIndices(
+                Math.floor(this.position.x),
+                Math.floor(this.position.y),
+            ),
+        );
         return robot.relativeMove(this.position.sub(robot.position));
     }
 }

src/server/robot/path-materializer.ts

@@ -10,7 +10,7 @@ import {
     AbsoluteMoveCommand,
     DriveCommand,
     MoveCommand,
-    RelativeRotateCommand,
+    ReversibleAbsoluteRotateCommand,
 } from "../command/move-command";
 import { MovePiece, ReversibleRobotCommand } from "../command/move-piece";
 import { Position } from "./position";
@@ -308,20 +308,21 @@ function constructDriveCommand(
 function constructRotateCommand(
     pieceId: string,
     location: Position,
-): RelativeRotateCommand {
+): ReversibleRobotCommand {
     const robot = robotManager.getRobot(pieceId);
     const offset = location.sub(robot.position);
-    const angle = Math.atan2(-offset.x, offset.y);
-    return new RelativeRotateCommand(pieceId, angle);
+    const angle = Math.atan2(offset.y, offset.x);
+    console.log("rotate cmd construct", robot.position, offset, angle);
+    return new ReversibleAbsoluteRotateCommand(pieceId, () => angle);
 }
 
 function constructFinalCommand(
     move: GridMove,
     driveCommands: DriveCommand[],
-    rotateCommands: RelativeRotateCommand[],
+    rotateCommands: ReversibleRobotCommand[],
 ): MovePiece {
     const from = move.from;
-    console.log(from, robotManager.indicesToIds);
+    // console.log(from, robotManager.indicesToIds);
     const mainPiece = robotManager.getRobotAtIndices(from).id;
 
     if (mainPiece !== undefined) {
@@ -343,7 +344,7 @@ function constructFinalCommand(
 // If there are pieces in the way, it shimmy's them out, and move them back after main piece passes
 function moveMainPiece(move: GridMove): MovePiece {
     const driveCommands: DriveCommand[] = [];
-    const rotateCommands: RelativeRotateCommand[] = [];
+    const rotateCommands: ReversibleRobotCommand[] = [];
     const collisionType = calcCollisionType(move);
     const collisions: string[] = detectCollisions(move, collisionType);
     for (let i = 0; i < collisions.length; i++) {
@@ -423,6 +424,10 @@ function directionToEdge(position: GridIndices) {
     return DirectionTuple;
 }
 
+function findGridIndicesInArray(array: GridIndices[], obj: GridIndices): number {
+    return array.findIndex((o) => o.i == obj.i && o.j == obj.j);
+}
+
 function returnToHome(from: GridIndices, id: string): SequentialCommandGroup {
     //const capturedPiece: GridIndices = GridIndices.squareToGrid(from);
     const home: GridIndices = robotManager.getRobot(id).homeIndices;
@@ -442,20 +447,22 @@ function returnToHome(from: GridIndices, id: string): SequentialCommandGroup {
     for (const direction of checkDirections) {
         if (arrayOfDeadzone.find((dz) => dz.equals(home.addTuple(direction)))) {
             finalDestination = home.addTuple(direction);
+            break;
         }
     }
     if (!finalDestination) {
         throw new error("WHERE THE HELL ARE YOU GOING");
     }
-    const startInArray = arrayOfDeadzone.indexOf(startInDeadzone);
-    const endInArray = arrayOfDeadzone.indexOf(finalDestination);
+    const startInArray = findGridIndicesInArray(arrayOfDeadzone, startInDeadzone);
+    const endInArray = findGridIndicesInArray(arrayOfDeadzone, finalDestination);
     let differenceOfIndex = endInArray - startInArray;
 
     if (differenceOfIndex < 0) {
         differenceOfIndex += 36;
     }
 
     const botDirectionToHome = differenceOfIndex < 18 ? 1 : -1;
+    console.log("deadzone array checker", startInArray, endInArray, botDirectionToHome);
 
     let i = startInArray;
     const moveCommands: MoveCommand[] = [];

src/server/robot/robot.ts

@@ -74,12 +74,14 @@ export class Robot {
      */
     public async relativeMove(deltaPosition: Position): Promise<void> {
         // NOTE: the implementation of this is wrong. it doesn't work properly but it is not needed for now so just ignoring. if someone wants to use this in the future, we can fix it but we probably won't need it in the future anyway (or at least that is what Dylan says)
-        const offset = deltaPosition.sub(this.position);
-        const distance = Math.hypot(offset.x, offset.y);
-        const angle = clampHeading(Math.atan2(-offset.x, offset.y) * RADIAN);
+        const distance = Math.hypot(deltaPosition.x, deltaPosition.y);
+        const angle = clampHeading(
+            Math.atan2(deltaPosition.y, deltaPosition.x) * RADIAN,
+        );
         const promise = this.absoluteRotate(angle).then(() => {
             return this.sendDrivePacket(distance);
         });
+        this.position = this.position.add(deltaPosition);
         console.log(this.position);
         return promise;
     }