y2019/vision/server/www/robot.ts

import {IN_TO_M, FT_TO_M} from './constants';
import {CAMERA_POSES} from './camera_constants';

const ROBOT_WIDTH = 25 * IN_TO_M;
const ROBOT_LENGTH = 31 * IN_TO_M;
const CAMERA_SCALE = 0.3;

function drawCamera(ctx : canvasRenderingContext2d, pose : {x : number, y : number, theta : number}) : void {
  ctx.beginPath();
  ctx.moveTo(pose.x, pose.y);
  ctx.lineTo(pose.x + CAMERA_SCALE * Math.cos(pose.theta + Math.PI / 4.0),
             pose.y + CAMERA_SCALE * Math.sin(pose.theta + Math.PI / 4.0));
  ctx.lineTo(pose.x + CAMERA_SCALE * Math.cos(pose.theta - Math.PI / 4.0),
             pose.y + CAMERA_SCALE * Math.sin(pose.theta - Math.PI / 4.0));
  ctx.closePath();
  ctx.stroke();
}

export function drawRobot(ctx : CanvasRenderingContext2D, x : number, y : number, theta : number, camera_colors : string[]) : void {
  ctx.save();
  ctx.translate(x, y);
  ctx.rotate(theta);

  ctx.fillStyle = 'blue';
  ctx.fillRect(-ROBOT_LENGTH / 2, -ROBOT_WIDTH / 2, ROBOT_LENGTH, ROBOT_WIDTH);

  ctx.beginPath();
  ctx.strokeStyle = 'black';
  ctx.moveTo(ROBOT_LENGTH / 2, -ROBOT_WIDTH/2);
  ctx.lineTo(ROBOT_LENGTH / 2 + 0.1, 0);
  ctx.lineTo(ROBOT_LENGTH / 2, ROBOT_WIDTH/2);
  ctx.closePath();
  ctx.stroke();
  ctx.lineWidth = 3.0 * ctx.lineWidth;
  for (let ii of [0, 1, 2, 3, 4]) {
    ctx.strokeStyle = camera_colors[ii];
    drawCamera(ctx, CAMERA_POSES[ii]);
  }

  ctx.restore();
}