1 /*---------------------------------------------------------------------------*
2 Project: WPAD Library
3 File: WPADClamp.c
4
5 Copyright (C) 2006 Nintendo. All rights reserved.
6
7 These coded instructions, statements, and computer programs contain
8 proprietary information of Nintendo of America Inc. and/or Nintendo
9 Company Ltd., and are protected by Federal copyright law. They may
10 not be disclosed to third parties or copied or duplicated in any form,
11 in whole or in part, without the prior written consent of Nintendo.
12
13 $Log: WPADClamp.c,v $
14 Revision 1.4 2008/03/25 02:31:07 tojo
15 Fixed the clamp parameter.
16
17 Revision 1.3 2006/08/11 09:12:35 tojo
18 (none)
19
20 Revision 1.2 2006/08/03 11:58:40 tojo
21 Changed clamp apis.
22 Fixed acc clamp bugs.
23
24 Revision 1.1 2006/08/01 05:59:41 tojo
25 (none)
26
27
28 *---------------------------------------------------------------------------*/
29
30 #include <revolution/os.h>
31 #include <revolution/wpad.h>
32
33 #include <math.h>
34 #include <string.h>
35
36 /*---------------------------------------------------------------------------*
37 Static function definitions
38 *---------------------------------------------------------------------------*/
39
40 // clamp
41 static void __ClampStickOctagon ( void *px, void *py, s16 max, s16 xy, s16 min, u8 type );
42 static void __ClampStickCircle ( void *px, void *py, s16 radius, s16 min, u8 type );
43 static void __ClampTrigger ( u8 *trigger, u8 min, u8 max );
44 static void __ClampCube ( s16 *px, s16 *py, s16 *pz, WPADAcc unit, f32 max );
45 static void __ClampSphere ( s16 *px, s16 *py, s16 *pz, WPADAcc unit, f32 max );
46
47
48 // Parameters for controller
49 typedef struct WPADClampStickRegion
50 {
51 s16 minStick; // the play of the control stick
52 s16 maxStick;
53 s16 xyStick; // max on x = y
54 s16 radStick; // max radius of the control stick
55 } WPADClampStickRegion;
56
57 // Freestyle
58 static const WPADClampStickRegion FSRegion =
59 {
60 15, // minStick
61 67, // maxStick
62 43, // xyStick
63 56, // radius
64 };
65 // Freestyle Ex
66 static const WPADClampStickRegion FSRegion2 =
67 {
68 0,
69 82,
70 58,
71 75,
72 };
73
74 // Classic
75 static const WPADClampStickRegion CLRegion =
76 {
77 60,
78 288,
79 188,
80 248,
81 };
82 // Classic Ex
83 static const WPADClampStickRegion CLRegion2 =
84 {
85 0,
86 348,
87 248,
88 322,
89 };
90
91
92 /*---------------------------------------------------------------------------*
93 Clamp
94 *---------------------------------------------------------------------------*/
95 /*---------------------------------------------------------------------------*
96 Name: WPADClampStick
97
98 Description: Clamp the analog inputs according to device type.
99
100 Arguments: fmt data format.
101 status point to the data to be clamped.
102 type clamp type.
103
104 Returns: None.
105 *---------------------------------------------------------------------------*/
WPADClampStick(s32 chan,void * status,u32 type)106 void WPADClampStick( s32 chan, void *status, u32 type )
107 {
108 WPADFSStatus *fp;
109 WPADCLStatus *cp;
110 const WPADClampStickRegion *stkreg;
111 u32 fmt;
112
113 ASSERT( status != NULL );
114
115 if (((WPADStatus*)status)->err != WPAD_ERR_NONE)
116 {
117 return;
118 }
119
120 fmt = WPADGetDataFormat(chan);
121 switch(fmt)
122 {
123 case WPAD_FMT_FREESTYLE:
124 case WPAD_FMT_FREESTYLE_ACC:
125 case WPAD_FMT_FREESTYLE_ACC_DPD:
126 fp = (WPADFSStatus*)status;
127 if (type == WPAD_STICK_CLAMP_OCTA_WITH_PLAY
128 || type == WPAD_STICK_CLAMP_CIRCLE_WITH_PLAY)
129 {
130 stkreg = &FSRegion;
131 }
132 else
133 {
134 stkreg = &FSRegion2;
135 }
136
137 if (type == WPAD_STICK_CLAMP_OCTA_WITH_PLAY
138 || type == WPAD_STICK_CLAMP_OCTA_WITHOUT_PLAY)
139 {
140 __ClampStickOctagon(&fp->fsStickX,
141 &fp->fsStickY,
142 stkreg->maxStick,
143 stkreg->xyStick,
144 stkreg->minStick,
145 0);
146 }
147 else
148 {
149 __ClampStickCircle(&fp->fsStickX,
150 &fp->fsStickY,
151 stkreg->radStick,
152 stkreg->minStick,
153 0);
154 }
155 break;
156
157 case WPAD_FMT_CLASSIC:
158 case WPAD_FMT_CLASSIC_ACC:
159 case WPAD_FMT_CLASSIC_ACC_DPD:
160 cp = (WPADCLStatus*)status;
161 if (type == WPAD_STICK_CLAMP_OCTA_WITH_PLAY
162 || type == WPAD_STICK_CLAMP_CIRCLE_WITH_PLAY)
163 {
164 stkreg = &CLRegion;
165 }
166 else
167 {
168 stkreg = &CLRegion2;
169 }
170
171 if (type == WPAD_STICK_CLAMP_OCTA_WITH_PLAY
172 || type == WPAD_STICK_CLAMP_OCTA_WITHOUT_PLAY)
173 {
174 __ClampStickOctagon(&cp->clLStickX,
175 &cp->clLStickY,
176 stkreg->maxStick,
177 stkreg->xyStick,
178 stkreg->minStick,
179 1);
180 __ClampStickOctagon(&cp->clRStickX,
181 &cp->clRStickY,
182 stkreg->maxStick,
183 stkreg->xyStick,
184 stkreg->minStick,
185 1);
186 }
187 else
188 {
189 __ClampStickCircle(&cp->clLStickX,
190 &cp->clLStickY,
191 stkreg->radStick,
192 stkreg->minStick,
193 1);
194 __ClampStickCircle(&cp->clRStickX,
195 &cp->clRStickY,
196 stkreg->radStick,
197 stkreg->minStick,
198 1);
199
200 }
201 break;
202
203 default:
204 break;
205 }
206 }
207
208 /*---------------------------------------------------------------------------*
209 Name: WPADClampTrigger
210
211 Description: Clamp the trigger inputs according to device type.
212
213 Arguments: chan controller port
214 fmt data format.
215 status point to the data to be clamped.
216 type clamp type.
217
218 Returns: None.
219 *---------------------------------------------------------------------------*/
WPADClampTrigger(s32 chan,void * status,u32 type)220 void WPADClampTrigger( s32 chan, void *status, u32 type )
221 {
222 WPADCLStatus *cp;
223 u8 min_l, min_r;
224 u8 max_l, max_r;
225 u32 fmt;
226
227 ASSERT(status != NULL);
228
229 if (((WPADStatus*)status)->err != WPAD_ERR_NONE)
230 {
231 return;
232 }
233
234 fmt = WPADGetDataFormat(chan);
235 switch(fmt)
236 {
237 case WPAD_FMT_CLASSIC:
238 case WPAD_FMT_CLASSIC_ACC:
239 case WPAD_FMT_CLASSIC_ACC_DPD:
240 cp = (WPADCLStatus*)status;
241 if (type == WPAD_TRIGGER_FIXED_BASE)
242 {
243 min_l = min_r = 30;
244 max_l = max_r = 180;
245 }
246 else
247 {
248 WPADGetCLTriggerThreshold(chan, &min_l, &min_r);
249 max_l = (u8)((min_l < 76) ? min_l+180 : 255);
250 max_r = (u8)((min_r < 76) ? min_r+180 : 255);
251 }
252 __ClampTrigger(&cp->clTriggerL,min_l,max_l);
253 __ClampTrigger(&cp->clTriggerR,min_r,max_r);
254 break;
255
256 default:
257 break;
258 }
259 }
260
261
262 /*---------------------------------------------------------------------------*
263 Name: WPADClampAcc
264
265 Description: Clamp the accelerometer inputs according to device type
266
267 Arguments: fmt data format.
268 status point to the data to be clamped.
269 unit structure of gravity unit for each axes.
270 type clamp type.
271
272 Returns: None.
273 *---------------------------------------------------------------------------*/
WPADClampAcc(s32 chan,void * status,u32 type)274 void WPADClampAcc( s32 chan, void *status, u32 type )
275 {
276 WPADStatus *wp;
277 WPADFSStatus *fp;
278 WPADAcc wunit;
279 WPADAcc funit;
280 u32 fmt;
281
282 ASSERT(status != NULL);
283
284 if (((WPADStatus*)status)->err != WPAD_ERR_NONE)
285 {
286 return;
287 }
288
289 fmt = WPADGetDataFormat(chan);
290 switch(fmt)
291 {
292 case WPAD_FMT_CORE_ACC:
293 case WPAD_FMT_CORE_ACC_DPD:
294 wp = (WPADStatus*)status;
295 WPADGetAccGravityUnit(chan, WPAD_DEV_CORE, &wunit);
296 if (type == WPAD_ACC_CLAMP_CUBE)
297 {
298 __ClampCube(&wp->accX, &wp->accY, &wp->accZ, wunit, WPAD_ACC_MAX);
299 }
300 else
301 {
302 __ClampSphere(&wp->accX, &wp->accY, &wp->accZ, wunit, WPAD_ACC_MAX);
303 }
304 break;
305
306 case WPAD_FMT_FREESTYLE_ACC:
307 case WPAD_FMT_FREESTYLE_ACC_DPD:
308 fp = (WPADFSStatus*)status;
309 WPADGetAccGravityUnit(chan, WPAD_DEV_CORE, &wunit);
310 WPADGetAccGravityUnit(chan, WPAD_DEV_FREESTYLE, &funit);
311 if (type == WPAD_ACC_CLAMP_CUBE)
312 {
313 __ClampCube(&fp->accX, &fp->accY, &fp->accZ, wunit, WPAD_ACC_MAX);
314 __ClampCube(&fp->fsAccX, &fp->fsAccY, &fp->fsAccZ, funit, WPAD_FS_ACC_MAX);
315 }
316 else
317 {
318 __ClampSphere(&fp->accX, &fp->accY, &fp->accZ, wunit, WPAD_ACC_MAX);
319 __ClampSphere(&fp->fsAccX, &fp->fsAccY, &fp->fsAccZ, funit, WPAD_FS_ACC_MAX);
320 }
321 break;
322
323 default:
324 break;
325 }
326 }
327
328
329 /*---------------------------------------------------------------------------*
330 Name: __ClampStickOctagon
331
332 Description: Adjusts stick movement data within the octagon, or clamps
333 the data to the origin if stick is close to the origin as
334 the play.
335
336 Arguments: px pointer to movement data in terms of x-axis
337 py pointer to movement data in terms of y-axis
338 max max on y = 0
339 xy max on x = y
340 min deadzone
341
342 Returns: None.
343 *---------------------------------------------------------------------------*/
__ClampStickOctagon(void * px,void * py,s16 max,s16 xy,s16 min,u8 type)344 static void __ClampStickOctagon(void* px, void* py, s16 max, s16 xy, s16 min, u8 type)
345 {
346 int x;
347 int y;
348 int signX;
349 int signY;
350 int d;
351
352 if (type == 0)
353 {
354 x = *((s8*)px);
355 y = *((s8*)py);
356 }
357 else
358 {
359 x = *((s16*)px);
360 y = *((s16*)py);
361 }
362
363 if (0 <= x)
364 {
365 signX = 1;
366 }
367 else
368 {
369 signX = -1;
370 x = -x;
371 }
372
373 if (0 <= y)
374 {
375 signY = 1;
376 }
377 else
378 {
379 signY = -1;
380 y = -y;
381 }
382
383 // Clamp dead zone
384 if (x <= min)
385 {
386 x = 0;
387 }
388 else
389 {
390 x -= min;
391 }
392 if (y <= min)
393 {
394 y = 0;
395 }
396 else
397 {
398 y -= min;
399 }
400
401 if (x == 0 && y == 0)
402 {
403 if (type == 0)
404 {
405 *((s8*)px) = 0;
406 *((s8*)py) = 0;
407 }
408 else
409 {
410 *((s16*)px) = 0;
411 *((s16*)py) = 0;
412 }
413 return;
414 // NOT REACHED HERE
415 }
416
417 // Clamp outer octagon
418 if (xy * y <= xy * x)
419 {
420 d = xy * x + (max - xy) * y;
421 if (xy * max < d)
422 {
423 if (type == 0)
424 {
425 x = (s8) (xy * max * x / d);
426 y = (s8) (xy * max * y / d);
427 }
428 else
429 {
430 x = (s16) (xy * max * x / d);
431 y = (s16) (xy * max * y / d);
432 }
433 }
434 }
435 else
436 {
437 d = xy * y + (max - xy) * x;
438 if (xy * max < d)
439 {
440 if (type == 0)
441 {
442 x = (s8) (xy * max * x / d);
443 y = (s8) (xy * max * y / d);
444 }
445 else
446 {
447 x = (s16) (xy * max * x / d);
448 y = (s16) (xy * max * y / d);
449 }
450 }
451 }
452
453 if (type == 0)
454 {
455 *(s8*)px = (s8) (signX * x);
456 *(s8*)py = (s8) (signY * y);
457 }
458 else
459 {
460 *(s16*)px = (s16) (signX * x);
461 *(s16*)py = (s16) (signY * y);
462 }
463 }
464
465 /*---------------------------------------------------------------------------*
466 Name: __ClampStickCircle
467
468 Description: Adjusts stick movement data.
469
470 Arguments: px pointer to movement data in terms of x-axis
471 py pointer to movement data in terms of y-axis
472 radius max valid radius
473 min deadzone
474
475 Returns: None.
476 *---------------------------------------------------------------------------*/
__ClampStickCircle(void * px,void * py,s16 radius,s16 min,u8 type)477 static void __ClampStickCircle(void *px, void *py, s16 radius, s16 min, u8 type)
478 {
479 int x;
480 int y;
481 int squared;
482 int length;
483
484 if (type == 0)
485 {
486 x = *((s8*)px);
487 y = *((s8*)py);
488 }
489 else
490 {
491 x = *((s16*)px);
492 y = *((s16*)py);
493 }
494
495 // Remove vertical zone
496 if (-min < x && x < min)
497 {
498 x = 0;
499 }
500 else if (0 < x)
501 {
502 x -= min;
503 }
504 else
505 {
506 x += min;
507 }
508
509 // Remove horizontal zone
510 if (-min < y && y < min)
511 {
512 y = 0;
513 }
514 else if (0 < y)
515 {
516 y -= min;
517 }
518 else
519 {
520 y += min;
521 }
522
523 // Clamp input to unit circle of radius
524 squared = x*x + y*y;
525 if (radius*radius < squared)
526 {
527 // Vector too long - clamp
528 length = (int) sqrtf(squared);
529 x = (x * radius) / length;
530 y = (y * radius) / length;
531 }
532
533 if (type == 0)
534 {
535 *(s8*)px = (s8)x;
536 *(s8*)py = (s8)y;
537 }
538 else
539 {
540 *(s16*)px = (s16)x;
541 *(s16*)py = (s16)y;
542 }
543 }
544
545 /*---------------------------------------------------------------------------*
546 Name: __ClampTrigger
547
548 Description: Adjusts trigger movement data
549
550 Arguments: trigger trigger magnitude
551 min minimum valid value for trigger
552 max maximum valid value for trigger
553
554 Returns: None.
555 *---------------------------------------------------------------------------*/
__ClampTrigger(u8 * trigger,u8 min,u8 max)556 static void __ClampTrigger(u8* trigger, u8 min, u8 max)
557 {
558 if (*trigger <= min)
559 {
560 *trigger = 0;
561 }
562 else
563 {
564 if (max < *trigger)
565 {
566 *trigger = max;
567 }
568 *trigger -= min;
569 }
570 }
571
572
573 /*---------------------------------------------------------------------------*
574 Name: __ClampCube
575
576 Description: Adjusts accelerometer movement data
577
578 Arguments: px pointer to movement data in terms of x-axis
579 py pointer to movement data in terms of y-axis
580 py pointer to movement data in terms of z-axis
581 unit gravity unit for each axes
582 max max valid gravity
583
584 Returns: None.
585 *---------------------------------------------------------------------------*/
__ClampCube(s16 * px,s16 * py,s16 * pz,WPADAcc unit,f32 max)586 static void __ClampCube(s16 *px, s16 *py, s16 *pz, WPADAcc unit, f32 max)
587 {
588 f32 x = (f32)((f32)*px / (f32)unit.x);
589 f32 y = (f32)((f32)*py / (f32)unit.y);
590 f32 z = (f32)((f32)*pz / (f32)unit.z);
591 f32 signX = 1.0f;
592 f32 signY = 1.0f;
593 f32 signZ = 1.0f;
594
595 if (x < 0.0f)
596 {
597 signX = -1.0f;
598 x = -x;
599 }
600 if (y < 0.0f)
601 {
602 signY = -1.0f;
603 y = -y;
604 }
605 if (z < 0.0f)
606 {
607 signZ = -1.0f;
608 z = -z;
609 }
610
611 if (x > max)
612 {
613 x = max;
614 }
615 if (y > max)
616 {
617 y = max;
618 }
619 if (z > max)
620 {
621 z = max;
622 }
623
624 x *= signX;
625 y *= signY;
626 z *= signZ;
627
628 *px = (s16)(x * (f32)unit.x);
629 *py = (s16)(y * (f32)unit.y);
630 *pz = (s16)(z * (f32)unit.z);
631 }
632
633 /*---------------------------------------------------------------------------*
634 Name: __ClampSphere
635
636 Description: Adjusts accelerometer movement data
637
638 Arguments: px pointer to movement data in terms of x-axis
639 py pointer to movement data in terms of y-axis
640 py pointer to movement data in terms of z-axis
641 unit gravity unit for each axes
642 max max valid gravity
643
644 Returns: None.
645 *---------------------------------------------------------------------------*/
__ClampSphere(s16 * px,s16 * py,s16 * pz,WPADAcc unit,f32 max)646 static void __ClampSphere(s16 *px, s16 *py, s16 *pz, WPADAcc unit, f32 max)
647 {
648 f32 x = (f32)((f32)*px / (f32)unit.x);
649 f32 y = (f32)((f32)*py / (f32)unit.y);
650 f32 z = (f32)((f32)*pz / (f32)unit.z);
651 f32 length;
652 f32 squared;
653
654 squared = x*x + y*y + z*z;
655 if (max * max < squared)
656 {
657 length = sqrtf(squared);
658 x = x * max / length;
659 y = y * max / length;
660 z = z * max / length;
661 }
662
663 *px = (s16)(x * (f32)unit.x);
664 *py = (s16)(y * (f32)unit.y);
665 *pz = (s16)(z * (f32)unit.z);
666 }
667
668