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 */
calculate_orientation(struct accel_t * ac,struct vec3w_t * accel,struct orient_t * orient,int smooth)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 * @brief Calculate the gravity forces on each axis.
122 *
123 * @param ac An accelerometer (accel_t) structure.
124 * @param accel [in] Pointer to a vec3w_t structure that holds the raw acceleration data.
125 * @param gforce [out] Pointer to a gforce_t structure that will hold the gravity force data.
126 */
calculate_gforce(struct accel_t * ac,struct vec3w_t * accel,struct gforce_t * gforce)127 void calculate_gforce(struct accel_t* ac, struct vec3w_t* accel, struct gforce_t* gforce) {
128 float xg, yg, zg;
129
130 /* find out how much it has to move to be 1g */
131 xg = (float)ac->cal_g.x;
132 yg = (float)ac->cal_g.y;
133 zg = (float)ac->cal_g.z;
134
135 /* find out how much it actually moved and normalize to +/- 1g */
136 gforce->x = ((float)accel->x - (float)ac->cal_zero.x) / xg;
137 gforce->y = ((float)accel->y - (float)ac->cal_zero.y) / yg;
138 gforce->z = ((float)accel->z - (float)ac->cal_zero.z) / zg;
139 }
140
applyCalibration(float inval,float minval,float maxval,float centerval)141 static float applyCalibration(float inval,float minval, float maxval,float centerval)
142 {
143 float ret;
144 /* We don't use the exact ranges but the ranges +1 in case we get bad calibration
145 * data - avoid div0 */
146
147 if (inval == centerval)
148 ret = 0;
149 else if (inval < centerval)
150 ret = (inval - centerval) / (centerval - minval + 1);
151 else
152 ret = (inval - centerval) / (maxval - centerval + 1);
153 return ret;
154 }
155
156 /**
157 * @brief Calculate the angle and magnitude of a joystick.
158 *
159 * @param js [out] Pointer to a joystick_t structure.
160 * @param x The raw x-axis value.
161 * @param y The raw y-axis value.
162 */
calc_joystick_state(struct joystick_t * js,float x,float y)163 void calc_joystick_state(struct joystick_t* js, float x, float y) {
164 float rx, ry;
165
166 /*
167 * Since the joystick center may not be exactly:
168 * (min + max) / 2
169 * Then the range from the min to the center and the center to the max
170 * may be different.
171 * Because of this, depending on if the current x or y value is greater
172 * or less than the assoicated axis center value, it needs to be interpolated
173 * between the center and the minimum or maxmimum rather than between
174 * the minimum and maximum.
175 *
176 * So we have something like this:
177 * (x min) [-1] ---------*------ [0] (x center) [0] -------- [1] (x max)
178 * Where the * is the current x value.
179 * The range is therefore -1 to 1, 0 being the exact center rather than
180 * the middle of min and max.
181 */
182 if (x == js->center.x)
183 rx = 0;
184 else if (x >= js->center.x)
185 rx = ((float)(x - js->center.x) / (float)(js->max.x - js->center.x));
186 else
187 rx = ((float)(x - js->min.x) / (float)(js->center.x - js->min.x)) - 1.0f;
188
189 if (y == js->center.y)
190 ry = 0;
191 else if (y >= js->center.y)
192 ry = ((float)(y - js->center.y) / (float)(js->max.y - js->center.y));
193 else
194 ry = ((float)(y - js->min.y) / (float)(js->center.y - js->min.y)) - 1.0f;
195
196 /* calculate the joystick angle and magnitude */
197 js->ang = RAD_TO_DEGREE(atan2f(rx, ry));
198 js->mag = hypotf(rx, ry);
199 }
200
apply_smoothing(struct accel_t * ac,struct orient_t * orient,int type)201 void apply_smoothing(struct accel_t* ac, struct orient_t* orient, int type) {
202 switch (type) {
203 case SMOOTH_ROLL:
204 {
205 /* it's possible last iteration was nan or inf, so set it to 0 if that happened */
206 if (isnan(ac->st_roll) || isinf(ac->st_roll))
207 ac->st_roll = 0.0f;
208
209 /*
210 * If the sign changes (which will happen if going from -180 to 180)
211 * or from (-1 to 1) then don't smooth, just use the new angle.
212 */
213 if (((ac->st_roll < 0) && (orient->roll > 0)) || ((ac->st_roll > 0) && (orient->roll < 0))) {
214 ac->st_roll = orient->roll;
215 } else {
216 orient->roll = ac->st_roll + (ac->st_alpha * (orient->a_roll - ac->st_roll));
217 ac->st_roll = orient->roll;
218 }
219
220 return;
221 }
222
223 case SMOOTH_PITCH:
224 {
225 if (isnan(ac->st_pitch) || isinf(ac->st_pitch))
226 ac->st_pitch = 0.0f;
227
228 if (((ac->st_pitch < 0) && (orient->pitch > 0)) || ((ac->st_pitch > 0) && (orient->pitch < 0))) {
229 ac->st_pitch = orient->pitch;
230 } else {
231 orient->pitch = ac->st_pitch + (ac->st_alpha * (orient->a_pitch - ac->st_pitch));
232 ac->st_pitch = orient->pitch;
233 }
234
235 return;
236 }
237 }
238 }
239