/*---------------------------------------------------------------------------* Project: KPAD library version 2 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 "KPADinside.h" #include #include #include #include #include REVOLUTION_LIB_VERSION(KPAD); /******************************************************* 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 BOOL kp_stick_clamp_cross = FALSE ; // Perform cross clamping //----- 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 over which center of gravity coordinate is enabled //static f32 kp_err_dist_speed_1 ; // reciprocal //static f32 kp_err_dist_speedM_1 ; // negative reciprocal //static f32 kp_ah_circle_radius2 ; // square 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 GC3D stick clamp setting //static s32 kp_gc_mstick_max = 77 ; //static s32 kp_gc_cstick_min = 15 ; // Old GCC stick clamp setting //static s32 kp_gc_cstick_max = 64 ; //static s32 kp_gc_trigger_min = 30 ; // Old GC analog trigger clamp setting //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 } ; //----- For correction of Nunchuk acceleration static Mtx kp_fs_rot ; // Rotation matrix static f32 kp_fs_revise_deg = 24.0f ; // Correction angle (in degrees) static void KPADiSamplingCallback ( s32 chan ) ; static void KPADiControlDpdCallback( s32 chan, s32 result ) ; static void *get_ring_buffer_by_kpad1_style( s32 chan, void *buf, u32 size ) ; /******************************************************************************* Analog data clamp setting *******************************************************************************/ void KPADSetFSStickClamp( s8 min, s8 max ) { kp_fs_fstick_min = (s32)min ; kp_fs_fstick_max = (s32)max ; } // These APIs are not recommended. // Please use KPADGetUnifiedWpadStatus() instead. // These are provided for compatibility with KPAD1. But not 100% compatible. /******************************************************************************* Obtain the ring buffer of WPADStatus *******************************************************************************/ WPADStatus *KPADGetWPADRingBuffer( s32 chan ) { static WPADStatus status[ KPAD_RING_BUFS ] ; return (WPADStatus *)get_ring_buffer_by_kpad1_style( chan, &status, WPAD_DEV_CORE ) ; } WPADFSStatus *KPADGetWPADFSRingBuffer( s32 chan ) { static WPADFSStatus status[ KPAD_RING_BUFS ] ; return (WPADFSStatus *)get_ring_buffer_by_kpad1_style( chan, &status, WPAD_DEV_FREESTYLE ) ; } WPADCLStatus *KPADGetWPADCLRingBuffer( s32 chan ) { static WPADCLStatus status[ KPAD_RING_BUFS ] ; return (WPADCLStatus *)get_ring_buffer_by_kpad1_style( chan, &status, WPAD_DEV_CLASSIC ) ; } static void *get_ring_buffer_by_kpad1_style( s32 chan, void *buf, u32 dev ) { KPADInsideStatus *kp = &inside_kpads[ chan ] ; BOOL enabled ; s32 idx1 ; s32 idx2 ; u32 count ; u32 size ; u32 type ; ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ; switch ( dev ) { case WPAD_DEV_CORE : size = sizeof( WPADStatus ) ; break ; case WPAD_DEV_FREESTYLE : size = sizeof( WPADFSStatus ) ; break ; case WPAD_DEV_CLASSIC : size = sizeof( WPADCLStatus ) ; break ; default : return buf ; } enabled = OSDisableInterrupts() ; idx1 = kp->bufIdx - 1 ; idx2 = (s32)WPADGetLatestIndexInBuf( chan ) ; for ( count = 0; count < KPAD_RING_BUFS; count++ ) { if ( idx1 < 0 ) { idx1 = KPAD_RING_BUFS - 1 ; } if ( idx2 < 0 ) { idx2 = KPAD_RING_BUFS - 1 ; } switch( kp->uniRingBuf[idx1].u.core.dev ) { case WPAD_DEV_CORE: case WPAD_DEV_FUTURE: case WPAD_DEV_NOT_SUPPORTED: case WPAD_DEV_UNKNOWN: type = WPAD_DEV_CORE; break; case WPAD_DEV_FREESTYLE: type = WPAD_DEV_FREESTYLE; break; case WPAD_DEV_CLASSIC: type = WPAD_DEV_CLASSIC; break; default: // Unreachable here. type = WPAD_DEV_UNKNOWN; break; } if ( type == dev ) { if ( WPADGetStatus() != WPAD_STATE_SETUP ) { kp->uniRingBuf[idx1].u.core.err = WPAD_ERR_INVALID ; } memcpy((u8 *)buf + idx2 * size, &kp->uniRingBuf[ idx1 ].u, size) ; } idx1-- ; idx2-- ; } (void)OSRestoreInterrupts( enabled ) ; return buf ; } /******************************************************************************* Configure the repeat rate of the buttons *******************************************************************************/ void KPADSetBtnRepeat( s32 chan, f32 delay_sec, f32 pulse_sec ) { KPADInsideStatus *kp = &inside_kpads[ chan ] ; ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ; if ( pulse_sec ) { //----- Set repeat flag setting kp->btn_repeat_delay = (u16)(s32)( delay_sec * 200.0f + 0.5f ) ; kp->btn_repeat_pulse = (u16)(s32)( pulse_sec * 200.0f + 0.5f ) ; } else { //----- No repeat flag setting kp->btn_repeat_delay = KPAD_BTN_NO_RPT_DELAY ; kp->btn_repeat_pulse = 0 ; } //----- Reset kp->btn_repeat_time = 0 ; kp->btn_repeat_next = kp->btn_repeat_delay ; kp->btn_cl_repeat_time = 0 ; kp->btn_cl_repeat_next = kp->btn_repeat_delay ; } /******************************************************************************* Set marker placement interval (in meters) *******************************************************************************/ void KPADSetObjInterval( f32 interval ) { BOOL enabled = OSDisableInterrupts() ; 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 = interval / KPAD_CMOS_HFOV_TAN ; //----- Distance calculation constants kp_dist_vv1 = interval / KPAD_CMOS_HFOV_TAN ; (void)OSRestoreInterrupts( enabled ) ; } /******************************************************************************* 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 ; } /******************************************************************************* Get parameters *******************************************************************************/ void KPADGetPosParam( s32 chan, f32 *play_radius, f32 *sensitivity ) { ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ; *play_radius = inside_kpads[ chan ].pos_play_radius ; *sensitivity = inside_kpads[ chan ].pos_sensitivity ; } void KPADGetHoriParam( s32 chan, f32 *play_radius, f32 *sensitivity ) { ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ; *play_radius = inside_kpads[ chan ].hori_play_radius ; *sensitivity = inside_kpads[ chan ].hori_sensitivity ; } void KPADGetDistParam( s32 chan, f32 *play_radius, f32 *sensitivity ) { ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ; *play_radius = inside_kpads[ chan ].dist_play_radius ; *sensitivity = inside_kpads[ chan ].dist_sensitivity ; } void KPADGetAccParam( s32 chan, f32 *play_radius, f32 *sensitivity ) { ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ; *play_radius = inside_kpads[ chan ].acc_play_radius ; *sensitivity = inside_kpads[ chan ].acc_sensitivity ; } /******************************************************************************* Obtain an easy-to-use scale value from the calibrated center position *******************************************************************************/ static void calc_dpd2pos_scale( KPADInsideStatus *kp ) { f32 scale ; f32 sx,sy ; //----- 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 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. kp->dpd2pos_scale = scale / ( ( sx < sy ) ? sx : sy ) ; } /******************************************************************************* Initialize the KPAD value *******************************************************************************/ static void reset_kpad( KPADInsideStatus *kp ) { KPADObject *op ; KPADStatus *sp = &kp->status ; KPADEXStatus *ep = &kp->status.ex_status ; kp->resetReq = FALSE ; //----- Recalculate constant kp->kobj_frame_min.x = -1.0f + kp_err_outside_frame ; kp->kobj_frame_max.x = 1.0f - kp_err_outside_frame ; kp->kobj_frame_min.y = -((f32)KPAD_DPD_RESO_WY / KPAD_DPD_RESO_WX) + kp_err_outside_frame ; kp->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 ; kp->err_dist_min = kp_err_dist_min ; kp->dist_vv1 = kp_dist_vv1 ; //----- 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->bufCount = 0 ; // Ring buffer unprocessed //----- Clear extension controller information kp->exResetReq = TRUE ; } /******************************************************************************* 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) ) ; inside_kpads[ chan ].aimReq = TRUE ; inside_kpads[ chan ].aimEnabled = TRUE ; } /******************************************************************************* Disable Wii Remote direction correction *******************************************************************************/ void KPADDisableAimingMode( s32 chan ) { ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ); inside_kpads[ chan ].aimReq = TRUE ; inside_kpads[ chan ].aimEnabled = FALSE ; } /******************************************************************************* 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, u32 dev_type, u32 count ) { KPADStatus *sp = &kp->status ; KPADEXStatus *ep = &kp->status.ex_status ; 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 += count ; 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 ; } } } if (dev_type == WPAD_DEV_CLASSIC) { if ( ep->cl.trig != 0 || ep->cl.release != 0 ) { //----- Reset because the button state changed kp->btn_cl_repeat_time = 0 ; kp->btn_cl_repeat_next = kp->btn_repeat_delay ; //----- Set flags at the beginning of push (only when repeat is set) if ( ep->cl.trig && kp->btn_repeat_pulse ) { ep->cl.hold |= KPAD_BUTTON_RPT ; } } else if ( ep->cl.hold != 0 ) { //----- Forward time because the button is pushed and the state is not changing kp->btn_cl_repeat_time += count ; 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 ) { ep->cl.hold |= KPAD_BUTTON_RPT ; //----- Configure the next repeat time kp->btn_cl_repeat_next += kp->btn_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 ; } } } } } /******************************************************************************* KPAD button information load process *******************************************************************************/ static void read_kpad_button( KPADInsideStatus *kp, u32 dev_type, u32 count, u32 core, u32 fs, u32 cl) { KPADStatus *sp = &kp->status ; KPADEXStatus *ep = &kp->status.ex_status ; u32 old_fg, change_fg ; u32 cl_old_fg, cl_change_fg ; //----- Store the previous value old_fg = sp->hold & KPAD_BUTTON_MASK ; //----- Load new value sp->hold = ( core & (KPAD_BUTTON_MASK & ~(WPAD_BUTTON_Z | WPAD_BUTTON_C)) ) | ( fs & (WPAD_BUTTON_Z | WPAD_BUTTON_C) ) ; //----- 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 if ( dev_type == WPAD_DEV_CLASSIC ) { //----- Store the previous value cl_old_fg = ep->cl.hold & KPAD_BUTTON_MASK ; //----- Load new value ep->cl.hold = cl & KPAD_BUTTON_MASK ; //----- Button state process 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 ; } //----- Repeat processing calc_button_repeat( kp, dev_type, count ) ; } /******************************************************************************* Acceleration tracking process *******************************************************************************/ static void calc_acc( KPADInsideStatus *kp, f32 *acc, f32 acc2 ) { f32 f1,f2 ; //----- Difference to the target f2 = acc2 - *acc ; if ( kp->acc_play_mode == KPAD_PLAY_MODE_LOOSE ) { 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 ; } else { if ( f2 < -kp->acc_play_radius ) { *acc += ( f2 + kp->acc_play_radius ) * kp->acc_sensitivity ; } else if ( f2 > kp->acc_play_radius ) { *acc += ( f2 - kp->acc_play_radius ) * kp->acc_sensitivity ; } } } /******************************************************************************* 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, KPADUnifiedWpadStatus *uwp ) { KPADStatus *sp = &kp->status ; Vec fsrc ; Vec vec ; switch ( uwp->fmt ) { case WPAD_FMT_CORE_ACC : case WPAD_FMT_CORE_ACC_DPD : case WPAD_FMT_FREESTYLE_ACC : case WPAD_FMT_FREESTYLE_ACC_DPD : case WPAD_FMT_CLASSIC_ACC : case WPAD_FMT_CLASSIC_ACC_DPD : // core ACC is OK break ; default : return ; } //----- Update raw value kp->hard_acc.x = clamp_acc( (f32)(s32)-uwp->u.core.accX * kp->acc_scale_x, kp_rm_acc_max ) ; kp->hard_acc.y = clamp_acc( (f32)(s32)-uwp->u.core.accZ * kp->acc_scale_z, kp_rm_acc_max ) ; kp->hard_acc.z = clamp_acc( (f32)(s32) uwp->u.core.accY * kp->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 ( uwp->u.fs.err != WPAD_ERR_NONE || uwp->u.fs.dev != WPAD_DEV_FREESTYLE || (uwp->fmt != WPAD_FMT_FREESTYLE_ACC && uwp->fmt != WPAD_FMT_FREESTYLE_ACC_DPD) ) { return ; } fsrc.x = clamp_acc( (f32)(s32)-uwp->u.fs.fsAccX * kp->fs_acc_scale_x, kp_fs_acc_max ) ; fsrc.y = clamp_acc( (f32)(s32)-uwp->u.fs.fsAccZ * kp->fs_acc_scale_z, kp_fs_acc_max ) ; fsrc.z = clamp_acc( (f32)(s32) uwp->u.fs.fsAccY * kp->fs_acc_scale_y, kp_fs_acc_max ) ; if ( kp->fsAccRevise ) { MTXMultVec( kp_fs_rot, &fsrc, &fsrc ) ; } //----- Temporary save vec = sp->ex_status.fs.acc ; //----- Acceleration tracking process for the application calc_acc( kp, &sp->ex_status.fs.acc.x, fsrc.x ) ; calc_acc( kp, &sp->ex_status.fs.acc.y, fsrc.y ) ; calc_acc( kp, &sp->ex_status.fs.acc.z, fsrc.z ) ; 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, DPDObject *wobj_p ) { 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 ; KPADObject *kobj_p ; //----- Store 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( KPADInsideStatus *kp, 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 <= kp->kobj_frame_min.x || kobj_p->center.x >= kp->kobj_frame_max.x || kobj_p->center.y <= kp->kobj_frame_min.y || kobj_p->center.y >= kp->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 <= kp->kobj_frame_min.x || p1.x >= kp->kobj_frame_max.x || p1.y <= kp->kobj_frame_min.y || p1.y >= kp->kobj_frame_max.y ) { //----- If the left-hand expected point is outside, the right-hand expected point needs to be inside if ( p2.x > kp->kobj_frame_min.x && p2.x < kp->kobj_frame_max.x && p2.y > kp->kobj_frame_min.y && p2.y < kp->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 <= kp->kobj_frame_min.x || p2.x >= kp->kobj_frame_max.x || p2.y <= kp->kobj_frame_min.y || p2.y >= kp->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 ) ; if ( kp->hori_play_mode == KPAD_PLAY_MODE_LOOSE ) { //----- 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 ; } else { if ( f1 > kp->hori_play_radius ) { //----- Interpolate because outside tolerance f1 = ( f1 - kp->hori_play_radius ) / f1 * kp->hori_sensitivity ; vec.x = vec.x * f1 + sp->horizon.x ; vec.y = vec.y * f1 + sp->horizon.y ; f1 = sqrtf( vec.x * vec.x + vec.y * vec.y ) ; 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 ; } else { //----- Does not move since it's inside tolerance sp->hori_vec = Vec2_0 ; sp->hori_speed = 0.0f ; } } } /*********************************************************************** 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 ; } if ( kp->dist_play_mode == KPAD_PLAY_MODE_LOOSE ) { //----- 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 ; } else { if ( f1 > kp->dist_play_radius ) { //----- Interpolate because outside tolerance f1 = ( f1 - kp->dist_play_radius ) / f1 * kp->dist_sensitivity ; 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 ; } else { //----- Does not move since it's inside tolerance sp->dist_vec = 0.0f ; sp->dist_speed = 0.0f ; } } } /*********************************************************************** 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 ) ; if ( kp->pos_play_mode == KPAD_PLAY_MODE_LOOSE ) { //----- 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 ; } else { if ( f1 > kp->pos_play_radius ) { //----- Interpolate because outside tolerance f1 = ( f1 - kp->pos_play_radius ) / f1 * kp->pos_sensitivity ; 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 ; } else { //----- Does not move since it's inside tolerance sp->vec = Vec2_0 ; sp->speed = 0.0f ; } } } /*********************************************************************** Update flags ***********************************************************************/ sp->dpd_valid_fg = valid_fg_next ; } /******************************************************************************* KPAD DPD information load process *******************************************************************************/ static void read_kpad_dpd( KPADInsideStatus *kp, KPADUnifiedWpadStatus *uwp ) { KPADStatus *sp = &kp->status ; KPADObject *op1 ; s8 valid_fg_next ; /*********************************************************************** Change the WPAD object to KPAD ***********************************************************************/ if ( uwp->fmt == WPAD_FMT_CORE_ACC_DPD || uwp->fmt == WPAD_FMT_FREESTYLE_ACC_DPD || uwp->fmt == WPAD_FMT_CLASSIC_ACC_DPD ) { //----- Change the WPAD object to KPAD get_kobj( kp, &uwp->u.core.obj[ WPAD_DPD_MAX_OBJECTS - 1 ] ) ; } else { // dpd data is not prepared op1 = &kp->kobj_sample[ WPAD_DPD_MAX_OBJECTS - 1 ] ; do { op1->error_fg = -1 ; } while ( --op1 >= kp->kobj_sample ) ; } /*********************************************************************** Select the normal object ***********************************************************************/ //----- Remove invalid objects check_kobj_outside_frame( kp, 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_circle( 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 ; } } static void clamp_stick_cross( Vec2 *stick, s32 sx, s32 sy, s32 min, s32 max ) { f32 length ; //----- Process clamp for each axis separately if ( sx < 0 ) { clamp_trigger( &stick->x, -sx, min, max ) ; stick->x = -stick->x ; } else { clamp_trigger( &stick->x, sx, min, max ) ; } if ( sy < 0 ) { clamp_trigger( &stick->y, -sy, min, max ) ; stick->y = -stick->y ; } else { clamp_trigger( &stick->y, sy, min, max ) ; } //----- Maximum length is set to 1 length = stick->x * stick->x + stick->y * stick->y ; if ( length > 1.0f ) { length = sqrtf( length ) ; stick->x /= length ; stick->y /= length ; } } /******************************************************************************* Load stick information *******************************************************************************/ static void read_kpad_stick( KPADInsideStatus *kp, KPADUnifiedWpadStatus *uwp ) { KPADEXStatus *ep = &kp->status.ex_status ; void (*clampStickFuncp)( Vec2 *stick, s32 sx, s32 sy, s32 min, s32 max ) ; clampStickFuncp = kp_stick_clamp_cross ? clamp_stick_circle : clamp_stick_cross ; if ( uwp->u.fs.dev == WPAD_DEV_FREESTYLE && (uwp->fmt == WPAD_FMT_FREESTYLE || uwp->fmt == WPAD_FMT_FREESTYLE_ACC || uwp->fmt == WPAD_FMT_FREESTYLE_ACC_DPD) ) { if ( kp->exResetReq ) { kp->exResetReq = FALSE ; //----- 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 ; } //----- Stick data processing clampStickFuncp( &ep->fs.stick, uwp->u.fs.fsStickX, uwp->u.fs.fsStickY, kp_fs_fstick_min, kp_fs_fstick_max ) ; } else if ( uwp->u.cl.dev == WPAD_DEV_CLASSIC && (uwp->fmt == WPAD_FMT_CLASSIC || uwp->fmt == WPAD_FMT_CLASSIC_ACC || uwp->fmt == WPAD_FMT_CLASSIC_ACC_DPD) ) { if ( kp->exResetReq ) { kp->exResetReq = FALSE ; //----- 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_repeat_delay ; } //----- Stick data processing clampStickFuncp( &ep->cl.lstick, uwp->u.cl.clLStickX, uwp->u.cl.clLStickY, kp_cl_stick_min, kp_cl_stick_max ) ; clampStickFuncp( &ep->cl.rstick, uwp->u.cl.clRStickX, uwp->u.cl.clRStickY, kp_cl_stick_min, kp_cl_stick_max ) ; clamp_trigger( &ep->cl.ltrigger, uwp->u.cl.clTriggerL, kp_cl_trigger_min, kp_cl_trigger_max ) ; clamp_trigger( &ep->cl.rtrigger, uwp->u.cl.clTriggerR, kp_cl_trigger_min, kp_cl_trigger_max ) ; } } /******************************************************************************* READ KPAD (return the number of loaded buffer) *******************************************************************************/ s32 KPADRead( s32 chan, KPADStatus samplingBufs[], u32 length ) { // This assumption is confirmed in KPADInit() // (sizeof(KPADTmpStatus) == sizeof(KPADStatus)) KPADTmpStatus *tp = (KPADTmpStatus *)samplingBufs ; KPADUnifiedWpadStatus uwStatus ; KPADInsideStatus *kp = &inside_kpads[ chan ] ; KPADUnifiedWpadStatus *uwp ; s32 wpad_err ; s32 idx ; u32 copy_ct ; u32 return_ct = 0 ; u32 bufCount ; BOOL enabled ; u32 lastCoreButton ; u32 lastFsButton ; u32 lastClButton ; u32 lastDev ; ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ; if ( WPADGetStatus() != WPAD_STATE_SETUP ) { return ( 0 ) ; } enabled = OSDisableInterrupts() ; if ( kp->readLocked ) { (void)OSRestoreInterrupts( enabled ) ; return ( 0 ) ; } kp->readLocked = TRUE ; wpad_err = WPADProbe( chan, NULL ) ; if ( wpad_err == WPAD_ERR_NO_CONTROLLER && kp->dpd_ctrl_callback && kp->dpdPreCallbackDone && !kp->dpdPostCallbackDone ) { KPADiControlDpdCallback( chan, WPAD_ERR_NONE ) ; } (void)OSRestoreInterrupts( enabled ) ; if ( kp->resetReq ) { kp->status.wpad_err = (s8)wpad_err ; reset_kpad( kp ) ; } // Sampling callback may have been unregistered // upon new connection WPADSetSamplingCallback( chan, KPADiSamplingCallback ) ; if ( kp->bufCount && samplingBufs && length ) { enabled = OSDisableInterrupts() ; bufCount = kp->bufCount ; //----- Check the number of times to save to the buffer if ( bufCount > length ) { copy_ct = length ; } else { copy_ct = bufCount ; } kp->bufCount = 0 ; return_ct = copy_ct ; tp += copy_ct ; // Start saving old items, beginning at the end idx = (s32)( kp->bufIdx - copy_ct ) ; if ( idx < 0 ) { idx += KPAD_RING_BUFS ; } --tp ; while ( --copy_ct ) { --tp ; tp->w = kp->uniRingBuf[ idx ] ; idx++ ; if ( idx >= KPAD_RING_BUFS ) { idx = 0 ; } } uwStatus = kp->uniRingBuf[ idx ] ; (void)OSRestoreInterrupts( enabled ) ; // Obtain 1G value from the controller to absorb the individual unit differences. // Although we'd like to get the value in one try, the value cannot be retrieved immediately after connect or attach. // For the time being, it is obtained here each time through the loop { WPADAcc core1G = {1, 1, 1} ; WPADAcc fs1G = {1, 1, 1} ; WPADGetAccGravityUnit( chan, WPAD_DEV_CORE, &core1G ) ; if(core1G.x * core1G.y * core1G.z != 0){ // Bug fix kp->acc_scale_x = 1.0f / core1G.x ; kp->acc_scale_y = 1.0f / core1G.y ; kp->acc_scale_z = 1.0f / core1G.z ; } else { // The values listed here should not be too far off from the precise value obtained through WPADGetAccGravityUnit(). // kp->acc_scale_x = 1.0f / 100 ; kp->acc_scale_y = 1.0f / 100 ; kp->acc_scale_z = 1.0f / 100 ; } WPADGetAccGravityUnit( chan, WPAD_DEV_FREESTYLE, &fs1G ) ; if( fs1G.x * fs1G.y * fs1G.z != 0 ){ // Bug fix kp->fs_acc_scale_x = 1.0f / fs1G.x ; kp->fs_acc_scale_y = 1.0f / fs1G.y ; kp->fs_acc_scale_z = 1.0f / fs1G.z ; } else { // The values listed here should not be too far off from the precise value obtained through WPADGetAccGravityUnit(). // kp->fs_acc_scale_x = 1.0f / 200 ; kp->fs_acc_scale_y = 1.0f / 200 ; kp->fs_acc_scale_z = 1.0f / 200 ; } } // find last button data and device type copy_ct = return_ct ; lastCoreButton = lastFsButton = lastClButton = KPAD_BUTTON_MASK ; lastDev = WPAD_DEV_NOT_FOUND ; tp = (KPADTmpStatus *)samplingBufs + copy_ct ; --tp ; do { --tp ; uwp = ( copy_ct > 1 ) ? &tp->w : &uwStatus ; switch ( uwp->u.core.err ) { case WPAD_ERR_NONE : lastDev = uwp->u.core.dev ; if ( lastDev == WPAD_DEV_FREESTYLE ) { lastFsButton = uwp->u.fs.button ; lastClButton = 0 ; } else if ( lastDev == WPAD_DEV_CLASSIC ) { lastFsButton = 0 ; lastClButton = uwp->u.cl.clButton ; } else { lastFsButton = lastClButton = 0 ; } // thru case WPAD_ERR_CORRUPTED : case WPAD_ERR_BUSY : lastCoreButton = uwp->u.core.button ; break ; default : break ; } } while ( --copy_ct ) ; /*************************************************************** Processes to be performed for each game frame ***************************************************************/ //----- Load button information if ( lastCoreButton == KPAD_BUTTON_MASK ) { do { memcpy( samplingBufs, &kp->status, sizeof(KPADStatus) ) ; samplingBufs++ ; } while ( --return_ct ) ; goto finish ; } if ( lastFsButton == KPAD_BUTTON_MASK ) { lastFsButton = kp->status.hold ; } if ( lastClButton == KPAD_BUTTON_MASK ) { lastClButton = kp->status.ex_status.cl.hold ; } read_kpad_button( kp, lastDev, bufCount, lastCoreButton, lastFsButton, lastClButton ) ; copy_ct = return_ct ; tp = (KPADTmpStatus *)samplingBufs + copy_ct ; --tp ; do { --tp ; uwp = ( copy_ct > 1 ) ? &tp->w : &uwStatus ; //----- Copy error code kp->status.wpad_err = uwp->u.core.err ; if ( kp->status.dev_type != uwp->u.core.dev ) { if ( uwp->u.core.err == WPAD_ERR_NONE || uwp->u.core.err == WPAD_ERR_BUSY || uwp->u.core.err == WPAD_ERR_NO_CONTROLLER ) { kp->status.dev_type = uwp->u.core.dev ; kp->exResetReq = TRUE ; } } kp->status.data_format = uwp->fmt ; switch ( uwp->u.core.err ) { case WPAD_ERR_NONE : read_kpad_stick( kp, uwp ) ; // thru case WPAD_ERR_CORRUPTED : read_kpad_acc( kp, uwp ) ; read_kpad_dpd( kp, uwp ) ; break ; default : kp->status.dpd_valid_fg = FALSE ; break ; } tp[ 1 ].k = kp->status ; } while ( --copy_ct ) ; } finish : kp->readLocked = FALSE ; return (s32)return_ct ; } /******************************************************************************* INIT KPAD *******************************************************************************/ void KPADInit( void ) { s32 i ; KPADInsideStatus *kp ; GXColor black = {0, 0, 0, 0} ; GXColor white = {255, 255, 255, 255} ; u32 idx ; // SDK 2.0 or later is required (see SDK release note 41) // Also KPADRead() assumes: (sizeof(KPADTmpStatus) == sizeof(KPADStatus)) if (offsetof( WPADFSStatus, err ) == offsetof( WPADStatus, err ) && sizeof(KPADTmpStatus) == sizeof(KPADStatus) ) { // OK } else { OSFatal( black, white, "KPADInit error" ) ; // Never reach here. } //----- WPAD WPADInit() ; //----- KPAD memset( &inside_kpads, 0, sizeof(inside_kpads) ) ; kp_err_dist_max = (f32)( 1.0f + (f32)WPADGetDpdSensitivity() ) ; i = 0 ; do { kp = &inside_kpads[ i ] ; //----- The DPD is enabled by default kp->dpdEnabled = TRUE ; kp->dpdCmd = WPAD_DPD_OFF ; //----- Check device kp->status.dev_type = WPAD_DEV_NOT_FOUND ; kp->status.data_format = WPAD_FMT_CORE ; 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->pos_play_mode = kp->hori_play_mode = kp->dist_play_mode = kp->acc_play_mode = KPAD_PLAY_MODE_LOOSE ; //----- Initialize to no button repeat KPADSetBtnRepeat( i, 0.0f, 0.0f ) ; //----- Enabling error correction for the Sensor Bar placement position KPADEnableAimingMode( i ) ; //----- Acceleration correction for the Nunchuk is disabled by default kp->fsAccRevise = 0 ; //----- Initialize the rotation matrix MTXRowCol( kp_fs_rot, 0, 0 ) = 1 ; MTXRowCol( kp_fs_rot, 0, 1 ) = 0 ; MTXRowCol( kp_fs_rot, 0, 2 ) = 0 ; MTXRowCol( kp_fs_rot, 0, 3 ) = 0 ; MTXRowCol( kp_fs_rot, 1, 0 ) = 0 ; MTXRowCol( kp_fs_rot, 1, 1 ) = (f32) cos( MTXDegToRad(kp_fs_revise_deg) ) ; MTXRowCol( kp_fs_rot, 1, 2 ) = (f32)-sin( MTXDegToRad(kp_fs_revise_deg) ) ; MTXRowCol( kp_fs_rot, 1, 3 ) = 0 ; MTXRowCol( kp_fs_rot, 2, 0 ) = 0 ; MTXRowCol( kp_fs_rot, 2, 1 ) = (f32) sin( MTXDegToRad(kp_fs_revise_deg) ) ; MTXRowCol( kp_fs_rot, 2, 2 ) = (f32) cos( MTXDegToRad(kp_fs_revise_deg) ) ; MTXRowCol( kp_fs_rot, 2, 3 ) = 0 ; idx = 0 ; do { kp->uniRingBuf[idx].u.core.err = WPAD_ERR_NO_CONTROLLER ; } while ( ++idx < KPAD_RING_BUFS ) ; } while ( ++i < WPAD_MAX_CONTROLLERS ) ; //---- Initialize value KPADReset() ; OSRegisterVersion( __KPADVersion ) ; } /******************************************************************************* RESET KPAD *******************************************************************************/ void KPADReset( void ) { s32 chan ; //----- Recalculate constant KPADSetObjInterval( kp_obj_interval ) ; //---- Reset all KPADs chan = WPAD_MAX_CONTROLLERS - 1 ; do { if ( WPADGetStatus() == WPAD_STATE_SETUP ) { WPADStopMotor( chan ) ; } inside_kpads[ chan ].resetReq = TRUE ; } while ( --chan >= 0 ) ; } void KPADDisableDPD( const s32 chan ) { inside_kpads[ chan ].dpdEnabled = FALSE ; } void KPADEnableDPD ( const s32 chan ) { inside_kpads[ chan ].dpdEnabled = TRUE ; } void KPADSetControlDpdCallback( s32 chan, KPADControlDpdCallback callback ) { KPADInsideStatus *kp = &inside_kpads[ chan ] ; BOOL enabled ; enabled = OSDisableInterrupts() ; kp->dpd_ctrl_callback = callback ; (void)OSRestoreInterrupts( enabled ) ; } static void KPADiSamplingCallback(s32 chan) { KPADInsideStatus *kp = &inside_kpads[ chan ] ; KPADUnifiedWpadStatus *uwp ; u32 idx ; u32 type ; u32 curDpd ; f32 aimClbr ; static struct { u8 dpd ; u8 fmt ; } table[] = { { WPAD_DPD_OFF, WPAD_FMT_CORE_ACC }, { WPAD_DPD_EXP, WPAD_FMT_CORE_ACC_DPD }, { WPAD_DPD_OFF, WPAD_FMT_FREESTYLE_ACC }, { WPAD_DPD_STD, WPAD_FMT_FREESTYLE_ACC_DPD }, { WPAD_DPD_OFF, WPAD_FMT_CLASSIC_ACC }, { WPAD_DPD_STD, WPAD_FMT_CLASSIC_ACC_DPD } } ; ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ); if ( WPADProbe( chan, &type ) == WPAD_ERR_NO_CONTROLLER ) { goto finish ; } idx = kp->bufIdx ; if ( idx >= KPAD_RING_BUFS ) { idx = 0 ; } uwp = &kp->uniRingBuf[ idx ] ; WPADRead( chan, &uwp->u ) ; uwp->fmt = (u8)WPADGetDataFormat( chan ) ; kp->bufIdx = (u8)( idx + 1 ) ; if ( kp->bufCount < KPAD_RING_BUFS ) { kp->bufCount++ ; } if ( kp->aimReq ) { if ( kp->aimEnabled ) { if ( WPAD_SENSOR_BAR_POS_TOP == WPADGetSensorBarPosition() ) { aimClbr = 0.2f ; } else { aimClbr = -0.2f ; } } else { aimClbr = 0.0f ; } KPADSetSensorHeight( chan, aimClbr ) ; kp->aimReq = FALSE ; } //----- Check DPD settings switch ( type ) { case WPAD_DEV_CORE : case WPAD_DEV_FUTURE : case WPAD_DEV_NOT_SUPPORTED : case WPAD_DEV_UNKNOWN : idx = 0 ; break ; case WPAD_DEV_FREESTYLE : idx = 2 ; break ; case WPAD_DEV_CLASSIC : idx = 4 ; break ; default : goto finish ; } if ( kp->dpdEnabled ) { idx += 1 ; } curDpd = (u32)( WPADIsDpdEnabled( chan ) ? kp->dpdCmd : WPAD_DPD_OFF ); if ( curDpd != table[idx].dpd ) { if ( kp->dpd_ctrl_callback && !kp->dpdPreCallbackDone ) { kp->dpdPreCallbackDone = TRUE ; kp->dpd_ctrl_callback( chan, KPAD_STATE_CTRL_DPD_START ) ; kp->dpdPostCallbackDone = FALSE ; } if ( !kp->dpdIssued ) { kp->dpdIssued = TRUE ; if (WPADControlDpd( chan, table[ idx ].dpd, KPADiControlDpdCallback ) == WPAD_ERR_NONE ) { kp->dpdCmd = table[ idx ].dpd ; } } } else { if ( uwp->fmt != table[ idx ].fmt ) { WPADSetDataFormat( chan, table[ idx ].fmt ); } } finish : if ( kp->appSamplingCallback ) { kp->appSamplingCallback( chan ) ; } } static void KPADiControlDpdCallback( s32 chan, s32 result ) { KPADInsideStatus *kp = &inside_kpads[ chan ] ; if ( result == WPAD_ERR_NONE ) { if ( kp->dpd_ctrl_callback && !kp->dpdPostCallbackDone ) { kp->dpdPostCallbackDone = TRUE ; kp->dpd_ctrl_callback( chan, KPAD_STATE_CTRL_DPD_FINISHED ) ; kp->dpdPreCallbackDone = FALSE ; } } kp->dpdIssued = FALSE ; } void KPADGetUnifiedWpadStatus( s32 chan, KPADUnifiedWpadStatus *dst, u32 count ) { KPADInsideStatus *kp = &inside_kpads[ chan ] ; BOOL enabled ; u32 idx ; ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ; if ( count > KPAD_RING_BUFS ) { count = KPAD_RING_BUFS ; } enabled = OSDisableInterrupts() ; idx = kp->bufIdx ; while ( count-- ) { if ( idx == 0 ) { idx = KPAD_RING_BUFS - 1 ; } else { idx-- ; } // <== latest ... oldest ==> // dst[0] dst[1] ... dst[KPAD_RING_BUFS - 1] if ( WPADGetStatus() != WPAD_STATE_SETUP ) { kp->uniRingBuf[ idx ].u.core.err = WPAD_ERR_INVALID ; } memcpy( dst, &kp->uniRingBuf[idx], sizeof(KPADUnifiedWpadStatus) ) ; dst++ ; } (void)OSRestoreInterrupts( enabled ) ; } void KPADEnableStickCrossClamp( void ) { kp_stick_clamp_cross = TRUE ; } void KPADDisableStickCrossClamp( void ) { kp_stick_clamp_cross = FALSE ; } void KPADSetSamplingCallback( s32 chan, WPADSamplingCallback callback ) { inside_kpads[ chan ].appSamplingCallback = callback ; } void KPADSetReviseMode( s32 chan, BOOL sw ) { KPADInsideStatus *kp = &inside_kpads[ chan ] ; kp->fsAccRevise = (u8)sw ; } f32 KPADReviseAcc( Vec *acc ) { MTXMultVec( kp_fs_rot, acc, acc ) ; return kp_fs_revise_deg ; // Return the correction angle (in degrees) } f32 KPADGetReviseAngle( void ) { return kp_fs_revise_deg ; // Return the correction angle (in degrees) } /******************************************************************************* Control for all calculation methods *******************************************************************************/ void KPADSetPosPlayMode( s32 chan, KPADPlayMode mode ) { ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ; inside_kpads[ chan ].pos_play_mode = mode ; } void KPADSetHoriPlayMode( s32 chan, KPADPlayMode mode ) { ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ; inside_kpads[ chan ].hori_play_mode = mode ; } void KPADSetDistPlayMode( s32 chan, KPADPlayMode mode ) { ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ; inside_kpads[ chan ].dist_play_mode = mode ; } void KPADSetAccPlayMode( s32 chan, KPADPlayMode mode ) { ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ; inside_kpads[ chan ].acc_play_mode = mode ; } KPADPlayMode KPADGetPosPlayMode( s32 chan ) { ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ; return ( inside_kpads[ chan ].pos_play_mode ) ; } KPADPlayMode KPADGetHoriPlayMode( s32 chan ) { ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ; return ( inside_kpads[ chan ].hori_play_mode ) ; } KPADPlayMode KPADGetDistPlayMode( s32 chan ) { ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ; return ( inside_kpads[ chan ].dist_play_mode ) ; } KPADPlayMode KPADGetAccPlayMode( s32 chan ) { ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ; return ( inside_kpads[ chan ].acc_play_mode ) ; }