/*---------------------------------------------------------------------------* Project: KPAD library File: KPAD.c Programmers: Keizo Ohta HIRATSU Daisuke 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(KPAD); /******************************************************* VARIABLE *******************************************************/ //----- Initial values for ＤＰＤ calibration static Vec2 icenter_org = { 0.000f, 0.000f } ; // Center coordinate of the 2 marks on CMOS static f32 idist_org = 1.000f ; // Distance after calibration (in meters) static Vec2 iaccXY_nrm_hori = { 0.000f,-1.000f } ; // XY acceleration direction when the controller is in the horizontal plane static Vec2 isec_nrm_hori = { 1.000f, 0.000f } ; // Direction from left mark to right mark when the controller is in the horizontal plane f32 kp_obj_interval = 0.200f ; // Distance between both marks (in meters) //----- Various adjustments f32 kp_acc_horizon_pw = 0.050f ; // Conditions when calculating the rotation from the acceleration f32 kp_ah_circle_radius = 0.070f ; // Radius used to determine if the controller is at rest f32 kp_ah_circle_pw = 0.060f ; // Sensitivity conditions used to determine if the controller is at rest u16 kp_ah_circle_ct = 100 ; // Count for determining if the controller is at rest //----- Numeric values viewed as errors f32 kp_err_outside_frame = 0.050f ; // Peripheral width where center of gravity coordinates are disabled (peripheral light may not be entirely displayed) f32 kp_err_dist_min ; // Automatically calculate minimum operation distance (in meters) f32 kp_err_dist_max = 3.000f ; // Maximum operation distance (in meters) f32 kp_err_dist_speed = 0.040f ; // Allowable range of variation in distance (in meters) f32 kp_err_first_inpr = 0.900f ; // Inner product of acceleration slope and object inclination when selecting first two points f32 kp_err_next_inpr = 0.900f ; // Allowable range in variation of inclination (inner product) f32 kp_err_acc_inpr = 0.900f ; // Allowable range of inner product with acceleration slope when controller has been brought to rest f32 kp_err_up_inpr = 0.700f ; // Allowable range of inner product when controller is facing up f32 kp_err_near_pos = 0.100f ; // Distance from last point when selecting the next point //----- Internal processing static Vec2 kobj_frame_min, kobj_frame_max ; // Range in which center of gravity coordinate is enabled static f32 kp_err_dist_speed_1 ; // Inverse static f32 kp_err_dist_speedM_1 ; // Inverse having negative value static f32 kp_ah_circle_radius2 ; // Raised to the 2nd power static f32 kp_dist_vv1 ; // Constant static s32 kp_fs_fstick_min = 15 ; // Set nunchaku unit stick clamp 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 ; // Set old GC3D stick clamp static s32 kp_gc_mstick_max = 77 ; static s32 kp_gc_cstick_min = 15 ; // Set old GCC stick clamp static s32 kp_gc_cstick_max = 64 ; static s32 kp_gc_trigger_min = 30 ; // Set old GC analog trigger clamp static s32 kp_gc_trigger_max = 180 ; static f32 kp_rm_acc_max = 3.4f ; // Remote acceleration clamp settings static f32 kp_fs_acc_max = 2.1f ; // Nunchuk acceleration clamp settings //----- ＫＰＡＤ unit KPADInsideStatus inside_kpads[ WPAD_MAX_CONTROLLERS ] ; //----- Zero vector static Vec2 Vec2_0={0.0f, 0.0f}; /******************************************************************************* Set analog data clamp *******************************************************************************/ void KPADSetFSStickClamp( s8 min, s8 max ) { kp_fs_fstick_min = (s32)min ; kp_fs_fstick_max = (s32)max ; } /******************************************************************************* Get WPADStatus ring buffer *******************************************************************************/ 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 ) ; } /******************************************************************************* Set conditions for button repeat *******************************************************************************/ void KPADSetBtnRepeat( s32 chan, f32 delay_sec, f32 pulse_sec ) { ASSERT((0 <= chan) && (chan < WPAD_MAX_CONTROLLERS)); if ( pulse_sec ) { //----- Set repeat flag 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 { //----- Unset repeat flag 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 distance between markers (in meters) *******************************************************************************/ void KPADSetObjInterval( f32 interval ) { kp_obj_interval = interval ; //----- Minimum ＤＰＤ operation distance (set so that the distance between marks is half the lens diameter) kp_err_dist_min = kp_obj_interval / KPAD_CMOS_HFOV_TAN ; //----- Constant used to calculate distances 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 ; } /******************************************************************************* Find an easy-to-use scale value based on the calibrated center position *******************************************************************************/ static void calc_dpd2pos_scale( KPADInsideStatus *kp ) { f32 sx,sy ; //----- Distance the controller is moved up/down/left/right sx = 1.0f ; // Horizontal sy = (f32)KPAD_DPD_RESO_WY / (f32)KPAD_DPD_RESO_WX ; // Vertical //----- Maximum distance the controller can be moved kp->dpd2pos_scale = sqrtf( sx * sx + sy * sy ) ;// Diagonal line //----- Correct the horizontal range of movement if ( kp->center_org.x < 0.0f ) { sx += kp->center_org.x ; } else { sx -= kp->center_org.x ; } //----- Correct the vertical range of movement if ( kp->center_org.y < 0.0f ) { sy += kp->center_org.y ; } else { sy -= kp->center_org.y ; } //----- Scale that can handle the longest distance possible when a small range of movement is allowed if ( sx < sy ) { kp->dpd2pos_scale /= sx ; } else { kp->dpd2pos_scale /= sy ; } } /******************************************************************************* Initialize KPAD *******************************************************************************/ static void reset_kpad( KPADInsideStatus *kp ) { KPADObject *op ; KPADStatus *sp = &kp->status ; KPADEXStatus *ep = &sp->ex_status ; //----- Clear button data sp->hold = sp->trig = sp->release = 0x00000000 ; kp->btn_repeat_time = 0 ; kp->btn_repeat_next = kp->btn_repeat_delay ; //----- Clear DPD data sp->dpd_valid_fg = 0 ; // Invalid 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 data 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 data of each object 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 ) ; //----- Miscellaneous kp->wpad_ring_idx = -1 ; // Process end of ring buffer kp->work_ct = 0 ; // Number of times DPD processing is performed per game frame sp->wpad_err = WPAD_ERR_NONE ; // Since there is no error for the time being if ( kp->wpad_chan_no < 0 ) { WPADStopMotor( kp->wpad_chan_no + WPAD_MAX_CONTROLLERS ) ; } else { WPADStopMotor( kp->wpad_chan_no ) ; } //----- Clear extended controller information switch( sp->dev_type ) { case WPAD_DEV_FREESTYLE: //----- Nunchaku 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: //----- for a 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 ; } } /******************************************************************************* Calibrate *******************************************************************************/ 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 position after calibration ***********************************************************************/ //----- Determine the order of marks by position 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 horizontal 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 become zero kp->sec_nrm_hori.x = vx * f1 ; kp->sec_nrm_hori.y = vy * f1 ; /*********************************************************************** Distance after calibration ***********************************************************************/ kp->dist_org = kp_dist_vv1 * f1 ; /*********************************************************************** Miscellaneous ***********************************************************************/ sp->dpd_valid_fg = 0 ; // Invalid for the time being return ( kp->valid_objs ) ; } /******************************************************************************* Calibrate *******************************************************************************/ 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 ) ; } /******************************************************************************* interpolation based on Sensor Bar position *******************************************************************************/ void KPADGetCalibratedDPD( Vec2* dst, const Vec2* src ) { f32 result_y; const f32 overlay = 0.25f; const f32 half = ( 1.0f + overlay ) / 2.0f; const f32 offset = half - overlay; if ( WPAD_SENSOR_BAR_POS_TOP == WPADGetSensorBarPosition() ) { // when the DPD marker is above result_y = ( src->y - offset ) / half; } else { // when the DPD marker is below result_y = ( src->y + offset ) / half; } if ( dst != src ) { *dst = *src; } dst->y = result_y; } /******************************************************************************* Repeat processing for digital buttons *******************************************************************************/ 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 due to change in button status kp->btn_cl_repeat_time = 0 ; kp->btn_cl_repeat_next = kp->btn_cl_repeat_delay ; //----- Set flag even when first being held (only when a repeat setting is used) 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 ) { //----- Time goes on since button is being held without change in button status 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 when repeat time is reached if ( kp->btn_cl_repeat_time >= kp->btn_cl_repeat_next ) { sp->ex_status.cl.hold |= KPAD_BUTTON_RPT ; //Set the following repeat time kp->btn_cl_repeat_next += kp->btn_cl_repeat_pulse ; //----- Loop here if the time exceeds the allowed range 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 due to change in button status kp->btn_repeat_time = 0 ; kp->btn_repeat_next = kp->btn_repeat_delay ; //----- Set flag even when first being held (only when a repeat setting is used) if ( sp->trig && kp->btn_repeat_pulse ) { sp->hold |= KPAD_BUTTON_RPT ; } } else if ( sp->hold != 0 ) { //----- Time goes on since button is being held without change in button status 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 when repeat time is reached if ( kp->btn_repeat_time >= kp->btn_repeat_next ) { sp->hold |= KPAD_BUTTON_RPT ; //Set the following repeat time kp->btn_repeat_next += kp->btn_repeat_pulse ; //----- Loop here if the time exceeds the allowed range 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 ; } } } } } /******************************************************************************* Process for loading KPAD button data *******************************************************************************/ 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 ; //----- Save 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 ) { //----- Disables the port if the controller is disconnected if ( fp->err == WPAD_ERR_NO_CONTROLLER ) { kp->wpad_chan_no -= WPAD_MAX_CONTROLLERS ; reset_kpad( kp ) ; return ; } //----- Inherits the previous status 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 ) { //----- Disables the port if the controller is disconnected if ( cp->err == WPAD_ERR_NO_CONTROLLER ) { kp->wpad_chan_no -= WPAD_MAX_CONTROLLERS ; reset_kpad( kp ); return ; } //----- Inherits the previous status 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 ) { //----- Disables the port if the controller is disconnected if ( wp->err == WPAD_ERR_NO_CONTROLLER ) { kp->wpad_chan_no -= WPAD_MAX_CONTROLLERS ; reset_kpad( kp ) ; return ; } //----- Inherits the previous status for other errors } else { //----- Load new value sp->hold = (u32)wp->button ; } } //----- Process button status change_fg = sp->hold ^ old_fg ; // Change button sp->trig = change_fg & sp->hold ; // Held 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 ); } } /******************************************************************************* Sensitivity processing for acceleration *******************************************************************************/ static void calc_acc( KPADInsideStatus *kp, f32 *acc, f32 acc2 ) { f32 f1,f2 ; //----- Amount short of target value f2 = acc2 - *acc ; if ( f2 < 0.0f ) { f1 = -f2 ; } else { f1 = f2 ; } //----- Calculate interpolation inside/outside play radius if ( f1 >= kp->acc_play_radius ) { //----- Apply 100% sensitivity if outside the play radius f1 = 1.0f ; } else { //----- Weaken sensitivity as the target is approached if inside the play radius f1 /= kp->acc_play_radius ; f1 *= f1 ; // Raised to the 2nd power f1 *= f1 ; // Raised to the 4th power } f1 *= kp->acc_sensitivity ; //----- Interpolate *acc += f1 * f2 ; } /******************************************************************************* Calculate orientation of controller based on acceleration *******************************************************************************/ static void calc_acc_horizon( KPADInsideStatus *kp ) { f32 f1, vx,vy, ax,ay ; //----- Normalization of XY acceleration 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 ; //----- Power increases as the length of XY acceleration approaches 1 if ( f1 > 1.0f ) { f1 = 2.0f - f1 ; } f1 *= f1 * kp_acc_horizon_pw ; //----- Target orientation 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 ; //----- Approach ax = ( vx - kp->acc_horizon.x ) * f1 + kp->acc_horizon.x ; ay = ( vy - kp->acc_horizon.y ) * f1 + kp->acc_horizon.y ; //----- Normalize f1 = sqrtf( ax * ax + ay * ay ) ; if ( f1 == 0.0f ) return ; kp->acc_horizon.x = ax / f1 ; kp->acc_horizon.y = ay / f1 ; //----- Update coordinates to use for determining the controller is at rest 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 orientation 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 ; //----- Power increases as the length of acceleration approaches 1 if ( f1 > 1.0f ) { f1 = 2.0f - f1 ; } f1 *= f1 * kp_acc_horizon_pw ; //----- Approach ax = ( ax - sp->acc_vertical.x ) * f1 + sp->acc_vertical.x ; ay = ( ay - sp->acc_vertical.y ) * f1 + sp->acc_vertical.y ; //----- Normalize f1 = sqrtf( ax * ax + ay * ay ) ; if ( f1 == 0.0f ) return ; sp->acc_vertical.x = ax / f1 ; sp->acc_vertical.y = ay / f1 ; } /******************************************************************************* Process for loading KPAD acceleration data *******************************************************************************/ 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 ; // ここに列挙している値は、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; if(WPADProbe(chan, &type)==WPAD_ERR_NONE) { // コントローラから1Gの値を取得し、個体差の吸収をねらう。 WPADAcc core1G = {1, 1, 1}; WPADGetAccGravityUnit( chan, WPAD_DEV_CORE, &core1G); if(core1G.x * core1G.y * core1G.z != 0){ // バグ対策 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){ // バグ対策 fs_acc_scale_x = 1.0f / fs1G.x; fs_acc_scale_y = 1.0f / fs1G.y; fs_acc_scale_z = 1.0f / fs1G.z; } } } if ( kp->wpad_chan_no < 0 ) return ; //----- Load data if ( dev_type == WPAD_DEV_FREESTYLE ) { fp = (WPADFSStatus*)vp ; if ( fp->err != WPAD_ERR_NONE ) { //----- Disables the port if the controller is disconnected if ( fp->err == WPAD_ERR_NO_CONTROLLER ) { kp->wpad_chan_no -= WPAD_MAX_CONTROLLERS ; reset_kpad( kp ) ; return ; } } else { //----- Update raw values 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 ( wp->err != WPAD_ERR_NONE ) { //----- Disables the port if the controller is disconnected if ( wp->err == WPAD_ERR_NO_CONTROLLER ) { kp->wpad_chan_no -= WPAD_MAX_CONTROLLERS ; reset_kpad( kp ) ; return ; } } else { //----- Update raw values 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 ) ; } } //----- Save temporarily vec = sp->acc ; //----- Acceleration sensitivity processing 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 ) ; //----- Amount of change in acceleration 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 orientation of controller based on raw velocity calc_acc_horizon( kp ) ; calc_acc_vertical( kp ) ; /*********************************************************************** Load acceleration for nunchaku unit ***********************************************************************/ if ( dev_type != WPAD_DEV_FREESTYLE ) return ; //----- Save temporarily vec = sp->ex_status.fs.acc ; //----- Acceleration sensitivity processing 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 ) ; //----- Amount of change in acceleration 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 ) ; } /******************************************************************************* Convert 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 data 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 ) ; } /******************************************************************************* Invalidate object outside frame *******************************************************************************/ 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 ) ; } /******************************************************************************* Invalidate object with same coordinates *******************************************************************************/ 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 ; // Take as an error only on one side } } while ( ++op2 <= &kobj_t[ WPAD_DPD_MAX_OBJECTS - 1 ] ) ; } while ( ++op1 < &kobj_t[ WPAD_DPD_MAX_OBJECTS - 1 ] ) ; } /******************************************************************************* Calculate controller orientation based on two points (returns 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 become 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 ) ; } /******************************************************************************* First, select two marks *******************************************************************************/ 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 ) ; //----- Check the operating distance range 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 ] ) ; //----- Confirm as regular marks? if ( max == kp_err_first_inpr ) return (0) ; kp->kobj_regular[0] = *rp1 ; kp->kobj_regular[1] = *rp2 ; return (2) ; // Two marks recognized } /******************************************************************************* Next, select two marks using only section information *******************************************************************************/ 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 ; //----- Search for the previous slope and the closest two points in distance op1 = kp->kobj_sample ; do { if ( op1->error_fg != 0 ) continue ; op2 = op1 + 1 ; do { if ( op2->error_fg != 0 ) continue ; //----- Calculate direction 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 ; //----- Check the operating distance range f1 *= kp_dist_vv1 ; // Distance if ( f1 <= kp_err_dist_min || f1 >= kp_err_dist_max ) continue ; //----- Check the amount of change in distance 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 //----- Check the amount of change in slope f2 = kp->sec_nrm.x * nrm.x + kp->sec_nrm.y * nrm.y ; if ( f2 < 0.0f ) { f2 = -f2 ; rev_fg = 1 ; // Handled by reversing orientation (op2 -> op1) } else { rev_fg = 0 ; // Handled as-is (op1 -> op2) } if ( f2 <= kp_err_next_inpr ) continue ; f2 = ( 1.0f - f2 ) / ( 1.0f - kp_err_next_inpr ) ; // Slope error rate //----- Record the object with the smallest error f1 += f2 ; // Make determination by adding the distance error rate and slope 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 ] ) ; //----- Confirm as regular marks? if ( min == 2.0f ) return (0) ; kp->kobj_regular[0] = *rp1 ; kp->kobj_regular[1] = *rp2 ; return (2) ; // Two marks recognized } /******************************************************************************* First, select one mark *******************************************************************************/ static s8 select_1obj_first( KPADInsideStatus *kp ) { KPADObject *op1 ; f32 vx,vy ; Vec2 p1,p2 ; //----- Find 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 ; //----- Find section vector vx *= kp->trust_sec_length ; vy *= kp->trust_sec_length ; //----- Search for a point where the predicted point lies on the outside op1 = kp->kobj_sample ; do { if ( op1->error_fg != 0 ) continue ; p1.x = op1->center.x - vx ; // Predicted point on the left p1.y = op1->center.y - vy ; p2.x = op1->center.x + vx ; // Predicted point on 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 predicted point on the left is on the outside, the predicted point on the right must be on the 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 might be the 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) ; // Tentative recognition of one mark } } else { //----- If the predicted point on the left is on the inside, the predicted point on the right must be on the 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 might be the 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) ; // Tentative recognition of one mark } } } while ( ++op1 < &kp->kobj_sample[ WPAD_DPD_MAX_OBJECTS ] ) ; return (0) ; } /******************************************************************************* Next, select one mark *******************************************************************************/ 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 the mark closest to the past regular mark op1 = kp->kobj_regular ; do { if ( op1->error_fg != 0 ) continue ; if ( op1->state_fg != 0 ) continue ; // Predicted point no good 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 ] ) ; //----- Confirm as regular marks? if ( min == kp_err_near_pos * kp_err_near_pos ) return (0) ; *rp1 = *rp2 ; //----- Calculate orientation based on velocity 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 the coordinates of the predicted point 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) ; // Tentative recognition of one mark } else { return (1) ; // One mark recognized } } /******************************************************************************* Calculate orientation of controller based on the 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 become 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 variables used in the application *******************************************************************************/ 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 the orientation of the controller ***********************************************************************/ //----- 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 ; //----- Take sensitivity and play into account when approaching the target value if ( sp->dpd_valid_fg == 0 ) { //----- Initialize using unchanged values during initial pointing sp->horizon = pos ; sp->hori_vec = Vec2_0 ; sp->hori_speed = 0.0f ; } else { //----- Amount short of target value vec.x = pos.x - sp->horizon.x ; vec.y = pos.y - sp->horizon.y ; f1 = sqrtf( vec.x * vec.x + vec.y * vec.y ) ; //----- Calculate interpolation inside/outside play radius if ( f1 >= kp->hori_play_radius ) { //----- Apply 100% sensitivity if outside the play radius f1 = 1.0f ; } else { //----- Weaken sensitivity as the target is approached if inside the play radius f1 /= kp->hori_play_radius ; f1 *= f1 ; // Raised to the 2nd power f1 *= f1 ; // Raised to the 4th power } f1 *= kp->hori_sensitivity ; //----- Interpolate 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 vector slope 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 to the TV ***********************************************************************/ //----- Calculate the target value dist = kp_dist_vv1 / kp->sec_length ; //----- Take sensitivity and play into account when approaching the target value if ( sp->dpd_valid_fg == 0 ) { //----- Initialize using unchanged values during initial pointing sp->dist = dist ; sp->dist_vec = 0.0f ; sp->dist_speed = 0.0f ; } else { //----- Amount short of target value f2 = dist - sp->dist ; if ( f2 < 0.0f ) { f1 = -f2 ; } else { f1 = f2 ; } //----- Calculate interpolation inside/outside play radius if ( f1 >= kp->dist_play_radius ) { //----- Apply 100% sensitivity if outside the play radius f1 = 1.0f ; } else { //----- Weaken sensitivity as the target is approached if inside the play radius f1 /= kp->dist_play_radius ; f1 *= f1 ; // Raised to the 2nd power f1 *= f1 ; // Raised to the 4th power } f1 *= kp->dist_sensitivity ; //----- Interpolate 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 position ***********************************************************************/ //----- Coordinate of center point between 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 by the amount the controller is twisted 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 ; //----- Scaling by correcting 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 gravity-oriented coordinate system during 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 ; //----- Take sensitivity and play into account when approaching the target value if ( sp->dpd_valid_fg == 0 ) { //----- Initialize using unchanged values during initial pointing sp->pos = pos ; sp->vec = Vec2_0 ; sp->speed = 0.0f ; } else { //----- Amount short of target value vec.x = pos.x - sp->pos.x ; vec.y = pos.y - sp->pos.y ; f1 = sqrtf( vec.x * vec.x + vec.y * vec.y ) ; //----- Calculate interpolation inside/outside play radius if ( f1 >= kp->pos_play_radius ) { //----- Apply 100% sensitivity if outside the play radius f1 = 1.0f ; } else { //----- Weaken sensitivity as the target is approached if inside the play radius f1 /= kp->pos_play_radius ; f1 *= f1 ; // Raised to the 2nd power f1 *= f1 ; // Raised to the 4th power } f1 *= kp->pos_sensitivity ; //----- Interpolate 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 ; } /******************************************************************************* Process for loading KPAD DPD data *******************************************************************************/ 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 ; if ( kp->wpad_chan_no < 0 ) return ; /*********************************************************************** Convert WPAD object to KPAD ***********************************************************************/ if ( dev_type == WPAD_DEV_FREESTYLE ) { fp = (WPADFSStatus*)vp ; if ( fp->err != WPAD_ERR_NONE ) { //----- Disables the port if the controller is disconnected if ( fp->err == WPAD_ERR_NO_CONTROLLER ) { kp->wpad_chan_no -= WPAD_MAX_CONTROLLERS ; reset_kpad( kp ) ; return ; } //----- Inherits the previous status for other errors } else { //----- Convert WPAD object to KPAD get_kobj( kp, fp, WPAD_DEV_FREESTYLE ) ; } } else if ( dev_type == WPAD_DEV_CLASSIC ) { cp = (WPADCLStatus*)vp ; if ( cp->err != WPAD_ERR_NONE ) { //----- Disables the port if the controller is disconnected if ( cp->err == WPAD_ERR_NO_CONTROLLER ) { kp->wpad_chan_no -= WPAD_MAX_CONTROLLERS ; reset_kpad( kp ) ; return ; } //----- Inherits the previous status for other errors } else { //----- Convert WPAD object to KPAD get_kobj( kp, cp, WPAD_DEV_CLASSIC ) ; } } else { wp = (WPADStatus*)vp ; if ( wp->err != WPAD_ERR_NONE ) { //----- Disables the port if the controller is disconnected if ( wp->err == WPAD_ERR_NO_CONTROLLER ) { kp->wpad_chan_no -= WPAD_MAX_CONTROLLERS ; reset_kpad( kp ) ; return ; } //----- Inherits the previous status for other errors } else { //----- Convert WPAD object to KPAD get_kobj( kp, wp, WPAD_DEV_CORE ) ; } } /*********************************************************************** Select out normal objects ***********************************************************************/ //----- Omit strange objects check_kobj_outside_frame( kp->kobj_sample ) ; // Invalidate object outside frame check_kobj_same_position( kp->kobj_sample ) ; // Invalidate objects with same coordinates //----- Check how many are being displayed 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 ) ; //----- Validation processing if ( sp->acc_vertical.x <= kp_err_up_inpr ) goto LABEL_select_NG ; if ( sp->dpd_valid_fg == 2 || sp->dpd_valid_fg == -2 ) { //----- Validated two objects on last pass 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 ) { //----- Validated previous one object 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 { //----- Couldn't validate any objects 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 ; // Could not be selected LABEL_select_OK: // Could perhaps be selected //----- Update section data if the object could be selected if ( valid_fg_next ) { //----- Calculate data for the two points and the object orientation calc_obj_horizon( kp ) ; //----- Error if clearly different from the acceleration slope 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 as suspected kp->kobj_regular[0].error_fg = kp->kobj_regular[1].error_fg = 1 ; } } //----- Count for two consecutive valid points 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 variables used in the application calc_dpd_variable( kp, valid_fg_next ) ; } /******************************************************************************* Process analog trigger clamp *******************************************************************************/ 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 ) ; } } /******************************************************************************* Process stick clamp *******************************************************************************/ 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 GC 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 ; //----- Process analog data 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 ) ; //----- Process button data old_fg = kstatus->hold & KPAD_BUTTON_MASK ; // Save last value kstatus->hold = (u32)pstatus.button ; // Load new value change_fg = kstatus->hold ^ old_fg ; // Change button kstatus->trig = change_fg & kstatus->hold ; // Held button kstatus->release = change_fg & old_fg ; // Released button //----- Repeat processing calc_button_repeat( &inside_kpads[ chan ], kstatus, 0 ) ; } /******************************************************************************* Read the stick information *******************************************************************************/ static void read_kpad_stick( KPADInsideStatus *kp, void *vp ) { KPADStatus *sp = &kp->status ; WPADFSStatus *fp; WPADCLStatus *cp; //----- Error check if ( kp->wpad_chan_no < 0 ) return ; if ( sp->dev_type == WPAD_DEV_FREESTYLE ) { fp = (WPADFSStatus*)vp; if ( fp->err != WPAD_ERR_NONE ) { //----- Disables the port if the controller is disconnected if ( fp->err == WPAD_ERR_NO_CONTROLLER ) { kp->wpad_chan_no -= WPAD_MAX_CONTROLLERS ; reset_kpad( kp ) ; return ; } } //----- Process stick data 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 ( cp->err != WPAD_ERR_NONE ) { //----- Disables the port if the controller is disconnected if ( cp->err == WPAD_ERR_NO_CONTROLLER ) { kp->wpad_chan_no -= WPAD_MAX_CONTROLLERS ; reset_kpad( kp ) ; return ; } } //----- Process stick data 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 ) ; } } /******************************************************************************* Device check (wpad_chan_no is modified) *******************************************************************************/ 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 ) { //----- Nothing found if ( kp->wpad_chan_no >= 0 ) { //----- Pause auto sampling WPADSetAutoSamplingBuf( chan, NULL, 0 ) ; kp->wpad_chan_no = (s16)( chan - WPAD_MAX_CONTROLLERS ) ; kp->status.dev_type = WPAD_DEV_UNKNOWN ; } return ( -1 ) ; // Unconnected } else if ( err != WPAD_ERR_NONE ) { //----- Check impossible return ( 0 ) ; // Assume no problem for the time being } else if ( type == kp->status.dev_type ) { //----- No problem with the same device as before return ( 0 ) ; // No problem } /*********************************************************************** Device has changed ***********************************************************************/ //----- Pause auto sampling WPADSetAutoSamplingBuf( chan, NULL, 0 ) ; //----- Initialize new device switch ( type ) { case WPAD_DEV_CORE: WPADControlDpd( chan, WPAD_DPD_EXP, NULL ) ; WPADSetDataFormat( chan, WPAD_FMT_CORE_ACC_DPD); WPADSetAutoSamplingBuf( chan, kp->wpad_ring_bf, KPAD_RING_BUFS ) ; break ; case WPAD_DEV_FREESTYLE: WPADControlDpd( chan, WPAD_DPD_STD, NULL ) ; WPADSetDataFormat( chan, WPAD_FMT_FREESTYLE_ACC_DPD ) ; WPADSetAutoSamplingBuf( chan, kp->wpad_ring_bf, KPAD_RING_BUFS ) ; break ; case WPAD_DEV_CLASSIC: WPADControlDpd( chan, WPAD_DPD_STD, NULL ) ; WPADSetDataFormat( chan, WPAD_FMT_CLASSIC_ACC_DPD ) ; WPADSetAutoSamplingBuf( chan, kp->wpad_ring_bf, KPAD_RING_BUFS ) ; break; #if 0 case WPAD_DEV_DOLPHIN: WPADSetDataFormat( chan, WPAD_FMT_DOLPHIN ) ; break ; #endif default: kp->wpad_chan_no = (s16)( chan - WPAD_MAX_CONTROLLERS ) ; return ( -1 ) ; // Unexpected device } //Switch to enabled kp->wpad_chan_no = (s16)chan ; kp->status.dev_type = type ; reset_kpad( kp ) ; return ( 1 ) ; // Change has occured with device } /******************************************************************************* READ KPAD (return the number of loaded buffers) *******************************************************************************/ 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)); /*************************************************************** Set device type ***************************************************************/ sp = &samplingBufs[ length - 1 ] ; do { sp->dev_type = kp->status.dev_type ; } while ( --sp >= samplingBufs ) ; /*************************************************************** Check device ***************************************************************/ if ( check_device( chan, kp ) ) { //----- Does nothing if there have been changes and the device is unconnected return ( 0 ) ; } /*************************************************************** Extended controller processing ***************************************************************/ if ( kp->status.dev_type != WPAD_DEV_CORE && kp->status.dev_type != WPAD_DEV_FREESTYLE && kp->status.dev_type != WPAD_DEV_CLASSIC) { //----- a device other than known ones ASSERT("Unknown device."); return ( 0 ) ; } /*************************************************************** Check the ring buffer inside the scope of current processing ***************************************************************/ //----- idx_old: Previously processed buffer -> idx_now: Most recent 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 ) { // At start of processing idx_old = idx_now - 1 ; if ( idx_old < 0 ) idx_old = KPAD_RING_BUFS - 1 ; } kp->wpad_ring_idx = (s16)idx_now ; //----- Check the number of internal processes sample_ct = idx_now - idx_old ; if ( sample_ct == 0 ) { return (0) ; // Still no data to be processed } else if ( sample_ct < 0 ) { sample_ct += KPAD_RING_BUFS ; } //----- Check the number of times data is saved to the buffer if ( sample_ct < length ) { copy_ct = sample_ct ; } else { copy_ct = (s32)length ; } /*************************************************************** Processing performed for each frame sampled ***************************************************************/ sp = &samplingBufs[ copy_ct ] ; // Continue saving with old data at the rear kp->work_ct = 0 ; idx = idx_old ; if ( kp->status.dev_type == WPAD_DEV_CORE ) { do { //----- Get 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 data 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 ; // Backup storage buffer *sp = kp->status ; // Actually, button data is copied unnecessarily } } while ( ++ kp->work_ct < sample_ct ) ; } else if ( kp->status.dev_type == WPAD_DEV_FREESTYLE) { do { //----- Get 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 data 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 ; // Backup storage buffer *sp = kp->status ; // Actually, button data is copied unnecessarily } } while ( ++ kp->work_ct < sample_ct ) ; } else if ( kp->status.dev_type == WPAD_DEV_CLASSIC ) { do { //----- Get 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 data 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 ; // Backup storage buffer *sp = kp->status ; // Actually, button data is copied unnecessarily } } while ( ++ kp->work_ct < sample_ct ) ; } /*************************************************************** Processing performed for each game frame ***************************************************************/ //----- Load button data if ( kp->status.dev_type == WPAD_DEV_CORE ) { 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 ; ++ sp ; // Proceed forward because we're at the start of the storage buffer 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; } } 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] ; //----- Check device kp->status.dev_type = WPAD_DEV_UNKNOWN ; kp->wpad_chan_no = (s16)( i - WPAD_MAX_CONTROLLERS ) ; (void)check_device( i, kp ) ; //----- Initialize 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 ) ; //----- Initialize KPAD pointing conditions 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 ; //----- Initialize without 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 values KPADReset() ; OSRegisterVersion(__KPADVersion); } /******************************************************************************* RESET KPAD *******************************************************************************/ void KPADReset( void ) { KPADInsideStatus *kp ; //----- Recalculate constants 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 entire KPAD kp = &inside_kpads[ WPAD_MAX_CONTROLLERS - 1 ] ; do { reset_kpad( kp ) ; } while ( --kp >= inside_kpads ) ; }