From 8d957f068792b4ee55f807930c975699c29922a2 Mon Sep 17 00:00:00 2001 From: erdgeist <> Date: Tue, 16 Sep 2008 15:40:46 +0000 Subject: Kickoff --- Makefile | 5 ++ display.c | 63 ++++++++++++++++++++++ display.h | 6 +++ gm.h | 5 ++ heuristics.c | 111 ++++++++++++++++++++++++++++++++++++++ heuristics.h | 13 +++++ locate.c | 91 +++++++++++++++++++++++++++++++ locate.h | 9 ++++ main.c | 172 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ vubars.c | 148 ++++++++++++++++++++++++++++++++++++++++++++++++++ vubars.h | 6 +++ 11 files changed, 629 insertions(+) create mode 100644 Makefile create mode 100644 display.c create mode 100644 display.h create mode 100644 gm.h create mode 100644 heuristics.c create mode 100644 heuristics.h create mode 100644 locate.c create mode 100644 locate.h create mode 100644 main.c create mode 100644 vubars.c create mode 100644 vubars.h diff --git a/Makefile b/Makefile new file mode 100644 index 0000000..bf43616 --- /dev/null +++ b/Makefile @@ -0,0 +1,5 @@ +main: main.c locate.c display.c vubars.c locate.h gm.h display.h vubars.h heuristics.c + gcc -O2 -o main main.c locate.c display.c vubars.c heuristics.c -l cwiid -lm -lSDL -lSDL_gfx + +clean: + rm main diff --git a/display.c b/display.c new file mode 100644 index 0000000..a4ac62b --- /dev/null +++ b/display.c @@ -0,0 +1,63 @@ +#include +#include + +#include "display.h" + +static SDL_Surface *screen; +static int g_width, g_height; + +void display_init( int width, int height) { + g_width = width; + g_height = height; + + if (SDL_Init (SDL_INIT_EVERYTHING) == -1) { + fprintf( stderr, "Can't initialize SDL.\n" ); + exit (1); + } + + screen = SDL_SetVideoMode ( width, height, 24, SDL_SWSURFACE | SDL_DOUBLEBUF ); + if( !screen ) { + fprintf( stderr, "Can't set video mode.\n" ); + exit( 1 ); + } + + memset( screen->pixels, 0, width * height * 3 ); + SDL_WM_SetCaption ( "GodMachine", "GodMachine" ); + +} + +void display_clear( ) { +// rectangleColor( screen, 0, 0, g_width, g_height, 0 ); + + memset( screen->pixels, 0, g_width * g_height * 3 ); + vlineColor( screen, g_width / 2, 0, g_height, 0xffffffff ); + hlineColor( screen, 0, g_width, g_height / 2, 0xffffffff ); +} + +void display_circle( int wii_id, int x, int y, int w ) { + int xoff = ( wii_id & 1 ) * g_width / 2; + int yoff = ( wii_id >> 1 ) * g_height / 2; + + filledCircleColor(screen, x + xoff, y + yoff, w, 0xffffffff ); + +} + +void display_line( int wii_id, int x0, int y0, int x1, int y1 ) { + int xoff = ( wii_id & 1 ) * g_width / 2; + int yoff = ( wii_id >> 1 ) * g_height / 2; + + aalineColor( screen, x0 + xoff, y0 + yoff, x1 + xoff, y1 + yoff, 0xffffffff ); + +} + +void display_rectangle( int wii_id, int x, int y, int w, int h ) { + int xoff = ( wii_id & 1 ) * g_width / 2; + int yoff = ( wii_id >> 1 ) * g_height / 2; + + boxColor( screen, x + xoff, y + yoff, x + xoff + w, y + yoff + h, 0xffffffff ); + +} + +void display_redraw() { + SDL_Flip( screen ); +} diff --git a/display.h b/display.h new file mode 100644 index 0000000..2b2464f --- /dev/null +++ b/display.h @@ -0,0 +1,6 @@ +void display_init( int width, int height); +void display_redraw( ); + +void display_clear( ); +void display_line( int wii_id, int x0, int y0, int x1, int y1 ); +void display_circle( int wii_id, int x, int y, int w ); diff --git a/gm.h b/gm.h new file mode 100644 index 0000000..95b54f3 --- /dev/null +++ b/gm.h @@ -0,0 +1,5 @@ +typedef struct { + double x; double y; +} wii_pt; + +#define WII_COUNT 4 diff --git a/heuristics.c b/heuristics.c new file mode 100644 index 0000000..429a612 --- /dev/null +++ b/heuristics.c @@ -0,0 +1,111 @@ +#include + +#include "vubars.h" + +static void heuristics_getGesture( int const amount, int const off, int const * const values, + int *dxl, int *dyl, int *dxr, int *dyr ); + +int heuristics_getEnergy( int const amount, int const * const values ) { + int energy = 0; + int i; + + for( i = 0; i < amount-1; ++i ) + energy += values[i]*values[i+1]; + + return energy; +} + +// Gets the two longest consecutive gestures found +// in frames. -1 in off[ab] means no gesture found + +// Here what counts: the bigger abs(dx[ab]), the longer the gesture +// the bigger dy[ab]/dx[ab], the slower the gesture +void heuristics_getGestures( int const amount, int const * const values, + int *offa, int *dxa, int *dya, int *dda, + int *offb, int *dxb, int *dyb, int *ddb ) +{ + int i, l = 0; + int dxl [ amount ], dyl [ amount ], dxr [ amount ], dyr [ amount ]; + + // Get the longest gestures for each point - both into left and rite + for( i=0; i *dxa ) { *offa = i; *dxa = dxr[ i ]; *dya = dyr[ i ]; *dda = 1; } + if( dxl[ i ] > *dxa ) { *offa = i; *dxa = dxl[ i ]; *dya = dyl[ i ]; *dda =-1; } + } + + // If no gesture found at all, invalidate off + // and return... second run wouldnt suddenly find a gesture + if( *dxa == 0 ) { *offa = *offb = -1; return; } + + // Now clear the best result - this will find us the second best result + i = *offa; + if( *dda == 1 ) { + for( i=*offa; i < *offa + dxr[ *offa ]; ++i ) + dxr[ i ] = 0; + } else { + for( i=*offa; i > *offa - dxl[ *offa ]; --i ) + dxl[ i ] = 0; + } + + // "Sort" remaining gestures by length + *offb = *dxb = *dyb = 0; + for( i=0; i *dxb ) { *offb = i; *dxb = dxr[ i ]; *dyb = dyr[ i ]; *ddb = 1;} + if( dxl[ i ] > *dxb ) { *offb = i; *dxb = -dxl[ i ]; *dyb = dyl[ i ]; *ddb =-1;} + } + + // If no secondary gesture found, invalidate off + if( *dxb == 0 ) *offb = -1; +} + +static void heuristics_getGesture( int const amount, int const off, int const * const values, + int * const dxl, int * const dyl, int * const dxr, int * const dyr ) +{ + int i; + + // Initialize as "nothing happened" + *dxl = *dxr = *dyl = *dyr = 0; + + // if this didn't peak in last frame, we're not starting + // a gesture here. + if( values[off] != VU_PEAK ) return; + + if( off > 0 ) *dyl = values[off]-values[off-1]; + if( off < amount-1 ) *dyr = values[off]-values[off+1]; + + if( !*dyl && !*dyr ) return; + + // Depending on where this peaks seems to have come from, + // chose direction where to follow it + // try to collect enough monotonic samples and calculate a + // slope. Since our sample peaks, all others cant be larger + if( (*dyl) && (*dyl < *dyr) ) { + (*dxl)++; + for( i=off-1; i>=0; --i ) { + int dy = values[i+1] - values[i]; + + // If it scrolled out of scope, ignore it + if( !values[i] || dy < 0 ) break; + + (*dxl)++; + (*dyl) += dy; + } + } else { + (*dxr)++; + // Do the same when going right + for( i=off+1; i +#include + +#include "gm.h" + +static wii_pt bounds[2][4]; +static int g_calibrated[4]; + +double get_dist( wii_pt *p1, wii_pt *p2 ) { + double d = (p1->x - p2->x)*(p1->x - p2->x) + + (p1->y - p2->y)*(p1->y - p2->y); + return sqrt(d); +} + +/* This call stores information about the expected + rectangle for each wii +*/ +void wii_calibrate( int wii_id, wii_pt *coords ) { + double max_dist = 0.0; + int pt1 = 0, pt2 = 0, i, j; + + /* Points with maximal distance span a rectangle */ + for( i=0; i<4; ++i ) + for( j=1; j<4; ++j ) { + double dist = get_dist( coords+i, coords+j ); + if( dist > max_dist ) { + pt1 = i; pt2 = j; + max_dist = dist; + } + } + + if( coords[pt1].x > coords[pt2].x ) { + bounds[0][wii_id] = coords[pt1]; + bounds[1][wii_id] = coords[pt2]; + } else { + bounds[1][wii_id] = coords[pt1]; + bounds[0][wii_id] = coords[pt2]; + } + + g_calibrated[wii_id] = 1; +} + +int wii_point_to_led( int wii_id, wii_pt *coords ) { + // Ick nenn die immer so. + const double x0 = bounds[0][wii_id].x; + const double y0 = bounds[0][wii_id].y; + const double x1 = bounds[1][wii_id].x; + const double y1 = bounds[1][wii_id].y; + const double xp = coords->x; + const double yp = coords->y; + +//printf( "%lf:%lf ", xp, yp ); + + // Where m is slope of orthogonal line + // And n is square of diagonal of bounding box + double m = (x1-x0)*(xp-x0)+(y1-y0)*(yp-y0); + double n = (x1-x0)*(x1-x0)+(y1-y0)*(y1-y0); + + // To find out distance of coord to our base line + double q = (xp-x0)*(y1-y0)-(yp-y0)*(x1-x0); + +//printf( "%lf %lf %lf\n", m, n, q ); + + double offs_on_line = m/n; + double dist_to_line = q/n; + + // If wii is not calibrated, we cannot detect point + if( !g_calibrated[wii_id] ) + return -1; + + // Point is too far away to be considered a match + if( ( dist_to_line > 0.1 ) || ( dist_to_line < -0.1 ) ) { +// puts( "too far " ); + return -1; + } + +// printf( "%1.3lf\n", offs_on_line ); + + // Check, which segment our line hits + if( ( offs_on_line > (-1.0/6.0) ) && ( offs_on_line <= (1.0/6.0) ) ) + return 0; + if( ( offs_on_line > ( 1.0/6.0) ) && ( offs_on_line <= (3.0/6.0) ) ) + return 1; + if( ( offs_on_line > ( 3.0/6.0) ) && ( offs_on_line <= (5.0/6.0) ) ) + return 2; + if( ( offs_on_line > ( 5.0/6.0) ) && ( offs_on_line <= (7.0/6.0) ) ) + return 3; + + // No match! + return -1; +} diff --git a/locate.h b/locate.h new file mode 100644 index 0000000..0b72f72 --- /dev/null +++ b/locate.h @@ -0,0 +1,9 @@ +#ifndef _WII_LOCATE_H_ +#define _WII_LOCATE_H_ + +#include "gm.h" + +void wii_calibrate( int wii_id, wii_pt *coords ); +int wii_point_to_led( int wii_id, wii_pt *coords ); + +#endif diff --git a/main.c b/main.c new file mode 100644 index 0000000..e645b48 --- /dev/null +++ b/main.c @@ -0,0 +1,172 @@ +#include +#include +#include +#include +#include + +#include +#include "gm.h" +#include "display.h" + +/* Our wii's bluetooth and wii handles */ +static bdaddr_t g_bdaddr[4]; +static cwiid_wiimote_t *g_wiimotes[4]; +static int g_num_controls = 0; +static int g_samples_received = 0; +static uint32_t g_starttime; + +static volatile int g_redraw_lock = 0; + +static const int g_width = 400, g_height = 300; + +cwiid_mesg_callback_t cwiid_callback; +cwiid_err_t err; +void err(cwiid_wiimote_t *wiimote, const char *s, va_list ap) +{ + if (wiimote) printf("%d:", cwiid_get_id(wiimote)); else printf("-1:"); + vprintf(s, ap); + printf("\n"); +} + +int main( int argc, char **argv ) { + int LEDs[4] = { CWIID_LED1_ON, CWIID_LED1_ON | CWIID_LED2_ON, + CWIID_LED1_ON | CWIID_LED2_ON | CWIID_LED3_ON, + CWIID_LED1_ON | CWIID_LED2_ON | CWIID_LED3_ON | CWIID_LED4_ON }; + int i; + struct timeval now; + + signal( SIGINT, exit ); + display_init( g_width, g_height); + + cwiid_set_err(err); + + /* If no bdaddrs given on command line, just connect to + first visible wii */ + if( argc <= 1 ) { + char bt_out[64]; + g_bdaddr[0] = *BDADDR_ANY; + + while( !(g_wiimotes[g_num_controls] = cwiid_open( &g_bdaddr[0], 0 ))) + fprintf( stderr, "Unable to connect to wiimote\n" ); + + ba2str( &g_bdaddr[0], bt_out ); + fprintf( stderr, "Connected to wiimote %i (BT-Addr: %s)\n", i, bt_out ); + g_num_controls++; + } + + /* ... else try to reach every single wii */ + for( i=0; i> j ) & 1 ); + } + + break; + default: + break; + } + } + g_redraw_lock = 0; +} + diff --git a/vubars.c b/vubars.c new file mode 100644 index 0000000..8fcf081 --- /dev/null +++ b/vubars.c @@ -0,0 +1,148 @@ +#include +#include + +#include "gm.h" +#include "vubars.h" +#include "heuristics.h" + +// This is our current idea of how we want to influence the weather +// 128 means: do nothing at all +// +// We want to default to softening the weather, so our value always +// flows back towards 100 +static double vu_value_main = 100.0; +static double vu_value_softening = 0.0; +static double vu_value_activating = 0.0; + +static int wii_last[WII_COUNT * 4]; +static int wii_bars[WII_COUNT * 4]; + +int vubars_peak( int wii_id, int led, int is_on ) { + if( is_on ) { + wii_last[wii_id*4+led] = 0; + } else { + if( ++wii_last[wii_id*4+led] == 1 ) + wii_bars[wii_id*4+led] = VU_PEAK; + } +} + +int vubars_getinfo( int wii_id, int led ) { + return wii_bars[wii_id*4+led]; +} + +int vubars_getweather( ) { + return (int)vu_value_main; +} + +static int GESTURE_A_dd = 0; +static int GESTURE_A_dx = 0; +static int GESTURE_A_dy = 0; + +static int GESTURE_A_off = 0; +static int GESTURE_A_ttl = 0; + +static int GESTURE_B_dd = 0; +static int GESTURE_B_dx = 0; +static int GESTURE_B_dy = 0; + +static int GESTURE_B_off = 0; +static int GESTURE_B_ttl = 0; + +static void match_gesture( int off, int dx, int dy, int dd ) { + if( ( GESTURE_A_dd == dd ) && ( GESTURE_A_off == off + dd ) ) { + printf( "...continued A\n" ); + + GESTURE_A_off = off; + GESTURE_A_dx = dx; + GESTURE_A_dy = dy; + GESTURE_A_ttl = VU_PEAK; + + } else if ( ( GESTURE_B_dd == dd ) && ( GESTURE_B_off == off + dd ) ) { + printf( "...continued B\n" ); + + GESTURE_B_off = off; + GESTURE_B_dx = dx; + GESTURE_B_dy = dy; + GESTURE_B_ttl = VU_PEAK; + + } else { + printf( "\n" ); + + if( GESTURE_B_ttl >= GESTURE_A_ttl ) { + GESTURE_A_off = off; + GESTURE_A_dd = dd; + GESTURE_A_dx = dx; + GESTURE_A_dy = dy; + GESTURE_A_ttl = VU_PEAK; + } else { + GESTURE_B_off = off; + GESTURE_B_dd = dd; + GESTURE_B_dx = dx; + GESTURE_B_dy = dy; + GESTURE_B_ttl = VU_PEAK; + } + } +} + +void vubars_reduce() { + int i; + double diff; + + // First do some heuristics voodoo + int offa, offb, dxa, dxb, dya, dyb, dda, ddb; + + heuristics_getGestures( 16, wii_bars, &offa, &dxa, &dya, &dda, &offb, &dxb, &dyb, &ddb ); + + if( ( offa != -1 ) && ( dxa > 2 ) ) { + printf( "1mary gesture detected: offset: % 2d direction: %s len: % 2d speed: %lf", + offa, dda < 0 ? "left" : "rite", dxa, (double)dya / (double)dxa ); + match_gesture( offa, dxa, dya, dda ); + } + + + if( ( offb != -1 ) && ( dxb > 2 ) ) { + printf( "2ndry gesture detected: offset: % 2d direction: %s len: % 2d speed: %lf\n", + offb, ddb < 0 ? "left" : "rite", dxb, (double)dyb / (double)dxb ); + match_gesture( offb, dxb, dyb, ddb ); + } + + // Have two gestures living at one + if( GESTURE_A_ttl && GESTURE_B_ttl ) { + if( GESTURE_A_dd == GESTURE_B_dd ) { +// printf( "Two gestures pointing into the same direction.\n" ); + } else { + // The more interesting case: hands move into opposite directions + if( GESTURE_A_dd == -1 ) { + if ( GESTURE_A_off < GESTURE_B_off ) + printf( "Slower! (Case 1)\n" ); + else + printf( "Faster! (Case 2)\n" ); + } else { + if ( GESTURE_A_off < GESTURE_B_off ) + printf( "Faster! (Case 3)\n" ); + else + printf( "Slower! (Case 4)\n" ); + } + } + } + + // Now try to modify the weather by using values from our vu-bars + diff = (double)heuristics_getEnergy( 16, wii_bars ); + vu_value_main += diff / 40000.0; + + diff = vu_value_main - 100.0; + if( diff<0 ) diff = -diff; + diff = sqrt( diff ); + + if( vu_value_main > 100.0 ) diff = -diff; + + vu_value_main += diff; + + for( i=0; i