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