judgement_ctrl.c

/*
  判定コマンド
  Satofumi KAMIMURA
  $Id$
*/

#include "judgement_ctrl.h"
#include "coordinate_ctrl.h"
#include "coordinateCtrl_target.h"
#define C_RUNCTRL_SOURCE
#include "commandCtrl.h"
#include <math.h>


int run_getDiffPos(int crd_id, int x, int y) {

  int pos_x, pos_y, pos_div16;
  int x_diff, y_diff;
  run_getBodyPos(crd_id, &pos_x, &pos_y, &pos_div16);
  x_diff = pos_x - x;
  y_diff = pos_y - y;

  return (int)sqrt((x_diff * x_diff) + (y_diff * y_diff));
}


int run_getDiffAng(int crd_id, int div16) {
  int pos_x, pos_y, pos_div16;
  run_getBodyPos(crd_id, &pos_x, &pos_y, &pos_div16);

  return getSignedDiv16((pos_div16 - div16) & 0xffff);
}


int run_getDiffStraightVel(int mm_vel) {
  int straight_vel;
  int ret_value = recvStraightVel(&straight_vel);
  if (ret_value < 0) {
    return ret_value;
  }
  return (straight_vel - mm_vel);
}


int run_getDiffRotateVel(int div16_vel) {
  int rotate_vel;
  int ret_value = recvRotateVel(&rotate_vel);
  if (ret_value < 0) {
    return ret_value;
  }
  return rotate_vel - div16_vel;
}


int run_getDiffLine(int crd_id, int x, int y, int div16) {
  int diff_x, diff_y;
  double radian;
  int gl_x, gl_y, gl_div16;
  int ret_value = run_getBodyPos(crd_id, &gl_x, &gl_y, &gl_div16);
  if (ret_value < 0) {
    return ret_value;
  }
  diff_x = gl_x - x;
  diff_y = gl_y - y;
  radian = 2.0 * M_PI * div16 / 65536.0;

  return (int)rint(diff_x * cos(radian) + diff_y * sin(radian));
}