wiiuse/ir.c

   1 #include <stdio.h>
   2 #include <math.h>
   3 #include <time.h>
   4
   5 #ifndef WIN32
   6         #include <unistd.h>
   7 #endif
   8 #ifdef GEKKO
   9         #include <ogcsys.h>
  10 #endif
  11 #include "definitions.h"
  12 #include "wiiuse_internal.h"
  13 #include "ir.h"
  14
  15 static int ir_correct_for_bounds(float* x, float* y, enum aspect_t aspect, int offset_x, int offset_y);
  16 static void ir_convert_to_vres(float* x, float* y, enum aspect_t aspect, unsigned int vx, unsigned int vy);
  17
  18 /**
  19  *      @brief  Get the IR sensitivity settings.
  20  *
  21  *      @param wm               Pointer to a wiimote_t structure.
  22  *      @param block1   [out] Pointer to where block1 will be set.
  23  *      @param block2   [out] Pointer to where block2 will be set.
  24  *
  25  *      @return Returns the sensitivity level.
  26  */
  27 static int get_ir_sens(struct wiimote_t* wm, char** block1, char** block2) {
  28         if (WIIMOTE_IS_SET(wm, WIIMOTE_STATE_IR_SENS_LVL1)) {
  29                 *block1 = WM_IR_BLOCK1_LEVEL1;
  30                 *block2 = WM_IR_BLOCK2_LEVEL1;
  31                 return 1;
  32         } else if (WIIMOTE_IS_SET(wm, WIIMOTE_STATE_IR_SENS_LVL2)) {
  33                 *block1 = WM_IR_BLOCK1_LEVEL2;
  34                 *block2 = WM_IR_BLOCK2_LEVEL2;
  35                 return 2;
  36         } else if (WIIMOTE_IS_SET(wm, WIIMOTE_STATE_IR_SENS_LVL3)) {
  37                 *block1 = WM_IR_BLOCK1_LEVEL3;
  38                 *block2 = WM_IR_BLOCK2_LEVEL3;
  39                 return 3;
  40         } else if (WIIMOTE_IS_SET(wm, WIIMOTE_STATE_IR_SENS_LVL4)) {
  41                 *block1 = WM_IR_BLOCK1_LEVEL4;
  42                 *block2 = WM_IR_BLOCK2_LEVEL4;
  43                 return 4;
  44         } else if (WIIMOTE_IS_SET(wm, WIIMOTE_STATE_IR_SENS_LVL5)) {
  45                 *block1 = WM_IR_BLOCK1_LEVEL5;
  46                 *block2 = WM_IR_BLOCK2_LEVEL5;
  47                 return 5;
  48         }
  49
  50         *block1 = NULL;
  51         *block2 = NULL;
  52         return 0;
  53 }
  54
  55 static void rotate_dots(struct fdot_t* in, struct fdot_t *out, int count, float ang) {
  56         float s, c;
  57         int i;
  58
  59         if (ang == 0) {
  60                 for (i = 0; i < count; ++i) {
  61                         out[i].x = in[i].x;
  62                         out[i].y = in[i].y;
  63                 }
  64                 return;
  65         }
  66
  67         s = sin(DEGREE_TO_RAD(ang));
  68         c = cos(DEGREE_TO_RAD(ang));
  69
  70         /*
  71          *      [ cos(theta)  -sin(theta) ][ ir->rx ]
  72          *      [ sin(theta)  cos(theta)  ][ ir->ry ]
  73          */
  74
  75         for (i = 0; i < count; ++i) {
  76                 out[i].x = (c * in[i].x) + (-s * in[i].y);
  77                 out[i].y = (s * in[i].x) + (c * in[i].y);
  78         }
  79 }
  80
  81 /**
  82  *      @brief Correct for the IR bounding box.
  83  *
  84  *      @param x                [out] The current X, it will be updated if valid.
  85  *      @param y                [out] The current Y, it will be updated if valid.
  86  *      @param aspect   Aspect ratio of the screen.
  87  *      @param offset_x The X offset of the bounding box.
  88  *      @param offset_y The Y offset of the bounding box.
  89  *
  90  *      @return Returns 1 if the point is valid and was updated.
  91  *
  92  *      Nintendo was smart with this bit. They sacrifice a little
  93  *      precision for a big increase in usability.
  94  */
  95 static int ir_correct_for_bounds(float* x, float* y, enum aspect_t aspect, int offset_x, int offset_y) {
  96         float x0, y0;
  97         int xs, ys;
  98
  99         if (aspect == WIIUSE_ASPECT_16_9) {
 100                 xs = WM_ASPECT_16_9_X;
 101                 ys = WM_ASPECT_16_9_Y;
 102         } else {
 103                 xs = WM_ASPECT_4_3_X;
 104                 ys = WM_ASPECT_4_3_Y;
 105         }
 106
 107         x0 = ((1024 - xs) / 2) + offset_x;
 108         y0 = ((768 - ys) / 2) + offset_y;
 109
 110         if ((*x >= x0)
 111                 && (*x <= (x0 + xs))
 112                 && (*y >= y0)
 113                 && (*y <= (y0 + ys)))
 114         {
 115                 *x -= offset_x;
 116                 *y -= offset_y;
 117
 118                 return 1;
 119         }
 120
 121         return 0;
 122 }
 123
 124
 125 /**
 126  *      @brief Interpolate the point to the user defined virtual screen resolution.
 127  */
 128 static void ir_convert_to_vres(float* x, float* y, enum aspect_t aspect, unsigned int vx, unsigned int vy) {
 129         int xs, ys;
 130
 131         if (aspect == WIIUSE_ASPECT_16_9) {
 132                 xs = WM_ASPECT_16_9_X;
 133                 ys = WM_ASPECT_16_9_Y;
 134         } else {
 135                 xs = WM_ASPECT_4_3_X;
 136                 ys = WM_ASPECT_4_3_Y;
 137         }
 138
 139         *x -= ((1024-xs)/2);
 140         *y -= ((768-ys)/2);
 141
 142         *x = (*x / (float)xs) * vx;
 143         *y = (*y / (float)ys) * vy;
 144 }
 145
 146 void wiiuse_set_ir_mode(struct wiimote_t *wm)
 147 {
 148         ubyte buf = 0x00;
 149
 150         if(!wm) return;
 151         if(!WIIMOTE_IS_SET(wm,WIIMOTE_STATE_IR)) return;
 152
 153         if(WIIMOTE_IS_SET(wm,WIIMOTE_STATE_EXP)) buf = WM_IR_TYPE_BASIC;
 154         else buf = WM_IR_TYPE_EXTENDED;
 155         wiiuse_write_data(wm,WM_REG_IR_MODENUM, &buf, 1, NULL);
 156 }
 157
 158 void wiiuse_set_ir(struct wiimote_t *wm,int status)
 159 {
 160         ubyte buf = 0x00;
 161         int ir_level = 0;
 162         char* block1 = NULL;
 163         char* block2 = NULL;
 164
 165         if(!wm) return;
 166
 167         /*
 168          *      Wait for the handshake to finish first.
 169          *      When it handshake finishes and sees that
 170          *      IR is enabled, it will call this function
 171          *      again to actually enable IR.
 172          */
 173         if(!WIIMOTE_IS_SET(wm,WIIMOTE_STATE_HANDSHAKE_COMPLETE)) {
 174                 WIIUSE_DEBUG("Tried to enable IR, will wait until handshake finishes.\n");
 175                 if(status)
 176                         WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_IR_INIT);
 177                 else
 178                         WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_IR_INIT);
 179                 return;
 180         }
 181
 182         /*
 183          *      Check to make sure a sensitivity setting is selected.
 184          */
 185         ir_level = get_ir_sens(wm, &block1, &block2);
 186         if (!ir_level) {
 187                 WIIUSE_ERROR("No IR sensitivity setting selected.");
 188                 return;
 189         }
 190
 191         if (status) {
 192                 /* if already enabled then stop */
 193                 if (WIIMOTE_IS_SET(wm, WIIMOTE_STATE_IR)) {
 194                         wiiuse_status(wm,NULL);
 195                         return;
 196                 }
 197         } else {
 198                 /* if already disabled then stop */
 199                 if (!WIIMOTE_IS_SET(wm, WIIMOTE_STATE_IR)) {
 200                         wiiuse_status(wm,NULL);
 201                         return;
 202                 }
 203         }
 204
 205         buf = (status ? 0x04 : 0x00);
 206         wiiuse_sendcmd(wm,WM_CMD_IR,&buf,1,NULL);
 207         wiiuse_sendcmd(wm,WM_CMD_IR_2,&buf,1,NULL);
 208
 209         if (!status) {
 210                 WIIUSE_DEBUG("Disabled IR cameras for wiimote id %i.", wm->unid);
 211                 wiiuse_status(wm,NULL);
 212                 return;
 213         }
 214
 215         /* enable IR, set sensitivity */
 216         buf = 0x08;
 217         wiiuse_write_data(wm,WM_REG_IR,&buf,1,NULL);
 218
 219         wiiuse_write_data(wm, WM_REG_IR_BLOCK1, (ubyte*)block1, 9, NULL);
 220         wiiuse_write_data(wm, WM_REG_IR_BLOCK2, (ubyte*)block2, 2, NULL);
 221
 222         if(WIIMOTE_IS_SET(wm,WIIMOTE_STATE_EXP)) buf = WM_IR_TYPE_BASIC;
 223         else buf = WM_IR_TYPE_EXTENDED;
 224         wiiuse_write_data(wm,WM_REG_IR_MODENUM, &buf, 1, NULL);
 225
 226         wiiuse_status(wm,NULL);
 227         return;
 228 }
 229
 230 /**
 231  *      @brief  Set the virtual screen resolution for IR tracking.
 232  *
 233  *      @param wm               Pointer to a wiimote_t structure.
 234  *      @param status   1 to enable, 0 to disable.
 235  */
 236 void wiiuse_set_ir_vres(struct wiimote_t* wm, unsigned int x, unsigned int y) {
 237         if (!wm)        return;
 238
 239         wm->ir.vres[0] = (x-1);
 240         wm->ir.vres[1] = (y-1);
 241 }
 242
 243 /**
 244  *      @brief  Set the XY position for the IR cursor.
 245  *
 246  *      @param wm               Pointer to a wiimote_t structure.
 247  */
 248 void wiiuse_set_ir_position(struct wiimote_t* wm, enum ir_position_t pos) {
 249         if (!wm)        return;
 250
 251         wm->ir.pos = pos;
 252
 253         switch (pos) {
 254
 255                 case WIIUSE_IR_ABOVE:
 256                         wm->ir.offset[0] = 0;
 257
 258                         if (wm->ir.aspect == WIIUSE_ASPECT_16_9)
 259                                 wm->ir.offset[1] = WM_ASPECT_16_9_Y/2 - 70;
 260                         else if (wm->ir.aspect == WIIUSE_ASPECT_4_3)
 261                                 wm->ir.offset[1] = WM_ASPECT_4_3_Y/2 - 100;
 262
 263                         return;
 264
 265                 case WIIUSE_IR_BELOW:
 266                         wm->ir.offset[0] = 0;
 267
 268                         if (wm->ir.aspect == WIIUSE_ASPECT_16_9)
 269                                 wm->ir.offset[1] = -WM_ASPECT_16_9_Y/2 + 70;
 270                         else if (wm->ir.aspect == WIIUSE_ASPECT_4_3)
 271                                 wm->ir.offset[1] = -WM_ASPECT_4_3_Y/2 + 100;
 272
 273                         return;
 274
 275                 default:
 276                         return;
 277         };
 278 }
 279
 280 /**
 281  *      @brief  Set the aspect ratio of the TV/monitor.
 282  *
 283  *      @param wm               Pointer to a wiimote_t structure.
 284  *      @param aspect   Either WIIUSE_ASPECT_16_9 or WIIUSE_ASPECT_4_3
 285  */
 286 void wiiuse_set_aspect_ratio(struct wiimote_t* wm, enum aspect_t aspect) {
 287         if (!wm)        return;
 288
 289         wm->ir.aspect = aspect;
 290
 291         if (aspect == WIIUSE_ASPECT_4_3) {
 292                 wm->ir.vres[0] = WM_ASPECT_4_3_X;
 293                 wm->ir.vres[1] = WM_ASPECT_4_3_Y;
 294         } else {
 295                 wm->ir.vres[0] = WM_ASPECT_16_9_X;
 296                 wm->ir.vres[1] = WM_ASPECT_16_9_Y;
 297         }
 298
 299         /* reset the position offsets */
 300         wiiuse_set_ir_position(wm, wm->ir.pos);
 301 }
 302
 303
 304 /**
 305  *      @brief  Set the IR sensitivity.
 306  *
 307  *      @param wm               Pointer to a wiimote_t structure.
 308  *      @param level    1-5, same as Wii system sensitivity setting.
 309  *
 310  *      If the level is < 1, then level will be set to 1.
 311  *      If the level is > 5, then level will be set to 5.
 312  */
 313 void wiiuse_set_ir_sensitivity(struct wiimote_t* wm, int level) {
 314         char* block1 = NULL;
 315         char* block2 = NULL;
 316
 317         if (!wm)        return;
 318
 319         if (level > 5)          level = 5;
 320         if (level < 1)          level = 1;
 321
 322         WIIMOTE_DISABLE_STATE(wm, (WIIMOTE_STATE_IR_SENS_LVL1 |
 323                                                                 WIIMOTE_STATE_IR_SENS_LVL2 |
 324                                                                 WIIMOTE_STATE_IR_SENS_LVL3 |
 325                                                                 WIIMOTE_STATE_IR_SENS_LVL4 |
 326                                                                 WIIMOTE_STATE_IR_SENS_LVL5));
 327
 328         switch (level) {
 329                 case 1:
 330                         WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_IR_SENS_LVL1);
 331                         break;
 332                 case 2:
 333                         WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_IR_SENS_LVL2);
 334                         break;
 335                 case 3:
 336                         WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_IR_SENS_LVL3);
 337                         break;
 338                 case 4:
 339                         WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_IR_SENS_LVL4);
 340                         break;
 341                 case 5:
 342                         WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_IR_SENS_LVL5);
 343                         break;
 344                 default:
 345                         return;
 346         }
 347
 348         if(!WIIMOTE_IS_SET(wm,WIIMOTE_STATE_IR)) return;
 349
 350         /* set the new sensitivity */
 351         get_ir_sens(wm, &block1, &block2);
 352
 353         wiiuse_write_data(wm, WM_REG_IR_BLOCK1, (ubyte*)block1, 9,NULL);
 354         wiiuse_write_data(wm, WM_REG_IR_BLOCK2, (ubyte*)block2, 2,NULL);
 355
 356         WIIUSE_DEBUG("Set IR sensitivity to level %i (unid %i)", level, wm->unid);
 357 }
 358
 359
 360 /**
 361  *      @brief Calculate the data from the IR spots.  Basic IR mode.
 362  *
 363  *      @param wm               Pointer to a wiimote_t structure.
 364  *      @param data             Data returned by the wiimote for the IR spots.
 365  */
 366 void calculate_basic_ir(struct wiimote_t* wm, ubyte* data) {
 367         struct ir_dot_t* dot = wm->ir.dot;
 368         int i;
 369
 370         dot[0].rx = 1023 - (data[0] | ((data[2] & 0x30) << 4));
 371         dot[0].ry = data[1] | ((data[2] & 0xC0) << 2);
 372
 373         dot[1].rx = 1023 - (data[3] | ((data[2] & 0x03) << 8));
 374         dot[1].ry = data[4] | ((data[2] & 0x0C) << 6);
 375
 376         dot[2].rx = 1023 - (data[5] | ((data[7] & 0x30) << 4));
 377         dot[2].ry = data[6] | ((data[7] & 0xC0) << 2);
 378
 379         dot[3].rx = 1023 - (data[8] | ((data[7] & 0x03) << 8));
 380         dot[3].ry = data[9] | ((data[7] & 0x0C) << 6);
 381
 382         /* set each IR spot to visible if spot is in range */
 383         for (i = 0; i < 4; ++i) {
 384                 dot[i].rx = BIG_ENDIAN_SHORT(dot[i].rx);
 385                 dot[i].ry = BIG_ENDIAN_SHORT(dot[i].ry);
 386
 387                 if (dot[i].ry == 1023)
 388                         dot[i].visible = 0;
 389                 else {
 390                         dot[i].visible = 1;
 391                         dot[i].size = 0;                /* since we don't know the size, set it as 0 */
 392                 }
 393         }
 394 #ifndef GEKKO
 395         interpret_ir_data(&wm->ir,&wm->orient,WIIMOTE_IS_SET(wm, WIIMOTE_STATE_ACC));
 396 #endif
 397 }
 398
 399 /**
 400  *      @brief Calculate the data from the IR spots.  Extended IR mode.
 401  *
 402  *      @param wm               Pointer to a wiimote_t structure.
 403  *      @param data             Data returned by the wiimote for the IR spots.
 404  */
 405 void calculate_extended_ir(struct wiimote_t* wm, ubyte* data) {
 406         struct ir_dot_t* dot = wm->ir.dot;
 407         int i;
 408
 409         for (i = 0; i < 4; ++i) {
 410                 dot[i].rx = 1023 - (data[3*i] | ((data[(3*i)+2] & 0x30) << 4));
 411                 dot[i].ry = data[(3*i)+1] | ((data[(3*i)+2] & 0xC0) << 2);
 412
 413                 dot[i].size = data[(3*i)+2];
 414
 415                 dot[i].rx = BIG_ENDIAN_SHORT(dot[i].rx);
 416                 dot[i].ry = BIG_ENDIAN_SHORT(dot[i].ry);
 417
 418                 dot[i].size = dot[i].size&0x0f;
 419
 420                 /* if in range set to visible */
 421                 if (dot[i].ry == 1023)
 422                         dot[i].visible = 0;
 423                 else
 424                         dot[i].visible = 1;
 425         }
 426 #ifndef GEKKO
 427         interpret_ir_data(&wm->ir,&wm->orient,WIIMOTE_IS_SET(wm, WIIMOTE_STATE_ACC));
 428 #endif
 429 }
 430
 431 enum {
 432         IR_STATE_DEAD = 0,
 433         IR_STATE_GOOD,
 434         IR_STATE_SINGLE,
 435         IR_STATE_LOST,
 436 };
 437
 438 // half-height of the IR sensor if half-width is 1
 439 #define HEIGHT (384.0f / 512.0f)
 440 // maximum sensor bar slope (tan(35 degrees))
 441 #define MAX_SB_SLOPE 0.7f
 442 // minimum sensor bar width in view, relative to half of the IR sensor area
 443 #define MIN_SB_WIDTH 0.1f
 444 // reject "sensor bars" that happen to have a dot towards the middle
 445 #define SB_MIDDOT_REJECT 0.05f
 446
 447 // physical dimensions
 448 // cm center to center of emitters
 449 #define SB_WIDTH        19.5f
 450 // half-width in cm of emitters
 451 #define SB_DOT_WIDTH 2.25f
 452 // half-height in cm of emitters (with some tolerance)
 453 #define SB_DOT_HEIGHT 1.0f
 454
 455 #define SB_DOT_WIDTH_RATIO (SB_DOT_WIDTH / SB_WIDTH)
 456 #define SB_DOT_HEIGHT_RATIO (SB_DOT_HEIGHT / SB_WIDTH)
 457
 458 // dots further out than these coords are allowed to not be picked up
 459 // otherwise assume something's wrong
 460 //#define SB_OFF_SCREEN_X 0.8f
 461 //#define SB_OFF_SCREEN_Y (0.8f * HEIGHT)
 462
 463 // disable, may be doing more harm than good due to sensor pickup glitches
 464 #define SB_OFF_SCREEN_X 0.0f
 465 #define SB_OFF_SCREEN_Y 0.0f
 466
 467 // if a point is closer than this to one of the previous SB points
 468 // when it reappears, consider it the same instead of trying to guess
 469 // which one of the two it is
 470 #define SB_SINGLE_NOGUESS_DISTANCE (100.0 * 100.0)
 471
 472 // width of the sensor bar in pixels at one meter from the Wiimote
 473 #define SB_Z_COEFFICIENT 256.0f
 474
 475 // distance in meters from the center of the FOV to the left or right edge,
 476 // when the wiimote is at one meter
 477 #define WIIMOTE_FOV_COEFFICIENT 0.39f
 478
 479 #define SQUARED(x) ((x)*(x))
 480 #define WMAX(x,y) ((x>y)?(x):(y))
 481 #define WMIN(x,y) ((x<y)?(x):(y))
 482
 483 /**
 484  *      @brief Interpret IR data into more user friendly variables.
 485  *
 486  *      @param wm               Pointer to a wiimote_t structure.
 487  */
 488 void find_sensorbar(struct ir_t* ir, struct orient_t *orient) {
 489         struct fdot_t dots[4];
 490         struct fdot_t acc_dots[4];
 491         struct sb_t cand;
 492         struct sb_t candidates[6];
 493         struct sb_t sb;
 494
 495         fdot_t difference;
 496
 497         int num_candidates = 0;
 498
 499         int i;
 500         int j;
 501         int first, second;
 502
 503         WIIUSE_DEBUG("IR: orient angle: %.02f\n",orient->roll);
 504
 505         /* count visible dots and populate dots structure */
 506         /* dots[] is in -1..1 units for width */
 507         ir->num_dots = 0;
 508         for (i = 0; i < 4; i++) {
 509                 if (ir->dot[i].visible) {
 510                         dots[ir->num_dots].x = (ir->dot[i].rx - 512.0f) / 512.0f;
 511                         dots[ir->num_dots].y = (ir->dot[i].ry - 384.0f) / 512.0f;
 512                         WIIUSE_DEBUG("IR: dot %d at (%d,%d) (%.03f,%.03f)\n",ir->num_dots,ir->dot[i].rx,ir->dot[i].ry,dots[ir->num_dots].x,dots[ir->num_dots].y);
 513                         ir->num_dots++;
 514                 }
 515         }
 516
 517         WIIUSE_DEBUG("IR: found %d dots\n",ir->num_dots);
 518
 519         // nothing to track
 520         if(ir->num_dots == 0) {
 521                 if(ir->state != IR_STATE_DEAD)
 522                         ir->state = IR_STATE_LOST;
 523                 ir->ax = 0;
 524                 ir->ay = 0;
 525                 ir->distance = 0.0f;
 526                 ir->raw_valid = 0;
 527                 return;
 528         }
 529
 530         /* ==== Find the Sensor Bar ==== */
 531
 532         // first rotate according to accelerometer orientation
 533         rotate_dots(dots, acc_dots, ir->num_dots, orient->roll);
 534         if(ir->num_dots > 1) {
 535                 WIIUSE_DEBUG("IR: locating sensor bar candidates\n");
 536
 537                 // iterate through all dot pairs
 538                 for(first=0; first < (ir->num_dots-1); first++) {
 539                         for(second=(first+1); second < ir->num_dots; second++) {
 540                                 WIIUSE_DEBUG("IR: trying dots %d and %d\n",first,second);
 541                                 // order the dots leftmost first into cand
 542                                 // storing both the raw dots and the accel-rotated dots
 543                                 if(acc_dots[first].x > acc_dots[second].x) {
 544                                         cand.dots[0] = dots[second];
 545                                         cand.dots[1] = dots[first];
 546                                         cand.acc_dots[0] = acc_dots[second];
 547                                         cand.acc_dots[1] = acc_dots[first];
 548                                 } else {
 549                                         cand.dots[0] = dots[first];
 550                                         cand.dots[1] = dots[second];
 551                                         cand.acc_dots[0] = acc_dots[first];
 552                                         cand.acc_dots[1] = acc_dots[second];
 553                                 }
 554                                 difference.x = cand.acc_dots[1].x - cand.acc_dots[0].x;
 555                                 difference.y = cand.acc_dots[1].y - cand.acc_dots[0].y;
 556
 557                                 // check angle
 558                                 if(fabsf(difference.y / difference.x) > MAX_SB_SLOPE)
 559                                         continue;
 560                                 WIIUSE_DEBUG("IR: passed angle check\n");
 561                                 // rotate to the true sensor bar angle
 562                                 cand.off_angle = -RAD_TO_DEGREE(atan2(difference.y, difference.x));
 563                                 cand.angle = cand.off_angle + orient->roll;
 564                                 rotate_dots(cand.dots, cand.rot_dots, 2, cand.angle);
 565                                 WIIUSE_DEBUG("IR: off_angle: %.02f, angle: %.02f\n", cand.off_angle, cand.angle);
 566                                 // recalculate x distance - y should be zero now, so ignore it
 567                                 difference.x = cand.rot_dots[1].x - cand.rot_dots[0].x;
 568
 569                                 // check distance
 570                                 if(difference.x < MIN_SB_WIDTH)
 571                                         continue;
 572                                 // middle dot check. If there's another source somewhere in the
 573                                 // middle of this candidate, then this can't be a sensor bar
 574
 575                                 for(i=0; i<ir->num_dots; i++) {
 576                                         float wadj, hadj;
 577                                         struct fdot_t tdot;
 578                                         if(i==first || i==second) continue;
 579                                         hadj = SB_DOT_HEIGHT_RATIO * difference.x;
 580                                         wadj = SB_DOT_WIDTH_RATIO * difference.x;
 581                                         rotate_dots(&dots[i], &tdot, 1, cand.angle);
 582                                         if( ((cand.rot_dots[0].x + wadj) < tdot.x) &&
 583                                                 ((cand.rot_dots[1].x - wadj) > tdot.x) &&
 584                                                 ((cand.rot_dots[0].y + hadj) > tdot.y) &&
 585                                                 ((cand.rot_dots[0].y - hadj) < tdot.y))
 586                                                 break;
 587                                 }
 588                                 // failed middle dot check
 589                                 if(i < ir->num_dots) continue;
 590                                 WIIUSE_DEBUG("IR: passed middle dot check\n");
 591
 592                                 cand.score = 1 / (cand.rot_dots[1].x - cand.rot_dots[0].x);
 593
 594                                 // we have a candidate, store it
 595                                 WIIUSE_DEBUG("IR: new candidate %d\n",num_candidates);
 596                                 candidates[num_candidates++] = cand;
 597                         }
 598                 }
 599         }
 600
 601         if(num_candidates == 0) {
 602                 int closest = -1;
 603                 int closest_to = 0;
 604                 float best = 999.0f;
 605                 float d;
 606                 float dx[2];
 607                 struct sb_t sbx[2];
 608                 // no sensor bar candidates, try to work with a lone dot
 609                 WIIUSE_DEBUG("IR: no candidates\n");
 610                 switch(ir->state) {
 611                         case IR_STATE_DEAD:
 612                                 WIIUSE_DEBUG("IR: we're dead\n");
 613                                 // we've never seen a sensor bar before, so we're screwed
 614                                 ir->ax = 0.0f;
 615                                 ir->ay = 0.0f;
 616                                 ir->distance = 0.0f;
 617                                 ir->raw_valid = 0;
 618                                 return;
 619                         case IR_STATE_GOOD:
 620                         case IR_STATE_SINGLE:
 621                         case IR_STATE_LOST:
 622                                 WIIUSE_DEBUG("IR: trying to keep track of single dot\n");
 623                                 // try to find the dot closest to the previous sensor bar position
 624                                 for(i=0; i<ir->num_dots; i++) {
 625                                         WIIUSE_DEBUG("IR: checking dot %d (%.02f, %.02f)\n",i, acc_dots[i].x,acc_dots[i].y);
 626                                         for(j=0; j<2; j++) {
 627                                                 WIIUSE_DEBUG("      to dot %d (%.02f, %.02f)\n",j, ir->sensorbar.acc_dots[j].x,ir->sensorbar.acc_dots[j].y);
 628                                                 d = SQUARED(acc_dots[i].x - ir->sensorbar.acc_dots[j].x);
 629                                                 d += SQUARED(acc_dots[i].y - ir->sensorbar.acc_dots[j].y);
 630                                                 if(d < best) {
 631                                                         best = d;
 632                                                         closest_to = j;
 633                                                         closest = i;
 634                                                 }
 635                                         }
 636                                 }
 637                                 WIIUSE_DEBUG("IR: closest dot is %d to %d\n",closest,closest_to);
 638                                 if(ir->state != IR_STATE_LOST || best < SB_SINGLE_NOGUESS_DISTANCE) {
 639                                         // now work out where the other dot would be, in the acc frame
 640                                         sb.acc_dots[closest_to] = acc_dots[closest];
 641                                         sb.acc_dots[closest_to^1].x = ir->sensorbar.acc_dots[closest_to^1].x - ir->sensorbar.acc_dots[closest_to].x + acc_dots[closest].x;
 642                                         sb.acc_dots[closest_to^1].y = ir->sensorbar.acc_dots[closest_to^1].y - ir->sensorbar.acc_dots[closest_to].y + acc_dots[closest].y;
 643                                         // get the raw frame
 644                                         rotate_dots(sb.acc_dots, sb.dots, 2, -orient->roll);
 645                                         if((fabsf(sb.dots[closest_to^1].x) < SB_OFF_SCREEN_X) && (fabsf(sb.dots[closest_to^1].y) < SB_OFF_SCREEN_Y)) {
 646                                                 // this dot should be visible but isn't, since the candidate section failed.
 647                                                 // fall through and try to pick out the sensor bar without previous information
 648                                                 WIIUSE_DEBUG("IR: dot falls on screen, falling through\n");
 649                                         } else {
 650                                                 // calculate the rotated dots frame
 651                                                 // angle tends to drift, so recalculate
 652                                                 sb.off_angle = -RAD_TO_DEGREE(atan2(sb.acc_dots[1].y - sb.acc_dots[0].y, sb.acc_dots[1].x - sb.acc_dots[0].x));
 653                                                 sb.angle = ir->sensorbar.off_angle + orient->roll;
 654                                                 rotate_dots(sb.acc_dots, sb.rot_dots, 2, ir->sensorbar.off_angle);
 655                                                 WIIUSE_DEBUG("IR: kept track of single dot\n");
 656                                                 break;
 657                                         }
 658                                 } else {
 659                                         WIIUSE_DEBUG("IR: lost the dot and new one is too far away\n");
 660                                 }
 661                                 // try to find the dot closest to the sensor edge
 662                                 WIIUSE_DEBUG("IR: trying to find best dot\n");
 663                                 for(i=0; i<ir->num_dots; i++) {
 664                                         d = WMIN(1.0f - fabsf(dots[i].x), HEIGHT - fabsf(dots[i].y));
 665                                         if(d < best) {
 666                                                 best = d;
 667                                                 closest = i;
 668                                         }
 669                                 }
 670                                 WIIUSE_DEBUG("IR: best dot: %d\n",closest);
 671                                 // now try it as both places in the sensor bar
 672                                 // and pick the one that places the other dot furthest off-screen
 673                                 for(i=0; i<2; i++) {
 674                                         sbx[i].acc_dots[i] = acc_dots[closest];
 675                                         sbx[i].acc_dots[i^1].x = ir->sensorbar.acc_dots[i^1].x - ir->sensorbar.acc_dots[i].x + acc_dots[closest].x;
 676                                         sbx[i].acc_dots[i^1].y = ir->sensorbar.acc_dots[i^1].y - ir->sensorbar.acc_dots[i].y + acc_dots[closest].y;
 677                                         rotate_dots(sbx[i].acc_dots, sbx[i].dots, 2, -orient->roll);
 678                                         dx[i] = WMAX(fabsf(sbx[i].dots[i^1].x),fabsf(sbx[i].dots[i^1].y / HEIGHT));
 679                                 }
 680                                 if(dx[0] > dx[1]) {
 681                                         WIIUSE_DEBUG("IR: dot is LEFT: %.02f > %.02f\n",dx[0],dx[1]);
 682                                         sb = sbx[0];
 683                                 } else {
 684                                         WIIUSE_DEBUG("IR: dot is RIGHT: %.02f < %.02f\n",dx[0],dx[1]);
 685                                         sb = sbx[1];
 686                                 }
 687                                 // angle tends to drift, so recalculate
 688                                 sb.off_angle = -RAD_TO_DEGREE(atan2(sb.acc_dots[1].y - sb.acc_dots[0].y, sb.acc_dots[1].x - sb.acc_dots[0].x));
 689                                 sb.angle = ir->sensorbar.off_angle + orient->roll;
 690                                 rotate_dots(sb.acc_dots, sb.rot_dots, 2, ir->sensorbar.off_angle);
 691                                 WIIUSE_DEBUG("IR: found new dot to track\n");
 692                                 break;
 693                 }
 694                 sb.score = 0;
 695                 ir->state = IR_STATE_SINGLE;
 696         } else {
 697                 int bestidx = 0;
 698                 float best = 0.0f;
 699                 WIIUSE_DEBUG("IR: finding best candidate\n");
 700                 // look for the best candidate
 701                 // for now, the formula is simple: pick the one with the smallest distance
 702                 for(i=0; i<num_candidates; i++) {
 703                         if(candidates[i].score > best) {
 704                                 bestidx = i;
 705                                 best = candidates[i].score;
 706                         }
 707                 }
 708                 WIIUSE_DEBUG("IR: best candidate: %d\n",bestidx);
 709                 sb = candidates[bestidx];
 710                 ir->state = IR_STATE_GOOD;
 711         }
 712
 713         ir->raw_valid = 1;
 714         ir->ax = ((sb.rot_dots[0].x + sb.rot_dots[1].x) / 2) * 512.0 + 512.0;
 715         ir->ay = ((sb.rot_dots[0].y + sb.rot_dots[1].y) / 2) * 512.0 + 384.0;
 716         ir->sensorbar = sb;
 717         ir->distance = (sb.rot_dots[1].x - sb.rot_dots[0].x) * 512.0;
 718
 719 }
 720
 721 #define SMOOTH_IR_RADIUS 8.0f
 722 #define SMOOTH_IR_SPEED 0.25f
 723 #define SMOOTH_IR_DEADZONE 2.5f
 724
 725 /**
 726  *      @brief Smooth the IR pointer position
 727  *
 728  *      @param ir               Pointer to an ir_t structure.
 729  */
 730 void apply_ir_smoothing(struct ir_t *ir) {
 731         f32 dx, dy, d, theta;
 732
 733         WIIUSE_DEBUG("Smooth: OK (%.02f, %.02f) LAST (%.02f, %.02f) ", ir->ax, ir->ay, ir->sx, ir->sy);
 734         dx = ir->ax - ir->sx;
 735         dy = ir->ay - ir->sy;
 736         d = sqrtf(dx*dx + dy*dy);
 737         if (d > SMOOTH_IR_DEADZONE) {
 738                 if (d < SMOOTH_IR_RADIUS) {
 739                         WIIUSE_DEBUG("INSIDE\n");
 740                         ir->sx += dx * SMOOTH_IR_SPEED;
 741                         ir->sy += dy * SMOOTH_IR_SPEED;
 742                 } else {
 743                         WIIUSE_DEBUG("OUTSIDE\n");
 744                         theta = atan2f(dy, dx);
 745                         ir->sx = ir->ax - cosf(theta) * SMOOTH_IR_RADIUS;
 746                         ir->sy = ir->ay - sinf(theta) * SMOOTH_IR_RADIUS;
 747                 }
 748         } else {
 749                 WIIUSE_DEBUG("DEADZONE\n");
 750         }
 751 }
 752
 753 // max number of errors before cooked data drops out
 754 #define ERROR_MAX_COUNT 8
 755 // max number of glitches before cooked data updates
 756 #define GLITCH_MAX_COUNT 5
 757 // squared delta over which we consider something a glitch
 758 #define GLITCH_DIST (150.0f * 150.0f)
 759
 760 /**
 761  *      @brief Interpret IR data into more user friendly variables.
 762  *
 763  *      @param ir               Pointer to an ir_t structure.
 764  *      @param orient   Pointer to an orient_t structure.
 765  */
 766 void interpret_ir_data(struct ir_t* ir, struct orient_t *orient) {
 767
 768         float x,y;
 769         float d;
 770
 771         find_sensorbar(ir, orient);
 772
 773         if(ir->raw_valid) {
 774                 ir->angle = ir->sensorbar.angle;
 775                 ir->z = SB_Z_COEFFICIENT / ir->distance;
 776                 orient->yaw = calc_yaw(ir);
 777                 if(ir->error_cnt >= ERROR_MAX_COUNT) {
 778                         ir->sx = ir->ax;
 779                         ir->sy = ir->ay;
 780                         ir->glitch_cnt = 0;
 781                 } else {
 782                         d = SQUARED(ir->ax - ir->sx) + SQUARED(ir->ay - ir->sy);
 783                         if(d > GLITCH_DIST) {
 784                                 if(ir->glitch_cnt > GLITCH_MAX_COUNT) {
 785                                         apply_ir_smoothing(ir);
 786                                         ir->glitch_cnt = 0;
 787                                 } else {
 788                                         ir->glitch_cnt++;
 789                                 }
 790                         } else {
 791                                 ir->glitch_cnt = 0;
 792                                 apply_ir_smoothing(ir);
 793                         }
 794                 }
 795                 ir->smooth_valid = 1;
 796                 ir->error_cnt = 0;
 797         } else {
 798                 if(ir->error_cnt >= ERROR_MAX_COUNT) {
 799                         ir->smooth_valid = 0;
 800                 } else {
 801                         ir->smooth_valid = 1;
 802                         ir->error_cnt++;
 803                 }
 804         }
 805         if(ir->smooth_valid) {
 806                 x = ir->sx;
 807                 y = ir->sy;
 808                 if (ir_correct_for_bounds(&x, &y, ir->aspect, ir->offset[0], ir->offset[1])) {
 809                         ir_convert_to_vres(&x, &y, ir->aspect, ir->vres[0], ir->vres[1]);
 810                         ir->x = x;
 811                         ir->y = y;
 812                         ir->valid = 1;
 813                 } else {
 814                         ir->valid = 0;
 815                 }
 816         } else {
 817                 ir->valid = 0;
 818         }
 819 }
 820
 821 /**
 822  *      @brief Calculate yaw given the IR data.
 823  *
 824  *      @param ir       IR data structure.
 825  */
 826 float calc_yaw(struct ir_t* ir) {
 827         float x;
 828
 829         x = ir->ax - 512;
 830         x *= WIIMOTE_FOV_COEFFICIENT / 512.0;
 831
 832         return RAD_TO_DEGREE( atanf(x) );
 833 }
 834