1 /*---------------------------------------------------------------------------*
2 Project: KPAD library
3 File: KPAD.c
4 Programmers: Keizo Ohta
5 HIRATSU Daisuke
6
7 Copyright 2005-2006 Nintendo. All rights reserved.
8
9 These coded instructions, statements, and computer programs contain
10 proprietary information of Nintendo of America Inc. and/or Nintendo
11 Company Ltd., and are protected by Federal copyright law. They may
12 not be disclosed to third parties or copied or duplicated in any form,
13 in whole or in part, without the prior written consent of Nintendo.
14 *---------------------------------------------------------------------------*/
15
16 #include <revolution/kpad.h>
17 #include "KPADinside.h"
18
19 #include <revolution.h>
20 #include <math.h>
21
22 #include <revolution/revodefs.h>
23 REVOLUTION_LIB_VERSION(KPAD);
24
25
26
27 /*******************************************************
28 VARIABLE
29 *******************************************************/
30 //----- Initial values for �c�o�c calibration
31 static Vec2 icenter_org = { 0.000f, 0.000f } ; // Center coordinate of the 2 marks on CMOS
32 static f32 idist_org = 1.000f ; // Distance after calibration (in meters)
33 static Vec2 iaccXY_nrm_hori = { 0.000f,-1.000f } ; // XY acceleration direction when the controller is in the horizontal plane
34 static Vec2 isec_nrm_hori = { 1.000f, 0.000f } ; // Direction from left mark to right mark when the controller is in the horizontal plane
35 f32 kp_obj_interval = 0.200f ; // Distance between both marks (in meters)
36
37 //----- Various adjustments
38 f32 kp_acc_horizon_pw = 0.050f ; // Conditions when calculating the rotation from the acceleration
39 f32 kp_ah_circle_radius = 0.070f ; // Radius used to determine if the controller is at rest
40 f32 kp_ah_circle_pw = 0.060f ; // Sensitivity conditions used to determine if the controller is at rest
41 u16 kp_ah_circle_ct = 100 ; // Count for determining if the controller is at rest
42
43 //----- Numeric values viewed as errors
44 f32 kp_err_outside_frame = 0.050f ; // Peripheral width where center of gravity coordinates are disabled (peripheral light may not be entirely displayed)
45 f32 kp_err_dist_min ; // Automatically calculate minimum operation distance (in meters)
46 f32 kp_err_dist_max = 3.000f ; // Maximum operation distance (in meters)
47 f32 kp_err_dist_speed = 0.040f ; // Allowable range of variation in distance (in meters)
48 f32 kp_err_first_inpr = 0.900f ; // Inner product of acceleration slope and object inclination when selecting first two points
49 f32 kp_err_next_inpr = 0.900f ; // Allowable range in variation of inclination (inner product)
50 f32 kp_err_acc_inpr = 0.900f ; // Allowable range of inner product with acceleration slope when controller has been brought to rest
51 f32 kp_err_up_inpr = 0.700f ; // Allowable range of inner product when controller is facing up
52 f32 kp_err_near_pos = 0.100f ; // Distance from last point when selecting the next point
53
54 //----- Internal processing
55 static Vec2 kobj_frame_min, kobj_frame_max ; // Range in which center of gravity coordinate is enabled
56 static f32 kp_err_dist_speed_1 ; // Inverse
57 static f32 kp_err_dist_speedM_1 ; // Inverse having negative value
58 static f32 kp_ah_circle_radius2 ; // Raised to the 2nd power
59 static f32 kp_dist_vv1 ; // Constant
60
61 static s32 kp_fs_fstick_min = 15 ; // Set nunchaku unit stick clamp
62 static s32 kp_fs_fstick_max = 71 ;
63 static s32 kp_cl_stick_min = 60 ; // Classic Controller unit stick clamp settings
64 static s32 kp_cl_stick_max = 308 ;
65 static s32 kp_cl_trigger_min = 30 ; // Classic Controller unit analog trigger clamp settings
66 static s32 kp_cl_trigger_max = 180 ;
67 static s32 kp_gc_mstick_min = 15 ; // Set old GC3D stick clamp
68 static s32 kp_gc_mstick_max = 77 ;
69 static s32 kp_gc_cstick_min = 15 ; // Set old GCC stick clamp
70 static s32 kp_gc_cstick_max = 64 ;
71 static s32 kp_gc_trigger_min = 30 ; // Set old GC analog trigger clamp
72 static s32 kp_gc_trigger_max = 180 ;
73 static f32 kp_rm_acc_max = 3.4f ; // Remote acceleration clamp settings
74 static f32 kp_fs_acc_max = 2.1f ; // Nunchuk acceleration clamp settings
75
76
77 //----- �j�o�`�c unit
78 KPADInsideStatus inside_kpads[ WPAD_MAX_CONTROLLERS ] ;
79
80 //----- Zero vector
81 static Vec2 Vec2_0={0.0f, 0.0f};
82
83
84 /*******************************************************************************
85 Set analog data clamp
86 *******************************************************************************/
KPADSetFSStickClamp(s8 min,s8 max)87 void KPADSetFSStickClamp( s8 min, s8 max )
88 {
89 kp_fs_fstick_min = (s32)min ;
90 kp_fs_fstick_max = (s32)max ;
91 }
92
93
94 /*******************************************************************************
95 Get WPADStatus ring buffer
96 *******************************************************************************/
KPADGetWPADRingBuffer(s32 chan)97 WPADStatus *KPADGetWPADRingBuffer( s32 chan )
98 {
99 ASSERT((0 <= chan) && (chan < WPAD_MAX_CONTROLLERS));
100 return ( (WPADStatus*)inside_kpads[ chan ].wpad_ring_bf ) ;
101 }
102
KPADGetWPADFSRingBuffer(s32 chan)103 WPADFSStatus *KPADGetWPADFSRingBuffer( s32 chan )
104 {
105 ASSERT((0 <= chan) && (chan < WPAD_MAX_CONTROLLERS));
106 return ( (WPADFSStatus*)inside_kpads[ chan ].wpad_ring_bf ) ;
107 }
108
KPADGetWPADCLRingBuffer(s32 chan)109 WPADCLStatus *KPADGetWPADCLRingBuffer( s32 chan )
110 {
111 ASSERT((0 <= chan) && (chan < WPAD_MAX_CONTROLLERS));
112 return ( (WPADCLStatus*)inside_kpads[ chan ].wpad_ring_bf ) ;
113 }
114
115 /*******************************************************************************
116 Set conditions for button repeat
117 *******************************************************************************/
KPADSetBtnRepeat(s32 chan,f32 delay_sec,f32 pulse_sec)118 void KPADSetBtnRepeat( s32 chan, f32 delay_sec, f32 pulse_sec )
119 {
120 ASSERT((0 <= chan) && (chan < WPAD_MAX_CONTROLLERS));
121 if ( pulse_sec ) {
122 //----- Set repeat flag
123 inside_kpads[ chan ].btn_repeat_delay = (u16)(s32)( delay_sec * 200.0f + 0.5f ) ;
124 inside_kpads[ chan ].btn_repeat_pulse = (u16)(s32)( pulse_sec * 200.0f + 0.5f ) ;
125 inside_kpads[ chan ].btn_cl_repeat_delay = (u16)(s32)( delay_sec * 200.0f + 0.5f ) ;
126 inside_kpads[ chan ].btn_cl_repeat_pulse = (u16)(s32)( pulse_sec * 200.0f + 0.5f ) ;
127 } else {
128 //----- Unset repeat flag
129 inside_kpads[ chan ].btn_repeat_delay = KPAD_BTN_NO_RPT_DELAY ;
130 inside_kpads[ chan ].btn_repeat_pulse = 0 ;
131 inside_kpads[ chan ].btn_cl_repeat_delay = KPAD_BTN_NO_RPT_DELAY ;
132 inside_kpads[ chan ].btn_cl_repeat_pulse = 0 ;
133 }
134
135 //----- Reset
136 inside_kpads[ chan ].btn_repeat_time = 0 ;
137 inside_kpads[ chan ].btn_repeat_next = inside_kpads[ chan ].btn_repeat_delay ;
138 inside_kpads[ chan ].btn_cl_repeat_time = 0 ;
139 inside_kpads[ chan ].btn_cl_repeat_next = inside_kpads[ chan ].btn_cl_repeat_delay ;
140 }
141
142
143 /*******************************************************************************
144 Set distance between markers (in meters)
145 *******************************************************************************/
KPADSetObjInterval(f32 interval)146 void KPADSetObjInterval( f32 interval )
147 {
148 kp_obj_interval = interval ;
149
150 //----- Minimum �c�o�c operation distance (set so that the distance between marks is half the lens diameter)
151 kp_err_dist_min = kp_obj_interval / KPAD_CMOS_HFOV_TAN ;
152
153 //----- Constant used to calculate distances
154 kp_dist_vv1 = kp_obj_interval / KPAD_CMOS_HFOV_TAN ;
155 }
156
157
158 /*******************************************************************************
159 Set parameters
160 *******************************************************************************/
KPADSetPosParam(s32 chan,f32 play_radius,f32 sensitivity)161 void KPADSetPosParam( s32 chan, f32 play_radius, f32 sensitivity )
162 {
163 ASSERT((0 <= chan) && (chan < WPAD_MAX_CONTROLLERS));
164 inside_kpads[ chan ].pos_play_radius = play_radius ;
165 inside_kpads[ chan ].pos_sensitivity = sensitivity ;
166 }
167
KPADSetHoriParam(s32 chan,f32 play_radius,f32 sensitivity)168 void KPADSetHoriParam( s32 chan, f32 play_radius, f32 sensitivity )
169 {
170 ASSERT((0 <= chan) && (chan < WPAD_MAX_CONTROLLERS));
171 inside_kpads[ chan ].hori_play_radius = play_radius ;
172 inside_kpads[ chan ].hori_sensitivity = sensitivity ;
173 }
174
KPADSetDistParam(s32 chan,f32 play_radius,f32 sensitivity)175 void KPADSetDistParam( s32 chan, f32 play_radius, f32 sensitivity )
176 {
177 ASSERT((0 <= chan) && (chan < WPAD_MAX_CONTROLLERS));
178 inside_kpads[ chan ].dist_play_radius = play_radius ;
179 inside_kpads[ chan ].dist_sensitivity = sensitivity ;
180 }
181
KPADSetAccParam(s32 chan,f32 play_radius,f32 sensitivity)182 void KPADSetAccParam( s32 chan, f32 play_radius, f32 sensitivity )
183 {
184 ASSERT((0 <= chan) && (chan < WPAD_MAX_CONTROLLERS));
185 inside_kpads[ chan ].acc_play_radius = play_radius ;
186 inside_kpads[ chan ].acc_sensitivity = sensitivity ;
187 }
188
189
190 /*******************************************************************************
191 Find an easy-to-use scale value based on the calibrated center position
192 *******************************************************************************/
calc_dpd2pos_scale(KPADInsideStatus * kp)193 static void calc_dpd2pos_scale( KPADInsideStatus *kp )
194 {
195 f32 sx,sy ;
196
197
198 //----- Distance the controller is moved up/down/left/right
199 sx = 1.0f ; // Horizontal
200 sy = (f32)KPAD_DPD_RESO_WY / (f32)KPAD_DPD_RESO_WX ; // Vertical
201
202 //----- Maximum distance the controller can be moved
203 kp->dpd2pos_scale = sqrtf( sx * sx + sy * sy ) ;// Diagonal line
204
205 //----- Correct the horizontal range of movement
206 if ( kp->center_org.x < 0.0f ) {
207 sx += kp->center_org.x ;
208 } else {
209 sx -= kp->center_org.x ;
210 }
211
212 //----- Correct the vertical range of movement
213 if ( kp->center_org.y < 0.0f ) {
214 sy += kp->center_org.y ;
215 } else {
216 sy -= kp->center_org.y ;
217 }
218
219 //----- Scale that can handle the longest distance possible when a small range of movement is allowed
220 if ( sx < sy ) {
221 kp->dpd2pos_scale /= sx ;
222 } else {
223 kp->dpd2pos_scale /= sy ;
224 }
225 }
226
227
228 /*******************************************************************************
229 Initialize KPAD
230 *******************************************************************************/
reset_kpad(KPADInsideStatus * kp)231 static void reset_kpad( KPADInsideStatus *kp )
232 {
233 KPADObject *op ;
234 KPADStatus *sp = &kp->status ;
235 KPADEXStatus *ep = &sp->ex_status ;
236
237
238 //----- Clear button data
239 sp->hold = sp->trig = sp->release = 0x00000000 ;
240 kp->btn_repeat_time = 0 ;
241 kp->btn_repeat_next = kp->btn_repeat_delay ;
242
243 //----- Clear DPD data
244 sp->dpd_valid_fg = 0 ; // Invalid
245 kp->dpd_valid2_ct = 0 ;
246
247 sp->pos = sp->vec = Vec2_0 ;
248 sp->speed = 0.0f ;
249
250 sp->horizon.x = kp->acc_horizon.x = kp->obj_horizon.x = 1.0f ;
251 sp->horizon.y = kp->acc_horizon.y = kp->obj_horizon.y = 0.0f ;
252 sp->hori_vec = Vec2_0 ;
253 sp->hori_speed = 0.0f ;
254
255 sp->acc_vertical.x = 1.0f ;
256 sp->acc_vertical.y = 0.0f ;
257
258 sp->dist = kp->dist_org ;
259 sp->dist_vec = sp->dist_speed = 0.0f ;
260
261 kp->sec_dist = sp->dist ;
262 kp->sec_length = kp->trust_sec_length = kp_dist_vv1 / kp->sec_dist ;
263 kp->sec_nrm = kp->sec_nrm_hori ;
264
265 //----- Clear acceleration data
266 sp->acc.x = sp->acc.z = 0.0f ;
267 sp->acc.y = -1.0f ;
268 sp->acc_value = 1.0f ;
269 sp->acc_speed = 0.0f ;
270 kp->hard_acc = sp->acc ;
271
272 kp->ah_circle_pos = kp->acc_horizon ;
273 kp->ah_circle_ct = kp_ah_circle_ct ;
274
275 //----- Clear data of each object
276 kp->valid_objs = 0 ;
277
278 op = &kp->kobj_sample[ WPAD_DPD_MAX_OBJECTS - 1 ] ;
279 do {
280 op->error_fg = -1 ; // Not displayed
281 } while ( --op >= kp->kobj_sample ) ;
282
283 op = &kp->kobj_regular[ KPAD_USE_OBJECTS - 1 ] ;
284 do {
285 op->error_fg = -1 ; // Not displayed
286 } while ( --op >= kp->kobj_regular ) ;
287
288 //----- Miscellaneous
289 kp->wpad_ring_idx = -1 ; // Process end of ring buffer
290 kp->work_ct = 0 ; // Number of times DPD processing is performed per game frame
291
292 sp->wpad_err = WPAD_ERR_NONE ; // Since there is no error for the time being
293
294 if ( kp->wpad_chan_no < 0 ) {
295 WPADStopMotor( kp->wpad_chan_no + WPAD_MAX_CONTROLLERS ) ;
296 } else {
297 WPADStopMotor( kp->wpad_chan_no ) ;
298 }
299
300 //----- Clear extended controller information
301 switch( sp->dev_type ) {
302 case WPAD_DEV_FREESTYLE:
303 //----- Nunchaku unit
304 ep->fs.stick = Vec2_0 ;
305 ep->fs.acc.x = ep->fs.acc.z = 0.0f ;
306 ep->fs.acc.y = -1.0f ;
307 ep->fs.acc_value = 1.0f ;
308 ep->fs.acc_speed = 0.0f ;
309 break ;
310
311 case WPAD_DEV_CLASSIC:
312 //----- for a Classic Controller unit
313 ep->cl.lstick = Vec2_0;
314 ep->cl.rstick = Vec2_0;
315 ep->cl.ltrigger = ep->cl.rtrigger = 0.0f;
316 ep->cl.hold = ep->cl.trig = ep->cl.release = 0x00000000 ;
317 kp->btn_cl_repeat_time = 0 ;
318 kp->btn_cl_repeat_next = kp->btn_cl_repeat_delay ;
319
320 default:
321 break ;
322 }
323 }
324
325
326 /*******************************************************************************
327 Calibrate
328 *******************************************************************************/
KPADCalibrateDPD(s32 chan)329 s32 KPADCalibrateDPD( s32 chan )
330 {
331 KPADInsideStatus *kp = &inside_kpads[ chan ] ;
332 KPADStatus *sp = &kp->status ;
333 KPADObject *op1, *op2 ;
334 f32 f1, vx,vy ;
335
336 ASSERT((0 <= chan) && (chan < WPAD_MAX_CONTROLLERS));
337
338 if ( kp->valid_objs != KPAD_USE_OBJECTS ) return ( kp->valid_objs ) ;
339
340 /***********************************************************************
341 Acceleration after calibration
342 ***********************************************************************/
343 vx = kp->hard_acc.x ;
344 vy = kp->hard_acc.y ;
345 f1 = sqrtf( vx * vx + vy * vy ) ;
346 if ( f1 <= 0.5f ) return (-1) ; // Abnormal acceleration
347 kp->accXY_nrm_hori.x = vx / f1 ;
348 kp->accXY_nrm_hori.y = vy / f1 ;
349
350 /***********************************************************************
351 Object position after calibration
352 ***********************************************************************/
353 //----- Determine the order of marks by position
354 op1 = kp->kobj_sample ;
355 while ( op1->error_fg != 0 ) ++op1 ;
356 op2 = op1 + 1 ;
357 while ( op2->error_fg != 0 ) ++op2 ;
358
359 if ( op1->center.x < op2->center.x ) goto LABEL_cp12 ;
360 else if ( op2->center.x < op1->center.x ) goto LABEL_cp21 ;
361 else if ( op2->center.y < op1->center.y ) goto LABEL_cp21 ;
362
363 LABEL_cp12:
364 kp->kobj_regular[0] = *op1 ;
365 kp->kobj_regular[1] = *op2 ;
366 goto LABEL_cpend ;
367
368 LABEL_cp21:
369 kp->kobj_regular[0] = *op2 ;
370 kp->kobj_regular[1] = *op1 ;
371
372 LABEL_cpend:
373
374 //kp->center_org.x = ( kp->kobj_regular[0].center.x + kp->kobj_regular[1].center.x ) * 0.5f ;
375 //kp->center_org.y = ( kp->kobj_regular[0].center.y + kp->kobj_regular[1].center.y ) * 0.5f ;
376 //kp->center_org.x = kp->center_org.y = 0.0f ;
377 calc_dpd2pos_scale( kp ) ;
378
379 //----- Section direction when the controller is horizontal
380 vx = kp->kobj_regular[KPAD_USE_OBJECTS-1].center.x - kp->kobj_regular[0].center.x ;
381 vy = kp->kobj_regular[KPAD_USE_OBJECTS-1].center.y - kp->kobj_regular[0].center.y ;
382 f1 = 1.0f / sqrtf( vx * vx + vy * vy ) ; // Should not become zero
383 kp->sec_nrm_hori.x = vx * f1 ;
384 kp->sec_nrm_hori.y = vy * f1 ;
385
386 /***********************************************************************
387 Distance after calibration
388 ***********************************************************************/
389 kp->dist_org = kp_dist_vv1 * f1 ;
390
391 /***********************************************************************
392 Miscellaneous
393 ***********************************************************************/
394 sp->dpd_valid_fg = 0 ; // Invalid for the time being
395
396 return ( kp->valid_objs ) ;
397 }
398
399
400 /*******************************************************************************
401 Calibrate
402 *******************************************************************************/
KPADSetSensorHeight(s32 chan,f32 level)403 void KPADSetSensorHeight( s32 chan, f32 level )
404 {
405 KPADInsideStatus *kp = &inside_kpads[ chan ] ;
406
407 kp->center_org.x = 0.0f ;
408 kp->center_org.y = -level ;
409 calc_dpd2pos_scale( kp ) ;
410 }
411
412 /*******************************************************************************
413 interpolation based on Sensor Bar position
414 *******************************************************************************/
KPADGetCalibratedDPD(Vec2 * dst,const Vec2 * src)415 void KPADGetCalibratedDPD( Vec2* dst, const Vec2* src )
416 {
417 f32 result_y;
418 const f32 overlay = 0.25f;
419 const f32 half = ( 1.0f + overlay ) / 2.0f;
420 const f32 offset = half - overlay;
421
422 if ( WPAD_SENSOR_BAR_POS_TOP == WPADGetSensorBarPosition() )
423 {
424 // when the DPD marker is above
425 result_y = ( src->y - offset ) / half;
426 }
427 else
428 {
429 // when the DPD marker is below
430 result_y = ( src->y + offset ) / half;
431 }
432
433 if ( dst != src )
434 {
435 *dst = *src;
436 }
437
438 dst->y = result_y;
439 }
440
441
442 /*******************************************************************************
443 Repeat processing for digital buttons
444 *******************************************************************************/
calc_button_repeat(KPADInsideStatus * kp,KPADStatus * sp,u32 dev_type)445 static void calc_button_repeat( KPADInsideStatus *kp, KPADStatus *sp, u32 dev_type )
446 {
447 if ( dev_type == WPAD_DEV_CLASSIC ) {
448 if ( sp->ex_status.cl.trig != 0 || sp->ex_status.cl.release != 0 ) {
449 //----- Reset due to change in button status
450 kp->btn_cl_repeat_time = 0 ;
451 kp->btn_cl_repeat_next = kp->btn_cl_repeat_delay ;
452
453 //----- Set flag even when first being held (only when a repeat setting is used)
454 if ( sp->ex_status.cl.trig && kp->btn_cl_repeat_pulse ) {
455 sp->ex_status.cl.hold |= KPAD_BUTTON_RPT ;
456 }
457 } else if ( sp->ex_status.cl.hold != 0 ) {
458 //----- Time goes on since button is being held without change in button status
459 kp->btn_cl_repeat_time += kp->work_ct ;
460 if ( kp->btn_cl_repeat_time >= KPAD_BTN_NO_RPT_DELAY ) {
461 kp->btn_cl_repeat_time -= KPAD_BTN_NO_RPT_DELAY ;
462 }
463
464 //----- Set flag when repeat time is reached
465 if ( kp->btn_cl_repeat_time >= kp->btn_cl_repeat_next ) {
466 sp->ex_status.cl.hold |= KPAD_BUTTON_RPT ;
467
468 //Set the following repeat time
469 kp->btn_cl_repeat_next += kp->btn_cl_repeat_pulse ;
470
471 //----- Loop here if the time exceeds the allowed range
472 if ( kp->btn_cl_repeat_time >= KPAD_BTN_RPT_TIME_MAX ) {
473 kp->btn_cl_repeat_time -= KPAD_BTN_RPT_TIME_MAX ;
474 kp->btn_cl_repeat_next -= KPAD_BTN_RPT_TIME_MAX ;
475 }
476 }
477 }
478 } else {
479 if ( sp->trig != 0 || sp->release != 0 ) {
480 //----- Reset due to change in button status
481 kp->btn_repeat_time = 0 ;
482 kp->btn_repeat_next = kp->btn_repeat_delay ;
483
484 //----- Set flag even when first being held (only when a repeat setting is used)
485 if ( sp->trig && kp->btn_repeat_pulse ) {
486 sp->hold |= KPAD_BUTTON_RPT ;
487 }
488
489 } else if ( sp->hold != 0 ) {
490 //----- Time goes on since button is being held without change in button status
491 kp->btn_repeat_time += kp->work_ct ;
492 if ( kp->btn_repeat_time >= KPAD_BTN_NO_RPT_DELAY ) {
493 kp->btn_repeat_time -= KPAD_BTN_NO_RPT_DELAY ;
494 }
495
496 //----- Set flag when repeat time is reached
497 if ( kp->btn_repeat_time >= kp->btn_repeat_next ) {
498 sp->hold |= KPAD_BUTTON_RPT ;
499
500 //Set the following repeat time
501 kp->btn_repeat_next += kp->btn_repeat_pulse ;
502
503 //----- Loop here if the time exceeds the allowed range
504 if ( kp->btn_repeat_time >= KPAD_BTN_RPT_TIME_MAX ) {
505 kp->btn_repeat_time -= KPAD_BTN_RPT_TIME_MAX ;
506 kp->btn_repeat_next -= KPAD_BTN_RPT_TIME_MAX ;
507 }
508 }
509 }
510 }
511 }
512
513
514 /*******************************************************************************
515 Process for loading KPAD button data
516 *******************************************************************************/
read_kpad_button(KPADInsideStatus * kp,void * vp,u32 dev_type)517 static void read_kpad_button( KPADInsideStatus *kp, void *vp, u32 dev_type )
518 {
519 KPADStatus *sp = &kp->status ;
520 KPADEXStatus *ep = &kp->status.ex_status;
521 u32 old_fg, change_fg ;
522 u32 cl_old_fg, cl_change_fg;
523 WPADStatus *wp ;
524 WPADFSStatus *fp ;
525 WPADCLStatus *cp ;
526
527
528 if ( kp->wpad_chan_no < 0 ) return ;
529
530 //----- Save previous value
531 old_fg = sp->hold & KPAD_BUTTON_MASK ;
532 if ( dev_type == WPAD_DEV_CLASSIC )
533 {
534 cl_old_fg = ep->cl.hold & KPAD_BUTTON_MASK;
535 }
536
537 //----- Load data
538 if ( dev_type == WPAD_DEV_FREESTYLE ) {
539 fp = (WPADFSStatus*)vp ;
540
541 if ( fp->err != WPAD_ERR_NONE ) {
542 //----- Disables the port if the controller is disconnected
543 if ( fp->err == WPAD_ERR_NO_CONTROLLER ) {
544 kp->wpad_chan_no -= WPAD_MAX_CONTROLLERS ;
545 reset_kpad( kp ) ;
546 return ;
547 }
548 //----- Inherits the previous status for other errors
549 } else {
550 //----- Load new value
551 sp->hold = (u32)fp->button ;
552 }
553 } else if ( dev_type == WPAD_DEV_CLASSIC ) {
554 cp = (WPADCLStatus*)vp;
555
556 if ( cp->err != WPAD_ERR_NONE ) {
557 //----- Disables the port if the controller is disconnected
558 if ( cp->err == WPAD_ERR_NO_CONTROLLER ) {
559 kp->wpad_chan_no -= WPAD_MAX_CONTROLLERS ;
560 reset_kpad( kp );
561 return ;
562 }
563 //----- Inherits the previous status for other errors
564 } else {
565 //----- Load new value
566 sp->hold = (u32)cp->button ;
567 ep->cl.hold = (u32)cp->clButton;
568 }
569 } else {
570 wp = (WPADStatus*)vp ;
571
572 if ( wp->err != WPAD_ERR_NONE ) {
573 //----- Disables the port if the controller is disconnected
574 if ( wp->err == WPAD_ERR_NO_CONTROLLER ) {
575 kp->wpad_chan_no -= WPAD_MAX_CONTROLLERS ;
576 reset_kpad( kp ) ;
577 return ;
578 }
579 //----- Inherits the previous status for other errors
580 } else {
581 //----- Load new value
582 sp->hold = (u32)wp->button ;
583 }
584 }
585
586 //----- Process button status
587 change_fg = sp->hold ^ old_fg ; // Change button
588 sp->trig = change_fg & sp->hold ; // Held button
589 sp->release = change_fg & old_fg ; // Released button
590
591 //----- Repeat processing
592 calc_button_repeat( kp, sp, WPAD_DEV_CORE ) ;
593
594 if ( dev_type == WPAD_DEV_CLASSIC )
595 {
596 cl_change_fg = ep->cl.hold ^ cl_old_fg ;
597 ep->cl.trig = cl_change_fg & ep->cl.hold ;
598 ep->cl.release = cl_change_fg & cl_old_fg ;
599
600 calc_button_repeat( kp, sp, WPAD_DEV_CLASSIC );
601 }
602
603 }
604
605
606 /*******************************************************************************
607 Sensitivity processing for acceleration
608 *******************************************************************************/
calc_acc(KPADInsideStatus * kp,f32 * acc,f32 acc2)609 static void calc_acc( KPADInsideStatus *kp, f32 *acc, f32 acc2 )
610 {
611 f32 f1,f2 ;
612
613
614 //----- Amount short of target value
615 f2 = acc2 - *acc ;
616 if ( f2 < 0.0f ) {
617 f1 = -f2 ;
618 } else {
619 f1 = f2 ;
620 }
621
622 //----- Calculate interpolation inside/outside play radius
623 if ( f1 >= kp->acc_play_radius ) {
624 //----- Apply 100% sensitivity if outside the play radius
625 f1 = 1.0f ;
626 } else {
627 //----- Weaken sensitivity as the target is approached if inside the play radius
628 f1 /= kp->acc_play_radius ;
629 f1 *= f1 ; // Raised to the 2nd power
630 f1 *= f1 ; // Raised to the 4th power
631 }
632 f1 *= kp->acc_sensitivity ;
633
634 //----- Interpolate
635 *acc += f1 * f2 ;
636 }
637
638
639 /*******************************************************************************
640 Calculate orientation of controller based on acceleration
641 *******************************************************************************/
calc_acc_horizon(KPADInsideStatus * kp)642 static void calc_acc_horizon( KPADInsideStatus *kp )
643 {
644 f32 f1, vx,vy, ax,ay ;
645
646
647 //----- Normalization of XY acceleration
648 f1 = sqrtf( kp->hard_acc.x * kp->hard_acc.x + kp->hard_acc.y * kp->hard_acc.y ) ;
649 if ( f1 == 0.0f || f1 >= 2.0f ) return ;
650 ax = kp->hard_acc.x / f1 ;
651 ay = kp->hard_acc.y / f1 ;
652
653 //----- Power increases as the length of XY acceleration approaches 1
654 if ( f1 > 1.0f ) {
655 f1 = 2.0f - f1 ;
656 }
657 f1 *= f1 * kp_acc_horizon_pw ;
658
659 //----- Target orientation
660 vx = kp->accXY_nrm_hori.x * ax + kp->accXY_nrm_hori.y * ay ;
661 vy = kp->accXY_nrm_hori.y * ax - kp->accXY_nrm_hori.x * ay ;
662
663 //----- Approach
664 ax = ( vx - kp->acc_horizon.x ) * f1 + kp->acc_horizon.x ;
665 ay = ( vy - kp->acc_horizon.y ) * f1 + kp->acc_horizon.y ;
666
667 //----- Normalize
668 f1 = sqrtf( ax * ax + ay * ay ) ;
669 if ( f1 == 0.0f ) return ;
670 kp->acc_horizon.x = ax / f1 ;
671 kp->acc_horizon.y = ay / f1 ;
672
673
674 //----- Update coordinates to use for determining the controller is at rest
675 kp->ah_circle_pos.x += ( kp->acc_horizon.x - kp->ah_circle_pos.x ) * kp_ah_circle_pw ;
676 kp->ah_circle_pos.y += ( kp->acc_horizon.y - kp->ah_circle_pos.y ) * kp_ah_circle_pw ;
677
678 vx = kp->acc_horizon.x - kp->ah_circle_pos.x ;
679 vy = kp->acc_horizon.y - kp->ah_circle_pos.y ;
680 if ( vx*vx + vy*vy <= kp_ah_circle_radius2 ) {
681 if ( kp->ah_circle_ct ) -- kp->ah_circle_ct ;
682 } else {
683 kp->ah_circle_ct = kp_ah_circle_ct ;
684 }
685 }
686
calc_acc_vertical(KPADInsideStatus * kp)687 static void calc_acc_vertical( KPADInsideStatus *kp )
688 {
689 KPADStatus *sp = &kp->status ;
690 f32 f1,f2, ax,ay ;
691
692
693 //----- Target orientation
694 ax = sqrtf( f2 = kp->hard_acc.x * kp->hard_acc.x + kp->hard_acc.y * kp->hard_acc.y ) ;
695 ay = - kp->hard_acc.z ;
696 f1 = sqrtf( f2 + ay * ay ) ;
697 if ( f1 == 0.0f || f1 >= 2.0f ) return ;
698 ax /= f1 ;
699 ay /= f1 ;
700
701 //----- Power increases as the length of acceleration approaches 1
702 if ( f1 > 1.0f ) {
703 f1 = 2.0f - f1 ;
704 }
705 f1 *= f1 * kp_acc_horizon_pw ;
706
707 //----- Approach
708 ax = ( ax - sp->acc_vertical.x ) * f1 + sp->acc_vertical.x ;
709 ay = ( ay - sp->acc_vertical.y ) * f1 + sp->acc_vertical.y ;
710
711 //----- Normalize
712 f1 = sqrtf( ax * ax + ay * ay ) ;
713 if ( f1 == 0.0f ) return ;
714 sp->acc_vertical.x = ax / f1 ;
715 sp->acc_vertical.y = ay / f1 ;
716 }
717
718
719 /*******************************************************************************
720 Process for loading KPAD acceleration data
721 *******************************************************************************/
clamp_acc(f32 acc,f32 clamp)722 static f32 clamp_acc( f32 acc, f32 clamp )
723 {
724 if ( acc < 0.0f ) {
725 if ( acc < -clamp ) return ( -clamp ) ;
726 } else {
727 if ( acc > clamp ) return ( clamp ) ;
728 }
729 return ( acc ) ;
730 }
731
read_kpad_acc(KPADInsideStatus * kp,void * vp,u32 dev_type,s32 chan)732 static void read_kpad_acc( KPADInsideStatus *kp, void *vp, u32 dev_type, s32 chan )
733 {
734 KPADStatus *sp = &kp->status ;
735 Vec vec ;
736 WPADStatus *wp ;
737 WPADFSStatus *fp ;
738 u32 type ;
739
740 // �����ɗ����Ă���l�́AWPADGetAccGravityUnit()�ɂ���ē�����
741 // ���m�Ȓl�Ƃ���قǑ傫������Ă͂��Ȃ��͂��B
742 f32 acc_scale_x = 1.0f/100;
743 f32 acc_scale_y = 1.0f/100;
744 f32 acc_scale_z = 1.0f/100;
745 f32 fs_acc_scale_x = 1.0f/200;
746 f32 fs_acc_scale_y = 1.0f/200;
747 f32 fs_acc_scale_z = 1.0f/200;
748
749 if(WPADProbe(chan, &type)==WPAD_ERR_NONE)
750 {
751 // �R���g���[������1G�̒l���擾���A�‘̍��̋z�����˂炤�B
752 WPADAcc core1G = {1, 1, 1};
753 WPADGetAccGravityUnit( chan, WPAD_DEV_CORE, &core1G);
754 if(core1G.x * core1G.y * core1G.z != 0){ // �o�O��
755 acc_scale_x = 1.0f / core1G.x;
756 acc_scale_y = 1.0f / core1G.y;
757 acc_scale_z = 1.0f / core1G.z;
758 }
759
760 if ( dev_type == WPAD_DEV_FREESTYLE ) {
761 WPADAcc fs1G = {1, 1, 1};
762 WPADGetAccGravityUnit( chan, WPAD_DEV_FREESTYLE, &fs1G);
763 if(fs1G.x * fs1G.y * fs1G.z != 0){ // �o�O��
764 fs_acc_scale_x = 1.0f / fs1G.x;
765 fs_acc_scale_y = 1.0f / fs1G.y;
766 fs_acc_scale_z = 1.0f / fs1G.z;
767 }
768 }
769 }
770
771 if ( kp->wpad_chan_no < 0 ) return ;
772
773 //----- Load data
774 if ( dev_type == WPAD_DEV_FREESTYLE ) {
775 fp = (WPADFSStatus*)vp ;
776
777 if ( fp->err != WPAD_ERR_NONE ) {
778 //----- Disables the port if the controller is disconnected
779 if ( fp->err == WPAD_ERR_NO_CONTROLLER ) {
780 kp->wpad_chan_no -= WPAD_MAX_CONTROLLERS ;
781 reset_kpad( kp ) ;
782 return ;
783 }
784 } else {
785 //----- Update raw values
786 kp->hard_acc.x = clamp_acc( (f32)(s32)-fp->accX * acc_scale_x, kp_rm_acc_max ) ;
787 kp->hard_acc.y = clamp_acc( (f32)(s32)-fp->accZ * acc_scale_z, kp_rm_acc_max ) ;
788 kp->hard_acc.z = clamp_acc( (f32)(s32) fp->accY * acc_scale_y, kp_rm_acc_max ) ;
789 }
790 } else {
791 wp = (WPADStatus*)vp ;
792
793 if ( wp->err != WPAD_ERR_NONE ) {
794 //----- Disables the port if the controller is disconnected
795 if ( wp->err == WPAD_ERR_NO_CONTROLLER ) {
796 kp->wpad_chan_no -= WPAD_MAX_CONTROLLERS ;
797 reset_kpad( kp ) ;
798 return ;
799 }
800 } else {
801 //----- Update raw values
802 kp->hard_acc.x = clamp_acc( (f32)(s32)-wp->accX * acc_scale_x, kp_rm_acc_max ) ;
803 kp->hard_acc.y = clamp_acc( (f32)(s32)-wp->accZ * acc_scale_z, kp_rm_acc_max ) ;
804 kp->hard_acc.z = clamp_acc( (f32)(s32) wp->accY * acc_scale_y, kp_rm_acc_max ) ;
805 }
806 }
807
808 //----- Save temporarily
809 vec = sp->acc ;
810
811 //----- Acceleration sensitivity processing for the application
812 calc_acc( kp, &sp->acc.x, kp->hard_acc.x ) ;
813 calc_acc( kp, &sp->acc.y, kp->hard_acc.y ) ;
814 calc_acc( kp, &sp->acc.z, kp->hard_acc.z ) ;
815 sp->acc_value = sqrtf( sp->acc.x * sp->acc.x + sp->acc.y * sp->acc.y + sp->acc.z * sp->acc.z ) ;
816
817 //----- Amount of change in acceleration for the application
818 vec.x -= sp->acc.x ;
819 vec.y -= sp->acc.y ;
820 vec.z -= sp->acc.z ;
821 sp->acc_speed = sqrtf( vec.x * vec.x + vec.y * vec.y + vec.z * vec.z ) ;
822
823 //----- Calculate orientation of controller based on raw velocity
824 calc_acc_horizon( kp ) ;
825 calc_acc_vertical( kp ) ;
826
827
828 /***********************************************************************
829 Load acceleration for nunchaku unit
830 ***********************************************************************/
831 if ( dev_type != WPAD_DEV_FREESTYLE ) return ;
832
833 //----- Save temporarily
834 vec = sp->ex_status.fs.acc ;
835
836 //----- Acceleration sensitivity processing for the application
837 calc_acc( kp, &sp->ex_status.fs.acc.x, clamp_acc( (f32)(s32)-fp->fsAccX * fs_acc_scale_x, kp_fs_acc_max ) ) ;
838 calc_acc( kp, &sp->ex_status.fs.acc.y, clamp_acc( (f32)(s32)-fp->fsAccZ * fs_acc_scale_z, kp_fs_acc_max ) ) ;
839 calc_acc( kp, &sp->ex_status.fs.acc.z, clamp_acc( (f32)(s32) fp->fsAccY * fs_acc_scale_y, kp_fs_acc_max ) ) ;
840 sp->ex_status.fs.acc_value = sqrtf( sp->ex_status.fs.acc.x * sp->ex_status.fs.acc.x + sp->ex_status.fs.acc.y * sp->ex_status.fs.acc.y + sp->ex_status.fs.acc.z * sp->ex_status.fs.acc.z ) ;
841
842 //----- Amount of change in acceleration for the application
843 vec.x -= sp->ex_status.fs.acc.x ;
844 vec.y -= sp->ex_status.fs.acc.y ;
845 vec.z -= sp->ex_status.fs.acc.z ;
846 sp->ex_status.fs.acc_speed = sqrtf( vec.x * vec.x + vec.y * vec.y + vec.z * vec.z ) ;
847 }
848
849
850 /*******************************************************************************
851 Convert WPAD object to KPAD
852 *******************************************************************************/
get_kobj(KPADInsideStatus * kp,void * vp,u32 dev_type)853 static void get_kobj( KPADInsideStatus *kp, void *vp, u32 dev_type )
854 {
855 const f32 dpd_scale = 2.0f / (f32)KPAD_DPD_RESO_WX ;
856 const f32 dpd_cx = (f32)( KPAD_DPD_RESO_WX - 1 ) / (f32)KPAD_DPD_RESO_WX ;
857 const f32 dpd_cy = (f32)( KPAD_DPD_RESO_WY - 1 ) / (f32)KPAD_DPD_RESO_WX ;
858
859 DPDObject *wobj_p ;
860 KPADObject *kobj_p ;
861
862
863 //----- Store data
864 if ( dev_type == WPAD_DEV_FREESTYLE ) {
865 wobj_p = &((WPADFSStatus*)vp)->obj[ WPAD_DPD_MAX_OBJECTS - 1 ] ;
866 } else if ( dev_type == WPAD_DEV_CLASSIC ) {
867 wobj_p = &((WPADCLStatus*)vp)->obj[ WPAD_DPD_MAX_OBJECTS - 1 ] ;
868 } else {
869 wobj_p = &((WPADStatus*)vp)->obj[ WPAD_DPD_MAX_OBJECTS - 1 ] ;
870 }
871 kobj_p = &kp->kobj_sample[ WPAD_DPD_MAX_OBJECTS - 1 ] ;
872 do {
873 if ( wobj_p->size ) {
874 //----- Valid object
875 kobj_p->center.x = (f32)(s32)wobj_p->x * dpd_scale - dpd_cx ;
876 kobj_p->center.y = (f32)(s32)wobj_p->y * dpd_scale - dpd_cy ;
877
878 kobj_p->error_fg = 0 ; // Displayed
879 kobj_p->state_fg = 0 ; // Normal
880 } else {
881 //----- Invalid object
882 kobj_p->error_fg = -1 ; // Not displayed
883 }
884
885 -- wobj_p ;
886 } while ( --kobj_p >= kp->kobj_sample ) ;
887 }
888
889
890 /*******************************************************************************
891 Invalidate object outside frame
892 *******************************************************************************/
check_kobj_outside_frame(KPADObject * kobj_t)893 static void check_kobj_outside_frame( KPADObject *kobj_t )
894 {
895 KPADObject *kobj_p = &kobj_t[ WPAD_DPD_MAX_OBJECTS - 1 ] ;
896
897 do {
898 if ( kobj_p->error_fg < 0 ) continue ;
899
900 if ( kobj_p->center.x <= kobj_frame_min.x || kobj_p->center.x >= kobj_frame_max.x ||
901 kobj_p->center.y <= kobj_frame_min.y || kobj_p->center.y >= kobj_frame_max.y ) {
902 kobj_p->error_fg |= 1 ;
903 }
904 } while ( --kobj_p >= kobj_t ) ;
905 }
906
907
908 /*******************************************************************************
909 Invalidate object with same coordinates
910 *******************************************************************************/
check_kobj_same_position(KPADObject * kobj_t)911 static void check_kobj_same_position( KPADObject *kobj_t )
912 {
913 KPADObject *op1, *op2 ;
914
915
916 op1 = kobj_t ;
917 do {
918 if ( op1->error_fg != 0 ) continue ;
919
920 op2 = op1 + 1 ;
921 do {
922 if ( op2->error_fg != 0 ) continue ;
923
924 if ( op1->center.x == op2->center.x && op1->center.y == op2->center.y ) {
925 op2->error_fg |= 2 ; // Take as an error only on one side
926 }
927 } while ( ++op2 <= &kobj_t[ WPAD_DPD_MAX_OBJECTS - 1 ] ) ;
928 } while ( ++op1 < &kobj_t[ WPAD_DPD_MAX_OBJECTS - 1 ] ) ;
929 }
930
931
932 /*******************************************************************************
933 Calculate controller orientation based on two points (returns distance from TV)
934 *******************************************************************************/
calc_horizon(KPADInsideStatus * kp,Vec2 * p1,Vec2 * p2,Vec2 * hori)935 static f32 calc_horizon( KPADInsideStatus *kp, Vec2 *p1, Vec2 *p2, Vec2 *hori )
936 {
937 f32 f1, vx,vy ;
938
939
940 vx = p2->x - p1->x ;
941 vy = p2->y - p1->y ;
942 f1 = 1.0f / sqrtf( vx * vx + vy * vy ) ; // Should not become zero
943 vx *= f1 ;
944 vy *= f1 ;
945
946 hori->x = kp->sec_nrm_hori.x * vx + kp->sec_nrm_hori.y * vy ;
947 hori->y = kp->sec_nrm_hori.y * vx - kp->sec_nrm_hori.x * vy ;
948
949 return ( kp_dist_vv1 * f1 ) ;
950 }
951
952
953 /*******************************************************************************
954 First, select two marks
955 *******************************************************************************/
select_2obj_first(KPADInsideStatus * kp)956 static s8 select_2obj_first( KPADInsideStatus *kp )
957 {
958 KPADObject *op1,*op2, *rp1,*rp2 ;
959 Vec2 hori ;
960 f32 f1, max = kp_err_first_inpr ;
961
962 op1 = kp->kobj_sample ;
963 do {
964 if ( op1->error_fg != 0 ) continue ;
965
966 op2 = op1 + 1 ;
967 do {
968 if ( op2->error_fg != 0 ) continue ;
969
970 f1 = calc_horizon( kp, &op1->center, &op2->center, &hori ) ;
971
972 //----- Check the operating distance range
973 if ( f1 <= kp_err_dist_min || f1 >= kp_err_dist_max ) continue ;
974
975 f1 = kp->acc_horizon.x * hori.x + kp->acc_horizon.y * hori.y ;
976 if ( f1 < 0.0f ) {
977 if ( -f1 > max ) {
978 max = -f1 ;
979 rp1 = op2 ;
980 rp2 = op1 ;
981 }
982 } else {
983 if ( f1 > max ) {
984 max = f1 ;
985 rp1 = op1 ;
986 rp2 = op2 ;
987 }
988 }
989
990 } while ( ++op2 <= &kp->kobj_sample[ WPAD_DPD_MAX_OBJECTS - 1 ] ) ;
991 } while ( ++op1 < &kp->kobj_sample[ WPAD_DPD_MAX_OBJECTS - 1 ] ) ;
992
993 //----- Confirm as regular marks?
994 if ( max == kp_err_first_inpr ) return (0) ;
995
996 kp->kobj_regular[0] = *rp1 ;
997 kp->kobj_regular[1] = *rp2 ;
998
999 return (2) ; // Two marks recognized
1000 }
1001
1002
1003 /*******************************************************************************
1004 Next, select two marks using only section information
1005 *******************************************************************************/
select_2obj_continue(KPADInsideStatus * kp)1006 static s8 select_2obj_continue( KPADInsideStatus *kp )
1007 {
1008 KPADObject *op1,*op2, *rp1,*rp2 ;
1009 Vec2 nrm ;
1010 s32 rev_fg ;
1011 f32 f1,f2, vx,vy, min = 2.0f ;
1012
1013
1014 //----- Search for the previous slope and the closest two points in distance
1015 op1 = kp->kobj_sample ;
1016 do {
1017 if ( op1->error_fg != 0 ) continue ;
1018
1019 op2 = op1 + 1 ;
1020 do {
1021 if ( op2->error_fg != 0 ) continue ;
1022
1023 //----- Calculate direction
1024 vx = op2->center.x - op1->center.x ;
1025 vy = op2->center.y - op1->center.y ;
1026 f1 = 1.0f / sqrtf( vx*vx + vy*vy ) ; // Should not be zero
1027 nrm.x = vx * f1 ;
1028 nrm.y = vy * f1 ;
1029
1030 //----- Check the operating distance range
1031 f1 *= kp_dist_vv1 ; // Distance
1032 if ( f1 <= kp_err_dist_min || f1 >= kp_err_dist_max ) continue ;
1033
1034 //----- Check the amount of change in distance
1035 f1 -= kp->sec_dist ;
1036 if ( f1 < 0.0f ) {
1037 f1 *= kp_err_dist_speedM_1 ;
1038 } else {
1039 f1 *= kp_err_dist_speed_1 ;
1040 }
1041 if ( f1 >= 1.0f ) continue ; // Distance error rate
1042
1043 //----- Check the amount of change in slope
1044 f2 = kp->sec_nrm.x * nrm.x + kp->sec_nrm.y * nrm.y ;
1045 if ( f2 < 0.0f ) {
1046 f2 = -f2 ;
1047 rev_fg = 1 ; // Handled by reversing orientation (op2 -> op1)
1048 } else {
1049 rev_fg = 0 ; // Handled as-is (op1 -> op2)
1050 }
1051 if ( f2 <= kp_err_next_inpr ) continue ;
1052 f2 = ( 1.0f - f2 ) / ( 1.0f - kp_err_next_inpr ) ; // Slope error rate
1053
1054 //----- Record the object with the smallest error
1055 f1 += f2 ; // Make determination by adding the distance error rate and slope error rate
1056 if ( f1 < min ) {
1057 min = f1 ;
1058 if ( rev_fg ) {
1059 rp1 = op2 ;
1060 rp2 = op1 ;
1061 } else {
1062 rp1 = op1 ;
1063 rp2 = op2 ;
1064 }
1065 }
1066
1067 } while ( ++op2 <= &kp->kobj_sample[ WPAD_DPD_MAX_OBJECTS - 1 ] ) ;
1068 } while ( ++op1 < &kp->kobj_sample[ WPAD_DPD_MAX_OBJECTS - 1 ] ) ;
1069
1070 //----- Confirm as regular marks?
1071 if ( min == 2.0f ) return (0) ;
1072
1073 kp->kobj_regular[0] = *rp1 ;
1074 kp->kobj_regular[1] = *rp2 ;
1075
1076 return (2) ; // Two marks recognized
1077 }
1078
1079
1080 /*******************************************************************************
1081 First, select one mark
1082 *******************************************************************************/
select_1obj_first(KPADInsideStatus * kp)1083 static s8 select_1obj_first( KPADInsideStatus *kp )
1084 {
1085 KPADObject *op1 ;
1086 f32 vx,vy ;
1087 Vec2 p1,p2 ;
1088
1089
1090 //----- Find section direction
1091 vx = kp->sec_nrm_hori.x * kp->acc_horizon.x + kp->sec_nrm_hori.y * kp->acc_horizon.y ;
1092 vy = kp->sec_nrm_hori.y * kp->acc_horizon.x - kp->sec_nrm_hori.x * kp->acc_horizon.y ;
1093
1094 //----- Find section vector
1095 vx *= kp->trust_sec_length ;
1096 vy *= kp->trust_sec_length ;
1097
1098 //----- Search for a point where the predicted point lies on the outside
1099 op1 = kp->kobj_sample ;
1100 do {
1101 if ( op1->error_fg != 0 ) continue ;
1102
1103 p1.x = op1->center.x - vx ; // Predicted point on the left
1104 p1.y = op1->center.y - vy ;
1105 p2.x = op1->center.x + vx ; // Predicted point on the right
1106 p2.y = op1->center.y + vy ;
1107
1108 if ( p1.x <= kobj_frame_min.x || p1.x >= kobj_frame_max.x ||
1109 p1.y <= kobj_frame_min.y || p1.y >= kobj_frame_max.y ) {
1110 //----- If the predicted point on the left is on the outside, the predicted point on the right must be on the inside
1111 if ( p2.x > kobj_frame_min.x && p2.x < kobj_frame_max.x &&
1112 p2.y > kobj_frame_min.y && p2.y < kobj_frame_max.y ) {
1113 //----- op1 might be the right mark
1114 kp->kobj_regular[1] = *op1 ;
1115
1116 kp->kobj_regular[0].center = p1 ;
1117 kp->kobj_regular[0].error_fg = 0 ;
1118 kp->kobj_regular[0].state_fg = -1 ;
1119
1120 return (-1) ; // Tentative recognition of one mark
1121 }
1122 } else {
1123 //----- If the predicted point on the left is on the inside, the predicted point on the right must be on the outside
1124 if ( p2.x <= kobj_frame_min.x || p2.x >= kobj_frame_max.x ||
1125 p2.y <= kobj_frame_min.y || p2.y >= kobj_frame_max.y ) {
1126 //----- op1 might be the left mark
1127 kp->kobj_regular[0] = *op1 ;
1128
1129 kp->kobj_regular[1].center = p2 ;
1130 kp->kobj_regular[1].error_fg = 0 ;
1131 kp->kobj_regular[1].state_fg = -1 ;
1132
1133 return (-1) ; // Tentative recognition of one mark
1134 }
1135 }
1136
1137 } while ( ++op1 < &kp->kobj_sample[ WPAD_DPD_MAX_OBJECTS ] ) ;
1138
1139 return (0) ;
1140 }
1141
1142
1143 /*******************************************************************************
1144 Next, select one mark
1145 *******************************************************************************/
select_1obj_continue(KPADInsideStatus * kp)1146 static s8 select_1obj_continue( KPADInsideStatus *kp )
1147 {
1148 KPADObject *op1,*op2, *rp1,*rp2 ;
1149 f32 f1, vx,vy ;
1150 f32 min = kp_err_near_pos * kp_err_near_pos ;
1151
1152
1153 //----- Select the mark closest to the past regular mark
1154 op1 = kp->kobj_regular ;
1155 do {
1156 if ( op1->error_fg != 0 ) continue ;
1157 if ( op1->state_fg != 0 ) continue ; // Predicted point no good
1158
1159 op2 = kp->kobj_sample ;
1160 do {
1161 if ( op2->error_fg != 0 ) continue ;
1162
1163 vx = op1->center.x - op2->center.x ;
1164 vy = op1->center.y - op2->center.y ;
1165 f1 = vx * vx + vy * vy ;
1166 if ( f1 < min ) {
1167 min = f1 ;
1168 rp1 = op1 ;
1169 rp2 = op2 ;
1170 }
1171 } while ( ++op2 < &kp->kobj_sample[ WPAD_DPD_MAX_OBJECTS ] ) ;
1172 } while ( ++op1 < &kp->kobj_regular[ KPAD_USE_OBJECTS ] ) ;
1173
1174 //----- Confirm as regular marks?
1175 if ( min == kp_err_near_pos * kp_err_near_pos ) return (0) ;
1176
1177 *rp1 = *rp2 ;
1178
1179 //----- Calculate orientation based on velocity
1180 kp->sec_nrm.x = kp->sec_nrm_hori.x * kp->acc_horizon.x + kp->sec_nrm_hori.y * kp->acc_horizon.y ;
1181 kp->sec_nrm.y = kp->sec_nrm_hori.y * kp->acc_horizon.x - kp->sec_nrm_hori.x * kp->acc_horizon.y ;
1182
1183 //----- Calculate the coordinates of the predicted point
1184 vx = kp->sec_length * kp->sec_nrm.x ;
1185 vy = kp->sec_length * kp->sec_nrm.y ;
1186 if ( rp1 == &kp->kobj_regular[0] ) {
1187 kp->kobj_regular[1].center.x = rp1->center.x + vx ;
1188 kp->kobj_regular[1].center.y = rp1->center.y + vy ;
1189 kp->kobj_regular[1].error_fg = 0 ;
1190 kp->kobj_regular[1].state_fg = -1 ;
1191 } else {
1192 kp->kobj_regular[0].center.x = rp1->center.x - vx ;
1193 kp->kobj_regular[0].center.y = rp1->center.y - vy ;
1194 kp->kobj_regular[0].error_fg = 0 ;
1195 kp->kobj_regular[0].state_fg = -1 ;
1196 }
1197
1198 if ( kp->status.dpd_valid_fg < 0 ) {
1199 return (-1) ; // Tentative recognition of one mark
1200 } else {
1201 return (1) ; // One mark recognized
1202 }
1203 }
1204
1205
1206 /*******************************************************************************
1207 Calculate orientation of controller based on the object
1208 *******************************************************************************/
calc_obj_horizon(KPADInsideStatus * kp)1209 static void calc_obj_horizon( KPADInsideStatus *kp )
1210 {
1211 f32 f1, vx,vy ;
1212
1213
1214 vx = kp->kobj_regular[1].center.x - kp->kobj_regular[0].center.x ;
1215 vy = kp->kobj_regular[1].center.y - kp->kobj_regular[0].center.y ;
1216 kp->sec_length = sqrtf( vx * vx + vy * vy ) ; // Should not become zero
1217
1218 f1 = 1.0f / kp->sec_length ;
1219 kp->sec_dist = kp_dist_vv1 * f1 ;
1220
1221 kp->sec_nrm.x = ( vx *= f1 ) ;
1222 kp->sec_nrm.y = ( vy *= f1 ) ;
1223
1224 kp->obj_horizon.x = kp->sec_nrm_hori.x * vx + kp->sec_nrm_hori.y * vy ;
1225 kp->obj_horizon.y = kp->sec_nrm_hori.y * vx - kp->sec_nrm_hori.x * vy ;
1226 }
1227
1228
1229 /*******************************************************************************
1230 Update variables used in the application
1231 *******************************************************************************/
calc_dpd_variable(KPADInsideStatus * kp,s8 valid_fg_next)1232 static void calc_dpd_variable( KPADInsideStatus *kp, s8 valid_fg_next )
1233 {
1234 KPADStatus *sp = &kp->status ;
1235 f32 f1,f2, dist ;
1236 Vec2 pos, vec ;
1237
1238
1239 if ( valid_fg_next == 0 ) {
1240 sp->dpd_valid_fg = 0 ;
1241 return ;
1242 }
1243
1244 /***********************************************************************
1245 Calculate the orientation of the controller
1246 ***********************************************************************/
1247 //----- Calculate the target value
1248 pos.x = kp->sec_nrm_hori.x * kp->sec_nrm.x + kp->sec_nrm_hori.y * kp->sec_nrm.y ;
1249 pos.y = kp->sec_nrm_hori.y * kp->sec_nrm.x - kp->sec_nrm_hori.x * kp->sec_nrm.y ;
1250
1251 //----- Take sensitivity and play into account when approaching the target value
1252 if ( sp->dpd_valid_fg == 0 ) {
1253 //----- Initialize using unchanged values during initial pointing
1254 sp->horizon = pos ;
1255 sp->hori_vec = Vec2_0 ;
1256 sp->hori_speed = 0.0f ;
1257 } else {
1258 //----- Amount short of target value
1259 vec.x = pos.x - sp->horizon.x ;
1260 vec.y = pos.y - sp->horizon.y ;
1261 f1 = sqrtf( vec.x * vec.x + vec.y * vec.y ) ;
1262
1263 //----- Calculate interpolation inside/outside play radius
1264 if ( f1 >= kp->hori_play_radius ) {
1265 //----- Apply 100% sensitivity if outside the play radius
1266 f1 = 1.0f ;
1267 } else {
1268 //----- Weaken sensitivity as the target is approached if inside the play radius
1269 f1 /= kp->hori_play_radius ;
1270 f1 *= f1 ; // Raised to the 2nd power
1271 f1 *= f1 ; // Raised to the 4th power
1272 }
1273 f1 *= kp->hori_sensitivity ;
1274
1275 //----- Interpolate
1276 vec.x = f1 * vec.x + sp->horizon.x ;
1277 vec.y = f1 * vec.y + sp->horizon.y ;
1278 f1 = sqrtf( vec.x * vec.x + vec.y * vec.y ) ; // Normalize vector slope
1279 vec.x /= f1 ;
1280 vec.y /= f1 ;
1281
1282 sp->hori_vec.x = vec.x - sp->horizon.x ;
1283 sp->hori_vec.y = vec.y - sp->horizon.y ;
1284 sp->hori_speed = sqrtf( sp->hori_vec.x * sp->hori_vec.x + sp->hori_vec.y * sp->hori_vec.y ) ;
1285
1286 sp->horizon = vec ;
1287 }
1288
1289 /***********************************************************************
1290 Calculate the distance to the TV
1291 ***********************************************************************/
1292 //----- Calculate the target value
1293 dist = kp_dist_vv1 / kp->sec_length ;
1294
1295 //----- Take sensitivity and play into account when approaching the target value
1296 if ( sp->dpd_valid_fg == 0 ) {
1297 //----- Initialize using unchanged values during initial pointing
1298 sp->dist = dist ;
1299 sp->dist_vec = 0.0f ;
1300 sp->dist_speed = 0.0f ;
1301 } else {
1302 //----- Amount short of target value
1303 f2 = dist - sp->dist ;
1304 if ( f2 < 0.0f ) {
1305 f1 = -f2 ;
1306 } else {
1307 f1 = f2 ;
1308 }
1309
1310 //----- Calculate interpolation inside/outside play radius
1311 if ( f1 >= kp->dist_play_radius ) {
1312 //----- Apply 100% sensitivity if outside the play radius
1313 f1 = 1.0f ;
1314 } else {
1315 //----- Weaken sensitivity as the target is approached if inside the play radius
1316 f1 /= kp->dist_play_radius ;
1317 f1 *= f1 ; // Raised to the 2nd power
1318 f1 *= f1 ; // Raised to the 4th power
1319 }
1320 f1 *= kp->dist_sensitivity ;
1321
1322 //----- Interpolate
1323 sp->dist_vec = f1 * f2 ;
1324 if ( sp->dist_vec < 0.0f ) {
1325 sp->dist_speed = -sp->dist_vec ;
1326 } else {
1327 sp->dist_speed = sp->dist_vec ;
1328 }
1329
1330 sp->dist += sp->dist_vec ;
1331 }
1332
1333 /***********************************************************************
1334 Calculate the pointing position
1335 ***********************************************************************/
1336 //----- Coordinate of center point between two marks
1337 pos.x = ( kp->kobj_regular[0].center.x + kp->kobj_regular[1].center.x ) * 0.5f ;
1338 pos.y = ( kp->kobj_regular[0].center.y + kp->kobj_regular[1].center.y ) * 0.5f ;
1339
1340 //----- Rotate by the amount the controller is twisted
1341 f1 = kp->sec_nrm.x * kp->sec_nrm_hori.x + kp->sec_nrm.y * kp->sec_nrm_hori.y ;
1342 f2 = -kp->sec_nrm.y * kp->sec_nrm_hori.x + kp->sec_nrm.x * kp->sec_nrm_hori.y ;
1343 vec.x = f1 * pos.x - f2 * pos.y ;
1344 vec.y = f2 * pos.x + f1 * pos.y ;
1345
1346 //----- Scaling by correcting center position
1347 vec.x = ( kp->center_org.x - vec.x ) * kp->dpd2pos_scale ;
1348 vec.y = ( kp->center_org.y - vec.y ) * kp->dpd2pos_scale ;
1349
1350 //----- Convert to gravity-oriented coordinate system during calibration
1351 pos.x = -kp->accXY_nrm_hori.y * vec.x + kp->accXY_nrm_hori.x * vec.y ;
1352 pos.y = -kp->accXY_nrm_hori.x * vec.x - kp->accXY_nrm_hori.y * vec.y ;
1353
1354 //----- Take sensitivity and play into account when approaching the target value
1355 if ( sp->dpd_valid_fg == 0 ) {
1356 //----- Initialize using unchanged values during initial pointing
1357 sp->pos = pos ;
1358 sp->vec = Vec2_0 ;
1359 sp->speed = 0.0f ;
1360 } else {
1361 //----- Amount short of target value
1362 vec.x = pos.x - sp->pos.x ;
1363 vec.y = pos.y - sp->pos.y ;
1364 f1 = sqrtf( vec.x * vec.x + vec.y * vec.y ) ;
1365
1366 //----- Calculate interpolation inside/outside play radius
1367 if ( f1 >= kp->pos_play_radius ) {
1368 //----- Apply 100% sensitivity if outside the play radius
1369 f1 = 1.0f ;
1370 } else {
1371 //----- Weaken sensitivity as the target is approached if inside the play radius
1372 f1 /= kp->pos_play_radius ;
1373 f1 *= f1 ; // Raised to the 2nd power
1374 f1 *= f1 ; // Raised to the 4th power
1375 }
1376 f1 *= kp->pos_sensitivity ;
1377
1378 //----- Interpolate
1379 sp->vec.x = f1 * vec.x ;
1380 sp->vec.y = f1 * vec.y ;
1381 sp->speed = sqrtf( sp->vec.x * sp->vec.x + sp->vec.y * sp->vec.y ) ;
1382
1383 sp->pos.x += sp->vec.x ;
1384 sp->pos.y += sp->vec.y ;
1385 }
1386
1387 /***********************************************************************
1388 Update flags
1389 ***********************************************************************/
1390 sp->dpd_valid_fg = valid_fg_next ;
1391 }
1392
1393
1394 /*******************************************************************************
1395 Process for loading KPAD DPD data
1396 *******************************************************************************/
read_kpad_dpd(KPADInsideStatus * kp,void * vp,u32 dev_type)1397 static void read_kpad_dpd( KPADInsideStatus *kp, void *vp, u32 dev_type )
1398 {
1399 KPADStatus *sp = &kp->status ;
1400 KPADObject *op1 ;
1401 s8 valid_fg_next ;
1402 WPADStatus *wp ;
1403 WPADFSStatus *fp ;
1404 WPADCLStatus *cp ;
1405
1406
1407 if ( kp->wpad_chan_no < 0 ) return ;
1408
1409 /***********************************************************************
1410 Convert WPAD object to KPAD
1411 ***********************************************************************/
1412 if ( dev_type == WPAD_DEV_FREESTYLE ) {
1413 fp = (WPADFSStatus*)vp ;
1414
1415 if ( fp->err != WPAD_ERR_NONE ) {
1416 //----- Disables the port if the controller is disconnected
1417 if ( fp->err == WPAD_ERR_NO_CONTROLLER ) {
1418 kp->wpad_chan_no -= WPAD_MAX_CONTROLLERS ;
1419 reset_kpad( kp ) ;
1420 return ;
1421 }
1422 //----- Inherits the previous status for other errors
1423 } else {
1424 //----- Convert WPAD object to KPAD
1425 get_kobj( kp, fp, WPAD_DEV_FREESTYLE ) ;
1426 }
1427 } else if ( dev_type == WPAD_DEV_CLASSIC ) {
1428 cp = (WPADCLStatus*)vp ;
1429
1430 if ( cp->err != WPAD_ERR_NONE ) {
1431 //----- Disables the port if the controller is disconnected
1432 if ( cp->err == WPAD_ERR_NO_CONTROLLER ) {
1433 kp->wpad_chan_no -= WPAD_MAX_CONTROLLERS ;
1434 reset_kpad( kp ) ;
1435 return ;
1436 }
1437 //----- Inherits the previous status for other errors
1438 } else {
1439 //----- Convert WPAD object to KPAD
1440 get_kobj( kp, cp, WPAD_DEV_CLASSIC ) ;
1441 }
1442
1443 } else {
1444 wp = (WPADStatus*)vp ;
1445
1446 if ( wp->err != WPAD_ERR_NONE ) {
1447 //----- Disables the port if the controller is disconnected
1448 if ( wp->err == WPAD_ERR_NO_CONTROLLER ) {
1449 kp->wpad_chan_no -= WPAD_MAX_CONTROLLERS ;
1450 reset_kpad( kp ) ;
1451 return ;
1452 }
1453 //----- Inherits the previous status for other errors
1454 } else {
1455 //----- Convert WPAD object to KPAD
1456 get_kobj( kp, wp, WPAD_DEV_CORE ) ;
1457 }
1458 }
1459
1460 /***********************************************************************
1461 Select out normal objects
1462 ***********************************************************************/
1463 //----- Omit strange objects
1464 check_kobj_outside_frame( kp->kobj_sample ) ; // Invalidate object outside frame
1465 check_kobj_same_position( kp->kobj_sample ) ; // Invalidate objects with same coordinates
1466
1467 //----- Check how many are being displayed
1468 kp->valid_objs = 0 ;
1469 op1 = &kp->kobj_sample[ WPAD_DPD_MAX_OBJECTS - 1 ] ;
1470 do {
1471 if ( op1->error_fg == 0 ) ++ kp->valid_objs ;
1472 } while ( --op1 >= kp->kobj_sample ) ;
1473
1474 //----- Validation processing
1475 if ( sp->acc_vertical.x <= kp_err_up_inpr ) goto LABEL_select_NG ;
1476
1477 if ( sp->dpd_valid_fg == 2 || sp->dpd_valid_fg == -2 ) {
1478 //----- Validated two objects on last pass
1479 if ( kp->valid_objs >= 2 )
1480 {
1481 valid_fg_next = select_2obj_continue( kp );
1482 if ( valid_fg_next ) goto LABEL_select_OK ;
1483 }
1484 if ( kp->valid_objs >= 1 )
1485 {
1486 valid_fg_next = select_1obj_continue( kp );
1487 if ( valid_fg_next ) goto LABEL_select_OK ;
1488 }
1489 } else if ( sp->dpd_valid_fg == 1 || sp->dpd_valid_fg == -1 ) {
1490 //----- Validated previous one object
1491 if ( kp->valid_objs >= 2 )
1492 {
1493 valid_fg_next = select_2obj_first( kp );
1494 if ( valid_fg_next ) goto LABEL_select_OK ;
1495 }
1496 if ( kp->valid_objs >= 1 )
1497 {
1498 valid_fg_next = select_1obj_continue( kp );
1499 if ( valid_fg_next ) goto LABEL_select_OK ;
1500 }
1501 } else {
1502 //----- Couldn't validate any objects
1503 if ( kp->valid_objs >= 2 )
1504 {
1505 valid_fg_next = select_2obj_first( kp );
1506
1507 if ( valid_fg_next ) goto LABEL_select_OK ;
1508 }
1509 if ( kp->valid_objs == 1 )
1510 {
1511 valid_fg_next = select_1obj_first( kp );
1512 if ( valid_fg_next ) goto LABEL_select_OK ;
1513 }
1514 }
1515
1516 LABEL_select_NG:
1517
1518 valid_fg_next = 0 ; // Could not be selected
1519
1520 LABEL_select_OK: // Could perhaps be selected
1521
1522 //----- Update section data if the object could be selected
1523 if ( valid_fg_next ) {
1524 //----- Calculate data for the two points and the object orientation
1525 calc_obj_horizon( kp ) ;
1526
1527 //----- Error if clearly different from the acceleration slope
1528 if ( kp->ah_circle_ct == 0 ) {
1529 if ( kp->obj_horizon.x * kp->acc_horizon.x + kp->obj_horizon.y * kp->acc_horizon.y <= kp_err_acc_inpr ) {
1530 valid_fg_next = 0 ; // Invalid as suspected
1531
1532 kp->kobj_regular[0].error_fg =
1533 kp->kobj_regular[1].error_fg = 1 ;
1534 }
1535 }
1536
1537 //----- Count for two consecutive valid points
1538 if ( sp->dpd_valid_fg == 2 && valid_fg_next == 2 ) {
1539 if ( kp->dpd_valid2_ct == 200 ) {
1540 kp->trust_sec_length = kp->sec_length ;
1541 } else {
1542 ++ kp->dpd_valid2_ct ;
1543 }
1544 } else {
1545 kp->dpd_valid2_ct = 0 ;
1546 }
1547 } else {
1548 kp->dpd_valid2_ct = 0 ;
1549 }
1550
1551 //----- Update variables used in the application
1552 calc_dpd_variable( kp, valid_fg_next ) ;
1553 }
1554
1555
1556 /*******************************************************************************
1557 Process analog trigger clamp
1558 *******************************************************************************/
clamp_trigger(f32 * trigger,s32 tr,s32 min,s32 max)1559 static void clamp_trigger( f32 *trigger, s32 tr, s32 min, s32 max )
1560 {
1561 if ( tr <= min ) {
1562 *trigger = 0.0f ;
1563 } else if ( tr >= max ) {
1564 *trigger = 1.0f ;
1565 } else {
1566 *trigger = (f32)( tr - min ) / (f32)( max - min ) ;
1567 }
1568 }
1569
1570
1571 /*******************************************************************************
1572 Process stick clamp
1573 *******************************************************************************/
clamp_stick(Vec2 * stick,s32 sx,s32 sy,s32 min,s32 max)1574 static void clamp_stick( Vec2 *stick, s32 sx, s32 sy, s32 min, s32 max )
1575 {
1576 f32 length ;
1577 f32 fx = (f32)sx ;
1578 f32 fy = (f32)sy ;
1579 f32 fmin = (f32)min ;
1580 f32 fmax = (f32)max ;
1581
1582
1583 length = sqrtf( fx * fx + fy * fy ) ;
1584
1585 if ( length <= fmin ) {
1586 stick->x = stick->y = 0.0f ;
1587
1588 } else if ( length >= fmax ) {
1589 stick->x = fx / length ;
1590 stick->y = fy / length ;
1591
1592 } else {
1593 length = ( length - fmin ) / ( fmax - fmin ) / length ;
1594 stick->x = fx * length ;
1595 stick->y = fy * length ;
1596 }
1597 }
1598
1599
1600 /*******************************************************************************
1601 Load old GC controller
1602 *******************************************************************************/
read_dolphin(s32 chan,KPADStatus * kstatus)1603 static void read_dolphin( s32 chan, KPADStatus *kstatus )
1604 {
1605 PADStatus pstatus ;
1606 u32 old_fg, change_fg ;
1607
1608 ASSERT((0 <= chan) && (chan < WPAD_MAX_CONTROLLERS));
1609
1610 WPADRead( chan, &pstatus ) ;
1611 if ( pstatus.err != PAD_ERR_NONE ) return ;
1612
1613 //----- Process analog data
1614 clamp_stick( &kstatus->ex_status.gc.stick, pstatus.stickX, pstatus.stickY, kp_gc_mstick_min, kp_gc_mstick_max ) ;
1615 clamp_stick( &kstatus->ex_status.gc.substick, pstatus.substickX, pstatus.substickY, kp_gc_cstick_min, kp_gc_cstick_max ) ;
1616
1617 clamp_trigger( &kstatus->ex_status.gc.ltrigger, pstatus.triggerLeft, kp_gc_trigger_min, kp_gc_trigger_max ) ;
1618 clamp_trigger( &kstatus->ex_status.gc.rtrigger, pstatus.triggerRight, kp_gc_trigger_min, kp_gc_trigger_max ) ;
1619
1620 //----- Process button data
1621 old_fg = kstatus->hold & KPAD_BUTTON_MASK ; // Save last value
1622 kstatus->hold = (u32)pstatus.button ; // Load new value
1623
1624 change_fg = kstatus->hold ^ old_fg ; // Change button
1625 kstatus->trig = change_fg & kstatus->hold ; // Held button
1626 kstatus->release = change_fg & old_fg ; // Released button
1627
1628 //----- Repeat processing
1629 calc_button_repeat( &inside_kpads[ chan ], kstatus, 0 ) ;
1630 }
1631
1632
1633 /*******************************************************************************
1634 Read the stick information
1635 *******************************************************************************/
read_kpad_stick(KPADInsideStatus * kp,void * vp)1636 static void read_kpad_stick( KPADInsideStatus *kp, void *vp )
1637 {
1638 KPADStatus *sp = &kp->status ;
1639 WPADFSStatus *fp;
1640 WPADCLStatus *cp;
1641
1642 //----- Error check
1643 if ( kp->wpad_chan_no < 0 ) return ;
1644
1645 if ( sp->dev_type == WPAD_DEV_FREESTYLE ) {
1646 fp = (WPADFSStatus*)vp;
1647
1648 if ( fp->err != WPAD_ERR_NONE ) {
1649 //----- Disables the port if the controller is disconnected
1650 if ( fp->err == WPAD_ERR_NO_CONTROLLER ) {
1651 kp->wpad_chan_no -= WPAD_MAX_CONTROLLERS ;
1652 reset_kpad( kp ) ;
1653 return ;
1654 }
1655 }
1656
1657 //----- Process stick data
1658 clamp_stick( &sp->ex_status.fs.stick, fp->fsStickX, fp->fsStickY, kp_fs_fstick_min, kp_fs_fstick_max ) ;
1659 } else if ( sp->dev_type == WPAD_DEV_CLASSIC ) {
1660 cp = (WPADCLStatus*)vp;
1661
1662 if ( cp->err != WPAD_ERR_NONE ) {
1663 //----- Disables the port if the controller is disconnected
1664 if ( cp->err == WPAD_ERR_NO_CONTROLLER ) {
1665 kp->wpad_chan_no -= WPAD_MAX_CONTROLLERS ;
1666 reset_kpad( kp ) ;
1667 return ;
1668 }
1669 }
1670
1671 //----- Process stick data
1672 clamp_stick( &sp->ex_status.cl.lstick, cp->clLStickX, cp->clLStickY, kp_cl_stick_min, kp_cl_stick_max ) ;
1673 clamp_stick( &sp->ex_status.cl.rstick, cp->clRStickX, cp->clRStickY, kp_cl_stick_min, kp_cl_stick_max ) ;
1674 clamp_trigger( &sp->ex_status.cl.ltrigger, cp->clTriggerL, kp_cl_trigger_min, kp_cl_trigger_max ) ;
1675 clamp_trigger( &sp->ex_status.cl.rtrigger, cp->clTriggerR, kp_cl_trigger_min, kp_cl_trigger_max ) ;
1676
1677 }
1678 }
1679
1680
1681 /*******************************************************************************
1682 Device check (wpad_chan_no is modified)
1683 *******************************************************************************/
check_device(s32 chan,KPADInsideStatus * kp)1684 static s32 check_device( s32 chan, KPADInsideStatus *kp )
1685 {
1686 u32 type ;
1687 s32 err ;
1688
1689 ASSERT((0 <= chan) && (chan < WPAD_MAX_CONTROLLERS));
1690
1691 err = WPADProbe( chan, &type ) ;
1692
1693 if ( err == WPAD_ERR_NO_CONTROLLER ) {
1694 //----- Nothing found
1695 if ( kp->wpad_chan_no >= 0 ) {
1696 //----- Pause auto sampling
1697 WPADSetAutoSamplingBuf( chan, NULL, 0 ) ;
1698 kp->wpad_chan_no = (s16)( chan - WPAD_MAX_CONTROLLERS ) ;
1699 kp->status.dev_type = WPAD_DEV_UNKNOWN ;
1700 }
1701 return ( -1 ) ; // Unconnected
1702 } else if ( err != WPAD_ERR_NONE ) {
1703 //----- Check impossible
1704 return ( 0 ) ; // Assume no problem for the time being
1705
1706 } else if ( type == kp->status.dev_type ) {
1707 //----- No problem with the same device as before
1708 return ( 0 ) ; // No problem
1709 }
1710
1711 /***********************************************************************
1712 Device has changed
1713 ***********************************************************************/
1714 //----- Pause auto sampling
1715 WPADSetAutoSamplingBuf( chan, NULL, 0 ) ;
1716
1717 //----- Initialize new device
1718 switch ( type ) {
1719 case WPAD_DEV_CORE:
1720 WPADControlDpd( chan, WPAD_DPD_EXP, NULL ) ;
1721 WPADSetDataFormat( chan, WPAD_FMT_CORE_ACC_DPD);
1722 WPADSetAutoSamplingBuf( chan, kp->wpad_ring_bf, KPAD_RING_BUFS ) ;
1723 break ;
1724 case WPAD_DEV_FREESTYLE:
1725 WPADControlDpd( chan, WPAD_DPD_STD, NULL ) ;
1726 WPADSetDataFormat( chan, WPAD_FMT_FREESTYLE_ACC_DPD ) ;
1727 WPADSetAutoSamplingBuf( chan, kp->wpad_ring_bf, KPAD_RING_BUFS ) ;
1728 break ;
1729 case WPAD_DEV_CLASSIC:
1730 WPADControlDpd( chan, WPAD_DPD_STD, NULL ) ;
1731 WPADSetDataFormat( chan, WPAD_FMT_CLASSIC_ACC_DPD ) ;
1732 WPADSetAutoSamplingBuf( chan, kp->wpad_ring_bf, KPAD_RING_BUFS ) ;
1733 break;
1734 #if 0
1735 case WPAD_DEV_DOLPHIN:
1736 WPADSetDataFormat( chan, WPAD_FMT_DOLPHIN ) ;
1737 break ;
1738 #endif
1739 default:
1740 kp->wpad_chan_no = (s16)( chan - WPAD_MAX_CONTROLLERS ) ;
1741 return ( -1 ) ; // Unexpected device
1742 }
1743
1744 //Switch to enabled
1745 kp->wpad_chan_no = (s16)chan ;
1746 kp->status.dev_type = type ;
1747 reset_kpad( kp ) ;
1748
1749 return ( 1 ) ; // Change has occured with device
1750 }
1751
1752
1753 /*******************************************************************************
1754 READ KPAD (return the number of loaded buffers)
1755 *******************************************************************************/
KPADRead(s32 chan,KPADStatus samplingBufs[],u32 length)1756 s32 KPADRead( s32 chan, KPADStatus samplingBufs[], u32 length )
1757 {
1758 KPADInsideStatus *kp = &inside_kpads[ chan ] ;
1759 KPADStatus *sp ;
1760 WPADStatus *wp ;
1761 WPADFSStatus *fp ;
1762 WPADCLStatus *cp ;
1763 s32 idx, idx_old, idx_now ;
1764 s32 sample_ct, copy_ct ;
1765
1766 ASSERT((0 <= chan) && (chan < WPAD_MAX_CONTROLLERS));
1767
1768 /***************************************************************
1769 Set device type
1770 ***************************************************************/
1771 sp = &samplingBufs[ length - 1 ] ;
1772 do {
1773 sp->dev_type = kp->status.dev_type ;
1774 } while ( --sp >= samplingBufs ) ;
1775
1776 /***************************************************************
1777 Check device
1778 ***************************************************************/
1779 if ( check_device( chan, kp ) ) {
1780 //----- Does nothing if there have been changes and the device is unconnected
1781 return ( 0 ) ;
1782 }
1783
1784 /***************************************************************
1785 Extended controller processing
1786 ***************************************************************/
1787 if ( kp->status.dev_type != WPAD_DEV_CORE &&
1788 kp->status.dev_type != WPAD_DEV_FREESTYLE &&
1789 kp->status.dev_type != WPAD_DEV_CLASSIC) {
1790 //----- a device other than known ones
1791 ASSERT("Unknown device.");
1792 return ( 0 ) ;
1793 }
1794
1795 /***************************************************************
1796 Check the ring buffer inside the scope of current processing
1797 ***************************************************************/
1798 //----- idx_old: Previously processed buffer -> idx_now: Most recent buffer
1799 if( kp->wpad_chan_no >= 0 ) {
1800 idx_now = (s32)WPADGetLatestIndexInBuf( kp->wpad_chan_no ) ;
1801 } else {
1802 idx_now = (s32)WPADGetLatestIndexInBuf( kp->wpad_chan_no + WPAD_MAX_CONTROLLERS ) ;
1803 }
1804
1805 idx_old = kp->wpad_ring_idx ;
1806 if ( idx_old < 0 ) { // At start of processing
1807 idx_old = idx_now - 1 ;
1808 if ( idx_old < 0 ) idx_old = KPAD_RING_BUFS - 1 ;
1809 }
1810 kp->wpad_ring_idx = (s16)idx_now ;
1811
1812 //----- Check the number of internal processes
1813 sample_ct = idx_now - idx_old ;
1814 if ( sample_ct == 0 ) {
1815 return (0) ; // Still no data to be processed
1816 } else if ( sample_ct < 0 ) {
1817 sample_ct += KPAD_RING_BUFS ;
1818 }
1819
1820 //----- Check the number of times data is saved to the buffer
1821 if ( sample_ct < length ) {
1822 copy_ct = sample_ct ;
1823 } else {
1824 copy_ct = (s32)length ;
1825 }
1826
1827 /***************************************************************
1828 Processing performed for each frame sampled
1829 ***************************************************************/
1830 sp = &samplingBufs[ copy_ct ] ; // Continue saving with old data at the rear
1831 kp->work_ct = 0 ;
1832 idx = idx_old ;
1833 if ( kp->status.dev_type == WPAD_DEV_CORE ) {
1834 do {
1835 //----- Get SDK ring buffer
1836 if ( ++idx == KPAD_RING_BUFS ) idx = 0 ;
1837 wp = &((WPADStatus*)kp->wpad_ring_bf)[ idx ] ;
1838
1839 //----- Copy error code
1840 kp->status.wpad_err = wp->err ;
1841
1842 //----- Load acceleration and DPD data
1843 read_kpad_acc( kp, wp, WPAD_DEV_CORE, chan ) ;
1844 read_kpad_dpd( kp, wp, WPAD_DEV_CORE ) ;
1845
1846 //----- Copy to KPAD buffer
1847 if ( kp->work_ct >= sample_ct - copy_ct ) {
1848 -- sp ; // Backup storage buffer
1849 *sp = kp->status ; // Actually, button data is copied unnecessarily
1850 }
1851 } while ( ++ kp->work_ct < sample_ct ) ;
1852 } else if ( kp->status.dev_type == WPAD_DEV_FREESTYLE) {
1853 do {
1854 //----- Get SDK ring buffer
1855 if ( ++idx == KPAD_RING_BUFS ) idx = 0 ;
1856 fp = &((WPADFSStatus*)kp->wpad_ring_bf)[ idx ] ;
1857
1858 //----- Copy error code
1859 kp->status.wpad_err = fp->err ;
1860
1861 //----- Load acceleration and DPD data
1862 read_kpad_acc( kp, fp, WPAD_DEV_FREESTYLE, chan ) ;
1863 read_kpad_dpd( kp, fp, WPAD_DEV_FREESTYLE ) ;
1864 read_kpad_stick( kp, fp ) ;
1865
1866 //----- Copy to KPAD buffer
1867 if ( kp->work_ct >= sample_ct - copy_ct ) {
1868 -- sp ; // Backup storage buffer
1869 *sp = kp->status ; // Actually, button data is copied unnecessarily
1870 }
1871 } while ( ++ kp->work_ct < sample_ct ) ;
1872 } else if ( kp->status.dev_type == WPAD_DEV_CLASSIC ) {
1873 do {
1874 //----- Get SDK ring buffer
1875 if ( ++idx == KPAD_RING_BUFS ) idx = 0 ;
1876 cp = &((WPADCLStatus*)kp->wpad_ring_bf)[ idx ] ;
1877
1878 //----- Copy error code
1879 kp->status.wpad_err = cp->err ;
1880
1881 //----- Load acceleration and DPD data
1882 read_kpad_acc( kp, cp, WPAD_DEV_CLASSIC, chan ) ;
1883 read_kpad_dpd( kp, cp, WPAD_DEV_CLASSIC ) ;
1884 read_kpad_stick( kp, cp ) ;
1885
1886 //----- Copy to KPAD buffer
1887 if ( kp->work_ct >= sample_ct - copy_ct ) {
1888 -- sp ; // Backup storage buffer
1889 *sp = kp->status ; // Actually, button data is copied unnecessarily
1890 }
1891 } while ( ++ kp->work_ct < sample_ct ) ;
1892 }
1893
1894
1895 /***************************************************************
1896 Processing performed for each game frame
1897 ***************************************************************/
1898 //----- Load button data
1899 if ( kp->status.dev_type == WPAD_DEV_CORE ) {
1900 read_kpad_button( kp, wp, WPAD_DEV_CORE ) ;
1901 } else if ( kp->status.dev_type == WPAD_DEV_FREESTYLE ) {
1902 read_kpad_button( kp, fp, WPAD_DEV_FREESTYLE ) ;
1903 } else if ( kp->status.dev_type == WPAD_DEV_CLASSIC ) {
1904 read_kpad_button( kp, cp, WPAD_DEV_CLASSIC ) ;
1905 }
1906
1907 //----- Copy to buffer
1908 idx = copy_ct ;
1909 do {
1910 sp->hold = kp->status.hold ;
1911 sp->trig = kp->status.trig ;
1912 sp->release = kp->status.release ;
1913 ++ sp ; // Proceed forward because we're at the start of the storage buffer
1914
1915 if ( kp->status.dev_type == WPAD_DEV_CLASSIC ) {
1916 sp->ex_status.cl.hold = kp->status.ex_status.cl.hold;
1917 sp->ex_status.cl.trig = kp->status.ex_status.cl.trig;
1918 sp->ex_status.cl.release = kp->status.ex_status.cl.release;
1919 }
1920 } while ( --idx != 0 ) ;
1921
1922 return ( copy_ct ) ;
1923 }
1924
1925
1926 /*******************************************************************************
1927 INIT KPAD
1928 *******************************************************************************/
KPADInit(void)1929 void KPADInit( void )
1930 {
1931 s32 i ;
1932 KPADInsideStatus *kp ;
1933
1934 //----- WPAD
1935 WPADInit() ;
1936 while (WPADGetStatus() != WPAD_STATE_SETUP)
1937 ;
1938
1939 //----- KPAD
1940 i = 0 ;
1941 do {
1942 kp = &inside_kpads[i] ;
1943
1944 //----- Check device
1945 kp->status.dev_type = WPAD_DEV_UNKNOWN ;
1946 kp->wpad_chan_no = (s16)( i - WPAD_MAX_CONTROLLERS ) ;
1947 (void)check_device( i, kp ) ;
1948
1949 //----- Initialize KPAD calibration value
1950 kp->dist_org = idist_org ;
1951 kp->accXY_nrm_hori = iaccXY_nrm_hori ;
1952 kp->sec_nrm_hori = isec_nrm_hori ;
1953 kp->center_org = icenter_org ;
1954 calc_dpd2pos_scale( kp ) ;
1955
1956 //----- Initialize KPAD pointing conditions
1957 kp->pos_play_radius =
1958 kp->hori_play_radius =
1959 kp->dist_play_radius =
1960 kp->acc_play_radius = 0.0f ;
1961
1962 kp->pos_sensitivity =
1963 kp->hori_sensitivity =
1964 kp->dist_sensitivity =
1965 kp->acc_sensitivity = 1.0f ;
1966
1967 //----- Initialize without button repeat
1968 kp->btn_repeat_delay = KPAD_BTN_NO_RPT_DELAY ;
1969 kp->btn_repeat_pulse = 0 ;
1970
1971 kp->btn_cl_repeat_delay = KPAD_BTN_NO_RPT_DELAY ;
1972 kp->btn_cl_repeat_pulse = 0 ;
1973
1974 } while ( ++i < WPAD_MAX_CONTROLLERS ) ;
1975
1976 //----- Initialize values
1977 KPADReset() ;
1978 OSRegisterVersion(__KPADVersion);
1979 }
1980
1981
1982 /*******************************************************************************
1983 RESET KPAD
1984 *******************************************************************************/
KPADReset(void)1985 void KPADReset( void )
1986 {
1987 KPADInsideStatus *kp ;
1988
1989
1990 //----- Recalculate constants
1991 KPADSetObjInterval( kp_obj_interval ) ;
1992
1993 kobj_frame_min.x = -1.0f + kp_err_outside_frame ;
1994 kobj_frame_max.x = 1.0f - kp_err_outside_frame ;
1995 kobj_frame_min.y = -((f32)KPAD_DPD_RESO_WY/KPAD_DPD_RESO_WX) + kp_err_outside_frame ;
1996 kobj_frame_max.y = ((f32)KPAD_DPD_RESO_WY/KPAD_DPD_RESO_WX) - kp_err_outside_frame ;
1997
1998 kp_err_dist_speed_1 = 1.0f / kp_err_dist_speed ;
1999 kp_err_dist_speedM_1 = -1.0f / kp_err_dist_speed ;
2000 kp_ah_circle_radius2 = kp_ah_circle_radius * kp_ah_circle_radius ;
2001
2002
2003 //----- Reset entire KPAD
2004 kp = &inside_kpads[ WPAD_MAX_CONTROLLERS - 1 ] ;
2005 do {
2006 reset_kpad( kp ) ;
2007 } while ( --kp >= inside_kpads ) ;
2008 }
2009