/*---------------------------------------------------------------------------* Project: KPAD library File: KPAD.c Programmers: Keizo Ohta HIRATSU Daisuke Tojo Haruki Tetsuya Sasaki Copyright 2005-2006 Nintendo. All rights reserved. These coded instructions, statements, and computer programs contain proprietary information of Nintendo of America Inc. and/or Nintendo Company Ltd., and are protected by Federal copyright law. They may not be disclosed to third parties or copied or duplicated in any form, in whole or in part, without the prior written consent of Nintendo. *---------------------------------------------------------------------------*/ #include #include "KPADinside.h" #include #include #include REVOLUTION_LIB_VERSION(KPADOld); static BOOL is_valid_device( u32 type ); static void set_dpd_disable( s32 chan, u32 type ); static void set_dpd_enable ( s32 chan, u32 type ); static void control_dpd_start_( const s32 chan ); static void control_dpd_end_( const s32 chan ); /******************************************************* VARIABLE *******************************************************/ //----- DPD calibration default value static Vec2 icenter_org = { 0.000f, 0.000f } ; // Center coordinate of two marks in CMOS static f32 idist_org = 1.000f ; // Distance at calibration (in meters) static Vec2 iaccXY_nrm_hori = { 0.000f,-1.000f } ; // XY acceleration direction when the controller is placed horizontally static Vec2 isec_nrm_hori = { 1.000f, 0.000f } ; // Direction from left mark to right mark when the controller is placed horizontally f32 kp_obj_interval = 0.200f ; // Separation between marks at each extremes (in meters) //----- Various adjustments f32 kp_acc_horizon_pw = 0.050f ; // Calculating twist from acceleration f32 kp_ah_circle_radius = 0.070f ; // Stationary state determination radius f32 kp_ah_circle_pw = 0.060f ; // Stationary state determination tracking level u16 kp_ah_circle_ct = 100 ; // Stationary state determination count //----- Values determined to be an error. f32 kp_err_outside_frame = 0.050f ; // Surrounding area width where center of mass coordinate will be invalid (not all surrounding lights are necessarily shown) f32 kp_err_dist_min ; // Minimum operational distance automatic calculation (in meters) f32 kp_err_dist_max = 3.000f ; // Maximum operational distance (in meters) f32 kp_err_dist_speed = 0.040f ; // Accepted range of distance change (in meters) f32 kp_err_first_inpr = 0.900f ; // Internal product of acceleration tilt and object tilt when selecting two points for the first time f32 kp_err_next_inpr = 0.900f ; // Accepted range of tilt change (internal product value) f32 kp_err_acc_inpr = 0.900f ; // Accepted range of acceleration tilt and internal product value at stationary state f32 kp_err_up_inpr = 0.700f ; // Accepted internal product range with controller pointed upwards f32 kp_err_near_pos = 0.100f ; // Distance from the previous point when selecting one point as a continuation //----- For internal processing static Vec2 kobj_frame_min, kobj_frame_max ; // Range where the center of mass coordinate is valid static f32 kp_err_dist_speed_1 ; // Inverse static f32 kp_err_dist_speedM_1 ; // Inverse of negative value static f32 kp_ah_circle_radius2 ; // Second power static f32 kp_dist_vv1 ; // Constant static s32 kp_fs_fstick_min = 15 ; // Nunchuk unit stick clamp settings static s32 kp_fs_fstick_max = 71 ; static s32 kp_cl_stick_min = 60 ; // Classic Controller unit stick clamp settings static s32 kp_cl_stick_max = 308 ; static s32 kp_cl_trigger_min = 30 ; // Classic Controller unit analog trigger clamp settings static s32 kp_cl_trigger_max = 180 ; static s32 kp_gc_mstick_min = 15 ; // Old Nintendo GameCube 3D stick clamp settings static s32 kp_gc_mstick_max = 77 ; static s32 kp_gc_cstick_min = 15 ; // Old Nintendo GameCube C-stick clamp settings static s32 kp_gc_cstick_max = 64 ; static s32 kp_gc_trigger_min = 30 ; // Old Nintendo GameCube analog trigger clamp settings static s32 kp_gc_trigger_max = 180 ; static f32 kp_rm_acc_max = 3.4f ; // Wii Remote acceleration clamp settings static f32 kp_fs_acc_max = 2.1f ; // Nunchuk acceleration clamp settings //----- KPAD KPADInsideStatus inside_kpads[ WPAD_MAX_CONTROLLERS ] ; //----- Zero vector static Vec2 Vec2_0={0.0f, 0.0f}; static void KPADiControlDpdCallback( s32 chan, s32 result ); /******************************************************************************* Analog data clamp setting *******************************************************************************/ void KPADSetFSStickClamp( s8 min, s8 max ) { kp_fs_fstick_min = (s32)min; kp_fs_fstick_max = (s32)max; } /******************************************************************************* Obtain the ring buffer of WPADStatus *******************************************************************************/ WPADStatus *KPADGetWPADRingBuffer( s32 chan ) { ASSERT((0 <= chan) && (chan < WPAD_MAX_CONTROLLERS)); return ( (WPADStatus*)inside_kpads[ chan ].wpad_ring_bf ) ; } WPADFSStatus *KPADGetWPADFSRingBuffer( s32 chan ) { ASSERT((0 <= chan) && (chan < WPAD_MAX_CONTROLLERS)); return ( (WPADFSStatus*)inside_kpads[ chan ].wpad_ring_bf ) ; } WPADCLStatus *KPADGetWPADCLRingBuffer( s32 chan ) { ASSERT((0 <= chan) && (chan < WPAD_MAX_CONTROLLERS)); return ( (WPADCLStatus*)inside_kpads[ chan ].wpad_ring_bf ) ; } /******************************************************************************* Configure the repeat rate of the buttons *******************************************************************************/ void KPADSetBtnRepeat( s32 chan, f32 delay_sec, f32 pulse_sec ) { ASSERT((0 <= chan) && (chan < WPAD_MAX_CONTROLLERS)); if ( pulse_sec ) { //----- Set repeat flag setting inside_kpads[ chan ].btn_repeat_delay = (u16)(s32)( delay_sec * 200.0f + 0.5f ) ; inside_kpads[ chan ].btn_repeat_pulse = (u16)(s32)( pulse_sec * 200.0f + 0.5f ) ; inside_kpads[ chan ].btn_cl_repeat_delay = (u16)(s32)( delay_sec * 200.0f + 0.5f ) ; inside_kpads[ chan ].btn_cl_repeat_pulse = (u16)(s32)( pulse_sec * 200.0f + 0.5f ) ; } else { //----- No repeat flag setting inside_kpads[ chan ].btn_repeat_delay = KPAD_BTN_NO_RPT_DELAY ; inside_kpads[ chan ].btn_repeat_pulse = 0 ; inside_kpads[ chan ].btn_cl_repeat_delay = KPAD_BTN_NO_RPT_DELAY ; inside_kpads[ chan ].btn_cl_repeat_pulse = 0 ; } //----- Reset inside_kpads[ chan ].btn_repeat_time = 0 ; inside_kpads[ chan ].btn_repeat_next = inside_kpads[ chan ].btn_repeat_delay ; inside_kpads[ chan ].btn_cl_repeat_time = 0 ; inside_kpads[ chan ].btn_cl_repeat_next = inside_kpads[ chan ].btn_cl_repeat_delay ; } /******************************************************************************* Set marker placement interval (in meters) *******************************************************************************/ void KPADSetObjInterval( f32 interval ) { kp_obj_interval = interval ; //----- DPD operation minimum distance (so that the length between marks will be half that of lens diameter) kp_err_dist_min = kp_obj_interval / KPAD_CMOS_HFOV_TAN ; //----- Distance calculation constants kp_dist_vv1 = kp_obj_interval / KPAD_CMOS_HFOV_TAN ; } /******************************************************************************* Set parameters *******************************************************************************/ void KPADSetPosParam( s32 chan, f32 play_radius, f32 sensitivity ) { ASSERT((0 <= chan) && (chan < WPAD_MAX_CONTROLLERS)); inside_kpads[ chan ].pos_play_radius = play_radius ; inside_kpads[ chan ].pos_sensitivity = sensitivity ; } void KPADSetHoriParam( s32 chan, f32 play_radius, f32 sensitivity ) { ASSERT((0 <= chan) && (chan < WPAD_MAX_CONTROLLERS)); inside_kpads[ chan ].hori_play_radius = play_radius ; inside_kpads[ chan ].hori_sensitivity = sensitivity ; } void KPADSetDistParam( s32 chan, f32 play_radius, f32 sensitivity ) { ASSERT((0 <= chan) && (chan < WPAD_MAX_CONTROLLERS)); inside_kpads[ chan ].dist_play_radius = play_radius ; inside_kpads[ chan ].dist_sensitivity = sensitivity ; } void KPADSetAccParam( s32 chan, f32 play_radius, f32 sensitivity ) { ASSERT((0 <= chan) && (chan < WPAD_MAX_CONTROLLERS)); inside_kpads[ chan ].acc_play_radius = play_radius ; inside_kpads[ chan ].acc_sensitivity = sensitivity ; } /******************************************************************************* Obtain an easy-to-use scale value from the calibrated center position *******************************************************************************/ static void calc_dpd2pos_scale( KPADInsideStatus *kp ) { BOOL old; f32 sx,sy ; old = OSDisableInterrupts(); //----- Movable distance of the controller in the vertical and horizontal direction sx = 1.0f ; // Horizontal sy = (f32)KPAD_DPD_RESO_WY / (f32)KPAD_DPD_RESO_WX ; // Vertical //----- Longest movable distance of the controller kp->dpd2pos_scale = sqrtf( sx * sx + sy * sy ) ;// Diagonal //----- Correct the horizontal movable distance if ( kp->center_org.x < 0.0f ) { sx += kp->center_org.x ; } else { sx -= kp->center_org.x ; } //----- Correct the vertical movable distance if ( kp->center_org.y < 0.0f ) { sy += kp->center_org.y ; } else { sy -= kp->center_org.y ; } //----- A scale that will cover the longest distance of the smaller of the range of movable distance. if ( sx < sy ) { kp->dpd2pos_scale /= sx ; } else { kp->dpd2pos_scale /= sy ; } OSRestoreInterrupts(old); } /******************************************************************************* Initialize the KPAD value *******************************************************************************/ static void reset_kpad( KPADInsideStatus *kp ) { KPADObject *op ; KPADStatus *sp = &kp->status ; KPADEXStatus *ep = &sp->ex_status ; BOOL old; //----- Clear button information sp->hold = sp->trig = sp->release = 0x00000000 ; kp->btn_repeat_time = 0 ; kp->btn_repeat_next = kp->btn_repeat_delay ; //----- Clear DPD information sp->dpd_valid_fg = 0 ; // Disabled kp->dpd_valid2_ct = 0 ; sp->pos = sp->vec = Vec2_0 ; sp->speed = 0.0f ; sp->horizon.x = kp->acc_horizon.x = kp->obj_horizon.x = 1.0f ; sp->horizon.y = kp->acc_horizon.y = kp->obj_horizon.y = 0.0f ; sp->hori_vec = Vec2_0 ; sp->hori_speed = 0.0f ; sp->acc_vertical.x = 1.0f ; sp->acc_vertical.y = 0.0f ; sp->dist = kp->dist_org ; sp->dist_vec = sp->dist_speed = 0.0f ; kp->sec_dist = sp->dist ; kp->sec_length = kp->trust_sec_length = kp_dist_vv1 / kp->sec_dist ; kp->sec_nrm = kp->sec_nrm_hori ; //----- Clear acceleration information sp->acc.x = sp->acc.z = 0.0f ; sp->acc.y = -1.0f ; sp->acc_value = 1.0f ; sp->acc_speed = 0.0f ; kp->hard_acc = sp->acc ; kp->ah_circle_pos = kp->acc_horizon ; kp->ah_circle_ct = kp_ah_circle_ct ; //----- Clear individual object information kp->valid_objs = 0 ; op = &kp->kobj_sample[ WPAD_DPD_MAX_OBJECTS - 1 ] ; do { op->error_fg = -1 ; // Not displayed } while ( --op >= kp->kobj_sample ) ; op = &kp->kobj_regular[ KPAD_USE_OBJECTS - 1 ] ; do { op->error_fg = -1 ; // Not displayed } while ( --op >= kp->kobj_regular ) ; //----- Other kp->wpad_ring_idx = -1 ; // Ring buffer unprocessed kp->work_ct = 0 ; // Number of DPD processes in a single game frame //sp->wpad_err = WPAD_ERR_NONE ; //No errors for now old = OSDisableInterrupts(); if ( kp->wpad_chan_no < 0 ) { sp->wpad_err = (s8)WPADProbe( kp->wpad_chan_no + WPAD_MAX_CONTROLLERS, NULL ) ; WPADStopMotor( kp->wpad_chan_no + WPAD_MAX_CONTROLLERS ) ; if ( sp->wpad_err != WPAD_ERR_NO_CONTROLLER ) { kp->wpad_chan_no += WPAD_MAX_CONTROLLERS; } } else { sp->wpad_err = (s8)WPADProbe( kp->wpad_chan_no, NULL ) ; WPADStopMotor( kp->wpad_chan_no ) ; } OSRestoreInterrupts(old); //----- Clear extension controller information switch( sp->dev_type ) { case WPAD_DEV_FREESTYLE: //----- Nunchuk unit ep->fs.stick = Vec2_0 ; ep->fs.acc.x = ep->fs.acc.z = 0.0f ; ep->fs.acc.y = -1.0f ; ep->fs.acc_value = 1.0f ; ep->fs.acc_speed = 0.0f ; break ; case WPAD_DEV_CLASSIC: //----- Classic Controller unit ep->cl.lstick = Vec2_0; ep->cl.rstick = Vec2_0; ep->cl.ltrigger = ep->cl.rtrigger = 0.0f; ep->cl.hold = ep->cl.trig = ep->cl.release = 0x00000000 ; kp->btn_cl_repeat_time = 0 ; kp->btn_cl_repeat_next = kp->btn_cl_repeat_delay ; default: break ; } } /******************************************************************************* Convert the DPD coordinate to projection coordinate system *******************************************************************************/ void KPADGetProjectionPos( Vec2 *dst, const Vec2 *src, const Rect *projRect, f32 viRatio ) { f32 projection_height = projRect->bottom - projRect->top; // Convert the normalized values into projection coordinates (*dst).x = src->x * (projection_height / 2.0f) * 1.2f; (*dst).y = src->y * (projection_height / 2.0f) * 1.2f; // Horizontal direction pixel ratio correction (*dst).x *= viRatio * 0.908; } /******************************************************************************* Calibration *******************************************************************************/ s32 KPADCalibrateDPD( s32 chan ) { KPADInsideStatus *kp = &inside_kpads[ chan ] ; KPADStatus *sp = &kp->status ; KPADObject *op1, *op2 ; f32 f1, vx,vy ; ASSERT((0 <= chan) && (chan < WPAD_MAX_CONTROLLERS)); if ( kp->valid_objs != KPAD_USE_OBJECTS ) return ( kp->valid_objs ) ; /*********************************************************************** Acceleration after calibration ***********************************************************************/ vx = kp->hard_acc.x ; vy = kp->hard_acc.y ; f1 = sqrtf( vx * vx + vy * vy ) ; if ( f1 <= 0.5f ) return (-1) ; // Abnormal acceleration kp->accXY_nrm_hori.x = vx / f1 ; kp->accXY_nrm_hori.y = vy / f1 ; /*********************************************************************** Object location after calibration ***********************************************************************/ //----- Determine the mark order by location op1 = kp->kobj_sample ; while ( op1->error_fg != 0 ) ++op1 ; op2 = op1 + 1 ; while ( op2->error_fg != 0 ) ++op2 ; if ( op1->center.x < op2->center.x ) goto LABEL_cp12 ; else if ( op2->center.x < op1->center.x ) goto LABEL_cp21 ; else if ( op2->center.y < op1->center.y ) goto LABEL_cp21 ; LABEL_cp12: kp->kobj_regular[0] = *op1 ; kp->kobj_regular[1] = *op2 ; goto LABEL_cpend ; LABEL_cp21: kp->kobj_regular[0] = *op2 ; kp->kobj_regular[1] = *op1 ; LABEL_cpend: //kp->center_org.x = ( kp->kobj_regular[0].center.x + kp->kobj_regular[1].center.x ) * 0.5f ; //kp->center_org.y = ( kp->kobj_regular[0].center.y + kp->kobj_regular[1].center.y ) * 0.5f ; //kp->center_org.x = kp->center_org.y = 0.0f ; calc_dpd2pos_scale( kp ) ; //----- Section direction when the controller is in horizontal position vx = kp->kobj_regular[KPAD_USE_OBJECTS-1].center.x - kp->kobj_regular[0].center.x ; vy = kp->kobj_regular[KPAD_USE_OBJECTS-1].center.y - kp->kobj_regular[0].center.y ; f1 = 1.0f / sqrtf( vx * vx + vy * vy ) ; // Should not be zero kp->sec_nrm_hori.x = vx * f1 ; kp->sec_nrm_hori.y = vy * f1 ; /*********************************************************************** Distance at calibration ***********************************************************************/ kp->dist_org = kp_dist_vv1 * f1 ; /*********************************************************************** Other ***********************************************************************/ sp->dpd_valid_fg = 0 ; // Invalid temporarily return ( kp->valid_objs ) ; } /******************************************************************************* Enable Wii Remote direction correction *******************************************************************************/ void KPADEnableAimingMode( s32 chan ) { ASSERT((0 <= chan) && (chan < WPAD_MAX_CONTROLLERS)); if ( WPAD_SENSOR_BAR_POS_TOP == WPADGetSensorBarPosition() ) { KPADSetSensorHeight( chan, 0.2f ); } else { KPADSetSensorHeight( chan, -0.2f ); } } /******************************************************************************* Disable Wii Remote direction correction *******************************************************************************/ void KPADDisableAimingMode( s32 chan ) { ASSERT((0 <= chan) && (chan < WPAD_MAX_CONTROLLERS)); KPADSetSensorHeight( chan, 0.0f ); } /******************************************************************************* Calibration *******************************************************************************/ void KPADSetSensorHeight( s32 chan, f32 level ) { KPADInsideStatus *kp = &inside_kpads[ chan ] ; kp->center_org.x = 0.0f ; kp->center_org.y = -level ; calc_dpd2pos_scale( kp ) ; } /******************************************************************************* Digital button repeat process *******************************************************************************/ static void calc_button_repeat( KPADInsideStatus *kp, KPADStatus *sp, u32 dev_type ) { if ( dev_type == WPAD_DEV_CLASSIC ) { if ( sp->ex_status.cl.trig != 0 || sp->ex_status.cl.release != 0 ) { //----- Reset because the button state changed kp->btn_cl_repeat_time = 0 ; kp->btn_cl_repeat_next = kp->btn_cl_repeat_delay ; //----- Set flags at the beginning of push (only when repeat is set) if ( sp->ex_status.cl.trig && kp->btn_cl_repeat_pulse ) { sp->ex_status.cl.hold |= KPAD_BUTTON_RPT ; } } else if ( sp->ex_status.cl.hold != 0 ) { //----- Forward time because the button is pushed and the state is not changing kp->btn_cl_repeat_time += kp->work_ct ; if ( kp->btn_cl_repeat_time >= KPAD_BTN_NO_RPT_DELAY ) { kp->btn_cl_repeat_time -= KPAD_BTN_NO_RPT_DELAY ; } //----- Set flag at the repeat time if ( kp->btn_cl_repeat_time >= kp->btn_cl_repeat_next ) { sp->ex_status.cl.hold |= KPAD_BUTTON_RPT ; //----- Configure the next repeat time kp->btn_cl_repeat_next += kp->btn_cl_repeat_pulse ; //----- If the time has exceeded the range here, make it loop if ( kp->btn_cl_repeat_time >= KPAD_BTN_RPT_TIME_MAX ) { kp->btn_cl_repeat_time -= KPAD_BTN_RPT_TIME_MAX ; kp->btn_cl_repeat_next -= KPAD_BTN_RPT_TIME_MAX ; } } } } else { if ( sp->trig != 0 || sp->release != 0 ) { //----- Reset because the button state changed kp->btn_repeat_time = 0 ; kp->btn_repeat_next = kp->btn_repeat_delay ; //----- Set flags at the beginning of push (only when repeat is set) if ( sp->trig && kp->btn_repeat_pulse ) { sp->hold |= KPAD_BUTTON_RPT ; } } else if ( sp->hold != 0 ) { //----- Forward time because the button is pushed and the state is not changing kp->btn_repeat_time += kp->work_ct ; if ( kp->btn_repeat_time >= KPAD_BTN_NO_RPT_DELAY ) { kp->btn_repeat_time -= KPAD_BTN_NO_RPT_DELAY ; } //----- Set flag at the repeat time if ( kp->btn_repeat_time >= kp->btn_repeat_next ) { sp->hold |= KPAD_BUTTON_RPT ; //----- Configure the next repeat time kp->btn_repeat_next += kp->btn_repeat_pulse ; //----- If the time has exceeded the range here, make it loop if ( kp->btn_repeat_time >= KPAD_BTN_RPT_TIME_MAX ) { kp->btn_repeat_time -= KPAD_BTN_RPT_TIME_MAX ; kp->btn_repeat_next -= KPAD_BTN_RPT_TIME_MAX ; } } } } } /******************************************************************************* KPAD button information load process *******************************************************************************/ static void read_kpad_button( KPADInsideStatus *kp, void *vp, u32 dev_type ) { KPADStatus *sp = &kp->status ; KPADEXStatus *ep = &kp->status.ex_status; u32 old_fg, change_fg ; u32 cl_old_fg, cl_change_fg; WPADStatus *wp ; WPADFSStatus *fp ; WPADCLStatus *cp ; if ( kp->wpad_chan_no < 0 ) return ; //----- Store the previous value old_fg = sp->hold & KPAD_BUTTON_MASK ; if ( dev_type == WPAD_DEV_CLASSIC ) { cl_old_fg = ep->cl.hold & KPAD_BUTTON_MASK; } //----- Load data if ( dev_type == WPAD_DEV_FREESTYLE ) { fp = (WPADFSStatus*)vp ; if ( fp->err != WPAD_ERR_NONE ) { //----- Disable port if there is no controller if ( fp->err == WPAD_ERR_NO_CONTROLLER ) { kp->wpad_chan_no -= WPAD_MAX_CONTROLLERS ; reset_kpad( kp ) ; return ; } //----- Inherit the previous states for other errors } else { //----- Load new value sp->hold = (u32)fp->button ; } } else if ( dev_type == WPAD_DEV_CLASSIC ) { cp = (WPADCLStatus*)vp; if ( cp->err != WPAD_ERR_NONE ) { //----- Disable port if there is no controller if ( cp->err == WPAD_ERR_NO_CONTROLLER ) { kp->wpad_chan_no -= WPAD_MAX_CONTROLLERS ; reset_kpad( kp ); return ; } //----- Inherit the previous states for other errors } else { //----- Load new value sp->hold = (u32)cp->button ; ep->cl.hold = (u32)cp->clButton; } } else { wp = (WPADStatus*)vp ; if ( wp->err != WPAD_ERR_NONE ) { //----- Disable port if there is no controller if ( wp->err == WPAD_ERR_NO_CONTROLLER ) { kp->wpad_chan_no -= WPAD_MAX_CONTROLLERS ; reset_kpad( kp ) ; return ; } //----- Inherit the previous states for other errors } else { //----- Load new value sp->hold = (u32)wp->button ; } } //----- Button state process change_fg = sp->hold ^ old_fg ; // Changed button sp->trig = change_fg & sp->hold ; // Pushed button sp->release = change_fg & old_fg ; // Released button //----- Repeat processing calc_button_repeat( kp, sp, WPAD_DEV_CORE ) ; if ( dev_type == WPAD_DEV_CLASSIC ) { cl_change_fg = ep->cl.hold ^ cl_old_fg ; ep->cl.trig = cl_change_fg & ep->cl.hold ; ep->cl.release = cl_change_fg & cl_old_fg ; calc_button_repeat( kp, sp, WPAD_DEV_CLASSIC ); } } /******************************************************************************* Acceleration tracking process *******************************************************************************/ static void calc_acc( KPADInsideStatus *kp, f32 *acc, f32 acc2 ) { f32 f1,f2 ; //----- Difference to the target f2 = acc2 - *acc ; if ( f2 < 0.0f ) { f1 = -f2 ; } else { f1 = f2 ; } //----- Tracking rate inside/outside the tolerance if ( f1 >= kp->acc_play_radius ) { //----- Apply 100% tracking if outside tolerance f1 = 1.0f ; } else { //----- If inside tolerance, weaken tracking as target gets closer f1 /= kp->acc_play_radius ; f1 *= f1 ; // Second power f1 *= f1 ; // Fourth power } f1 *= kp->acc_sensitivity ; //----- Tracking *acc += f1 * f2 ; } /******************************************************************************* Calculate controller tilt from acceleration *******************************************************************************/ static void calc_acc_horizon( KPADInsideStatus *kp ) { f32 f1, vx,vy, ax,ay ; //----- xy acceleration normalization f1 = sqrtf( kp->hard_acc.x * kp->hard_acc.x + kp->hard_acc.y * kp->hard_acc.y ) ; if ( f1 == 0.0f || f1 >= 2.0f ) return ; ax = kp->hard_acc.x / f1 ; ay = kp->hard_acc.y / f1 ; //----- There will be more power the closer the xy acceleration length is to one (1). if ( f1 > 1.0f ) { f1 = 2.0f - f1 ; } f1 *= f1 * kp_acc_horizon_pw ; //----- Target tilt vx = kp->accXY_nrm_hori.x * ax + kp->accXY_nrm_hori.y * ay ; vy = kp->accXY_nrm_hori.y * ax - kp->accXY_nrm_hori.x * ay ; //----- Set closer ax = ( vx - kp->acc_horizon.x ) * f1 + kp->acc_horizon.x ; ay = ( vy - kp->acc_horizon.y ) * f1 + kp->acc_horizon.y ; //----- Normalization f1 = sqrtf( ax * ax + ay * ay ) ; if ( f1 == 0.0f ) return ; kp->acc_horizon.x = ax / f1 ; kp->acc_horizon.y = ay / f1 ; //----- Update stationary state determination coordinate kp->ah_circle_pos.x += ( kp->acc_horizon.x - kp->ah_circle_pos.x ) * kp_ah_circle_pw ; kp->ah_circle_pos.y += ( kp->acc_horizon.y - kp->ah_circle_pos.y ) * kp_ah_circle_pw ; vx = kp->acc_horizon.x - kp->ah_circle_pos.x ; vy = kp->acc_horizon.y - kp->ah_circle_pos.y ; if ( vx*vx + vy*vy <= kp_ah_circle_radius2 ) { if ( kp->ah_circle_ct ) -- kp->ah_circle_ct ; } else { kp->ah_circle_ct = kp_ah_circle_ct ; } } static void calc_acc_vertical( KPADInsideStatus *kp ) { KPADStatus *sp = &kp->status ; f32 f1,f2, ax,ay ; //----- Target tilt ax = sqrtf( f2 = kp->hard_acc.x * kp->hard_acc.x + kp->hard_acc.y * kp->hard_acc.y ) ; ay = - kp->hard_acc.z ; f1 = sqrtf( f2 + ay * ay ) ; if ( f1 == 0.0f || f1 >= 2.0f ) return ; ax /= f1 ; ay /= f1 ; //----- There will be more power the closer the acceleration length is to one (1). if ( f1 > 1.0f ) { f1 = 2.0f - f1 ; } f1 *= f1 * kp_acc_horizon_pw ; //----- Set closer ax = ( ax - sp->acc_vertical.x ) * f1 + sp->acc_vertical.x ; ay = ( ay - sp->acc_vertical.y ) * f1 + sp->acc_vertical.y ; //----- Normalization f1 = sqrtf( ax * ax + ay * ay ) ; if ( f1 == 0.0f ) return ; sp->acc_vertical.x = ax / f1 ; sp->acc_vertical.y = ay / f1 ; } /******************************************************************************* KPAD acceleration information load process *******************************************************************************/ static f32 clamp_acc( f32 acc, f32 clamp ) { if ( acc < 0.0f ) { if ( acc < -clamp ) return ( -clamp ) ; } else { if ( acc > clamp ) return ( clamp ) ; } return ( acc ) ; } static void read_kpad_acc( KPADInsideStatus *kp, void *vp, u32 dev_type, s32 chan ) { KPADStatus *sp = &kp->status ; Vec vec ; WPADStatus *wp ; WPADFSStatus *fp ; //u32 type ; s32 err; // The values listed here should not be too far off from the precise value obtained through WPADGetAccGravityUnit(). // f32 acc_scale_x = 1.0f/100; f32 acc_scale_y = 1.0f/100; f32 acc_scale_z = 1.0f/100; f32 fs_acc_scale_x = 1.0f/200; f32 fs_acc_scale_y = 1.0f/200; f32 fs_acc_scale_z = 1.0f/200; // @T - should not probe again, as state may have changed. //err = WPADProbe(chan, &type); if ( kp->wpad_chan_no < 0 ) return ; err = kp->status.wpad_err; if(err==WPAD_ERR_NONE) { // Obtain 1G value from the controller to absorb the individual unit differences. WPADAcc core1G = {1, 1, 1}; WPADGetAccGravityUnit( chan, WPAD_DEV_CORE, &core1G); if(core1G.x * core1G.y * core1G.z != 0){ // Bug fix acc_scale_x = 1.0f / core1G.x; acc_scale_y = 1.0f / core1G.y; acc_scale_z = 1.0f / core1G.z; } if ( dev_type == WPAD_DEV_FREESTYLE ) { WPADAcc fs1G = {1, 1, 1}; WPADGetAccGravityUnit( chan, WPAD_DEV_FREESTYLE, &fs1G); if(fs1G.x * fs1G.y * fs1G.z != 0){ // Bug fix fs_acc_scale_x = 1.0f / fs1G.x; fs_acc_scale_y = 1.0f / fs1G.y; fs_acc_scale_z = 1.0f / fs1G.z; } } } // @ moved to above //if ( kp->wpad_chan_no < 0 ) return ; //----- Load data if ( dev_type == WPAD_DEV_FREESTYLE ) { fp = (WPADFSStatus*)vp ; if ( err != WPAD_ERR_NONE ) { //----- Disable port if there is no controller if ( err == WPAD_ERR_NO_CONTROLLER ) { kp->wpad_chan_no -= WPAD_MAX_CONTROLLERS ; reset_kpad( kp ) ; return ; } } else { //----- Update raw value kp->hard_acc.x = clamp_acc( (f32)(s32)-fp->accX * acc_scale_x, kp_rm_acc_max ) ; kp->hard_acc.y = clamp_acc( (f32)(s32)-fp->accZ * acc_scale_z, kp_rm_acc_max ) ; kp->hard_acc.z = clamp_acc( (f32)(s32) fp->accY * acc_scale_y, kp_rm_acc_max ) ; } } else { wp = (WPADStatus*)vp ; if ( err != WPAD_ERR_NONE ) { //----- Disable port if there is no controller if ( err == WPAD_ERR_NO_CONTROLLER ) { kp->wpad_chan_no -= WPAD_MAX_CONTROLLERS ; reset_kpad( kp ) ; return ; } } else { //----- Update raw value kp->hard_acc.x = clamp_acc( (f32)(s32)-wp->accX * acc_scale_x, kp_rm_acc_max ) ; kp->hard_acc.y = clamp_acc( (f32)(s32)-wp->accZ * acc_scale_z, kp_rm_acc_max ) ; kp->hard_acc.z = clamp_acc( (f32)(s32) wp->accY * acc_scale_y, kp_rm_acc_max ) ; } } //----- Temporary save vec = sp->acc ; //----- Acceleration tracking process for the application calc_acc( kp, &sp->acc.x, kp->hard_acc.x ) ; calc_acc( kp, &sp->acc.y, kp->hard_acc.y ) ; calc_acc( kp, &sp->acc.z, kp->hard_acc.z ) ; sp->acc_value = sqrtf( sp->acc.x * sp->acc.x + sp->acc.y * sp->acc.y + sp->acc.z * sp->acc.z ) ; //----- Acceleration change for the application vec.x -= sp->acc.x ; vec.y -= sp->acc.y ; vec.z -= sp->acc.z ; sp->acc_speed = sqrtf( vec.x * vec.x + vec.y * vec.y + vec.z * vec.z ) ; //----- Calculate controller tilt from the raw acceleration calc_acc_horizon( kp ) ; calc_acc_vertical( kp ) ; /*********************************************************************** Load Nunchuk unit acceleration ***********************************************************************/ if ( dev_type != WPAD_DEV_FREESTYLE ) return ; //----- Temporary save vec = sp->ex_status.fs.acc ; //----- Acceleration tracking process for the application calc_acc( kp, &sp->ex_status.fs.acc.x, clamp_acc( (f32)(s32)-fp->fsAccX * fs_acc_scale_x, kp_fs_acc_max ) ) ; calc_acc( kp, &sp->ex_status.fs.acc.y, clamp_acc( (f32)(s32)-fp->fsAccZ * fs_acc_scale_z, kp_fs_acc_max ) ) ; calc_acc( kp, &sp->ex_status.fs.acc.z, clamp_acc( (f32)(s32) fp->fsAccY * fs_acc_scale_y, kp_fs_acc_max ) ) ; 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 ) ; //----- Acceleration change for the application vec.x -= sp->ex_status.fs.acc.x ; vec.y -= sp->ex_status.fs.acc.y ; vec.z -= sp->ex_status.fs.acc.z ; sp->ex_status.fs.acc_speed = sqrtf( vec.x * vec.x + vec.y * vec.y + vec.z * vec.z ) ; } /******************************************************************************* Change the WPAD object to KPAD *******************************************************************************/ static void get_kobj( KPADInsideStatus *kp, void *vp, u32 dev_type ) { const f32 dpd_scale = 2.0f / (f32)KPAD_DPD_RESO_WX ; const f32 dpd_cx = (f32)( KPAD_DPD_RESO_WX - 1 ) / (f32)KPAD_DPD_RESO_WX ; const f32 dpd_cy = (f32)( KPAD_DPD_RESO_WY - 1 ) / (f32)KPAD_DPD_RESO_WX ; DPDObject *wobj_p ; KPADObject *kobj_p ; //----- Store if ( dev_type == WPAD_DEV_FREESTYLE ) { wobj_p = &((WPADFSStatus*)vp)->obj[ WPAD_DPD_MAX_OBJECTS - 1 ] ; } else if ( dev_type == WPAD_DEV_CLASSIC ) { wobj_p = &((WPADCLStatus*)vp)->obj[ WPAD_DPD_MAX_OBJECTS - 1 ] ; } else { wobj_p = &((WPADStatus*)vp)->obj[ WPAD_DPD_MAX_OBJECTS - 1 ] ; } kobj_p = &kp->kobj_sample[ WPAD_DPD_MAX_OBJECTS - 1 ] ; do { if ( wobj_p->size ) { //----- Valid object kobj_p->center.x = (f32)(s32)wobj_p->x * dpd_scale - dpd_cx ; kobj_p->center.y = (f32)(s32)wobj_p->y * dpd_scale - dpd_cy ; kobj_p->error_fg = 0 ; // Displayed kobj_p->state_fg = 0 ; // Normal } else { //----- Invalid object kobj_p->error_fg = -1 ; // Not displayed } -- wobj_p ; } while ( --kobj_p >= kp->kobj_sample ) ; } /******************************************************************************* Set surrounding objects to invalid *******************************************************************************/ static void check_kobj_outside_frame( KPADObject *kobj_t ) { KPADObject *kobj_p = &kobj_t[ WPAD_DPD_MAX_OBJECTS - 1 ] ; do { if ( kobj_p->error_fg < 0 ) continue ; if ( kobj_p->center.x <= kobj_frame_min.x || kobj_p->center.x >= kobj_frame_max.x || kobj_p->center.y <= kobj_frame_min.y || kobj_p->center.y >= kobj_frame_max.y ) { kobj_p->error_fg |= 1 ; } } while ( --kobj_p >= kobj_t ) ; } /******************************************************************************* Set objects at the same coordinate to invalid *******************************************************************************/ static void check_kobj_same_position( KPADObject *kobj_t ) { KPADObject *op1, *op2 ; op1 = kobj_t ; do { if ( op1->error_fg != 0 ) continue ; op2 = op1 + 1 ; do { if ( op2->error_fg != 0 ) continue ; if ( op1->center.x == op2->center.x && op1->center.y == op2->center.y ) { op2->error_fg |= 2 ; // Set just one as error } } while ( ++op2 <= &kobj_t[ WPAD_DPD_MAX_OBJECTS - 1 ] ) ; } while ( ++op1 < &kobj_t[ WPAD_DPD_MAX_OBJECTS - 1 ] ) ; } /******************************************************************************* Calculate controller tilt from two points (return the distance from TV) *******************************************************************************/ static f32 calc_horizon( KPADInsideStatus *kp, Vec2 *p1, Vec2 *p2, Vec2 *hori ) { f32 f1, vx,vy ; vx = p2->x - p1->x ; vy = p2->y - p1->y ; f1 = 1.0f / sqrtf( vx * vx + vy * vy ) ; // Should not be zero vx *= f1 ; vy *= f1 ; hori->x = kp->sec_nrm_hori.x * vx + kp->sec_nrm_hori.y * vy ; hori->y = kp->sec_nrm_hori.y * vx - kp->sec_nrm_hori.x * vy ; return ( kp_dist_vv1 * f1 ) ; } /******************************************************************************* Select two marks for the first time *******************************************************************************/ static s8 select_2obj_first( KPADInsideStatus *kp ) { KPADObject *op1,*op2, *rp1,*rp2 ; Vec2 hori ; f32 f1, max = kp_err_first_inpr ; op1 = kp->kobj_sample ; do { if ( op1->error_fg != 0 ) continue ; op2 = op1 + 1 ; do { if ( op2->error_fg != 0 ) continue ; f1 = calc_horizon( kp, &op1->center, &op2->center, &hori ) ; //----- Control distance range check if ( f1 <= kp_err_dist_min || f1 >= kp_err_dist_max ) continue ; f1 = kp->acc_horizon.x * hori.x + kp->acc_horizon.y * hori.y ; if ( f1 < 0.0f ) { if ( -f1 > max ) { max = -f1 ; rp1 = op2 ; rp2 = op1 ; } } else { if ( f1 > max ) { max = f1 ; rp1 = op1 ; rp2 = op2 ; } } } while ( ++op2 <= &kp->kobj_sample[ WPAD_DPD_MAX_OBJECTS - 1 ] ) ; } while ( ++op1 < &kp->kobj_sample[ WPAD_DPD_MAX_OBJECTS - 1 ] ) ; //----- Confirmed regular mark? if ( max == kp_err_first_inpr ) return (0) ; kp->kobj_regular[0] = *rp1 ; kp->kobj_regular[1] = *rp2 ; return (2) ; // Two points recognition } /******************************************************************************* Select two marks using only the interval information subsequently *******************************************************************************/ static s8 select_2obj_continue( KPADInsideStatus *kp ) { KPADObject *op1,*op2, *rp1,*rp2 ; Vec2 nrm ; s32 rev_fg ; f32 f1,f2, vx,vy, min = 2.0f ; //----- Find two points closest to the last tilt and distance op1 = kp->kobj_sample ; do { if ( op1->error_fg != 0 ) continue ; op2 = op1 + 1 ; do { if ( op2->error_fg != 0 ) continue ; //----- Direction calculation vx = op2->center.x - op1->center.x ; vy = op2->center.y - op1->center.y ; f1 = 1.0f / sqrtf( vx*vx + vy*vy ) ; // Should not be zero nrm.x = vx * f1 ; nrm.y = vy * f1 ; //----- Control distance range check f1 *= kp_dist_vv1 ; // Distance if ( f1 <= kp_err_dist_min || f1 >= kp_err_dist_max ) continue ; //----- Distance change check f1 -= kp->sec_dist ; if ( f1 < 0.0f ) { f1 *= kp_err_dist_speedM_1 ; } else { f1 *= kp_err_dist_speed_1 ; } if ( f1 >= 1.0f ) continue ; // Distance error rate //----- Tilt change check f2 = kp->sec_nrm.x * nrm.x + kp->sec_nrm.y * nrm.y ; if ( f2 < 0.0f ) { f2 = -f2 ; rev_fg = 1 ; // Handle with direction inverted (op2 -> op1) } else { rev_fg = 0 ; // Handle as is (op1 -> op2) } if ( f2 <= kp_err_next_inpr ) continue ; f2 = ( 1.0f - f2 ) / ( 1.0f - kp_err_next_inpr ) ; // Tilt error rate //----- Record the object with minimum error f1 += f2 ; // Determine through the sum of distance error rate and tilt error rate if ( f1 < min ) { min = f1 ; if ( rev_fg ) { rp1 = op2 ; rp2 = op1 ; } else { rp1 = op1 ; rp2 = op2 ; } } } while ( ++op2 <= &kp->kobj_sample[ WPAD_DPD_MAX_OBJECTS - 1 ] ) ; } while ( ++op1 < &kp->kobj_sample[ WPAD_DPD_MAX_OBJECTS - 1 ] ) ; //----- Confirmed regular mark? if ( min == 2.0f ) return (0) ; kp->kobj_regular[0] = *rp1 ; kp->kobj_regular[1] = *rp2 ; return (2) ; // Two points recognition } /******************************************************************************* Select one mark for the first time *******************************************************************************/ static s8 select_1obj_first( KPADInsideStatus *kp ) { KPADObject *op1 ; f32 vx,vy ; Vec2 p1,p2 ; //----- Determine the section direction vx = kp->sec_nrm_hori.x * kp->acc_horizon.x + kp->sec_nrm_hori.y * kp->acc_horizon.y ; vy = kp->sec_nrm_hori.y * kp->acc_horizon.x - kp->sec_nrm_hori.x * kp->acc_horizon.y ; //----- Determine the section vector vx *= kp->trust_sec_length ; vy *= kp->trust_sec_length ; //----- Search for a point where the expected point is outside op1 = kp->kobj_sample ; do { if ( op1->error_fg != 0 ) continue ; p1.x = op1->center.x - vx ; // Expected point to the left p1.y = op1->center.y - vy ; p2.x = op1->center.x + vx ; // Expected point to the right p2.y = op1->center.y + vy ; if ( p1.x <= kobj_frame_min.x || p1.x >= kobj_frame_max.x || p1.y <= kobj_frame_min.y || p1.y >= kobj_frame_max.y ) { //----- If the left-hand expected point is outside, the right-hand expected point needs to be inside if ( p2.x > kobj_frame_min.x && p2.x < kobj_frame_max.x && p2.y > kobj_frame_min.y && p2.y < kobj_frame_max.y ) { //----- op1 may be right mark kp->kobj_regular[1] = *op1 ; kp->kobj_regular[0].center = p1 ; kp->kobj_regular[0].error_fg = 0 ; kp->kobj_regular[0].state_fg = -1 ; return (-1) ; // Found one point of worry } } else { //----- If the left-hand expected point is inside, the right-hand expected point needs to be outside. if ( p2.x <= kobj_frame_min.x || p2.x >= kobj_frame_max.x || p2.y <= kobj_frame_min.y || p2.y >= kobj_frame_max.y ) { //----- op1 may be left mark kp->kobj_regular[0] = *op1 ; kp->kobj_regular[1].center = p2 ; kp->kobj_regular[1].error_fg = 0 ; kp->kobj_regular[1].state_fg = -1 ; return (-1) ; // Found one point of worry } } } while ( ++op1 < &kp->kobj_sample[ WPAD_DPD_MAX_OBJECTS ] ) ; return (0) ; } /******************************************************************************* Select one mark subsequently *******************************************************************************/ static s8 select_1obj_continue( KPADInsideStatus *kp ) { KPADObject *op1,*op2, *rp1,*rp2 ; f32 f1, vx,vy ; f32 min = kp_err_near_pos * kp_err_near_pos ; //----- Select a mark closest to the past regular mark op1 = kp->kobj_regular ; do { if ( op1->error_fg != 0 ) continue ; if ( op1->state_fg != 0 ) continue ; // No expected points op2 = kp->kobj_sample ; do { if ( op2->error_fg != 0 ) continue ; vx = op1->center.x - op2->center.x ; vy = op1->center.y - op2->center.y ; f1 = vx * vx + vy * vy ; if ( f1 < min ) { min = f1 ; rp1 = op1 ; rp2 = op2 ; } } while ( ++op2 < &kp->kobj_sample[ WPAD_DPD_MAX_OBJECTS ] ) ; } while ( ++op1 < &kp->kobj_regular[ KPAD_USE_OBJECTS ] ) ; //----- Confirmed regular mark? if ( min == kp_err_near_pos * kp_err_near_pos ) return (0) ; *rp1 = *rp2 ; //----- Calculate tilt from the acceleration kp->sec_nrm.x = kp->sec_nrm_hori.x * kp->acc_horizon.x + kp->sec_nrm_hori.y * kp->acc_horizon.y ; kp->sec_nrm.y = kp->sec_nrm_hori.y * kp->acc_horizon.x - kp->sec_nrm_hori.x * kp->acc_horizon.y ; //----- Calculate expected point coordinate vx = kp->sec_length * kp->sec_nrm.x ; vy = kp->sec_length * kp->sec_nrm.y ; if ( rp1 == &kp->kobj_regular[0] ) { kp->kobj_regular[1].center.x = rp1->center.x + vx ; kp->kobj_regular[1].center.y = rp1->center.y + vy ; kp->kobj_regular[1].error_fg = 0 ; kp->kobj_regular[1].state_fg = -1 ; } else { kp->kobj_regular[0].center.x = rp1->center.x - vx ; kp->kobj_regular[0].center.y = rp1->center.y - vy ; kp->kobj_regular[0].error_fg = 0 ; kp->kobj_regular[0].state_fg = -1 ; } if ( kp->status.dpd_valid_fg < 0 ) { return (-1) ; // Found one point of worry } else { return (1) ; // One point recognition } } /******************************************************************************* Calculate controller tilt from object *******************************************************************************/ static void calc_obj_horizon( KPADInsideStatus *kp ) { f32 f1, vx,vy ; vx = kp->kobj_regular[1].center.x - kp->kobj_regular[0].center.x ; vy = kp->kobj_regular[1].center.y - kp->kobj_regular[0].center.y ; kp->sec_length = sqrtf( vx * vx + vy * vy ) ; // Should not be zero f1 = 1.0f / kp->sec_length ; kp->sec_dist = kp_dist_vv1 * f1 ; kp->sec_nrm.x = ( vx *= f1 ) ; kp->sec_nrm.y = ( vy *= f1 ) ; kp->obj_horizon.x = kp->sec_nrm_hori.x * vx + kp->sec_nrm_hori.y * vy ; kp->obj_horizon.y = kp->sec_nrm_hori.y * vx - kp->sec_nrm_hori.x * vy ; } /******************************************************************************* Update application variables *******************************************************************************/ static void calc_dpd_variable( KPADInsideStatus *kp, s8 valid_fg_next ) { KPADStatus *sp = &kp->status ; f32 f1,f2, dist ; Vec2 pos, vec ; if ( valid_fg_next == 0 ) { sp->dpd_valid_fg = 0 ; return ; } /*********************************************************************** Calculate controller tilt ***********************************************************************/ //----- Calculate the target value pos.x = kp->sec_nrm_hori.x * kp->sec_nrm.x + kp->sec_nrm_hori.y * kp->sec_nrm.y ; pos.y = kp->sec_nrm_hori.y * kp->sec_nrm.x - kp->sec_nrm_hori.x * kp->sec_nrm.y ; //----- Consider the tracking and tolerance for the target value if ( sp->dpd_valid_fg == 0 ) { //----- Because this is the first pointing, initialize with the given value sp->horizon = pos ; sp->hori_vec = Vec2_0 ; sp->hori_speed = 0.0f ; } else { //----- Difference to the target vec.x = pos.x - sp->horizon.x ; vec.y = pos.y - sp->horizon.y ; f1 = sqrtf( vec.x * vec.x + vec.y * vec.y ) ; //----- Tracking rate inside/outside the tolerance if ( f1 >= kp->hori_play_radius ) { //----- Apply 100% tracking if outside tolerance f1 = 1.0f ; } else { //----- If inside tolerance, weaken tracking as target gets closer f1 /= kp->hori_play_radius ; f1 *= f1 ; // Second power f1 *= f1 ; // Fourth power } f1 *= kp->hori_sensitivity ; //----- Tracking vec.x = f1 * vec.x + sp->horizon.x ; vec.y = f1 * vec.y + sp->horizon.y ; f1 = sqrtf( vec.x * vec.x + vec.y * vec.y ) ; // Normalize because this is tilt vec.x /= f1 ; vec.y /= f1 ; sp->hori_vec.x = vec.x - sp->horizon.x ; sp->hori_vec.y = vec.y - sp->horizon.y ; sp->hori_speed = sqrtf( sp->hori_vec.x * sp->hori_vec.x + sp->hori_vec.y * sp->hori_vec.y ) ; sp->horizon = vec ; } /*********************************************************************** Calculate the distance from the TV ***********************************************************************/ //----- Calculate the target value dist = kp_dist_vv1 / kp->sec_length ; //----- Consider the tracking and tolerance for the target value if ( sp->dpd_valid_fg == 0 ) { //----- Because this is the first pointing, initialize with the given value sp->dist = dist ; sp->dist_vec = 0.0f ; sp->dist_speed = 0.0f ; } else { //----- Difference to the target f2 = dist - sp->dist ; if ( f2 < 0.0f ) { f1 = -f2 ; } else { f1 = f2 ; } //----- Tracking rate inside/outside the tolerance if ( f1 >= kp->dist_play_radius ) { //----- Apply 100% tracking if outside tolerance f1 = 1.0f ; } else { //----- If inside tolerance, weaken tracking as target gets closer f1 /= kp->dist_play_radius ; f1 *= f1 ; // Second power f1 *= f1 ; // Fourth power } f1 *= kp->dist_sensitivity ; //----- Tracking sp->dist_vec = f1 * f2 ; if ( sp->dist_vec < 0.0f ) { sp->dist_speed = -sp->dist_vec ; } else { sp->dist_speed = sp->dist_vec ; } sp->dist += sp->dist_vec ; } /*********************************************************************** Calculate the pointing location ***********************************************************************/ //----- Center coordinate of two marks pos.x = ( kp->kobj_regular[0].center.x + kp->kobj_regular[1].center.x ) * 0.5f ; pos.y = ( kp->kobj_regular[0].center.y + kp->kobj_regular[1].center.y ) * 0.5f ; //----- Rotate for the amount of twist f1 = kp->sec_nrm.x * kp->sec_nrm_hori.x + kp->sec_nrm.y * kp->sec_nrm_hori.y ; f2 = -kp->sec_nrm.y * kp->sec_nrm_hori.x + kp->sec_nrm.x * kp->sec_nrm_hori.y ; vec.x = f1 * pos.x - f2 * pos.y ; vec.y = f2 * pos.x + f1 * pos.y ; //----- Apply scaling after correcting the center position vec.x = ( kp->center_org.x - vec.x ) * kp->dpd2pos_scale ; vec.y = ( kp->center_org.y - vec.y ) * kp->dpd2pos_scale ; //----- Convert to the gravitational direction coordinate system at calibration pos.x = -kp->accXY_nrm_hori.y * vec.x + kp->accXY_nrm_hori.x * vec.y ; pos.y = -kp->accXY_nrm_hori.x * vec.x - kp->accXY_nrm_hori.y * vec.y ; //----- Consider the tracking and tolerance for the target value if ( sp->dpd_valid_fg == 0 ) { //----- Because this is the first pointing, initialize with the given value sp->pos = pos ; sp->vec = Vec2_0 ; sp->speed = 0.0f ; } else { //----- Difference to the target vec.x = pos.x - sp->pos.x ; vec.y = pos.y - sp->pos.y ; f1 = sqrtf( vec.x * vec.x + vec.y * vec.y ) ; //----- Tracking rate inside/outside the tolerance if ( f1 >= kp->pos_play_radius ) { //----- Apply 100% tracking if outside tolerance f1 = 1.0f ; } else { //----- If inside tolerance, weaken tracking as target gets closer f1 /= kp->pos_play_radius ; f1 *= f1 ; // Second power f1 *= f1 ; // Fourth power } f1 *= kp->pos_sensitivity ; //----- Tracking sp->vec.x = f1 * vec.x ; sp->vec.y = f1 * vec.y ; sp->speed = sqrtf( sp->vec.x * sp->vec.x + sp->vec.y * sp->vec.y ) ; sp->pos.x += sp->vec.x ; sp->pos.y += sp->vec.y ; } /*********************************************************************** Update flags ***********************************************************************/ sp->dpd_valid_fg = valid_fg_next ; } /******************************************************************************* KPAD DPD information load process *******************************************************************************/ static void read_kpad_dpd( KPADInsideStatus *kp, void *vp, u32 dev_type ) { KPADStatus *sp = &kp->status ; KPADObject *op1 ; s8 valid_fg_next ; WPADStatus *wp ; WPADFSStatus *fp ; WPADCLStatus *cp ; s32 err ; //u32 type ; if ( kp->wpad_chan_no < 0 ) return ; //@T keep sync with kpad status //err = WPADProbe( kp->wpad_chan_no, &type ) ; err = kp->status.wpad_err; /*********************************************************************** Change the WPAD object to KPAD ***********************************************************************/ if ( dev_type == WPAD_DEV_FREESTYLE ) { fp = (WPADFSStatus*)vp ; if ( err != WPAD_ERR_NONE ) { //----- Disable port if there is no controller if ( err == WPAD_ERR_NO_CONTROLLER ) { kp->wpad_chan_no -= WPAD_MAX_CONTROLLERS ; reset_kpad( kp ) ; return ; } //----- Inherit the previous states for other errors } else { //----- Change the WPAD object to KPAD get_kobj( kp, fp, WPAD_DEV_FREESTYLE ) ; } } else if ( dev_type == WPAD_DEV_CLASSIC ) { cp = (WPADCLStatus*)vp ; if ( err != WPAD_ERR_NONE ) { //----- Disable port if there is no controller if ( err == WPAD_ERR_NO_CONTROLLER ) { kp->wpad_chan_no -= WPAD_MAX_CONTROLLERS ; reset_kpad( kp ) ; return ; } //----- Inherit the previous states for other errors } else { //----- Change the WPAD object to KPAD get_kobj( kp, cp, WPAD_DEV_CLASSIC ) ; } } else { wp = (WPADStatus*)vp ; if ( err != WPAD_ERR_NONE ) { //----- Disable port if there is no controller if ( err == WPAD_ERR_NO_CONTROLLER ) { kp->wpad_chan_no -= WPAD_MAX_CONTROLLERS ; reset_kpad( kp ) ; return ; } //----- Inherit the previous states for other errors } else { //----- Change the WPAD object to KPAD get_kobj( kp, wp, WPAD_DEV_CORE ) ; } } /*********************************************************************** Select the normal object ***********************************************************************/ //----- Remove invalid objects check_kobj_outside_frame( kp->kobj_sample ) ; // Set surrounding objects to invalid check_kobj_same_position( kp->kobj_sample ) ; // Set objects at the same coordinate to be invalid //----- Determine the number shown kp->valid_objs = 0 ; op1 = &kp->kobj_sample[ WPAD_DPD_MAX_OBJECTS - 1 ] ; do { if ( op1->error_fg == 0 ) ++ kp->valid_objs ; } while ( --op1 >= kp->kobj_sample ) ; //----- Recognition process if ( sp->acc_vertical.x <= kp_err_up_inpr ) goto LABEL_select_NG ; if ( sp->dpd_valid_fg == 2 || sp->dpd_valid_fg == -2 ) { //----- Recognized using two objects the last time if ( kp->valid_objs >= 2 ) { valid_fg_next = select_2obj_continue( kp ); if ( valid_fg_next ) goto LABEL_select_OK ; } if ( kp->valid_objs >= 1 ) { valid_fg_next = select_1obj_continue( kp ); if ( valid_fg_next ) goto LABEL_select_OK ; } } else if ( sp->dpd_valid_fg == 1 || sp->dpd_valid_fg == -1 ) { //----- Recognized using one object the last time if ( kp->valid_objs >= 2 ) { valid_fg_next = select_2obj_first( kp ); if ( valid_fg_next ) goto LABEL_select_OK ; } if ( kp->valid_objs >= 1 ) { valid_fg_next = select_1obj_continue( kp ); if ( valid_fg_next ) goto LABEL_select_OK ; } } else { //----- Not recognized the last time if ( kp->valid_objs >= 2 ) { valid_fg_next = select_2obj_first( kp ); if ( valid_fg_next ) goto LABEL_select_OK ; } if ( kp->valid_objs == 1 ) { valid_fg_next = select_1obj_first( kp ); if ( valid_fg_next ) goto LABEL_select_OK ; } } LABEL_select_NG: valid_fg_next = 0 ; // Not selected LABEL_select_OK: // Maybe selected //----- Update section information if successfully selected if ( valid_fg_next ) { //----- Calculate the information of two points and object tilt. calc_obj_horizon( kp ) ; //----- Error if obviously different from the acceleration tilt if ( kp->ah_circle_ct == 0 ) { if ( kp->obj_horizon.x * kp->acc_horizon.x + kp->obj_horizon.y * kp->acc_horizon.y <= kp_err_acc_inpr ) { valid_fg_next = 0 ; // Invalid after all kp->kobj_regular[0].error_fg = kp->kobj_regular[1].error_fg = 1 ; } } //----- Consecutive two point recognition count if ( sp->dpd_valid_fg == 2 && valid_fg_next == 2 ) { if ( kp->dpd_valid2_ct == 200 ) { kp->trust_sec_length = kp->sec_length ; } else { ++ kp->dpd_valid2_ct ; } } else { kp->dpd_valid2_ct = 0 ; } } else { kp->dpd_valid2_ct = 0 ; } //----- Update application variables calc_dpd_variable( kp, valid_fg_next ) ; } /******************************************************************************* Clamp processing of analog trigger *******************************************************************************/ static void clamp_trigger( f32 *trigger, s32 tr, s32 min, s32 max ) { if ( tr <= min ) { *trigger = 0.0f ; } else if ( tr >= max ) { *trigger = 1.0f ; } else { *trigger = (f32)( tr - min ) / (f32)( max - min ) ; } } /******************************************************************************* Clamp processing of stick *******************************************************************************/ static void clamp_stick( Vec2 *stick, s32 sx, s32 sy, s32 min, s32 max ) { f32 length ; f32 fx = (f32)sx ; f32 fy = (f32)sy ; f32 fmin = (f32)min ; f32 fmax = (f32)max ; length = sqrtf( fx * fx + fy * fy ) ; if ( length <= fmin ) { stick->x = stick->y = 0.0f ; } else if ( length >= fmax ) { stick->x = fx / length ; stick->y = fy / length ; } else { length = ( length - fmin ) / ( fmax - fmin ) / length ; stick->x = fx * length ; stick->y = fy * length ; } } /******************************************************************************* Load old Nintendo GameCube controller *******************************************************************************/ static void read_dolphin( s32 chan, KPADStatus *kstatus ) { PADStatus pstatus ; u32 old_fg, change_fg ; ASSERT((0 <= chan) && (chan < WPAD_MAX_CONTROLLERS)); WPADRead( chan, &pstatus ) ; if ( pstatus.err != PAD_ERR_NONE ) return ; //----- Analog data processing clamp_stick( &kstatus->ex_status.gc.stick, pstatus.stickX, pstatus.stickY, kp_gc_mstick_min, kp_gc_mstick_max ) ; clamp_stick( &kstatus->ex_status.gc.substick, pstatus.substickX, pstatus.substickY, kp_gc_cstick_min, kp_gc_cstick_max ) ; clamp_trigger( &kstatus->ex_status.gc.ltrigger, pstatus.triggerLeft, kp_gc_trigger_min, kp_gc_trigger_max ) ; clamp_trigger( &kstatus->ex_status.gc.rtrigger, pstatus.triggerRight, kp_gc_trigger_min, kp_gc_trigger_max ) ; //----- Button data processing old_fg = kstatus->hold & KPAD_BUTTON_MASK ; // Save previous value kstatus->hold = (u32)pstatus.button ; // Load new value change_fg = kstatus->hold ^ old_fg ; // Changed button kstatus->trig = change_fg & kstatus->hold ; // Pushed button kstatus->release = change_fg & old_fg ; // Released button //----- Repeat processing calc_button_repeat( &inside_kpads[ chan ], kstatus, 0 ) ; } /******************************************************************************* Load stick information *******************************************************************************/ static void read_kpad_stick( KPADInsideStatus *kp, void *vp ) { KPADStatus *sp = &kp->status ; WPADFSStatus *fp; WPADCLStatus *cp; s32 err; //u32 type ; //----- Error check if ( kp->wpad_chan_no < 0 ) return ; // @T // err = WPADProbe( kp->wpad_chan_no, &type ) ; err = kp->status.wpad_err; if ( sp->dev_type == WPAD_DEV_FREESTYLE ) { fp = (WPADFSStatus*)vp; if ( err != WPAD_ERR_NONE ) { //----- Disable port if there is no controller if ( err == WPAD_ERR_NO_CONTROLLER ) { kp->wpad_chan_no -= WPAD_MAX_CONTROLLERS ; reset_kpad( kp ) ; return ; } } //----- Stick data processing clamp_stick( &sp->ex_status.fs.stick, fp->fsStickX, fp->fsStickY, kp_fs_fstick_min, kp_fs_fstick_max ) ; } else if ( sp->dev_type == WPAD_DEV_CLASSIC ) { cp = (WPADCLStatus*)vp; if ( err != WPAD_ERR_NONE ) { //----- Disable port if there is no controller if ( err == WPAD_ERR_NO_CONTROLLER ) { kp->wpad_chan_no -= WPAD_MAX_CONTROLLERS ; reset_kpad( kp ) ; return ; } } //----- Stick data processing clamp_stick( &sp->ex_status.cl.lstick, cp->clLStickX, cp->clLStickY, kp_cl_stick_min, kp_cl_stick_max ) ; clamp_stick( &sp->ex_status.cl.rstick, cp->clRStickX, cp->clRStickY, kp_cl_stick_min, kp_cl_stick_max ) ; clamp_trigger( &sp->ex_status.cl.ltrigger, cp->clTriggerL, kp_cl_trigger_min, kp_cl_trigger_max ) ; clamp_trigger( &sp->ex_status.cl.rtrigger, cp->clTriggerR, kp_cl_trigger_min, kp_cl_trigger_max ) ; } } /******************************************************************************* Check DPD settings *******************************************************************************/ static s32 check_dpd_setting( s32 chan, u32 type ) { KPADInsideStatus *kp = &inside_kpads[ chan ] ; s32 ret; BOOL enabled = OSDisableInterrupts(); if ( kp->dpd_ctrl_busy ) // Currently setting device/changing DPD settings { reset_kpad( kp ); ret = 1; } else { //Check whether DPD ON/OFF is being requested BOOL dpd_enabled = WPADIsDpdEnabled( chan ); if ( kp->dpd_set_enabled && ! dpd_enabled ) { set_dpd_enable( chan, type ); reset_kpad( kp ); ret = 1; // The setting has been changed } else if ( !(kp->dpd_set_enabled) && dpd_enabled ) { set_dpd_disable( chan, type ); reset_kpad( kp ); ret = 1; // The setting has been changed } else if ( kp->dpd_ctrl_retry_flag ) // If a WPADControlDPD() retry is required { if ( kp->dpd_set_enabled ) { set_dpd_enable(chan, type); } else { set_dpd_disable(chan, type); } reset_kpad( kp ); ret = 1; // The setting has been changed } else if ( kp->dpd_request_flag ) // If DPD settings are not required and a complete callback is not being called { control_dpd_end_( chan ); ret = 0; } else { ret = 0; // No problem } } (void)OSRestoreInterrupts( enabled ); return ret; } /******************************************************************************* Device check (wpad_chan_no is changed) *******************************************************************************/ static s32 check_device( s32 chan, KPADInsideStatus *kp ) { u32 type ; s32 err ; ASSERT((0 <= chan) && (chan < WPAD_MAX_CONTROLLERS)); err = WPADProbe( chan, &type ); if ( err == WPAD_ERR_NO_CONTROLLER ) //----- There is nothing { if ( kp->wpad_chan_no >= 0 ) { //----- Stop auto-sampling WPADSetAutoSamplingBuf( chan, NULL, 0 ) ; kp->wpad_chan_no = (s16)( chan - WPAD_MAX_CONTROLLERS ); kp->status.dev_type = WPAD_DEV_NOT_FOUND ; control_dpd_end_( chan ); } return ( -1 ); } else if ( err != WPAD_ERR_NONE ) //----- Check not possible { return ( 0 ) ; // Treat as no problem for now } else if ( type == kp->status.dev_type ) //----- The same device as before { // Check DPD ON/OFF return check_dpd_setting( chan, type ); } else if ( is_valid_device(type) ) // The device changed { if ( kp->dpd_set_enabled ) { set_dpd_enable(chan, type); } else { set_dpd_disable(chan, type); } //----- Valid switching kp->wpad_chan_no = (s16)chan ; kp->status.dev_type = type ; reset_kpad( kp ) ; return ( 1 ) ; // The device is changed } else //Unexpected device { kp->wpad_chan_no = (s16)( chan - WPAD_MAX_CONTROLLERS ) ; return ( -1 ) ; } } /******************************************************************************* READ KPAD (return the number of loaded buffer) *******************************************************************************/ s32 KPADRead( s32 chan, KPADStatus samplingBufs[], u32 length ) { KPADInsideStatus *kp = &inside_kpads[ chan ] ; KPADStatus *sp ; WPADStatus *wp ; WPADFSStatus *fp ; WPADCLStatus *cp ; s32 idx, idx_old, idx_now ; s32 sample_ct, copy_ct ; ASSERT((0 <= chan) && (chan < WPAD_MAX_CONTROLLERS)); kp_err_dist_max = (f32)(1.0f + (f32)WPADGetDpdSensitivity()); /*************************************************************** Set device type ***************************************************************/ sp = &samplingBufs[ length - 1 ] ; do { sp->dev_type = kp->status.dev_type ; } while ( --sp >= samplingBufs ) ; /*************************************************************** Device check ***************************************************************/ if ( check_device( chan, kp ) ) { //----- Do not process if changed or unconnected. return ( 0 ) ; } /*************************************************************** Extension controller processing ***************************************************************/ // If an unknown attachment is found that may appear in the future, control as if only the core controller existed. // // The unknown attachment will just be ignored. if ( !is_valid_device( kp->status.dev_type ) ){ //----- Some kind of abnormal operation // ASSERT("Unknown device."); return ( 0 ) ; } /*************************************************************** Check the ring buffer that will be the current processing range ***************************************************************/ //----- idx_old: previously processed buffer -> idx_now: newest buffer if( kp->wpad_chan_no >= 0 ) { idx_now = (s32)WPADGetLatestIndexInBuf( kp->wpad_chan_no ) ; } else { idx_now = (s32)WPADGetLatestIndexInBuf( kp->wpad_chan_no + WPAD_MAX_CONTROLLERS ) ; } idx_old = kp->wpad_ring_idx ; if ( idx_old < 0 ) { // Processing start time idx_old = idx_now - 1 ; if ( idx_old < 0 ) idx_old = KPAD_RING_BUFS - 1 ; } kp->wpad_ring_idx = (s16)idx_now ; //----- Check the internal processing count sample_ct = idx_now - idx_old ; if ( sample_ct == 0 ) { return (0) ; // Data to be processed is not yet available } else if ( sample_ct < 0 ) { sample_ct += KPAD_RING_BUFS ; } //----- Check the number of times to save to the buffer if ( sample_ct < length ) { copy_ct = sample_ct ; } else { copy_ct = (s32)length ; } /*************************************************************** Processes to be performed for each sampling frame ***************************************************************/ sp = &samplingBufs[ copy_ct ] ; // Start saving old items, beginning at the end kp->work_ct = 0 ; idx = idx_old ; if ( kp->status.dev_type == WPAD_DEV_CORE || kp->status.dev_type == WPAD_DEV_FUTURE || kp->status.dev_type == WPAD_DEV_UNKNOWN || kp->status.dev_type == WPAD_DEV_NOT_SUPPORTED ) { // Treat the same way as core controller do { //----- Obtain SDK ring buffer if ( ++idx == KPAD_RING_BUFS ) idx = 0 ; wp = &((WPADStatus*)kp->wpad_ring_bf)[ idx ] ; //----- Copy error code kp->status.wpad_err = wp->err ; //----- Load acceleration and DPD information read_kpad_acc( kp, wp, WPAD_DEV_CORE, chan ) ; read_kpad_dpd( kp, wp, WPAD_DEV_CORE ) ; //----- Copy to KPAD buffer if ( kp->work_ct >= sample_ct - copy_ct ) { -- sp ; // Recede the storage buffer *sp = kp->status ; // The button information is actually unnecessary copying } } while ( ++ kp->work_ct < sample_ct ) ; } else if ( kp->status.dev_type == WPAD_DEV_FREESTYLE) { do { //----- Obtain SDK ring buffer if ( ++idx == KPAD_RING_BUFS ) idx = 0 ; fp = &((WPADFSStatus*)kp->wpad_ring_bf)[ idx ] ; //----- Copy error code kp->status.wpad_err = fp->err ; //----- Load acceleration and DPD information read_kpad_acc( kp, fp, WPAD_DEV_FREESTYLE, chan ) ; read_kpad_dpd( kp, fp, WPAD_DEV_FREESTYLE ) ; read_kpad_stick( kp, fp ) ; //----- Copy to KPAD buffer if ( kp->work_ct >= sample_ct - copy_ct ) { -- sp ; // Recede the storage buffer *sp = kp->status ; // The button information is actually unnecessary copying } } while ( ++ kp->work_ct < sample_ct ) ; } else if ( kp->status.dev_type == WPAD_DEV_CLASSIC ) { do { //----- Obtain SDK ring buffer if ( ++idx == KPAD_RING_BUFS ) idx = 0 ; cp = &((WPADCLStatus*)kp->wpad_ring_bf)[ idx ] ; //----- Copy error code kp->status.wpad_err = cp->err ; //----- Load acceleration and DPD information read_kpad_acc( kp, cp, WPAD_DEV_CLASSIC, chan ) ; read_kpad_dpd( kp, cp, WPAD_DEV_CLASSIC ) ; read_kpad_stick( kp, cp ) ; //----- Copy to KPAD buffer if ( kp->work_ct >= sample_ct - copy_ct ) { -- sp ; // Recede the storage buffer *sp = kp->status ; // The button information is actually unnecessary copying } } while ( ++ kp->work_ct < sample_ct ) ; } /*************************************************************** Processes to be performed for each game frame ***************************************************************/ //----- Load button information if ( kp->status.dev_type == WPAD_DEV_CORE || kp->status.dev_type == WPAD_DEV_FUTURE || kp->status.dev_type == WPAD_DEV_NOT_SUPPORTED || kp->status.dev_type == WPAD_DEV_UNKNOWN ) { // Treat the same way as core controller read_kpad_button( kp, wp, WPAD_DEV_CORE ) ; } else if ( kp->status.dev_type == WPAD_DEV_FREESTYLE ) { read_kpad_button( kp, fp, WPAD_DEV_FREESTYLE ) ; } else if ( kp->status.dev_type == WPAD_DEV_CLASSIC ) { read_kpad_button( kp, cp, WPAD_DEV_CLASSIC ) ; } //----- Copy to buffer idx = copy_ct ; do { sp->hold = kp->status.hold ; sp->trig = kp->status.trig ; sp->release = kp->status.release ; if ( kp->status.dev_type == WPAD_DEV_CLASSIC ) { sp->ex_status.cl.hold = kp->status.ex_status.cl.hold; sp->ex_status.cl.trig = kp->status.ex_status.cl.trig; sp->ex_status.cl.release = kp->status.ex_status.cl.release; } ++ sp ; // Move forward because the storage buffer is at the beginning } while ( --idx != 0 ) ; return ( copy_ct ) ; } /******************************************************************************* INIT KPAD *******************************************************************************/ void KPADInit( void ) { s32 i ; KPADInsideStatus *kp ; //----- WPAD WPADInit() ; while (WPADGetStatus() != WPAD_STATE_SETUP) ; //----- KPAD i = 0 ; do { kp = &inside_kpads[i] ; //----- The DPD is enabled by default kp->dpd_set_enabled = TRUE; kp->dpd_ctrl_retry_flag = FALSE; kp->dpd_request_flag = FALSE; kp->dpd_ctrl_busy = FALSE; kp->dpd_ctrl_callback = NULL; //----- Check device kp->status.dev_type = WPAD_DEV_NOT_FOUND ; kp->wpad_chan_no = (s16)( i - WPAD_MAX_CONTROLLERS ) ; (void)check_device( i, kp ) ; //----- Initialization of KPAD calibration value kp->dist_org = idist_org ; kp->accXY_nrm_hori = iaccXY_nrm_hori ; kp->sec_nrm_hori = isec_nrm_hori ; kp->center_org = icenter_org ; calc_dpd2pos_scale( kp ) ; //----- Initialization of KPAD pointing status kp->pos_play_radius = kp->hori_play_radius = kp->dist_play_radius = kp->acc_play_radius = 0.0f ; kp->pos_sensitivity = kp->hori_sensitivity = kp->dist_sensitivity = kp->acc_sensitivity = 1.0f ; kp_err_dist_max = (f32)(1.0f + (f32)WPADGetDpdSensitivity()); //----- Initialize to no button repeat kp->btn_repeat_delay = KPAD_BTN_NO_RPT_DELAY ; kp->btn_repeat_pulse = 0 ; kp->btn_cl_repeat_delay = KPAD_BTN_NO_RPT_DELAY ; kp->btn_cl_repeat_pulse = 0 ; } while ( ++i < WPAD_MAX_CONTROLLERS ) ; //---- Initialize value KPADReset() ; OSRegisterVersion(__KPADOldVersion); } /******************************************************************************* RESET KPAD *******************************************************************************/ void KPADReset( void ) { KPADInsideStatus *kp ; //----- Recalculate constant KPADSetObjInterval( kp_obj_interval ) ; kobj_frame_min.x = -1.0f + kp_err_outside_frame ; kobj_frame_max.x = 1.0f - kp_err_outside_frame ; kobj_frame_min.y = -((f32)KPAD_DPD_RESO_WY/KPAD_DPD_RESO_WX) + kp_err_outside_frame ; kobj_frame_max.y = ((f32)KPAD_DPD_RESO_WY/KPAD_DPD_RESO_WX) - kp_err_outside_frame ; kp_err_dist_speed_1 = 1.0f / kp_err_dist_speed ; kp_err_dist_speedM_1 = -1.0f / kp_err_dist_speed ; kp_ah_circle_radius2 = kp_ah_circle_radius * kp_ah_circle_radius ; //---- Reset all KPADs kp = &inside_kpads[ WPAD_MAX_CONTROLLERS - 1 ] ; do { reset_kpad( kp ) ; } while ( --kp >= inside_kpads ) ; } //----- is 'type' a valid device? static BOOL is_valid_device( const u32 type ) { if(type==WPAD_DEV_NOT_FOUND) { return FALSE; } else { return TRUE; } } //----- Data format and sampling buffer settings (with DPD off) static void set_dpd_disable( const s32 chan, const u32 type ) { if ( is_valid_device(type) ) { s32 ret; u32 fmt; KPADInsideStatus *kp = &inside_kpads[ chan ] ; WPADSetAutoSamplingBuf( chan, NULL, 0 ) ; //----- Stop auto-sampling switch ( type ) { case WPAD_DEV_CORE: case WPAD_DEV_FUTURE: // Treat the same way as core controller case WPAD_DEV_NOT_SUPPORTED: case WPAD_DEV_UNKNOWN: fmt = WPAD_FMT_CORE_ACC; break; case WPAD_DEV_FREESTYLE: fmt = WPAD_FMT_FREESTYLE_ACC; break ; case WPAD_DEV_CLASSIC: fmt = WPAD_FMT_CLASSIC_ACC; break; default: ASSERT("Never reach here."); return; // Never reach here. } ret = WPADSetDataFormat( chan, fmt ); kp->dpd_ctrl_retry_flag = (ret == WPAD_ERR_BUSY)? TRUE : FALSE; control_dpd_start_( chan ); kp->dpd_ctrl_busy = TRUE; ret = WPADControlDpd(chan, WPAD_DPD_OFF, KPADiControlDpdCallback); if ( ret != WPAD_ERR_NONE ) { kp->dpd_ctrl_retry_flag = TRUE; } WPADSetAutoSamplingBuf( chan, kp->wpad_ring_bf, KPAD_RING_BUFS ); } } //----- Data format and sampling buffer settings (with DPD on) static void set_dpd_enable( const s32 chan, const u32 type ) { if ( is_valid_device(type) ) { s32 ret; u32 fmt; u32 dpd_command; KPADInsideStatus *kp = &inside_kpads[ chan ] ; WPADSetAutoSamplingBuf( chan, NULL, 0 ) ; //----- Stop auto-sampling switch(type) { case WPAD_DEV_CORE: case WPAD_DEV_FUTURE: // Treat the same way as core controller case WPAD_DEV_NOT_SUPPORTED: case WPAD_DEV_UNKNOWN: fmt = WPAD_FMT_CORE_ACC_DPD; dpd_command = WPAD_DPD_EXP; break; case WPAD_DEV_FREESTYLE: fmt = WPAD_FMT_FREESTYLE_ACC_DPD; dpd_command = WPAD_DPD_STD; break ; case WPAD_DEV_CLASSIC: fmt = WPAD_FMT_CLASSIC_ACC_DPD; dpd_command = WPAD_DPD_STD; break; default: //ASSERT("Never reach here."); return; // Never reach here. } ret = WPADSetDataFormat( chan, fmt ); kp->dpd_ctrl_retry_flag = (ret != WPAD_ERR_NONE)? TRUE : FALSE; control_dpd_start_( chan ); kp->dpd_ctrl_busy = TRUE; ret = WPADControlDpd( chan, dpd_command, KPADiControlDpdCallback ); if ( ret != WPAD_ERR_NONE ) { kp->dpd_ctrl_retry_flag = TRUE; } WPADSetAutoSamplingBuf( chan, kp->wpad_ring_bf, KPAD_RING_BUFS ); } } static void control_dpd_start_( const s32 chan ) { KPADInsideStatus *kp = &inside_kpads[ chan ]; BOOL enabled = OSDisableInterrupts(); if ( ! kp->dpd_request_flag ) { s32 err; err = WPADProbe( chan, NULL ); if ( err != WPAD_ERR_NO_CONTROLLER ) { kp->dpd_request_flag = TRUE; if ( kp->dpd_ctrl_callback ) { kp->dpd_ctrl_callback( chan, KPAD_STATE_CTRL_DPD_START ); } } } (void)OSRestoreInterrupts( enabled ); } static void control_dpd_end_( const s32 chan ) { KPADInsideStatus *kp = &inside_kpads[ chan ]; BOOL enabled = OSDisableInterrupts(); if ( kp->dpd_request_flag ) { kp->dpd_request_flag = FALSE; if ( kp->dpd_ctrl_callback ) { kp->dpd_ctrl_callback( chan, KPAD_STATE_CTRL_DPD_FINISHED ); } } (void)OSRestoreInterrupts( enabled ); } static inline BOOL KPADiRestoreDPD( const s32 chan, BOOL enable ) { KPADInsideStatus *kp = &inside_kpads[ chan ]; BOOL before; BOOL intrEnabled; ASSERT((0 <= chan) && (chan < WPAD_MAX_CONTROLLERS)); intrEnabled = OSDisableInterrupts(); before = kp->dpd_set_enabled; kp->dpd_set_enabled = enable; if ( before != enable ) { control_dpd_start_( chan ); } (void)OSRestoreInterrupts( intrEnabled ); return before; } void KPADDisableDPD( const s32 chan ) { (void)KPADiRestoreDPD( chan, FALSE ); } void KPADEnableDPD ( const s32 chan ) { (void)KPADiRestoreDPD( chan, TRUE ); } void KPADSetControlDpdCallback( s32 chan, KPADControlDpdCallback callback ) { BOOL enabled = OSDisableInterrupts(); inside_kpads[chan].dpd_ctrl_callback = callback; (void)OSRestoreInterrupts( enabled ); } static void KPADiControlDpdCallback( s32 chan, s32 result ) { #pragma unused( chan, result ) KPADInsideStatus *kp = &inside_kpads[ chan ]; BOOL dpdEnabled; kp->dpd_ctrl_busy = FALSE; if ( result != WPAD_ERR_NONE ) // Retry because of error { kp->dpd_ctrl_retry_flag = TRUE; return; } // Process when successful dpdEnabled = WPADIsDpdEnabled( chan ); if ( ( dpdEnabled == kp->dpd_set_enabled ) && ( ! kp->dpd_ctrl_retry_flag ) ) // Complete if there is no retry flag and the result is same as the desired value { control_dpd_end_( chan ); } }