/*---------------------------------------------------------------------------* Project: KPAD Sample Program File: mpls.c Programmer: Haruki Tojo Copyright 2008-2009 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 #include #include #include "main.h" #include "kfont.h" #include "graphic.h" #include "sample.h" /*************************************************************** Definitions ***************************************************************/ #define CMD_LEFT ( (kpads[0][0].trig & KPAD_BUTTON_LEFT ) != 0 ) // Cursor movement #define CMD_RIGHT ( (kpads[0][0].trig & KPAD_BUTTON_RIGHT) != 0 ) #define CMD_UP ( (kpads[0][0].trig & KPAD_BUTTON_UP ) != 0 ) #define CMD_DOWN ( (kpads[0][0].trig & KPAD_BUTTON_DOWN ) != 0 ) #define CMD_INC ( (kpads[0][0].trig & KPAD_BUTTON_A ) != 0 ) #define CMD_DEC ( (kpads[0][0].trig & KPAD_BUTTON_B ) != 0 ) #define CMD_RESET_ALL ( (kpads[0][0].trig & KPAD_BUTTON_1 ) != 0 ) #define CMD_RESET_ORIENT ( (kpads[0][0].trig & KPAD_BUTTON_2 ) != 0 ) #define CMD_CALIBRATE ( (kpads[0][0].trig & KPAD_BUTTON_HOME ) != 0 ) #define CMD_MOVE_CNT_INC ( (kpads[0][0].trig & KPAD_BUTTON_PLUS ) != 0 ) #define CMD_MOVE_CNT_DEC ( (kpads[0][0].trig & KPAD_BUTTON_MINUS) != 0 ) /*************************************************************** Adjustments ***************************************************************/ #define EDIT_DATA_MAX 13 // Edit data count static f32 zero_play_radius = 0.005f ; static f32 zero_play_mode =-1.000f ; static f32 zero_play_status ; static f32 zero_drift_mode = 1.00f ; static f32 zero_drift_status ; static f32 dir_rev_revise_pw = 0.500f ; static f32 dir_rev_mode =-1.000f ; static f32 dir_rev_status ; static f32 acc_rev_revise_pw = 0.030f ; static f32 acc_rev_revise_rg = 0.400f ; static f32 acc_rev_mode = 1.000f ; static f32 acc_rev_status ; static f32 dpd_rev_revise_pw = 0.050f ; static f32 dpd_rev_mode = 1.000f ; static f32 dpd_rev_status ; static f32 dps_mag_pitch = 1.000f ; static f32 dps_mag_yaw = 1.000f ; static f32 dps_mag_roll = 1.000f ; typedef struct { f32 *vp ; // Pointer to the variable f32 min ; // Variable minimum value f32 max ; // Variable maximum value u8 y ; // Y-cursor position u8 x[4] ; // X-cursor position for each digit f32 v[4] ; // Value change amount for each digit } EditData ; static EditData edit_data[ EDIT_DATA_MAX ] = { { &zero_play_radius, 0.000f,5.000f, 5, {23,25,26,27}, { 1.000f, 0.100f, 0.010f, 0.001f} }, // 0 { &zero_play_mode, -1.000f,1.000f, 5, {21,21,21,21}, { 2.000f, 2.000f, 2.000f, 2.000f} }, // 1 { &zero_drift_mode, 0.000f,2.000f, 6, {21,21,21,21}, { 1.000f, 1.000f, 1.000f, 1.000f} }, // 2 { &dir_rev_revise_pw, 0.000f,1.000f, 8, {23,25,26,27}, { 1.000f, 0.100f, 0.010f, 0.001f} }, // 3 { &dir_rev_mode, -1.000f,1.000f, 8, {21,21,21,21}, { 2.000f, 2.000f, 2.000f, 2.000f} }, // 4 { &acc_rev_revise_pw, 0.000f,1.000f, 9, {23,25,26,27}, { 1.000f, 0.100f, 0.010f, 0.001f} }, // 5 { &acc_rev_revise_rg, 0.000f,3.000f, 9, {29,31,32,33}, { 1.000f, 0.100f, 0.010f, 0.001f} }, // 6 { &acc_rev_mode, -1.000f,1.000f, 9, {21,21,21,21}, { 2.000f, 2.000f, 2.000f, 2.000f} }, // 7 { &dpd_rev_revise_pw, 0.000f,1.000f, 10, {23,25,26,27}, { 1.000f, 0.100f, 0.010f, 0.001f} }, // 8 { &dpd_rev_mode, -1.000f,1.000f, 10, {21,21,21,21}, { 2.000f, 2.000f, 2.000f, 2.000f} }, // 9 { &dps_mag_pitch, 0.000f,2.000f, 12, {17,19,20,21}, { 1.000f, 0.100f, 0.010f, 0.001f} }, // 10 { &dps_mag_yaw, 0.000f,2.000f, 12, {23,25,26,27}, { 1.000f, 0.100f, 0.010f, 0.001f} }, // 11 { &dps_mag_roll, 0.000f,2.000f, 12, {29,31,32,33}, { 1.000f, 0.100f, 0.010f, 0.001f} }, // 12 } ; /*************************************************************** Variables ***************************************************************/ //----- Screen rendering static const f32 O1Y = -1.5f ; static const f32 O2Y = -0.5f ; static const f32 O3Y = -1.0f ; static const f32 O4Y = -20.8f ; static const f32 O5Y = -20.8f ; static const f32 O6Y = -1.0f ; static const f32 O1X = 11.0f ; static const f32 O2X = 17.0f ; static const f32 O3X = 23.0f ; static const f32 O4X = 29.0f ; static const f32 O5X = 35.0f ; static const f32 dpd_disp_scale = 140.0f ; // DPD object display scale static const f32 dpd_disp_ox = 150.0f ; // DPD object display position static const f32 dpd_disp_oy = 195.0f+O5Y ; static const f32 PI = 3.141592653589793f ; //----- Base attitude for attitude correction (the attitude in which the Wii Remote is facing straight at the TV) static KPADMPDir dir_base = { 1.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 1.0f } ; //----- Calibration status static f32 mpls_calibration = -1.0f ; static Mtx view_mtx ; //----- Values for KPAD library settings static const f32 pointing_scale = 200.0f ; // Screen pointing scale static const f32 pos_play_radius = 0.00f ; // 'pos' sensitivity setting static const f32 pos_sensitivity = 1.00f ; static const f32 hori_play_radius = 0.00f ; // 'horizon' sensitivity setting static const f32 hori_sensitivity = 1.00f ; static const f32 dist_play_radius = 0.00f ; // 'dist' sensitivity setting static const f32 dist_sensitivity = 1.00f ; static const f32 acc_play_radius = 0.00f ; // 'acc' sensitivity setting static const f32 acc_sensitivity = 1.00f ; static const f32 repeat_delay_sec = 0.75f ; // Key repeat settings static const f32 repeat_pulse_sec = 0.25f ; //----- Cursor for adjustments static s32 csr_x, csr_y ; // Value adjustment cursor position static s32 csr_i, csr_k ; // Position of data index and digit currently being modified //----- Parameters for the cuboid static f32 rect_center = 0.0f ; // Center coordinates, depth-wise static f32 rect_length = 6.0f ; // Depth-wise length //----- WPAD data buffer static KPADUnifiedWpadStatus wpads[ WPAD_MAX_CONTROLLERS ][ KPAD_BUF_SIZE * 2 ] ; //----- Callback display s32 MyCallbackLatestIdx[ MY_CALLBACK_MAX ] = { -1, -1 } ; MyCallbackStatus MyCallbackStatusBuf[ MY_CALLBACK_MAX ][ MY_START_COUNTS_MAX ] ; vu32 MySamplingCount ; char *MyCbName[ MY_CALLBACK_MAX ] = { "DPDCB", "MPLSCB" } ; KPADUnifiedWpadStatus uniRingExBuf[ WPAD_MAX_CONTROLLERS ][ 32 ] ; /******************************************************************************* Initialization function *******************************************************************************/ void init_sample( void ) { u32 work_size ; void *work_ptr ; //----- Initialize KPAD WPADRegisterAllocator( alloc32, free32 ) ; KPADInitEx( (KPADUnifiedWpadStatus*)uniRingExBuf, sizeof(uniRingExBuf)/sizeof(KPADUnifiedWpadStatus) ) ; KPADSetControlMplsCallback( 0, mpls_callback ) ; KPADSetSamplingCallback( 0, sampling_callback ) ; //----- For the time being, start in the mode that uses the Wii MotionPlus only KPADEnableMpls( 0, WPAD_MPLS_STD ) ; //----- Set work area for calculating attitude work_size = KPADGetMplsWorkSize() ; work_ptr = alloc32( work_size ) ; if ( work_ptr == NULL ) { OSHalt("Cannot allocate work area!\n") ; } KPADSetMplsWorkarea( work_ptr ) ; //----- Default controller settings KPADReset() ; KPADSetPosParam ( 0, pos_play_radius, pos_sensitivity ) ; KPADSetHoriParam( 0, hori_play_radius, hori_sensitivity ) ; KPADSetDistParam( 0, dist_play_radius, dist_sensitivity ) ; KPADSetAccParam ( 0, acc_play_radius, acc_sensitivity ) ; KPADSetBtnRepeat( 0, repeat_delay_sec, repeat_pulse_sec ) ; //----- Cursor position initialization csr_i = 1 ; // Data index being edited csr_k = 0 ; // Digit being edited csr_x = edit_data[ csr_i ].x[ csr_k ] ; csr_y = edit_data[ csr_i ].y ; } /******************************************************************************* Cursor movement *******************************************************************************/ static void move_cursor( void ) { EditData *ep ; s32 vx,vy, wx,wy, fy ; s32 i,w,k, im,wm,km ; /*********************************************************************** Cursor movement ***********************************************************************/ //------ Temporary movement vector if ( CMD_LEFT ) vx = -1 ; else if ( CMD_RIGHT ) vx = 1 ; else vx = 0 ; if ( CMD_UP ) vy = -1 ; else if ( CMD_DOWN ) vy = 1 ; else vy = 0 ; //------ Move to the nearest digit location using edit data wm = 1 << 30 ; // Maximum distance i = EDIT_DATA_MAX - 1 ; do { ep = &edit_data[i] ; fy = ( wy = ep->y - csr_y ) * vy ; // Prepare to calculate dot product wy *= wy ; // Prepare to calculate distance k = 0 ; do { //----- Dot product must be positive wx = ep->x[k] - csr_x ; if ( wx * vx + fy <= 0 ) continue ; //----- Distance must be shortest w = wx * wx + wy ; if ( w >= wm ) continue ; //----- Record candidates wm = w ; im = i ; km = k ; } while ( ++k < 4 ) ; } while ( --i >= 0 ) ; //----- If candidates have been recorded, move cursor if ( wm != 1 << 30 ) { csr_i = im ; csr_k = km ; csr_x = edit_data[ csr_i ].x[ csr_k ] ; csr_y = edit_data[ csr_i ].y ; } } /******************************************************************************* Increasing/decreasing numerical values *******************************************************************************/ static void push_cursor( void ) { EditData *ep = &edit_data[ csr_i ] ; //----- Was a button pressed? if ( CMD_INC ) { *(ep->vp) += ep->v[ csr_k ] ; if ( *(ep->vp) > ep->max ) *(ep->vp) = ep->max ; } else if ( CMD_DEC ) { *(ep->vp) -= ep->v[ csr_k ] ; if ( *(ep->vp) < ep->min ) *(ep->vp) = ep->min ; } else { return ; // No value change } //----- Reset because value has changed switch ( csr_i ) { //----- zero play case 0 : KPADSetMplsZeroPlayParam( 0, zero_play_radius ) ; break ; case 1 : if ( zero_play_mode > 0.0f ) { KPADEnableMplsZeroPlay( 0 ) ; } else { KPADDisableMplsZeroPlay( 0 ) ; } break ; //----- zero drift case 2 : KPADSetMplsZeroDriftMode( 0, ( KPADMplsZeroDriftMode )zero_drift_mode ) ; break ; //----- dir revise case 3 : KPADSetMplsDirReviseParam( 0, dir_rev_revise_pw ) ; break ; case 4 : if ( dir_rev_mode > 0.0f ) { KPADSetMplsDirReviseBase( 0, &dir_base ) ; KPADEnableMplsDirRevise( 0 ) ; } else { KPADDisableMplsDirRevise( 0 ) ; } break ; //----- acc revise case 5 : case 6 : KPADSetMplsAccReviseParam( 0, acc_rev_revise_pw, acc_rev_revise_rg ) ; break ; case 7 : if ( acc_rev_mode > 0.0f ) { KPADEnableMplsAccRevise( 0 ) ; } else { KPADDisableMplsAccRevise( 0 ) ; } break ; //----- dpd revise case 8 : KPADSetMplsDpdReviseParam( 0, dpd_rev_revise_pw ) ; break ; case 9 : if ( dpd_rev_mode > 0.0f ) { KPADEnableMplsDpdRevise( 0 ) ; } else { KPADDisableMplsDpdRevise( 0 ) ; } break ; //----- dps magnification case 10 : case 11 : case 12 : KPADSetMplsMagnification( 0, dps_mag_pitch, dps_mag_yaw, dps_mag_roll ) ; break ; } } /******************************************************************************* KPAD Processing Functions *******************************************************************************/ void work_kpad( void ) { //----- Change the mode of the Wii MotionPlus if ( padsTrig[ 0 ].button & PAD_BUTTON_A ) { KPADEnableMpls( 0, WPAD_MPLS_STD ) ; } if ( padsTrig[ 0 ].button & PAD_BUTTON_B ) { KPADDisableMpls( 0 ) ; } if ( padsTrig[ 0 ].button & PAD_BUTTON_X ) { KPADEnableMpls( 0, WPAD_MPLS_FS ) ; } if ( padsTrig[ 0 ].button & PAD_BUTTON_Y ) { KPADEnableMpls( 0, WPAD_MPLS_CL ) ; } //----- Get KPAD data kpad_reads = KPADReadEx( 0, &kpads[ 0 ][ 0 ], KPAD_MAX_READ_BUFS, &kpad_err ) ; } /******************************************************************************* CPU Processing Functions *******************************************************************************/ void work_sample( void ) { KPADMplsZeroDriftMode zero_drift_init_mode ; if ( kpad_reads <= 0 ) { return ; } zero_play_status = KPADIsEnableMplsZeroPlay( 0 ) ; zero_drift_status = KPADIsEnableMplsZeroDrift( 0 ) ; dir_rev_status = KPADIsEnableMplsDirRevise( 0 ) ; acc_rev_status = KPADIsEnableMplsAccRevise( 0 ) ; dpd_rev_status = KPADIsEnableMplsDpdRevise( 0 ) ; mpls_calibration = KPADWorkMplsCalibration( 0 ) ; /*********************************************************************** Reset ***********************************************************************/ if ( CMD_RESET_ALL ) { KPADResetMpls( 0 ) ; KPADGetMplsZeroPlayParam ( 0, &zero_play_radius ) ; KPADGetMplsDirReviseParam( 0, &dir_rev_revise_pw ) ; KPADGetMplsAccReviseParam( 0, &acc_rev_revise_pw, &acc_rev_revise_rg ) ; KPADGetMplsDpdReviseParam( 0, &dpd_rev_revise_pw ) ; KPADGetMplsZeroDriftMode ( 0, &zero_drift_init_mode ) ; zero_play_mode = -1.0f ; zero_drift_mode = ( f32 ) zero_drift_init_mode ; dir_rev_mode =-1.0f ; acc_rev_mode = 1.0f ; dpd_rev_mode = 1.0f ; dps_mag_pitch = 1.000f ; dps_mag_yaw = 1.000f ; dps_mag_roll = 1.000f ; } /*********************************************************************** Reset the orientation ***********************************************************************/ if ( CMD_RESET_ORIENT ) { KPADSetMplsAngle( 0, 0.0f, 0.0f, 0.0f ) ; KPADSetMplsDirection( 0, &dir_base ) ; KPADSetMplsDirReviseBase( 0, &dir_base ) ; } /*********************************************************************** Change the mode of the Wii MotionPlus ***********************************************************************/ if ( CMD_CALIBRATE ) { KPADStartMplsCalibration( 0 ) ; } /*********************************************************************** Move the center of the cuboid ***********************************************************************/ if ( CMD_MOVE_CNT_INC ) { if ( ++ rect_center >= 3.0f ) { rect_center = 3.0f ; } } if ( CMD_MOVE_CNT_DEC ) { if ( -- rect_center <=-3.0f ) { rect_center = -3.0f ; } } /*********************************************************************** Value Editing ***********************************************************************/ move_cursor() ; // Cursor movement push_cursor() ; // Increasing/decreasing numerical values } /******************************************************************************* Render pointing coordinates *******************************************************************************/ static void draw_position( KPADStatus *sp ) { Vec2 pos, vec ; f32 scale ; if (sp->dpd_valid_fg <= 0) { return; } switch (sp->data_format) { case WPAD_FMT_CORE_ACC_DPD: case WPAD_FMT_FREESTYLE_ACC_DPD: case WPAD_FMT_CLASSIC_ACC_DPD: case WPAD_FMT_MPLS: break; default: return; } //------ Crosshair pos.x = pointing_scale * sp->pos.x ; // Center coordinate pos.y = pointing_scale * sp->pos.y ; scale = 72.0f ; vec.x = scale * sp->horizon.x ; // Vector pointing right vec.y = scale * sp->horizon.y ; draw_line( pos.x-vec.x,pos.y-vec.y, pos.x+vec.x,pos.y+vec.y, yellow_clr, 3 ) ; scale = 28.0f ; vec.x = scale * -sp->horizon.y ; // Vector pointing down vec.y = scale * sp->horizon.x ; draw_arrow( pos.x+vec.x,pos.y+vec.y, pos.x-vec.x,pos.y-vec.y, silver_clr, 3 ) ; //------ Center point draw_point( pos.x,pos.y, black_clr, 11 ) ; draw_point( pos.x,pos.y, cyan_clr, 9 ) ; } /******************************************************************************* Render Grids *******************************************************************************/ static void draw_grid( f32 ox, f32 oy, f32 radius, GXColor clr ) { f32 r = radius * 1.4f ; // Crosshair length draw_arrow( ox-r,oy, ox+r,oy, clr, 3 ) ; // Horizontal line draw_arrow( ox,oy+r, ox,oy-r, clr, 3 ) ; // Vertical line draw_circle( ox,oy, radius, clr, 3 ) ; // Circle } /******************************************************************************* Render Acceleration *******************************************************************************/ static void draw_acc( f32 ox, f32 oy, f32 radius, f32 ax, f32 ay, GXColor clr, BOOL dashed ) { f32 px = ax * radius + ox ; f32 py = ay * radius + oy ; if ( dashed ) { draw_dashed_arrow( ox,oy, px,py, clr, 4 ) ; } else { draw_arrow( ox,oy, px,py, clr, 4 ) ; } draw_point( px,py, white_clr, 4 ) ; } /******************************************************************************* Render DPD Sensor Frame *******************************************************************************/ static void draw_dpd_frame( void ) { f32 rx,ry, x1,y1, x2,y2 ; rx = dpd_disp_scale * 0.5f ; ry = rx * (f32)WPAD_DPD_IMG_RESO_WY / (f32)WPAD_DPD_IMG_RESO_WX ; x1 = dpd_disp_ox - rx ; y1 = dpd_disp_oy - ry ; x2 = dpd_disp_ox + rx ; y2 = dpd_disp_oy + ry ; GXSetTevColor( GX_TEVREG0, smoke_clr ) ; GXBegin( GX_QUADS, GX_VTXFMT0, 4 ) ; GXPosition2f32( x1, y1 ) ; GXPosition2f32( x2, y1 ) ; GXPosition2f32( x2, y2 ) ; GXPosition2f32( x1, y2 ) ; GXEnd() ; } /******************************************************************************* Render DPD objects *******************************************************************************/ static void draw_object( void ) { DPDObject *op ; f32 px,py, scale, ofx,ofy ; s32 cx ; KPADUnifiedWpadStatus *up ; //----- Calculate display position scale = dpd_disp_scale / (f32)WPAD_DPD_IMG_RESO_WX ; ofx = dpd_disp_scale * -0.5f ; ofy = ofx * (f32)WPAD_DPD_IMG_RESO_WY / (f32)WPAD_DPD_IMG_RESO_WX ; ofx += dpd_disp_ox ; ofy += dpd_disp_oy ; //----- Get WPAD data KPADGetUnifiedWpadStatus( 0, wpads[ 0 ], ( u32 )kpad_reads ) ; cx = kpad_reads ; while ( --cx >= 0 ) { up = &wpads[ 0 ][ cx ] ; if ( up->u.core.err == WPAD_ERR_NONE ) { op = up->u.core.obj + ( WPAD_DPD_MAX_OBJECTS - 1 ) ; do { if ( op->size == 0 ) continue ; //----- Change to screen coordinates px = (f32)op->x * scale + ofx ; py = (f32)op->y * scale + ofy ; //----- Render objects draw_point( px,py, white_clr, 5 ) ; draw_circle( px,py, 5, black_clr, 2 ) ; draw_circle( px,py, 4, smoke_clr, 2 ) ; draw_circle( px,py, 3, gray_clr, 2 ) ; } while ( --op >= up->u.core.obj ) ; } } } /******************************************************************************* Render analog stick *******************************************************************************/ static void draw_stick( f32 ox, f32 oy, f32 radius, f32 sx, f32 sy, GXColor clr, BOOL dashed ) { f32 px = sx * radius + ox; f32 py = sy * radius + oy; if ( dashed ) { draw_dashed_arrow( ox,oy, px,py, clr, 4 ) ; } else { draw_arrow( ox,oy, px,py, clr, 4 ) ; } draw_point( px,py, white_clr, 4 ) ; } /******************************************************************************* Render the Wii MotionPlus *******************************************************************************/ static void draw_mpls( f32 ox, f32 oy, f32 radius, f32 val, GXColor clr, BOOL dashed ) { f32 py =-val * radius + oy ; if ( dashed ) { draw_dashed_arrow( ox, oy, ox, py, clr, 3 ) ; } else { draw_arrow( ox, oy, ox, py, clr, 3 ) ; } draw_point( ox, py, white_clr, 3 ) ; } /******************************************************************************* Render the angle *******************************************************************************/ static void draw_angle( f32 ox, f32 oy, f32 radius, f32 angle, GXColor clr, BOOL dashed ) { f32 px = (f32)( sin(angle * 2 * PI) * radius + ox ) ; f32 py = (f32)( cos(angle * 2 * PI) * radius + oy ) ; if ( dashed ) { draw_dashed_arrow( ox,oy, px,py, clr, 3 ) ; } else { draw_arrow( ox,oy, px,py, clr, 3 ) ; } draw_point( px,py, white_clr, 3 ) ; } /******************************************************************************* Render value adjustment cursor *******************************************************************************/ static void draw_cursor( void ) { f32 x1,y1, x2,y2 ; f32 width = 0.0f ; f32 vp = *edit_data[ csr_i ].vp ; s32 cx = csr_x ; y1 = calc_kfont_y1( csr_y+O1Y ) ; y2 = calc_kfont_y2( csr_y+O1Y ) - 1.0f ; if ( csr_x == 21 && csr_y < 12 ) { cx -= 4 ; if ( csr_y == 6 ) width = 4.0f ; else if ( vp < 0.0 ) width = 2.0f ; else width = 1.0f ; } x1 = calc_kfont_x1( (f32)cx ) ; x2 = calc_kfont_x2( cx + width ) ; draw_line( x1,y1, x2,y1, gray_clr, 2 ) ; draw_line( x2,y1, x2,y2, gray_clr, 2 ) ; draw_line( x2,y2, x1,y2, gray_clr, 2 ) ; draw_line( x1,y2, x1,y1, gray_clr, 2 ) ; } /******************************************************************************* Render the sampling rate *******************************************************************************/ static void draw_sampling_rate( void ) { static OSTime time; static u32 countDisp; BOOL enabled; OSTime t; t = OSGetTime(); if (OSTicksToSeconds((OSTime)(t - time)) > 0) { time = t; enabled = OSDisableInterrupts(); countDisp = MySamplingCount; MySamplingCount = 0; (void)OSRestoreInterrupts( enabled ); } draw_kfont_letter( 48.0f,0.5f, white_clr, "SMP.RATE" ); draw_kfont_s32 ( 59.0f,0.5f, white_clr, (s32)countDisp ); } /******************************************************************************* Render the callback status *******************************************************************************/ static void draw_callback_status( void ) { u32 i ; MyCallbackStatus buf[ MY_START_COUNTS_MAX ] ; BOOL enabled ; char str[ 32 ] ; s32 latestIdx ; enabled = OSDisableInterrupts() ; for ( i = 0 ; i < MY_START_COUNTS_MAX ; i++ ) { if ( MyCallbackStatusBuf[ 1 ][ i ].count ) { MyCallbackStatusBuf[ 1 ][ i ].count-- ; } buf[ i ] = MyCallbackStatusBuf[ 1 ][ i ] ; } latestIdx = MyCallbackLatestIdx[ 1 ] ; ( void )OSRestoreInterrupts( enabled ) ; for ( i = 0 ; i < MY_START_COUNTS_MAX ; i++ ) { if ( (s32)i == latestIdx || buf[ i ].count ) { GXColor clr ; if ( buf[ i ].reason < 0 ) { clr = gray_clr ; } else { clr = white_clr ; } sprintf( str, "%s %d", MyCbName[ 1 ], buf[ i ].reason ) ; draw_kfont_letter( 0.0f, buf[ i ].count * 0.05f + 0.5f, clr, str ) ; } } } /******************************************************************************* Render the cuboid *******************************************************************************/ static void draw_rectangular_solid( f32 ox, f32 oy, f32 oz, f32 px, f32 py, f32 pz, Mtx v, KPADMPDir dir ) { Mtx ms ; // Scale matrix Mtx mr ; // Rotation matrix Mtx mv ; // View matrix Mtx mvi ; //----- Copy the dir values to the rotation matrix MTXRowCol( mr, 0, 0 ) = dir.X.x ; MTXRowCol( mr, 1, 0 ) = dir.X.y ; MTXRowCol( mr, 2, 0 ) = dir.X.z ; MTXRowCol( mr, 0, 1 ) = dir.Y.x ; MTXRowCol( mr, 1, 1 ) = dir.Y.y ; MTXRowCol( mr, 2, 1 ) = dir.Y.z ; MTXRowCol( mr, 0, 2 ) = dir.Z.x ; MTXRowCol( mr, 1, 2 ) = dir.Z.y ; MTXRowCol( mr, 2, 2 ) = dir.Z.z ; C_MTXConcat( v, mr, mv ) ; //----- Scaling C_MTXScale( ms, 25.0F, 25.0F, 25.0F ) ; C_MTXConcat( mv, ms, mv ) ; GXLoadPosMtxImm( mv, GX_PNMTX0 ) ; C_MTXInverse( mv, mvi ) ; C_MTXTranspose( mvi, mv ) ; GXLoadNrmMtxImm( mv, GX_PNMTX0 ) ; //----- Render the cuboid GXBegin(GX_QUADS, GX_VTXFMT3, 4*6) ; GXPosition3f32( ox, oy, oz ) ; GXNormal3f32 ( ox, 0.0f, 0.0f ); GXPosition3f32( ox, oy, pz ) ; GXNormal3f32 ( ox, 0.0f, 0.0f ); GXPosition3f32( ox, py, pz ) ; GXNormal3f32 ( ox, 0.0f, 0.0f ); GXPosition3f32( ox, py, oz ) ; GXNormal3f32 ( ox, 0.0f, 0.0f ); GXPosition3f32( px, oy, oz ) ; GXNormal3f32 ( px, 0.0f, 0.0f ); GXPosition3f32( px, py, oz ) ; GXNormal3f32 ( px, 0.0f, 0.0f ); GXPosition3f32( px, py, pz ) ; GXNormal3f32 ( px, 0.0f, 0.0f ); GXPosition3f32( px, oy, pz ) ; GXNormal3f32 ( px, 0.0f, 0.0f ); GXPosition3f32( ox, py, oz ) ; GXNormal3f32 ( 0.0f, py, 0.0f ); GXPosition3f32( ox, py, pz ) ; GXNormal3f32 ( 0.0f, py, 0.0f ); GXPosition3f32( px, py, pz ) ; GXNormal3f32 ( 0.0f, py, 0.0f ); GXPosition3f32( px, py, oz ) ; GXNormal3f32 ( 0.0f, py, 0.0f ); GXPosition3f32( ox, oy, pz ) ; GXNormal3f32 ( 0.0f, oy, 0.0f ); GXPosition3f32( ox, oy, oz ) ; GXNormal3f32 ( 0.0f, oy, 0.0f ); GXPosition3f32( px, oy, oz ) ; GXNormal3f32 ( 0.0f, oy, 0.0f ); GXPosition3f32( px, oy, pz ) ; GXNormal3f32 ( 0.0f, oy, 0.0f ); GXPosition3f32( px, py, oz ) ; GXNormal3f32 ( 0.0f, 0.0f, oz ); GXPosition3f32( px, oy, oz ) ; GXNormal3f32 ( 0.0f, 0.0f, oz ); GXPosition3f32( ox, oy, oz ) ; GXNormal3f32 ( 0.0f, 0.0f, oz ); GXPosition3f32( ox, py, oz ) ; GXNormal3f32 ( 0.0f, 0.0f, oz ); GXPosition3f32( px, py, pz ) ; GXNormal3f32 ( 0.0f, 0.0f, pz ); GXPosition3f32( ox, py, pz ) ; GXNormal3f32 ( 0.0f, 0.0f, pz ); GXPosition3f32( ox, oy, pz ) ; GXNormal3f32 ( 0.0f, 0.0f, pz ); GXPosition3f32( px, oy, pz ) ; GXNormal3f32 ( 0.0f, 0.0f, pz ); GXEnd() ; } /******************************************************************************* Render *******************************************************************************/ void draw_sample( void ) { s32 cx,cy ; f32 x1,x2, x3, y, r ; GXColor clr ; /*********************************************************************** Sampling rate ***********************************************************************/ init_draw_kfont( rmode_p->fbWidth, rmode_p->efbHeight, 10.6f, 15.8f ) ; draw_sampling_rate() ; /*********************************************************************** Manual ***********************************************************************/ init_draw_kfont( rmode_p->fbWidth, rmode_p->efbHeight, 10.6f, 15.8f ) ; cx = 38 ; cy = 3 ; draw_kfont_letter( (f32)cx,cy+O1Y, silver_clr, "-- KPADMPLS DEMO MAN --" ) ; ++ cy ; draw_kfont_letter( (f32)cx,cy+O1Y, yellow_clr, " WII REMOTE" ) ; ++ cy ; draw_kfont_letter( (f32)cx,cy+O1Y, silver_clr, "KEY :MOVE CURSOR" ) ; ++ cy ; draw_kfont_letter( (f32)cx,cy+O1Y, silver_clr, "A :INCREASE PARAM" ) ; ++ cy ; draw_kfont_letter( (f32)cx,cy+O1Y, silver_clr, "B :DECREASE PARAM" ) ; ++ cy ; draw_kfont_letter( (f32)cx,cy+O1Y, silver_clr, "1 :RESET" ) ; ++ cy ; draw_kfont_letter( (f32)cx,cy+O1Y, silver_clr, "2 :RESET ORIENT" ) ; ++ cy ; draw_kfont_letter( (f32)cx,cy+O1Y, silver_clr, "HOME :CALIBRATION" ) ; ++ cy ; draw_kfont_letter( (f32)cx,cy+O1Y, silver_clr, "+, - :MOVE OBJ" ) ; ++ cy ; draw_kfont_letter( (f32)cx,cy+O1Y, yellow_clr, " GC CONTROLLER" ) ; ++ cy ; draw_kfont_letter( (f32)cx,cy+O1Y, silver_clr, "A,X,Y:MPLS STD,FS,CL" ) ; ++ cy ; draw_kfont_letter( (f32)cx,cy+O1Y, silver_clr, "B :MPLS OFF" ) ; /*********************************************************************** Menu ***********************************************************************/ init_draw_kfont( rmode_p->fbWidth, rmode_p->efbHeight, 10.6f, 15.8f ) ; cx = 0 ; cy = 3 ; draw_kfont_letter( (f32)cx,cy+O1Y, silver_clr, "---- MPLS PRAMS ----" ) ; ++ cy ; draw_kfont_letter( cx+O1X,cy+O1Y, silver_clr, "STATS" ) ; draw_kfont_letter( cx+O2X,cy+O1Y, silver_clr, "MODE" ) ; draw_kfont_letter( cx+O3X,cy+O1Y, silver_clr, "RADIUS" ) ; ++ cy ; draw_kfont_letter( (f32)cx,cy+O1Y, silver_clr, "ZERO PLAY" ) ; (void)draw_kfont_f32 ( cx+O1X,cy+O1Y, cyan_clr, zero_play_status, 3 ) ; (void)draw_kfont_letter( cx+O2X,cy+O1Y, cyan_clr, (zero_play_mode < 0.0f) ? "OFF" : "ON" ) ; (void)draw_kfont_f32 ( cx+O3X,cy+O1Y, cyan_clr, zero_play_radius, 3 ) ; ++ cy ; draw_kfont_letter( (f32)cx,cy+O1Y, silver_clr, "ZERO DRIFT" ) ; (void)draw_kfont_f32 ( cx+O1X, cy+O1Y, violet_clr, zero_drift_status, 3 ) ; (void)draw_kfont_letter( cx+O2X, cy+O1Y, violet_clr,(zero_drift_mode == 0.0f) ? "LOOSE" : (zero_drift_mode == 1.0f) ? "STAND" : "TIGHT" ) ; ++ cy ; draw_kfont_letter( cx+O3X,cy+O1Y, silver_clr, "POWER" ) ; draw_kfont_letter( cx+O4X,cy+O1Y, silver_clr, "RANGE" ) ; ++ cy ; draw_kfont_letter( (f32)cx,cy+O1Y, silver_clr, "DIR REVISE" ) ; (void)draw_kfont_f32 ( cx+O1X,cy+O1Y, green_clr, dir_rev_status, 3 ) ; (void)draw_kfont_letter( cx+O2X,cy+O1Y, green_clr,(dir_rev_mode < 0.0f) ? "OFF" : "ON" ) ; (void)draw_kfont_f32 ( cx+O3X,cy+O1Y, green_clr, dir_rev_revise_pw, 3 ) ; ++ cy ; draw_kfont_letter( (f32)cx,cy+O1Y, silver_clr, "ACC REVISE" ) ; (void)draw_kfont_f32 ( cx+O1X,cy+O1Y, red_clr, acc_rev_status, 3 ) ; (void)draw_kfont_letter( cx+O2X,cy+O1Y, red_clr, (acc_rev_mode < 0.0f) ? "OFF" : "ON" ) ; (void)draw_kfont_f32 ( cx+O3X,cy+O1Y, red_clr, acc_rev_revise_pw, 3 ) ; (void)draw_kfont_f32 ( cx+O4X,cy+O1Y, red_clr, acc_rev_revise_rg, 3 ) ; ++ cy ; draw_kfont_letter( (f32)cx,cy+O1Y, silver_clr, "DPD REVISE" ) ; (void)draw_kfont_f32 ( cx+O1X,cy+O1Y, orange_clr, dpd_rev_status, 3 ) ; (void)draw_kfont_letter( cx+O2X,cy+O1Y, orange_clr, (dpd_rev_mode < 0.0f) ? "OFF" : "ON" ) ; (void)draw_kfont_f32 ( cx+O3X,cy+O1Y, orange_clr, dpd_rev_revise_pw, 3 ) ; ++ cy ; draw_kfont_letter( cx+O2X,cy+O1Y, silver_clr, "PITCH" ) ; draw_kfont_letter( cx+O3X,cy+O1Y, silver_clr, "YAW" ) ; draw_kfont_letter( cx+O4X,cy+O1Y, silver_clr, "ROLL" ) ; ++ cy ; draw_kfont_letter( (f32)cx,cy+O1Y, silver_clr, "DEGREE MAG" ) ; (void)draw_kfont_f32 ( cx+O2X,cy+O1Y, peagreen_clr, dps_mag_pitch, 3 ) ; (void)draw_kfont_f32 ( cx+O3X,cy+O1Y, peagreen_clr, dps_mag_yaw, 3 ) ; (void)draw_kfont_f32 ( cx+O4X,cy+O1Y, peagreen_clr, dps_mag_roll, 3 ) ; /*********************************************************************** Cursor ***********************************************************************/ init_draw_graphic( rmode_p->fbWidth, rmode_p->efbHeight ) ; draw_cursor() ; /*********************************************************************** Values for each direction ***********************************************************************/ cx = 4 ; cy = 14 ; init_draw_kfont( rmode_p->fbWidth, rmode_p->efbHeight, 12.6f, 16.0f ) ; draw_kfont_letter( (f32)cx,(f32)cy, gray_clr, "---- MPLS ----") ; cy = 19 ; draw_kfont_f32 ( (f32)cx ,(f32)cy, peagreen_clr, kpads[ 0 ][ 0 ].mpls.mpls.x, 2 ); draw_kfont_f32 ( cx+ 8.0f,(f32)cy, peagreen_clr, kpads[ 0 ][ 0 ].mpls.mpls.y, 2 ); draw_kfont_f32 ( cx+16.0f,(f32)cy, peagreen_clr, kpads[ 0 ][ 0 ].mpls.mpls.z, 2 ); init_draw_kfont( rmode_p->fbWidth, rmode_p->efbHeight, 12.0f, 15.0f ) ; draw_kfont_letter(5.0f, 17.2f+O3Y, gray_clr, "PIT YAW ROL") ; x1 = -250.0f ; // X-coordinate of left grid x2 = -160.0f ; // X-coordinate of the center grid x3 = -70.0f ; // X-coordinate of right grid y = 80.0f+O4Y ; // Common y-coordinate r = 25.0f ; // Common radius init_draw_graphic( rmode_p->fbWidth, rmode_p->efbHeight ) ; draw_grid( x1, y, r, gray_clr ) ; draw_grid( x2, y, r, gray_clr ) ; draw_grid( x3, y, r, gray_clr ) ; cx = kpad_reads ; while ( --cx >= 0 ) { draw_mpls( x1, y, r, kpads[ 0 ][ cx ].mpls.mpls.x, peagreen_clr, TRUE ) ; draw_mpls( x2, y, r, kpads[ 0 ][ cx ].mpls.mpls.y, peagreen_clr, TRUE ) ; draw_mpls( x3, y, r, kpads[ 0 ][ cx ].mpls.mpls.z, peagreen_clr, TRUE ) ; } /*********************************************************************** Angles in each direction ***********************************************************************/ cx = 4 ; cy = 20 ; init_draw_kfont( rmode_p->fbWidth, rmode_p->efbHeight, 12.6f, 16.0f ) ; draw_kfont_letter( (f32)cx,(f32)cy, gray_clr, "---- ANGLE ----") ; cy = 25 ; draw_kfont_f32 ( (f32)cx ,(f32)cy, yellow_clr, kpads[ 0 ][ 0 ].mpls.angle.x, 2 ); draw_kfont_f32 ( cx+ 8.0f,(f32)cy, yellow_clr, kpads[ 0 ][ 0 ].mpls.angle.y, 2 ); draw_kfont_f32 ( cx+16.0f,(f32)cy, yellow_clr, kpads[ 0 ][ 0 ].mpls.angle.z, 2 ); init_draw_kfont( rmode_p->fbWidth, rmode_p->efbHeight, 12.0f, 15.0f ) ; draw_kfont_letter(5.5f, 24.0f+O3Y, gray_clr, "PIT YAW ROL") ; x1 = -250.0f ; // X-coordinate of left grid x2 = -160.0f ; // X-coordinate of the center grid x3 = -70.0f ; // X-coordinate of right grid y = 180.0f+O4Y ; // Common y-coordinate r = 25.0f ; // Common radius init_draw_graphic( rmode_p->fbWidth, rmode_p->efbHeight ) ; draw_grid( x1, y, r, gray_clr ) ; draw_grid( x2, y, r, gray_clr ) ; draw_grid( x3, y, r, gray_clr ) ; cx = kpad_reads ; while ( --cx >= 0 ) { draw_angle( x1, y, r, kpads[ 0 ][ cx ].mpls.angle.x, yellow_clr, TRUE ) ; draw_angle( x2, y, r, kpads[ 0 ][ cx ].mpls.angle.y, yellow_clr, TRUE ) ; draw_angle( x3, y, r, kpads[ 0 ][ cx ].mpls.angle.z, yellow_clr, TRUE ) ; } /*********************************************************************** Acceleration ***********************************************************************/ cx = 32 ; cy = 14 ; init_draw_kfont( rmode_p->fbWidth, rmode_p->efbHeight, 12.6f, 16.0f ) ; draw_kfont_letter( (f32)cx,(f32)cy, gray_clr, "---- ACC ----") ; init_draw_kfont( rmode_p->fbWidth, rmode_p->efbHeight, 12.0f, 15.0f ) ; draw_kfont_letter(37.0f, 17.0f+O3Y, gray_clr, "Y Y") ; draw_kfont_letter(39.4f, 20.0f+O3Y, gray_clr, "X Z") ; x1 = 105.0f ; // X-coordinate of left grid x2 = 195.0f ; // X-coordinate of right grid y = 80.0f+O4Y ; // Common y-coordinate r = 25.0f ; // Common radius init_draw_graphic( rmode_p->fbWidth, rmode_p->efbHeight ) ; draw_grid( x1, y, r, gray_clr ) ; draw_grid( x2, y, r, gray_clr ) ; cx = kpad_reads ; while ( --cx >= 0 ) { draw_acc( x1, y, r, kpads[ 0 ][ cx ].acc.x, -kpads[ 0 ][ cx ].acc.y, red_clr, FALSE ) ; draw_acc( x2, y, r, kpads[ 0 ][ cx ].acc.z, -kpads[ 0 ][ cx ].acc.y, red_clr, FALSE ) ; if ( kpads[ 0 ][ cx ].wpad_err == WPAD_ERR_NONE && ( kpads[ 0 ][ cx ].dev_type == WPAD_DEV_FREESTYLE || ( kpads[ 0 ][ cx ].dev_type == WPAD_DEV_MPLS_FREESTYLE && KPADGetMplsStatus( 0 ) == WPAD_MPLS_FS ) ) ) { draw_acc( x1, y, r, kpads[ 0 ][ cx ].ex_status.fs.acc.x, -kpads[ 0 ][ cx ].ex_status.fs.acc.y, orange_clr, FALSE ) ; draw_acc( x2, y, r, kpads[ 0 ][ cx ].ex_status.fs.acc.z, -kpads[ 0 ][ cx ].ex_status.fs.acc.y, orange_clr, FALSE ) ; } } /*********************************************************************** Control Stick ***********************************************************************/ init_draw_graphic( rmode_p->fbWidth, rmode_p->efbHeight ) ; cx = kpad_reads ; while ( --cx >= 0 ) { if ( kpads[ 0 ][ cx ].wpad_err == WPAD_ERR_NONE ) { if ( kpads[ 0 ][ cx ].dev_type == WPAD_DEV_FREESTYLE || ( kpads[ 0 ][ cx ].dev_type == WPAD_DEV_MPLS_FREESTYLE && KPADGetMplsStatus( 0 ) == WPAD_MPLS_FS ) ) { draw_stick( x1, y, r, kpads[ 0 ][ cx ].ex_status.fs.stick.x, -kpads[ 0 ][ cx ].ex_status.fs.stick.y, orange_clr, TRUE ) ; } else if ( kpads[ 0 ][ cx ].dev_type == WPAD_DEV_CLASSIC || ( kpads[ 0 ][ cx ].dev_type == WPAD_DEV_MPLS_CLASSIC && KPADGetMplsStatus( 0 ) == WPAD_MPLS_CL ) ) { draw_stick( x1, y, r, kpads[ 0 ][ cx ].ex_status.cl.lstick.x, -kpads[ 0 ][ cx ].ex_status.cl.lstick.y, peagreen_clr, FALSE ) ; draw_stick( x2, y, r, kpads[ 0 ][ cx ].ex_status.cl.rstick.x, -kpads[ 0 ][ cx ].ex_status.cl.rstick.y, peagreen_clr, FALSE ) ; } } } /*********************************************************************** DPD Object Display ***********************************************************************/ cx = 32 ; cy = 20 ; init_draw_kfont( rmode_p->fbWidth, rmode_p->efbHeight, 12.6f, 16.0f ) ; draw_kfont_letter( (f32)cx,(f32)cy, gray_clr, "---- OBJ ----") ; init_draw_graphic( rmode_p->fbWidth, rmode_p->efbHeight ) ; draw_dpd_frame() ; draw_object() ; /*********************************************************************** Callbacks ***********************************************************************/ init_draw_kfont( rmode_p->fbWidth, rmode_p->efbHeight, 10.6f, 15.8f ) ; draw_callback_status() ; /*********************************************************************** Pointer ***********************************************************************/ init_draw_graphic( rmode_p->fbWidth, rmode_p->efbHeight ) ; cx = kpad_reads ; while ( --cx >= 0 ) { draw_position( &kpads[ 0 ][ cx ] ) ; } /*********************************************************************** Cuboid ***********************************************************************/ if ( ( kpads[ 0 ][ 0 ].dev_type != WPAD_DEV_MPLS && kpads[ 0 ][ 0 ].dev_type != WPAD_DEV_MPLS_FREESTYLE && kpads[ 0 ][ 0 ].dev_type != WPAD_DEV_MPLS_CLASSIC && kpads[ 0 ][ 0 ].dev_type != WPAD_DEV_MPLS_FUTURE ) || kpad_err == KPAD_READ_ERR_NO_CONTROLLER ) { clr = gray_clr ; } else { // Change the color depending on the device type switch ( kpads[ 0 ][ 0 ].dev_type ) { case WPAD_DEV_MPLS : clr = magenta_clr ; break ; case WPAD_DEV_MPLS_FREESTYLE : clr = orange_clr ; break ; case WPAD_DEV_MPLS_CLASSIC : clr = green_clr ; break ; case WPAD_DEV_MPLS_FUTURE : clr = violet_clr ; break ; default : clr = blue_clr ; break ; // Control will never reach here } // Change the alpha value to match the calibration status if ( mpls_calibration >= 0.0f ) { f32 alpha ; alpha = clr.a / 2.0f ; clr.a = ( u8 )( alpha * ( 2.0f - mpls_calibration ) ) ; } } // Display only the attitude with acceleration and DPD correction applied init_draw_graphic_3d( view_mtx, clr ) ; draw_rectangular_solid( -1.0f, 1.0f, rect_center - rect_length / 2.0f, 1.0f,-1.0f, rect_center + rect_length / 2.0f, view_mtx, kpads[ 0 ][ 0 ].mpls.dir ) ; }