1 /*---------------------------------------------------------------------------*
2 Project: KPAD Sample Program
3 File: mpls.c
4 Programmer: Haruki Tojo
5
6 Copyright 2008-2009 Nintendo. All rights reserved.
7
8 These coded instructions, statements, and computer programs contain
9 proprietary information of Nintendo of America Inc. and/or Nintendo
10 Company Ltd., and are protected by Federal copyright law. They may
11 not be disclosed to third parties or copied or duplicated in any form,
12 in whole or in part, without the prior written consent of Nintendo.
13 *---------------------------------------------------------------------------*/
14
15 #include <revolution.h>
16 #include <math.h>
17 #include <stdio.h>
18
19 #include <revolution/kpad.h>
20 #include "main.h"
21 #include "kfont.h"
22 #include "graphic.h"
23 #include "sample.h"
24
25 /***************************************************************
26 Definitions
27 ***************************************************************/
28 #define CMD_LEFT ( (kpads[0][0].trig & KPAD_BUTTON_LEFT ) != 0 ) // Cursor movement
29 #define CMD_RIGHT ( (kpads[0][0].trig & KPAD_BUTTON_RIGHT) != 0 )
30 #define CMD_UP ( (kpads[0][0].trig & KPAD_BUTTON_UP ) != 0 )
31 #define CMD_DOWN ( (kpads[0][0].trig & KPAD_BUTTON_DOWN ) != 0 )
32
33 #define CMD_INC ( (kpads[0][0].trig & KPAD_BUTTON_A ) != 0 )
34 #define CMD_DEC ( (kpads[0][0].trig & KPAD_BUTTON_B ) != 0 )
35
36 #define CMD_RESET_ALL ( (kpads[0][0].trig & KPAD_BUTTON_1 ) != 0 )
37 #define CMD_RESET_ORIENT ( (kpads[0][0].trig & KPAD_BUTTON_2 ) != 0 )
38 #define CMD_CALIBRATE ( (kpads[0][0].trig & KPAD_BUTTON_HOME ) != 0 )
39
40 #define CMD_MOVE_CNT_INC ( (kpads[0][0].trig & KPAD_BUTTON_PLUS ) != 0 )
41 #define CMD_MOVE_CNT_DEC ( (kpads[0][0].trig & KPAD_BUTTON_MINUS) != 0 )
42
43 /***************************************************************
44 Adjustments
45 ***************************************************************/
46 #define EDIT_DATA_MAX 13 // Edit data count
47
48 static f32 zero_play_radius = 0.005f ;
49 static f32 zero_play_mode =-1.000f ;
50 static f32 zero_play_status ;
51 static f32 zero_drift_mode = 1.00f ;
52 static f32 zero_drift_status ;
53 static f32 dir_rev_revise_pw = 0.500f ;
54 static f32 dir_rev_mode =-1.000f ;
55 static f32 dir_rev_status ;
56 static f32 acc_rev_revise_pw = 0.030f ;
57 static f32 acc_rev_revise_rg = 0.400f ;
58 static f32 acc_rev_mode = 1.000f ;
59 static f32 acc_rev_status ;
60 static f32 dpd_rev_revise_pw = 0.050f ;
61 static f32 dpd_rev_mode = 1.000f ;
62 static f32 dpd_rev_status ;
63 static f32 dps_mag_pitch = 1.000f ;
64 static f32 dps_mag_yaw = 1.000f ;
65 static f32 dps_mag_roll = 1.000f ;
66
67 typedef struct {
68 f32 *vp ; // Pointer to the variable
69 f32 min ; // Variable minimum value
70 f32 max ; // Variable maximum value
71 u8 y ; // Y-cursor position
72 u8 x[4] ; // X-cursor position for each digit
73 f32 v[4] ; // Value change amount for each digit
74 } EditData ;
75
76 static EditData edit_data[ EDIT_DATA_MAX ] = {
77 { &zero_play_radius, 0.000f,5.000f, 5, {23,25,26,27}, { 1.000f, 0.100f, 0.010f, 0.001f} }, // 0
78 { &zero_play_mode, -1.000f,1.000f, 5, {21,21,21,21}, { 2.000f, 2.000f, 2.000f, 2.000f} }, // 1
79 { &zero_drift_mode, 0.000f,2.000f, 6, {21,21,21,21}, { 1.000f, 1.000f, 1.000f, 1.000f} }, // 2
80 { &dir_rev_revise_pw, 0.000f,1.000f, 8, {23,25,26,27}, { 1.000f, 0.100f, 0.010f, 0.001f} }, // 3
81 { &dir_rev_mode, -1.000f,1.000f, 8, {21,21,21,21}, { 2.000f, 2.000f, 2.000f, 2.000f} }, // 4
82 { &acc_rev_revise_pw, 0.000f,1.000f, 9, {23,25,26,27}, { 1.000f, 0.100f, 0.010f, 0.001f} }, // 5
83 { &acc_rev_revise_rg, 0.000f,3.000f, 9, {29,31,32,33}, { 1.000f, 0.100f, 0.010f, 0.001f} }, // 6
84 { &acc_rev_mode, -1.000f,1.000f, 9, {21,21,21,21}, { 2.000f, 2.000f, 2.000f, 2.000f} }, // 7
85 { &dpd_rev_revise_pw, 0.000f,1.000f, 10, {23,25,26,27}, { 1.000f, 0.100f, 0.010f, 0.001f} }, // 8
86 { &dpd_rev_mode, -1.000f,1.000f, 10, {21,21,21,21}, { 2.000f, 2.000f, 2.000f, 2.000f} }, // 9
87 { &dps_mag_pitch, 0.000f,2.000f, 12, {17,19,20,21}, { 1.000f, 0.100f, 0.010f, 0.001f} }, // 10
88 { &dps_mag_yaw, 0.000f,2.000f, 12, {23,25,26,27}, { 1.000f, 0.100f, 0.010f, 0.001f} }, // 11
89 { &dps_mag_roll, 0.000f,2.000f, 12, {29,31,32,33}, { 1.000f, 0.100f, 0.010f, 0.001f} }, // 12
90 } ;
91
92
93 /***************************************************************
94 Variables
95 ***************************************************************/
96 //----- Screen rendering
97 static const f32 O1Y = -1.5f ;
98 static const f32 O2Y = -0.5f ;
99 static const f32 O3Y = -1.0f ;
100 static const f32 O4Y = -20.8f ;
101 static const f32 O5Y = -20.8f ;
102 static const f32 O6Y = -1.0f ;
103
104 static const f32 O1X = 11.0f ;
105 static const f32 O2X = 17.0f ;
106 static const f32 O3X = 23.0f ;
107 static const f32 O4X = 29.0f ;
108 static const f32 O5X = 35.0f ;
109
110 static const f32 dpd_disp_scale = 140.0f ; // DPD object display scale
111 static const f32 dpd_disp_ox = 150.0f ; // DPD object display position
112 static const f32 dpd_disp_oy = 195.0f+O5Y ;
113
114 static const f32 PI = 3.141592653589793f ;
115
116 //----- Base attitude for attitude correction (the attitude in which the Wii Remote is facing straight at the TV)
117 static KPADMPDir dir_base = {
118 1.0f, 0.0f, 0.0f,
119 0.0f, 1.0f, 0.0f,
120 0.0f, 0.0f, 1.0f
121 } ;
122
123 //----- Calibration status
124 static f32 mpls_calibration = -1.0f ;
125
126 static Mtx view_mtx ;
127
128 //----- Values for KPAD library settings
129 static const f32 pointing_scale = 200.0f ; // Screen pointing scale
130 static const f32 pos_play_radius = 0.00f ; // 'pos' sensitivity setting
131 static const f32 pos_sensitivity = 1.00f ;
132 static const f32 hori_play_radius = 0.00f ; // 'horizon' sensitivity setting
133 static const f32 hori_sensitivity = 1.00f ;
134 static const f32 dist_play_radius = 0.00f ; // 'dist' sensitivity setting
135 static const f32 dist_sensitivity = 1.00f ;
136 static const f32 acc_play_radius = 0.00f ; // 'acc' sensitivity setting
137 static const f32 acc_sensitivity = 1.00f ;
138 static const f32 repeat_delay_sec = 0.75f ; // Key repeat settings
139 static const f32 repeat_pulse_sec = 0.25f ;
140
141 //----- Cursor for adjustments
142 static s32 csr_x, csr_y ; // Value adjustment cursor position
143 static s32 csr_i, csr_k ; // Position of data index and digit currently being modified
144
145 //----- Parameters for the cuboid
146 static f32 rect_center = 0.0f ; // Center coordinates, depth-wise
147 static f32 rect_length = 6.0f ; // Depth-wise length
148
149 //----- WPAD data buffer
150 static KPADUnifiedWpadStatus wpads[ WPAD_MAX_CONTROLLERS ][ KPAD_BUF_SIZE * 2 ] ;
151
152 //----- Callback display
153 s32 MyCallbackLatestIdx[ MY_CALLBACK_MAX ] = { -1, -1 } ;
154 MyCallbackStatus MyCallbackStatusBuf[ MY_CALLBACK_MAX ][ MY_START_COUNTS_MAX ] ;
155 vu32 MySamplingCount ;
156 char *MyCbName[ MY_CALLBACK_MAX ] = { "DPDCB", "MPLSCB" } ;
157
158 KPADUnifiedWpadStatus uniRingExBuf[ WPAD_MAX_CONTROLLERS ][ 32 ] ;
159
160 /*******************************************************************************
161 Initialization function
162 *******************************************************************************/
init_sample(void)163 void init_sample( void )
164 {
165 u32 work_size ;
166 void *work_ptr ;
167
168 //----- Initialize KPAD
169 WPADRegisterAllocator( alloc32, free32 ) ;
170 KPADInitEx( (KPADUnifiedWpadStatus*)uniRingExBuf, sizeof(uniRingExBuf)/sizeof(KPADUnifiedWpadStatus) ) ;
171 KPADSetControlMplsCallback( 0, mpls_callback ) ;
172 KPADSetSamplingCallback( 0, sampling_callback ) ;
173 //----- For the time being, start in the mode that uses the Wii MotionPlus only
174 KPADEnableMpls( 0, WPAD_MPLS_STD ) ;
175 //----- Set work area for calculating attitude
176 work_size = KPADGetMplsWorkSize() ;
177 work_ptr = alloc32( work_size ) ;
178 if ( work_ptr == NULL ) {
179 OSHalt("Cannot allocate work area!\n") ;
180 }
181 KPADSetMplsWorkarea( work_ptr ) ;
182
183 //----- Default controller settings
184 KPADReset() ;
185 KPADSetPosParam ( 0, pos_play_radius, pos_sensitivity ) ;
186 KPADSetHoriParam( 0, hori_play_radius, hori_sensitivity ) ;
187 KPADSetDistParam( 0, dist_play_radius, dist_sensitivity ) ;
188 KPADSetAccParam ( 0, acc_play_radius, acc_sensitivity ) ;
189 KPADSetBtnRepeat( 0, repeat_delay_sec, repeat_pulse_sec ) ;
190
191 //----- Cursor position initialization
192 csr_i = 1 ; // Data index being edited
193 csr_k = 0 ; // Digit being edited
194 csr_x = edit_data[ csr_i ].x[ csr_k ] ;
195 csr_y = edit_data[ csr_i ].y ;
196 }
197
198 /*******************************************************************************
199 Cursor movement
200 *******************************************************************************/
move_cursor(void)201 static void move_cursor( void )
202 {
203 EditData *ep ;
204 s32 vx,vy, wx,wy, fy ;
205 s32 i,w,k, im,wm,km ;
206
207 /***********************************************************************
208 Cursor movement
209 ***********************************************************************/
210 //------ Temporary movement vector
211 if ( CMD_LEFT ) vx = -1 ;
212 else if ( CMD_RIGHT ) vx = 1 ;
213 else vx = 0 ;
214
215 if ( CMD_UP ) vy = -1 ;
216 else if ( CMD_DOWN ) vy = 1 ;
217 else vy = 0 ;
218
219 //------ Move to the nearest digit location using edit data
220 wm = 1 << 30 ; // Maximum distance
221 i = EDIT_DATA_MAX - 1 ;
222 do {
223 ep = &edit_data[i] ;
224
225 fy = ( wy = ep->y - csr_y ) * vy ; // Prepare to calculate dot product
226 wy *= wy ; // Prepare to calculate distance
227 k = 0 ;
228 do {
229 //----- Dot product must be positive
230 wx = ep->x[k] - csr_x ;
231 if ( wx * vx + fy <= 0 ) continue ;
232
233 //----- Distance must be shortest
234 w = wx * wx + wy ;
235 if ( w >= wm ) continue ;
236
237 //----- Record candidates
238 wm = w ;
239 im = i ;
240 km = k ;
241 } while ( ++k < 4 ) ;
242 } while ( --i >= 0 ) ;
243
244 //----- If candidates have been recorded, move cursor
245 if ( wm != 1 << 30 ) {
246 csr_i = im ;
247 csr_k = km ;
248 csr_x = edit_data[ csr_i ].x[ csr_k ] ;
249 csr_y = edit_data[ csr_i ].y ;
250 }
251 }
252
253
254 /*******************************************************************************
255 Increasing/decreasing numerical values
256 *******************************************************************************/
push_cursor(void)257 static void push_cursor( void )
258 {
259 EditData *ep = &edit_data[ csr_i ] ;
260
261 //----- Was a button pressed?
262 if ( CMD_INC ) {
263 *(ep->vp) += ep->v[ csr_k ] ;
264 if ( *(ep->vp) > ep->max ) *(ep->vp) = ep->max ;
265 } else if ( CMD_DEC ) {
266 *(ep->vp) -= ep->v[ csr_k ] ;
267 if ( *(ep->vp) < ep->min ) *(ep->vp) = ep->min ;
268 } else {
269 return ; // No value change
270 }
271
272 //----- Reset because value has changed
273 switch ( csr_i ) {
274 //----- zero play
275 case 0 : KPADSetMplsZeroPlayParam( 0, zero_play_radius ) ;
276 break ;
277 case 1 : if ( zero_play_mode > 0.0f ) {
278 KPADEnableMplsZeroPlay( 0 ) ;
279 } else {
280 KPADDisableMplsZeroPlay( 0 ) ;
281 }
282 break ;
283 //----- zero drift
284 case 2 : KPADSetMplsZeroDriftMode( 0, ( KPADMplsZeroDriftMode )zero_drift_mode ) ;
285 break ;
286 //----- dir revise
287 case 3 : KPADSetMplsDirReviseParam( 0, dir_rev_revise_pw ) ;
288 break ;
289 case 4 : if ( dir_rev_mode > 0.0f ) {
290 KPADSetMplsDirReviseBase( 0, &dir_base ) ;
291 KPADEnableMplsDirRevise( 0 ) ;
292 } else {
293 KPADDisableMplsDirRevise( 0 ) ;
294 }
295 break ;
296 //----- acc revise
297 case 5 :
298 case 6 : KPADSetMplsAccReviseParam( 0, acc_rev_revise_pw, acc_rev_revise_rg ) ;
299 break ;
300 case 7 : if ( acc_rev_mode > 0.0f ) {
301 KPADEnableMplsAccRevise( 0 ) ;
302 } else {
303 KPADDisableMplsAccRevise( 0 ) ;
304 }
305 break ;
306 //----- dpd revise
307 case 8 : KPADSetMplsDpdReviseParam( 0, dpd_rev_revise_pw ) ;
308 break ;
309 case 9 : if ( dpd_rev_mode > 0.0f ) {
310 KPADEnableMplsDpdRevise( 0 ) ;
311 } else {
312 KPADDisableMplsDpdRevise( 0 ) ;
313 }
314 break ;
315 //----- dps magnification
316 case 10 :
317 case 11 :
318 case 12 : KPADSetMplsMagnification( 0, dps_mag_pitch, dps_mag_yaw, dps_mag_roll ) ;
319 break ;
320 }
321 }
322
323 /*******************************************************************************
324 KPAD Processing Functions
325 *******************************************************************************/
work_kpad(void)326 void work_kpad( void )
327 {
328 //----- Change the mode of the Wii MotionPlus
329 if ( padsTrig[ 0 ].button & PAD_BUTTON_A ) {
330 KPADEnableMpls( 0, WPAD_MPLS_STD ) ;
331 }
332 if ( padsTrig[ 0 ].button & PAD_BUTTON_B ) {
333 KPADDisableMpls( 0 ) ;
334 }
335 if ( padsTrig[ 0 ].button & PAD_BUTTON_X ) {
336 KPADEnableMpls( 0, WPAD_MPLS_FS ) ;
337 }
338 if ( padsTrig[ 0 ].button & PAD_BUTTON_Y ) {
339 KPADEnableMpls( 0, WPAD_MPLS_CL ) ;
340 }
341
342 //----- Get KPAD data
343 kpad_reads = KPADReadEx( 0, &kpads[ 0 ][ 0 ], KPAD_MAX_READ_BUFS, &kpad_err ) ;
344 }
345
346 /*******************************************************************************
347 CPU Processing Functions
348 *******************************************************************************/
work_sample(void)349 void work_sample( void )
350 {
351 KPADMplsZeroDriftMode zero_drift_init_mode ;
352
353 if ( kpad_reads <= 0 ) {
354 return ;
355 }
356
357 zero_play_status = KPADIsEnableMplsZeroPlay( 0 ) ;
358 zero_drift_status = KPADIsEnableMplsZeroDrift( 0 ) ;
359 dir_rev_status = KPADIsEnableMplsDirRevise( 0 ) ;
360 acc_rev_status = KPADIsEnableMplsAccRevise( 0 ) ;
361 dpd_rev_status = KPADIsEnableMplsDpdRevise( 0 ) ;
362 mpls_calibration = KPADWorkMplsCalibration( 0 ) ;
363
364 /***********************************************************************
365 Reset
366 ***********************************************************************/
367 if ( CMD_RESET_ALL ) {
368 KPADResetMpls( 0 ) ;
369
370 KPADGetMplsZeroPlayParam ( 0, &zero_play_radius ) ;
371 KPADGetMplsDirReviseParam( 0, &dir_rev_revise_pw ) ;
372 KPADGetMplsAccReviseParam( 0, &acc_rev_revise_pw, &acc_rev_revise_rg ) ;
373 KPADGetMplsDpdReviseParam( 0, &dpd_rev_revise_pw ) ;
374 KPADGetMplsZeroDriftMode ( 0, &zero_drift_init_mode ) ;
375 zero_play_mode = -1.0f ;
376 zero_drift_mode = ( f32 ) zero_drift_init_mode ;
377 dir_rev_mode =-1.0f ;
378 acc_rev_mode = 1.0f ;
379 dpd_rev_mode = 1.0f ;
380 dps_mag_pitch = 1.000f ;
381 dps_mag_yaw = 1.000f ;
382 dps_mag_roll = 1.000f ;
383 }
384
385 /***********************************************************************
386 Reset the orientation
387 ***********************************************************************/
388 if ( CMD_RESET_ORIENT ) {
389 KPADSetMplsAngle( 0, 0.0f, 0.0f, 0.0f ) ;
390 KPADSetMplsDirection( 0, &dir_base ) ;
391 KPADSetMplsDirReviseBase( 0, &dir_base ) ;
392 }
393
394 /***********************************************************************
395 Change the mode of the Wii MotionPlus
396 ***********************************************************************/
397 if ( CMD_CALIBRATE ) {
398 KPADStartMplsCalibration( 0 ) ;
399 }
400
401 /***********************************************************************
402 Move the center of the cuboid
403 ***********************************************************************/
404 if ( CMD_MOVE_CNT_INC ) {
405 if ( ++ rect_center >= 3.0f ) {
406 rect_center = 3.0f ;
407 }
408 }
409 if ( CMD_MOVE_CNT_DEC ) {
410 if ( -- rect_center <=-3.0f ) {
411 rect_center = -3.0f ;
412 }
413 }
414
415 /***********************************************************************
416 Value Editing
417 ***********************************************************************/
418 move_cursor() ; // Cursor movement
419 push_cursor() ; // Increasing/decreasing numerical values
420 }
421
422
423 /*******************************************************************************
424 Render pointing coordinates
425 *******************************************************************************/
draw_position(KPADStatus * sp)426 static void draw_position( KPADStatus *sp )
427 {
428 Vec2 pos, vec ;
429 f32 scale ;
430
431 if (sp->dpd_valid_fg <= 0) {
432 return;
433 }
434 switch (sp->data_format) {
435 case WPAD_FMT_CORE_ACC_DPD:
436 case WPAD_FMT_FREESTYLE_ACC_DPD:
437 case WPAD_FMT_CLASSIC_ACC_DPD:
438 case WPAD_FMT_MPLS:
439 break;
440
441 default:
442 return;
443 }
444
445 //------ Crosshair
446 pos.x = pointing_scale * sp->pos.x ; // Center coordinate
447 pos.y = pointing_scale * sp->pos.y ;
448
449 scale = 72.0f ;
450 vec.x = scale * sp->horizon.x ; // Vector pointing right
451 vec.y = scale * sp->horizon.y ;
452 draw_line( pos.x-vec.x,pos.y-vec.y, pos.x+vec.x,pos.y+vec.y, yellow_clr, 3 ) ;
453
454 scale = 28.0f ;
455 vec.x = scale * -sp->horizon.y ; // Vector pointing down
456 vec.y = scale * sp->horizon.x ;
457 draw_arrow( pos.x+vec.x,pos.y+vec.y, pos.x-vec.x,pos.y-vec.y, silver_clr, 3 ) ;
458
459 //------ Center point
460 draw_point( pos.x,pos.y, black_clr, 11 ) ;
461 draw_point( pos.x,pos.y, cyan_clr, 9 ) ;
462 }
463
464
465 /*******************************************************************************
466 Render Grids
467 *******************************************************************************/
draw_grid(f32 ox,f32 oy,f32 radius,GXColor clr)468 static void draw_grid( f32 ox, f32 oy, f32 radius, GXColor clr )
469 {
470 f32 r = radius * 1.4f ; // Crosshair length
471
472 draw_arrow( ox-r,oy, ox+r,oy, clr, 3 ) ; // Horizontal line
473 draw_arrow( ox,oy+r, ox,oy-r, clr, 3 ) ; // Vertical line
474 draw_circle( ox,oy, radius, clr, 3 ) ; // Circle
475 }
476
477 /*******************************************************************************
478 Render Acceleration
479 *******************************************************************************/
draw_acc(f32 ox,f32 oy,f32 radius,f32 ax,f32 ay,GXColor clr,BOOL dashed)480 static void draw_acc( f32 ox, f32 oy, f32 radius, f32 ax, f32 ay, GXColor clr, BOOL dashed )
481 {
482 f32 px = ax * radius + ox ;
483 f32 py = ay * radius + oy ;
484
485 if ( dashed ) {
486 draw_dashed_arrow( ox,oy, px,py, clr, 4 ) ;
487 } else {
488 draw_arrow( ox,oy, px,py, clr, 4 ) ;
489 }
490 draw_point( px,py, white_clr, 4 ) ;
491 }
492
493 /*******************************************************************************
494 Render DPD Sensor Frame
495 *******************************************************************************/
draw_dpd_frame(void)496 static void draw_dpd_frame( void )
497 {
498 f32 rx,ry, x1,y1, x2,y2 ;
499
500 rx = dpd_disp_scale * 0.5f ;
501 ry = rx * (f32)WPAD_DPD_IMG_RESO_WY / (f32)WPAD_DPD_IMG_RESO_WX ;
502 x1 = dpd_disp_ox - rx ;
503 y1 = dpd_disp_oy - ry ;
504 x2 = dpd_disp_ox + rx ;
505 y2 = dpd_disp_oy + ry ;
506
507 GXSetTevColor( GX_TEVREG0, smoke_clr ) ;
508
509 GXBegin( GX_QUADS, GX_VTXFMT0, 4 ) ;
510 GXPosition2f32( x1, y1 ) ;
511 GXPosition2f32( x2, y1 ) ;
512 GXPosition2f32( x2, y2 ) ;
513 GXPosition2f32( x1, y2 ) ;
514 GXEnd() ;
515 }
516
517 /*******************************************************************************
518 Render DPD objects
519 *******************************************************************************/
draw_object(void)520 static void draw_object( void )
521 {
522 DPDObject *op ;
523 f32 px,py, scale, ofx,ofy ;
524 s32 cx ;
525 KPADUnifiedWpadStatus *up ;
526
527 //----- Calculate display position
528 scale = dpd_disp_scale / (f32)WPAD_DPD_IMG_RESO_WX ;
529 ofx = dpd_disp_scale * -0.5f ;
530 ofy = ofx * (f32)WPAD_DPD_IMG_RESO_WY / (f32)WPAD_DPD_IMG_RESO_WX ;
531 ofx += dpd_disp_ox ;
532 ofy += dpd_disp_oy ;
533
534 //----- Get WPAD data
535 KPADGetUnifiedWpadStatus( 0, wpads[ 0 ], ( u32 )kpad_reads ) ;
536
537 cx = kpad_reads ;
538 while ( --cx >= 0 ) {
539 up = &wpads[ 0 ][ cx ] ;
540 if ( up->u.core.err == WPAD_ERR_NONE ) {
541 op = up->u.core.obj + ( WPAD_DPD_MAX_OBJECTS - 1 ) ;
542 do {
543 if ( op->size == 0 ) continue ;
544
545 //----- Change to screen coordinates
546 px = (f32)op->x * scale + ofx ;
547 py = (f32)op->y * scale + ofy ;
548
549 //----- Render objects
550 draw_point( px,py, white_clr, 5 ) ;
551 draw_circle( px,py, 5, black_clr, 2 ) ;
552 draw_circle( px,py, 4, smoke_clr, 2 ) ;
553 draw_circle( px,py, 3, gray_clr, 2 ) ;
554 } while ( --op >= up->u.core.obj ) ;
555 }
556 }
557 }
558
559 /*******************************************************************************
560 Render analog stick
561 *******************************************************************************/
draw_stick(f32 ox,f32 oy,f32 radius,f32 sx,f32 sy,GXColor clr,BOOL dashed)562 static void draw_stick( f32 ox, f32 oy, f32 radius, f32 sx, f32 sy, GXColor clr, BOOL dashed )
563 {
564 f32 px = sx * radius + ox;
565 f32 py = sy * radius + oy;
566
567 if ( dashed ) {
568 draw_dashed_arrow( ox,oy, px,py, clr, 4 ) ;
569 } else {
570 draw_arrow( ox,oy, px,py, clr, 4 ) ;
571 }
572 draw_point( px,py, white_clr, 4 ) ;
573 }
574
575 /*******************************************************************************
576 Render the Wii MotionPlus
577 *******************************************************************************/
draw_mpls(f32 ox,f32 oy,f32 radius,f32 val,GXColor clr,BOOL dashed)578 static void draw_mpls( f32 ox, f32 oy, f32 radius, f32 val, GXColor clr, BOOL dashed )
579 {
580 f32 py =-val * radius + oy ;
581
582 if ( dashed ) {
583 draw_dashed_arrow( ox, oy, ox, py, clr, 3 ) ;
584 } else {
585 draw_arrow( ox, oy, ox, py, clr, 3 ) ;
586 }
587 draw_point( ox, py, white_clr, 3 ) ;
588 }
589
590 /*******************************************************************************
591 Render the angle
592 *******************************************************************************/
draw_angle(f32 ox,f32 oy,f32 radius,f32 angle,GXColor clr,BOOL dashed)593 static void draw_angle( f32 ox, f32 oy, f32 radius, f32 angle, GXColor clr, BOOL dashed )
594 {
595 f32 px = (f32)( sin(angle * 2 * PI) * radius + ox ) ;
596 f32 py = (f32)( cos(angle * 2 * PI) * radius + oy ) ;
597
598 if ( dashed ) {
599 draw_dashed_arrow( ox,oy, px,py, clr, 3 ) ;
600 } else {
601 draw_arrow( ox,oy, px,py, clr, 3 ) ;
602 }
603 draw_point( px,py, white_clr, 3 ) ;
604 }
605
606 /*******************************************************************************
607 Render value adjustment cursor
608 *******************************************************************************/
draw_cursor(void)609 static void draw_cursor( void )
610 {
611 f32 x1,y1, x2,y2 ;
612 f32 width = 0.0f ;
613 f32 vp = *edit_data[ csr_i ].vp ;
614 s32 cx = csr_x ;
615
616 y1 = calc_kfont_y1( csr_y+O1Y ) ;
617 y2 = calc_kfont_y2( csr_y+O1Y ) - 1.0f ;
618 if ( csr_x == 21 && csr_y < 12 ) {
619 cx -= 4 ;
620 if ( csr_y == 6 ) width = 4.0f ;
621 else if ( vp < 0.0 ) width = 2.0f ;
622 else width = 1.0f ;
623 }
624 x1 = calc_kfont_x1( (f32)cx ) ;
625 x2 = calc_kfont_x2( cx + width ) ;
626 draw_line( x1,y1, x2,y1, gray_clr, 2 ) ;
627 draw_line( x2,y1, x2,y2, gray_clr, 2 ) ;
628 draw_line( x2,y2, x1,y2, gray_clr, 2 ) ;
629 draw_line( x1,y2, x1,y1, gray_clr, 2 ) ;
630 }
631
632 /*******************************************************************************
633 Render the sampling rate
634 *******************************************************************************/
draw_sampling_rate(void)635 static void draw_sampling_rate( void )
636 {
637 static OSTime time;
638 static u32 countDisp;
639 BOOL enabled;
640 OSTime t;
641
642 t = OSGetTime();
643 if (OSTicksToSeconds((OSTime)(t - time)) > 0) {
644 time = t;
645 enabled = OSDisableInterrupts();
646 countDisp = MySamplingCount;
647 MySamplingCount = 0;
648 (void)OSRestoreInterrupts( enabled );
649 }
650
651 draw_kfont_letter( 48.0f,0.5f, white_clr, "SMP.RATE" );
652 draw_kfont_s32 ( 59.0f,0.5f, white_clr, (s32)countDisp );
653 }
654
655 /*******************************************************************************
656 Render the callback status
657 *******************************************************************************/
draw_callback_status(void)658 static void draw_callback_status( void )
659 {
660 u32 i ;
661 MyCallbackStatus buf[ MY_START_COUNTS_MAX ] ;
662 BOOL enabled ;
663 char str[ 32 ] ;
664 s32 latestIdx ;
665
666 enabled = OSDisableInterrupts() ;
667 for ( i = 0 ; i < MY_START_COUNTS_MAX ; i++ ) {
668 if ( MyCallbackStatusBuf[ 1 ][ i ].count ) {
669 MyCallbackStatusBuf[ 1 ][ i ].count-- ;
670 }
671 buf[ i ] = MyCallbackStatusBuf[ 1 ][ i ] ;
672 }
673 latestIdx = MyCallbackLatestIdx[ 1 ] ;
674 ( void )OSRestoreInterrupts( enabled ) ;
675
676 for ( i = 0 ; i < MY_START_COUNTS_MAX ; i++ ) {
677 if ( (s32)i == latestIdx ||
678 buf[ i ].count ) {
679 GXColor clr ;
680 if ( buf[ i ].reason < 0 ) {
681 clr = gray_clr ;
682 } else {
683 clr = white_clr ;
684 }
685 sprintf( str, "%s %d", MyCbName[ 1 ], buf[ i ].reason ) ;
686 draw_kfont_letter( 0.0f, buf[ i ].count * 0.05f + 0.5f, clr, str ) ;
687 }
688 }
689 }
690
691 /*******************************************************************************
692 Render the cuboid
693 *******************************************************************************/
draw_rectangular_solid(f32 ox,f32 oy,f32 oz,f32 px,f32 py,f32 pz,Mtx v,KPADMPDir dir)694 static void draw_rectangular_solid( f32 ox, f32 oy, f32 oz, f32 px, f32 py, f32 pz, Mtx v, KPADMPDir dir )
695 {
696 Mtx ms ; // Scale matrix
697 Mtx mr ; // Rotation matrix
698 Mtx mv ; // View matrix
699 Mtx mvi ;
700
701 //----- Copy the dir values to the rotation matrix
702 MTXRowCol( mr, 0, 0 ) = dir.X.x ;
703 MTXRowCol( mr, 1, 0 ) = dir.X.y ;
704 MTXRowCol( mr, 2, 0 ) = dir.X.z ;
705 MTXRowCol( mr, 0, 1 ) = dir.Y.x ;
706 MTXRowCol( mr, 1, 1 ) = dir.Y.y ;
707 MTXRowCol( mr, 2, 1 ) = dir.Y.z ;
708 MTXRowCol( mr, 0, 2 ) = dir.Z.x ;
709 MTXRowCol( mr, 1, 2 ) = dir.Z.y ;
710 MTXRowCol( mr, 2, 2 ) = dir.Z.z ;
711 C_MTXConcat( v, mr, mv ) ;
712 //----- Scaling
713 C_MTXScale( ms, 25.0F, 25.0F, 25.0F ) ;
714 C_MTXConcat( mv, ms, mv ) ;
715 GXLoadPosMtxImm( mv, GX_PNMTX0 ) ;
716 C_MTXInverse( mv, mvi ) ;
717 C_MTXTranspose( mvi, mv ) ;
718 GXLoadNrmMtxImm( mv, GX_PNMTX0 ) ;
719
720 //----- Render the cuboid
721 GXBegin(GX_QUADS, GX_VTXFMT3, 4*6) ;
722
723 GXPosition3f32( ox, oy, oz ) ;
724 GXNormal3f32 ( ox, 0.0f, 0.0f );
725 GXPosition3f32( ox, oy, pz ) ;
726 GXNormal3f32 ( ox, 0.0f, 0.0f );
727 GXPosition3f32( ox, py, pz ) ;
728 GXNormal3f32 ( ox, 0.0f, 0.0f );
729 GXPosition3f32( ox, py, oz ) ;
730 GXNormal3f32 ( ox, 0.0f, 0.0f );
731
732 GXPosition3f32( px, oy, oz ) ;
733 GXNormal3f32 ( px, 0.0f, 0.0f );
734 GXPosition3f32( px, py, oz ) ;
735 GXNormal3f32 ( px, 0.0f, 0.0f );
736 GXPosition3f32( px, py, pz ) ;
737 GXNormal3f32 ( px, 0.0f, 0.0f );
738 GXPosition3f32( px, oy, pz ) ;
739 GXNormal3f32 ( px, 0.0f, 0.0f );
740
741 GXPosition3f32( ox, py, oz ) ;
742 GXNormal3f32 ( 0.0f, py, 0.0f );
743 GXPosition3f32( ox, py, pz ) ;
744 GXNormal3f32 ( 0.0f, py, 0.0f );
745 GXPosition3f32( px, py, pz ) ;
746 GXNormal3f32 ( 0.0f, py, 0.0f );
747 GXPosition3f32( px, py, oz ) ;
748 GXNormal3f32 ( 0.0f, py, 0.0f );
749
750 GXPosition3f32( ox, oy, pz ) ;
751 GXNormal3f32 ( 0.0f, oy, 0.0f );
752 GXPosition3f32( ox, oy, oz ) ;
753 GXNormal3f32 ( 0.0f, oy, 0.0f );
754 GXPosition3f32( px, oy, oz ) ;
755 GXNormal3f32 ( 0.0f, oy, 0.0f );
756 GXPosition3f32( px, oy, pz ) ;
757 GXNormal3f32 ( 0.0f, oy, 0.0f );
758
759 GXPosition3f32( px, py, oz ) ;
760 GXNormal3f32 ( 0.0f, 0.0f, oz );
761 GXPosition3f32( px, oy, oz ) ;
762 GXNormal3f32 ( 0.0f, 0.0f, oz );
763 GXPosition3f32( ox, oy, oz ) ;
764 GXNormal3f32 ( 0.0f, 0.0f, oz );
765 GXPosition3f32( ox, py, oz ) ;
766 GXNormal3f32 ( 0.0f, 0.0f, oz );
767
768 GXPosition3f32( px, py, pz ) ;
769 GXNormal3f32 ( 0.0f, 0.0f, pz );
770 GXPosition3f32( ox, py, pz ) ;
771 GXNormal3f32 ( 0.0f, 0.0f, pz );
772 GXPosition3f32( ox, oy, pz ) ;
773 GXNormal3f32 ( 0.0f, 0.0f, pz );
774 GXPosition3f32( px, oy, pz ) ;
775 GXNormal3f32 ( 0.0f, 0.0f, pz );
776
777 GXEnd() ;
778 }
779
780 /*******************************************************************************
781 Render
782 *******************************************************************************/
draw_sample(void)783 void draw_sample( void )
784 {
785 s32 cx,cy ;
786 f32 x1,x2, x3, y, r ;
787 GXColor clr ;
788
789 /***********************************************************************
790 Sampling rate
791 ***********************************************************************/
792 init_draw_kfont( rmode_p->fbWidth, rmode_p->efbHeight, 10.6f, 15.8f ) ;
793 draw_sampling_rate() ;
794
795 /***********************************************************************
796 Manual
797 ***********************************************************************/
798 init_draw_kfont( rmode_p->fbWidth, rmode_p->efbHeight, 10.6f, 15.8f ) ;
799 cx = 38 ;
800 cy = 3 ;
801 draw_kfont_letter( (f32)cx,cy+O1Y, silver_clr, "-- KPADMPLS DEMO MAN --" ) ;
802 ++ cy ;
803 draw_kfont_letter( (f32)cx,cy+O1Y, yellow_clr, " WII REMOTE" ) ;
804 ++ cy ;
805 draw_kfont_letter( (f32)cx,cy+O1Y, silver_clr, "KEY :MOVE CURSOR" ) ;
806 ++ cy ;
807 draw_kfont_letter( (f32)cx,cy+O1Y, silver_clr, "A :INCREASE PARAM" ) ;
808 ++ cy ;
809 draw_kfont_letter( (f32)cx,cy+O1Y, silver_clr, "B :DECREASE PARAM" ) ;
810 ++ cy ;
811 draw_kfont_letter( (f32)cx,cy+O1Y, silver_clr, "1 :RESET" ) ;
812 ++ cy ;
813 draw_kfont_letter( (f32)cx,cy+O1Y, silver_clr, "2 :RESET ORIENT" ) ;
814 ++ cy ;
815 draw_kfont_letter( (f32)cx,cy+O1Y, silver_clr, "HOME :CALIBRATION" ) ;
816 ++ cy ;
817 draw_kfont_letter( (f32)cx,cy+O1Y, silver_clr, "+, - :MOVE OBJ" ) ;
818 ++ cy ;
819 draw_kfont_letter( (f32)cx,cy+O1Y, yellow_clr, " GC CONTROLLER" ) ;
820 ++ cy ;
821 draw_kfont_letter( (f32)cx,cy+O1Y, silver_clr, "A,X,Y:MPLS STD,FS,CL" ) ;
822 ++ cy ;
823 draw_kfont_letter( (f32)cx,cy+O1Y, silver_clr, "B :MPLS OFF" ) ;
824
825 /***********************************************************************
826 Menu
827 ***********************************************************************/
828 init_draw_kfont( rmode_p->fbWidth, rmode_p->efbHeight, 10.6f, 15.8f ) ;
829 cx = 0 ;
830 cy = 3 ;
831 draw_kfont_letter( (f32)cx,cy+O1Y, silver_clr, "---- MPLS PRAMS ----" ) ;
832 ++ cy ;
833 draw_kfont_letter( cx+O1X,cy+O1Y, silver_clr, "STATS" ) ;
834 draw_kfont_letter( cx+O2X,cy+O1Y, silver_clr, "MODE" ) ;
835 draw_kfont_letter( cx+O3X,cy+O1Y, silver_clr, "RADIUS" ) ;
836 ++ cy ;
837 draw_kfont_letter( (f32)cx,cy+O1Y, silver_clr, "ZERO PLAY" ) ;
838 (void)draw_kfont_f32 ( cx+O1X,cy+O1Y, cyan_clr, zero_play_status, 3 ) ;
839 (void)draw_kfont_letter( cx+O2X,cy+O1Y, cyan_clr, (zero_play_mode < 0.0f) ? "OFF" : "ON" ) ;
840 (void)draw_kfont_f32 ( cx+O3X,cy+O1Y, cyan_clr, zero_play_radius, 3 ) ;
841 ++ cy ;
842 draw_kfont_letter( (f32)cx,cy+O1Y, silver_clr, "ZERO DRIFT" ) ;
843 (void)draw_kfont_f32 ( cx+O1X, cy+O1Y, violet_clr, zero_drift_status, 3 ) ;
844 (void)draw_kfont_letter( cx+O2X, cy+O1Y, violet_clr,(zero_drift_mode == 0.0f) ? "LOOSE" : (zero_drift_mode == 1.0f) ? "STAND" : "TIGHT" ) ;
845 ++ cy ;
846 draw_kfont_letter( cx+O3X,cy+O1Y, silver_clr, "POWER" ) ;
847 draw_kfont_letter( cx+O4X,cy+O1Y, silver_clr, "RANGE" ) ;
848 ++ cy ;
849 draw_kfont_letter( (f32)cx,cy+O1Y, silver_clr, "DIR REVISE" ) ;
850 (void)draw_kfont_f32 ( cx+O1X,cy+O1Y, green_clr, dir_rev_status, 3 ) ;
851 (void)draw_kfont_letter( cx+O2X,cy+O1Y, green_clr,(dir_rev_mode < 0.0f) ? "OFF" : "ON" ) ;
852 (void)draw_kfont_f32 ( cx+O3X,cy+O1Y, green_clr, dir_rev_revise_pw, 3 ) ;
853 ++ cy ;
854 draw_kfont_letter( (f32)cx,cy+O1Y, silver_clr, "ACC REVISE" ) ;
855 (void)draw_kfont_f32 ( cx+O1X,cy+O1Y, red_clr, acc_rev_status, 3 ) ;
856 (void)draw_kfont_letter( cx+O2X,cy+O1Y, red_clr, (acc_rev_mode < 0.0f) ? "OFF" : "ON" ) ;
857 (void)draw_kfont_f32 ( cx+O3X,cy+O1Y, red_clr, acc_rev_revise_pw, 3 ) ;
858 (void)draw_kfont_f32 ( cx+O4X,cy+O1Y, red_clr, acc_rev_revise_rg, 3 ) ;
859 ++ cy ;
860 draw_kfont_letter( (f32)cx,cy+O1Y, silver_clr, "DPD REVISE" ) ;
861 (void)draw_kfont_f32 ( cx+O1X,cy+O1Y, orange_clr, dpd_rev_status, 3 ) ;
862 (void)draw_kfont_letter( cx+O2X,cy+O1Y, orange_clr, (dpd_rev_mode < 0.0f) ? "OFF" : "ON" ) ;
863 (void)draw_kfont_f32 ( cx+O3X,cy+O1Y, orange_clr, dpd_rev_revise_pw, 3 ) ;
864 ++ cy ;
865 draw_kfont_letter( cx+O2X,cy+O1Y, silver_clr, "PITCH" ) ;
866 draw_kfont_letter( cx+O3X,cy+O1Y, silver_clr, "YAW" ) ;
867 draw_kfont_letter( cx+O4X,cy+O1Y, silver_clr, "ROLL" ) ;
868 ++ cy ;
869 draw_kfont_letter( (f32)cx,cy+O1Y, silver_clr, "DEGREE MAG" ) ;
870 (void)draw_kfont_f32 ( cx+O2X,cy+O1Y, peagreen_clr, dps_mag_pitch, 3 ) ;
871 (void)draw_kfont_f32 ( cx+O3X,cy+O1Y, peagreen_clr, dps_mag_yaw, 3 ) ;
872 (void)draw_kfont_f32 ( cx+O4X,cy+O1Y, peagreen_clr, dps_mag_roll, 3 ) ;
873
874 /***********************************************************************
875 Cursor
876 ***********************************************************************/
877 init_draw_graphic( rmode_p->fbWidth, rmode_p->efbHeight ) ;
878 draw_cursor() ;
879
880 /***********************************************************************
881 Values for each direction
882 ***********************************************************************/
883 cx = 4 ;
884 cy = 14 ;
885 init_draw_kfont( rmode_p->fbWidth, rmode_p->efbHeight, 12.6f, 16.0f ) ;
886 draw_kfont_letter( (f32)cx,(f32)cy, gray_clr, "---- MPLS ----") ;
887 cy = 19 ;
888 draw_kfont_f32 ( (f32)cx ,(f32)cy, peagreen_clr, kpads[ 0 ][ 0 ].mpls.mpls.x, 2 );
889 draw_kfont_f32 ( cx+ 8.0f,(f32)cy, peagreen_clr, kpads[ 0 ][ 0 ].mpls.mpls.y, 2 );
890 draw_kfont_f32 ( cx+16.0f,(f32)cy, peagreen_clr, kpads[ 0 ][ 0 ].mpls.mpls.z, 2 );
891
892 init_draw_kfont( rmode_p->fbWidth, rmode_p->efbHeight, 12.0f, 15.0f ) ;
893 draw_kfont_letter(5.0f, 17.2f+O3Y, gray_clr, "PIT YAW ROL") ;
894
895 x1 = -250.0f ; // X-coordinate of left grid
896 x2 = -160.0f ; // X-coordinate of the center grid
897 x3 = -70.0f ; // X-coordinate of right grid
898 y = 80.0f+O4Y ; // Common y-coordinate
899 r = 25.0f ; // Common radius
900
901 init_draw_graphic( rmode_p->fbWidth, rmode_p->efbHeight ) ;
902 draw_grid( x1, y, r, gray_clr ) ;
903 draw_grid( x2, y, r, gray_clr ) ;
904 draw_grid( x3, y, r, gray_clr ) ;
905
906 cx = kpad_reads ;
907 while ( --cx >= 0 ) {
908 draw_mpls( x1, y, r, kpads[ 0 ][ cx ].mpls.mpls.x, peagreen_clr, TRUE ) ;
909 draw_mpls( x2, y, r, kpads[ 0 ][ cx ].mpls.mpls.y, peagreen_clr, TRUE ) ;
910 draw_mpls( x3, y, r, kpads[ 0 ][ cx ].mpls.mpls.z, peagreen_clr, TRUE ) ;
911 }
912
913 /***********************************************************************
914 Angles in each direction
915 ***********************************************************************/
916 cx = 4 ;
917 cy = 20 ;
918 init_draw_kfont( rmode_p->fbWidth, rmode_p->efbHeight, 12.6f, 16.0f ) ;
919 draw_kfont_letter( (f32)cx,(f32)cy, gray_clr, "---- ANGLE ----") ;
920 cy = 25 ;
921 draw_kfont_f32 ( (f32)cx ,(f32)cy, yellow_clr, kpads[ 0 ][ 0 ].mpls.angle.x, 2 );
922 draw_kfont_f32 ( cx+ 8.0f,(f32)cy, yellow_clr, kpads[ 0 ][ 0 ].mpls.angle.y, 2 );
923 draw_kfont_f32 ( cx+16.0f,(f32)cy, yellow_clr, kpads[ 0 ][ 0 ].mpls.angle.z, 2 );
924
925 init_draw_kfont( rmode_p->fbWidth, rmode_p->efbHeight, 12.0f, 15.0f ) ;
926 draw_kfont_letter(5.5f, 24.0f+O3Y, gray_clr, "PIT YAW ROL") ;
927
928 x1 = -250.0f ; // X-coordinate of left grid
929 x2 = -160.0f ; // X-coordinate of the center grid
930 x3 = -70.0f ; // X-coordinate of right grid
931 y = 180.0f+O4Y ; // Common y-coordinate
932 r = 25.0f ; // Common radius
933
934 init_draw_graphic( rmode_p->fbWidth, rmode_p->efbHeight ) ;
935 draw_grid( x1, y, r, gray_clr ) ;
936 draw_grid( x2, y, r, gray_clr ) ;
937 draw_grid( x3, y, r, gray_clr ) ;
938
939 cx = kpad_reads ;
940 while ( --cx >= 0 ) {
941 draw_angle( x1, y, r, kpads[ 0 ][ cx ].mpls.angle.x, yellow_clr, TRUE ) ;
942 draw_angle( x2, y, r, kpads[ 0 ][ cx ].mpls.angle.y, yellow_clr, TRUE ) ;
943 draw_angle( x3, y, r, kpads[ 0 ][ cx ].mpls.angle.z, yellow_clr, TRUE ) ;
944 }
945
946 /***********************************************************************
947 Acceleration
948 ***********************************************************************/
949 cx = 32 ;
950 cy = 14 ;
951
952 init_draw_kfont( rmode_p->fbWidth, rmode_p->efbHeight, 12.6f, 16.0f ) ;
953 draw_kfont_letter( (f32)cx,(f32)cy, gray_clr, "---- ACC ----") ;
954
955 init_draw_kfont( rmode_p->fbWidth, rmode_p->efbHeight, 12.0f, 15.0f ) ;
956 draw_kfont_letter(37.0f, 17.0f+O3Y, gray_clr, "Y Y") ;
957 draw_kfont_letter(39.4f, 20.0f+O3Y, gray_clr, "X Z") ;
958
959 x1 = 105.0f ; // X-coordinate of left grid
960 x2 = 195.0f ; // X-coordinate of right grid
961 y = 80.0f+O4Y ; // Common y-coordinate
962 r = 25.0f ; // Common radius
963
964 init_draw_graphic( rmode_p->fbWidth, rmode_p->efbHeight ) ;
965 draw_grid( x1, y, r, gray_clr ) ;
966 draw_grid( x2, y, r, gray_clr ) ;
967
968 cx = kpad_reads ;
969 while ( --cx >= 0 ) {
970 draw_acc( x1, y, r, kpads[ 0 ][ cx ].acc.x, -kpads[ 0 ][ cx ].acc.y, red_clr, FALSE ) ;
971 draw_acc( x2, y, r, kpads[ 0 ][ cx ].acc.z, -kpads[ 0 ][ cx ].acc.y, red_clr, FALSE ) ;
972 if ( kpads[ 0 ][ cx ].wpad_err == WPAD_ERR_NONE &&
973 ( kpads[ 0 ][ cx ].dev_type == WPAD_DEV_FREESTYLE ||
974 ( kpads[ 0 ][ cx ].dev_type == WPAD_DEV_MPLS_FREESTYLE &&
975 KPADGetMplsStatus( 0 ) == WPAD_MPLS_FS ) ) ) {
976 draw_acc( x1, y, r, kpads[ 0 ][ cx ].ex_status.fs.acc.x, -kpads[ 0 ][ cx ].ex_status.fs.acc.y, orange_clr, FALSE ) ;
977 draw_acc( x2, y, r, kpads[ 0 ][ cx ].ex_status.fs.acc.z, -kpads[ 0 ][ cx ].ex_status.fs.acc.y, orange_clr, FALSE ) ;
978 }
979 }
980
981 /***********************************************************************
982 Control Stick
983 ***********************************************************************/
984 init_draw_graphic( rmode_p->fbWidth, rmode_p->efbHeight ) ;
985 cx = kpad_reads ;
986 while ( --cx >= 0 ) {
987 if ( kpads[ 0 ][ cx ].wpad_err == WPAD_ERR_NONE ) {
988 if ( kpads[ 0 ][ cx ].dev_type == WPAD_DEV_FREESTYLE ||
989 ( kpads[ 0 ][ cx ].dev_type == WPAD_DEV_MPLS_FREESTYLE &&
990 KPADGetMplsStatus( 0 ) == WPAD_MPLS_FS ) ) {
991 draw_stick( x1, y, r, kpads[ 0 ][ cx ].ex_status.fs.stick.x, -kpads[ 0 ][ cx ].ex_status.fs.stick.y, orange_clr, TRUE ) ;
992 } else if ( kpads[ 0 ][ cx ].dev_type == WPAD_DEV_CLASSIC ||
993 ( kpads[ 0 ][ cx ].dev_type == WPAD_DEV_MPLS_CLASSIC &&
994 KPADGetMplsStatus( 0 ) == WPAD_MPLS_CL ) ) {
995 draw_stick( x1, y, r, kpads[ 0 ][ cx ].ex_status.cl.lstick.x, -kpads[ 0 ][ cx ].ex_status.cl.lstick.y, peagreen_clr, FALSE ) ;
996 draw_stick( x2, y, r, kpads[ 0 ][ cx ].ex_status.cl.rstick.x, -kpads[ 0 ][ cx ].ex_status.cl.rstick.y, peagreen_clr, FALSE ) ;
997 }
998 }
999 }
1000
1001
1002 /***********************************************************************
1003 DPD Object Display
1004 ***********************************************************************/
1005 cx = 32 ;
1006 cy = 20 ;
1007 init_draw_kfont( rmode_p->fbWidth, rmode_p->efbHeight, 12.6f, 16.0f ) ;
1008 draw_kfont_letter( (f32)cx,(f32)cy, gray_clr, "---- OBJ ----") ;
1009
1010 init_draw_graphic( rmode_p->fbWidth, rmode_p->efbHeight ) ;
1011 draw_dpd_frame() ;
1012 draw_object() ;
1013
1014 /***********************************************************************
1015 Callbacks
1016 ***********************************************************************/
1017 init_draw_kfont( rmode_p->fbWidth, rmode_p->efbHeight, 10.6f, 15.8f ) ;
1018 draw_callback_status() ;
1019
1020 /***********************************************************************
1021 Pointer
1022 ***********************************************************************/
1023 init_draw_graphic( rmode_p->fbWidth, rmode_p->efbHeight ) ;
1024 cx = kpad_reads ;
1025 while ( --cx >= 0 ) {
1026 draw_position( &kpads[ 0 ][ cx ] ) ;
1027 }
1028
1029 /***********************************************************************
1030 Cuboid
1031 ***********************************************************************/
1032 if ( ( kpads[ 0 ][ 0 ].dev_type != WPAD_DEV_MPLS &&
1033 kpads[ 0 ][ 0 ].dev_type != WPAD_DEV_MPLS_FREESTYLE &&
1034 kpads[ 0 ][ 0 ].dev_type != WPAD_DEV_MPLS_CLASSIC &&
1035 kpads[ 0 ][ 0 ].dev_type != WPAD_DEV_MPLS_FUTURE ) ||
1036 kpad_err == KPAD_READ_ERR_NO_CONTROLLER ) {
1037 clr = gray_clr ;
1038 } else {
1039 // Change the color depending on the device type
1040 switch ( kpads[ 0 ][ 0 ].dev_type ) {
1041 case WPAD_DEV_MPLS : clr = magenta_clr ; break ;
1042 case WPAD_DEV_MPLS_FREESTYLE : clr = orange_clr ; break ;
1043 case WPAD_DEV_MPLS_CLASSIC : clr = green_clr ; break ;
1044 case WPAD_DEV_MPLS_FUTURE : clr = violet_clr ; break ;
1045 default : clr = blue_clr ; break ; // Control will never reach here
1046 }
1047
1048 // Change the alpha value to match the calibration status
1049 if ( mpls_calibration >= 0.0f ) {
1050 f32 alpha ;
1051
1052 alpha = clr.a / 2.0f ;
1053 clr.a = ( u8 )( alpha * ( 2.0f - mpls_calibration ) ) ;
1054 }
1055 }
1056
1057 // Display only the attitude with acceleration and DPD correction applied
1058 init_draw_graphic_3d( view_mtx, clr ) ;
1059 draw_rectangular_solid(
1060 -1.0f, 1.0f, rect_center - rect_length / 2.0f,
1061 1.0f,-1.0f, rect_center + rect_length / 2.0f,
1062 view_mtx, kpads[ 0 ][ 0 ].mpls.dir ) ;
1063
1064 }
1065
1066