wiiuse/dynamics.c

   1 /*
   2  *      wiiuse
   3  *
   4  *      Written By:
   5  *              Michael Laforest        < para >
   6  *              Email: < thepara (--AT--) g m a i l [--DOT--] com >
   7  *
   8  *      Copyright 2006-2007
   9  *
  10  *      This file is part of wiiuse.
  11  *
  12  *      This program is free software; you can redistribute it and/or modify
  13  *      it under the terms of the GNU General Public License as published by
  14  *      the Free Software Foundation; either version 3 of the License, or
  15  *      (at your option) any later version.
  16  *
  17  *      This program is distributed in the hope that it will be useful,
  18  *      but WITHOUT ANY WARRANTY; without even the implied warranty of
  19  *      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  20  *      GNU General Public License for more details.
  21  *
  22  *      You should have received a copy of the GNU General Public License
  23  *      along with this program.  If not, see <http://www.gnu.org/licenses/>.
  24  *
  25  *      $Header: /lvm/shared/ds/ds/cvs/devkitpro-cvsbackup/libogc/wiiuse/dynamics.c,v 1.2 2008-11-14 13:34:57 shagkur Exp $
  26  *
  27  */
  28
  29 /**
  30  *      @file
  31  *      @brief Handles the dynamics of the wiimote.
  32  *
  33  *      The file includes functions that handle the dynamics
  34  *      of the wiimote.  Such dynamics include orientation and
  35  *      motion sensing.
  36  */
  37
  38 #include <stdio.h>
  39 #include <stdlib.h>
  40 #include <math.h>
  41
  42 #ifdef WIN32
  43         #include <float.h>
  44 #endif
  45
  46 #include "definitions.h"
  47 #include "wiiuse_internal.h"
  48 #include "ir.h"
  49 #include "dynamics.h"
  50
  51 /**
  52  *      @brief Calculate the roll, pitch, yaw.
  53  *
  54  *      @param ac                       An accelerometer (accel_t) structure.
  55  *      @param accel            [in] Pointer to a vec3w_t structure that holds the raw acceleration data.
  56  *      @param orient           [out] Pointer to a orient_t structure that will hold the orientation data.
  57  *      @param rorient          [out] Pointer to a orient_t structure that will hold the non-smoothed orientation data.
  58  *      @param smooth           If smoothing should be performed on the angles calculated. 1 to enable, 0 to disable.
  59  *
  60  *      Given the raw acceleration data from the accelerometer struct, calculate
  61  *      the orientation of the device and set it in the \a orient parameter.
  62  */
  63 void calculate_orientation(struct accel_t* ac, struct vec3w_t* accel, struct orient_t* orient, int smooth) {
  64         float xg, yg, zg;
  65         float x, y, z;
  66
  67         /*
  68          *      roll    - use atan(z / x)               [ ranges from -180 to 180 ]
  69          *      pitch   - use atan(z / y)               [ ranges from -180 to 180 ]
  70          *      yaw             - impossible to tell without IR
  71          */
  72
  73         /* yaw - set to 0, IR will take care of it if it's enabled */
  74         orient->yaw = 0.0f;
  75
  76         /* find out how much it has to move to be 1g */
  77         xg = (float)ac->cal_g.x;
  78         yg = (float)ac->cal_g.y;
  79         zg = (float)ac->cal_g.z;
  80
  81         /* find out how much it actually moved and normalize to +/- 1g */
  82         x = ((float)accel->x - (float)ac->cal_zero.x) / xg;
  83         y = ((float)accel->y - (float)ac->cal_zero.y) / yg;
  84         z = ((float)accel->z - (float)ac->cal_zero.z) / zg;
  85
  86         /* make sure x,y,z are between -1 and 1 for the tan functions */
  87         if (x < -1.0f)                  x = -1.0f;
  88         else if (x > 1.0f)              x = 1.0f;
  89         if (y < -1.0f)                  y = -1.0f;
  90         else if (y > 1.0f)              y = 1.0f;
  91         if (z < -1.0f)                  z = -1.0f;
  92         else if (z > 1.0f)              z = 1.0f;
  93
  94         /* if it is over 1g then it is probably accelerating and not reliable */
  95         if (abs(accel->x - ac->cal_zero.x) <= (ac->cal_g.x+10)) {
  96                 /* roll */
  97                 x = RAD_TO_DEGREE(atan2f(x, z));
  98                 if(isfinite(x)) {
  99                         orient->roll = x;
 100                         orient->a_roll = x;
 101                 }
 102         }
 103
 104         if (abs(accel->y - ac->cal_zero.y) <= (ac->cal_g.y+10)) {
 105                 /* pitch */
 106                 y = RAD_TO_DEGREE(atan2f(y, z));
 107                 if(isfinite(y)) {
 108                         orient->pitch = y;
 109                         orient->a_pitch = y;
 110                 }
 111         }
 112
 113         /* smooth the angles if enabled */
 114         if (smooth) {
 115                 apply_smoothing(ac, orient, SMOOTH_ROLL);
 116                 apply_smoothing(ac, orient, SMOOTH_PITCH);
 117         }
 118 }
 119
 120
 121 /**
 122  *      @brief Calculate the gravity forces on each axis.
 123  *
 124  *      @param ac                       An accelerometer (accel_t) structure.
 125  *      @param accel            [in] Pointer to a vec3w_t structure that holds the raw acceleration data.
 126  *      @param gforce           [out] Pointer to a gforce_t structure that will hold the gravity force data.
 127  */
 128 void calculate_gforce(struct accel_t* ac, struct vec3w_t* accel, struct gforce_t* gforce) {
 129         float xg, yg, zg;
 130
 131         /* find out how much it has to move to be 1g */
 132         xg = (float)ac->cal_g.x;
 133         yg = (float)ac->cal_g.y;
 134         zg = (float)ac->cal_g.z;
 135
 136         /* find out how much it actually moved and normalize to +/- 1g */
 137         gforce->x = ((float)accel->x - (float)ac->cal_zero.x) / xg;
 138         gforce->y = ((float)accel->y - (float)ac->cal_zero.y) / yg;
 139         gforce->z = ((float)accel->z - (float)ac->cal_zero.z) / zg;
 140 }
 141
 142
 143 /**
 144  *      @brief Calculate the angle and magnitude of a joystick.
 145  *
 146  *      @param js       [out] Pointer to a joystick_t structure.
 147  *      @param x        The raw x-axis value.
 148  *      @param y        The raw y-axis value.
 149  */
 150 void calc_joystick_state(struct joystick_t* js, float x, float y) {
 151         float rx, ry, ang;
 152
 153         /*
 154          *      Since the joystick center may not be exactly:
 155          *              (min + max) / 2
 156          *      Then the range from the min to the center and the center to the max
 157          *      may be different.
 158          *      Because of this, depending on if the current x or y value is greater
 159          *      or less than the assoicated axis center value, it needs to be interpolated
 160          *      between the center and the minimum or maxmimum rather than between
 161          *      the minimum and maximum.
 162          *
 163          *      So we have something like this:
 164          *              (x min) [-1] ---------*------ [0] (x center) [0] -------- [1] (x max)
 165          *      Where the * is the current x value.
 166          *      The range is therefore -1 to 1, 0 being the exact center rather than
 167          *      the middle of min and max.
 168          */
 169         if (x == js->center.x)
 170                 rx = 0;
 171         else if (x >= js->center.x)
 172                 rx = ((float)(x - js->center.x) / (float)(js->max.x - js->center.x));
 173         else
 174                 rx = ((float)(x - js->min.x) / (float)(js->center.x - js->min.x)) - 1.0f;
 175
 176         if (y == js->center.y)
 177                 ry = 0;
 178         else if (y >= js->center.y)
 179                 ry = ((float)(y - js->center.y) / (float)(js->max.y - js->center.y));
 180         else
 181                 ry = ((float)(y - js->min.y) / (float)(js->center.y - js->min.y)) - 1.0f;
 182
 183         /* calculate the joystick angle and magnitude */
 184         ang = RAD_TO_DEGREE(atanf(ry / rx));
 185         ang -= 90.0f;
 186         if (rx < 0.0f)
 187                 ang -= 180.0f;
 188         js->ang = absf(ang);
 189         js->mag = (float) sqrt((rx * rx) + (ry * ry));
 190 }
 191
 192
 193 void apply_smoothing(struct accel_t* ac, struct orient_t* orient, int type) {
 194         switch (type) {
 195                 case SMOOTH_ROLL:
 196                 {
 197                         /* it's possible last iteration was nan or inf, so set it to 0 if that happened */
 198                         if (isnan(ac->st_roll) || isinf(ac->st_roll))
 199                                 ac->st_roll = 0.0f;
 200
 201                         /*
 202                          *      If the sign changes (which will happen if going from -180 to 180)
 203                          *      or from (-1 to 1) then don't smooth, just use the new angle.
 204                          */
 205                         if (((ac->st_roll < 0) && (orient->roll > 0)) || ((ac->st_roll > 0) && (orient->roll < 0))) {
 206                                 ac->st_roll = orient->roll;
 207                         } else {
 208                                 orient->roll = ac->st_roll + (ac->st_alpha * (orient->a_roll - ac->st_roll));
 209                                 ac->st_roll = orient->roll;
 210                         }
 211
 212                         return;
 213                 }
 214
 215                 case SMOOTH_PITCH:
 216                 {
 217                         if (isnan(ac->st_pitch) || isinf(ac->st_pitch))
 218                                 ac->st_pitch = 0.0f;
 219
 220                         if (((ac->st_pitch < 0) && (orient->pitch > 0)) || ((ac->st_pitch > 0) && (orient->pitch < 0))) {
 221                                 ac->st_pitch = orient->pitch;
 222                         } else {
 223                                 orient->pitch = ac->st_pitch + (ac->st_alpha * (orient->a_pitch - ac->st_pitch));
 224                                 ac->st_pitch = orient->pitch;
 225                         }
 226
 227                         return;
 228                 }
 229         }
 230 }
 231
 232 void calc_balanceboard_state(struct wii_board_t *wb)
 233 {
 234         /*
 235         Interpolate values
 236         Calculations borrowed from wiili.org - No names to mention sadly :( http://www.wiili.org/index.php/Wii_Balance_Board_PC_Drivers
 237         */
 238
 239         if(wb->rtr<wb->ctr[1])
 240         {
 241                 wb->tr = 17.0f*(f32)(wb->rtr-wb->ctr[0])/(f32)(wb->ctr[1]-wb->ctr[0]);
 242         }
 243         else
 244         {
 245                 wb->tr = 17.0f*(f32)(wb->rtr-wb->ctr[1])/(f32)(wb->ctr[2]-wb->ctr[1]) + 17.0f;
 246         }
 247
 248         if(wb->rtl<wb->ctl[1])
 249         {
 250                 wb->tl = 17.0f*(f32)(wb->rtl-wb->ctl[0])/(f32)(wb->ctl[1]-wb->ctl[0]);
 251         }
 252         else
 253         {
 254                 wb->tl = 17.0f*(f32)(wb->rtl-wb->ctl[1])/(f32)(wb->ctl[2]-wb->ctl[1]) + 17.0f;
 255         }
 256
 257         if(wb->rbr<wb->cbr[1])
 258         {
 259                 wb->br = 17.0f*(f32)(wb->rbr-wb->cbr[0])/(f32)(wb->cbr[1]-wb->cbr[0]);
 260         }
 261         else
 262         {
 263                 wb->br = 17.0f*(f32)(wb->rbr-wb->cbr[1])/(f32)(wb->cbr[2]-wb->cbr[1]) + 17.0f;
 264         }
 265
 266         if(wb->rbl<wb->cbl[1])
 267         {
 268                 wb->bl = 17.0f*(f32)(wb->rbl-wb->cbl[0])/(f32)(wb->cbl[1]-wb->cbl[0]);
 269         }
 270         else
 271         {
 272                 wb->bl = 17.0f*(f32)(wb->rbl-wb->cbl[1])/(f32)(wb->cbl[2]-wb->cbl[1]) + 17.0f;
 273         }
 274
 275         wb->x = ((wb->tr+wb->br) - (wb->tl+wb->bl))/2.0f;
 276         wb->y = ((wb->bl+wb->br) - (wb->tl+wb->tr))/2.0f;
 277 }