From be571a486992bf01ddf72b536b7c3bc47a87cc78 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Thu, 15 Dec 2016 22:35:34 -0500 Subject: Fix library add two more utility functions --- redist/linmath.c | 17 ++++++++++++++++- redist/linmath.h | 9 ++++++++- 2 files changed, 24 insertions(+), 2 deletions(-) diff --git a/redist/linmath.c b/redist/linmath.c index 808bfbf..60fbc21 100644 --- a/redist/linmath.c +++ b/redist/linmath.c @@ -63,7 +63,22 @@ void copy3d( FLT * out, const FLT * in ) out[2] = in[2]; } +FLT magnitude3d( FLT * a ) +{ + return sqrt( a[0]*a[0] + a[1]*a[1] + a[2]*a[2] ); +} +FLT anglebetween3d( FLT * a, FLT * b ) +{ + FLT an[3]; + FLT bn[3]; + normalize3d( an, a ); + normalize3d( bn, b ); + FLT dot = dot3d( a, b ); + if( dot < -0.9999999 ) return LINMATHPI; + if( dot > 0.9999999 ) return 0; + return acos( dot ); +} /////////////////////////////////////QUATERNIONS////////////////////////////////////////// //Originally from Mercury (Copyright (C) 2009 by Joshua Allen, Charles Lohr, Adam Lowman) @@ -74,7 +89,7 @@ void copy3d( FLT * out, const FLT * in ) void quatsetnone( FLT * q ) { - q[0] = 0; q[1] = 0; q[2] = 0; q[3] = 1; + q[0] = 1; q[1] = 0; q[2] = 0; q[3] = 0; } void quatcopy( FLT * qout, const FLT * qin ) diff --git a/redist/linmath.h b/redist/linmath.h index 9b21347..5cc7b7d 100644 --- a/redist/linmath.h +++ b/redist/linmath.h @@ -6,6 +6,12 @@ //Yes, I know it's kind of arbitrary. #define DEFAULT_EPSILON 0.001 +//For printf +#define PFTHREE(x) x[0], x[1], x[2] +#define PFFOUR(x) x[0], x[1], x[2], x[3] + +#define LINMATHPI 3.141592653589 + //If you want, you can define FLT to be double for double precision. #ifndef FLT #define FLT float @@ -30,8 +36,9 @@ int compare3d( const FLT * a, const FLT * b, FLT epsilon ); void copy3d( FLT * out, const FLT * in ); +FLT magnitude3d( FLT * a ); - +FLT anglebetween3d( FLT * a, FLT * b ); //Quaternion things... -- cgit v1.2.3 From 890a3addcc936229541626e6748fadd142f58a6e Mon Sep 17 00:00:00 2001 From: cnlohr Date: Thu, 15 Dec 2016 22:35:51 -0500 Subject: rework some of the way we display data and the way magic codes are sent. --- src/survive_data.c | 17 ++++++++++------- src/survive_internal.h | 2 +- src/survive_process.c | 12 ++++++------ src/survive_usb.c | 52 +++++++++++++++++++++++++++++++++++++++----------- 4 files changed, 58 insertions(+), 25 deletions(-) diff --git a/src/survive_data.c b/src/survive_data.c index 11677e0..c84ab09 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -41,11 +41,14 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement * int32_t deltat = (uint32_t)le->timestamp - (uint32_t)ct->last_photo_time; if( deltat > 2000 || deltat < -2000 ) //New pulse. (may be inverted) { - ct->last_photo_time = le->timestamp; - ct->total_photo_time = 0; - ct->total_photos = 0; - ct->total_pulsecode_time = 0; - survive_light_process( so, le->sensor_id, -1, 0, le->timestamp ); + if( le->timestamp - ct->last_photo_time > 80000 ) + { + ct->last_photo_time = le->timestamp; + ct->total_photo_time = 0; + ct->total_photos = 0; + ct->total_pulsecode_time = 0; + survive_light_process( so, le->sensor_id, -1, 0, le->timestamp, deltat ); + } } else { @@ -54,7 +57,7 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement * ct->total_photos++; } } - else if( le->length < 900 && le->length > 50 && ct->total_photos ) + else if( le->length < 1200 && le->length > 40 && ct->total_photos ) { int32_t dl = (ct->total_photo_time/ct->total_photos); int32_t tpco = (ct->total_pulsecode_time/ct->total_photos); @@ -70,7 +73,7 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement * if( offset_from < 380000 ) { - survive_light_process( so, le->sensor_id, acode, offset_from, le->timestamp ); + survive_light_process( so, le->sensor_id, acode, offset_from, le->timestamp, le->length ); } } else diff --git a/src/survive_internal.h b/src/survive_internal.h index d6168b2..40659e8 100644 --- a/src/survive_internal.h +++ b/src/survive_internal.h @@ -113,7 +113,7 @@ int survive_get_config( char ** config, struct SurviveContext * ctx, int devno, void survive_data_cb( struct SurviveUSBInterface * si ); //Accept higher-level data. -void survive_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode ); +void survive_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ); void survive_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id ); diff --git a/src/survive_process.c b/src/survive_process.c index 268fdfc..046971f 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -8,13 +8,13 @@ int bufferpts[32*2]; char buffermts[32*128]; int buffertimeto[32]; -void survive_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode ) +void survive_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ) { if( acode == -1 ) return; if( acode == 0 || acode == 2 ) //data = 0 { - printf( "L, X, %s, %d, %d, %d, %d\n", so->codename, timecode, sensor_id, acode, timeinsweep ); + printf( "L X %s %d %d %d %d %d\n", so->codename, timecode, sensor_id, acode, timeinsweep, length ); bufferpts[sensor_id*2+0] = (timeinsweep-100000)/500; buffertimeto[sensor_id] = 0; //printf( "X: %d\n",bufferpts[sensor_id*2+0] ); @@ -22,7 +22,7 @@ void survive_light_process( struct SurviveObject * so, int sensor_id, int acode, } if( acode == 1 || acode == 3 ) //data = 1 { - printf( "L, Y, %s, %d, %d, %d, %d\n", so->codename, timecode, sensor_id, acode, timeinsweep ); + printf( "L Y %s %d %d %d %d %d\n", so->codename, timecode, sensor_id, acode, timeinsweep, length ); bufferpts[sensor_id*2+1] = (timeinsweep-100000)/500; //printf( "Y: %d\n",bufferpts[sensor_id*2+1] ); buffertimeto[sensor_id] = 0; @@ -33,7 +33,7 @@ void survive_light_process( struct SurviveObject * so, int sensor_id, int acode, if( acode == 4 || acode == 6 ) //data = 0 { - printf( "R, X, %s, %d, %d, %d, %d\n", so->codename, timecode, sensor_id, acode, timeinsweep ); + printf( "R X %s %d %d %d %d %d\n", so->codename, timecode, sensor_id, acode, timeinsweep, length ); bufferpts[sensor_id*2+0] = (timeinsweep-100000)/500; buffertimeto[sensor_id] = 0; //printf( "X: %d\n",bufferpts[sensor_id*2+0] ); @@ -41,7 +41,7 @@ void survive_light_process( struct SurviveObject * so, int sensor_id, int acode, } if( acode == 5 || acode == 7 ) //data = 1 { - printf( "R, Y, %s, %d, %d, %d, %d\n", so->codename, timecode, sensor_id, acode, timeinsweep ); + printf( "R Y %s %d %d %d %d %d\n", so->codename, timecode, sensor_id, acode, timeinsweep, length ); bufferpts[sensor_id*2+1] = (timeinsweep-100000)/500; //printf( "Y: %d\n",bufferpts[sensor_id*2+1] ); buffertimeto[sensor_id] = 0; @@ -58,6 +58,6 @@ void survive_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32 //if( so->codename[0] == 'H' ) if( 1 ) { - printf( "I, %s, %d, %d, %d, %d, %d, %d, %d, %d\n", so->codename, timecode, accelgyro[0], accelgyro[1], accelgyro[2], accelgyro[3], accelgyro[4], accelgyro[5], id ); + printf( "I %s %d %d %d %d %d %d %d %d\n", so->codename, timecode, accelgyro[0], accelgyro[1], accelgyro[2], accelgyro[3], accelgyro[4], accelgyro[5], id ); } } diff --git a/src/survive_usb.c b/src/survive_usb.c index 07e5cb4..4f341ec 100644 --- a/src/survive_usb.c +++ b/src/survive_usb.c @@ -233,12 +233,22 @@ int survive_usb_init( struct SurviveContext * ctx ) SV_INFO( "All devices attached." ); -#if 1 + + //libUSB initialized. Continue. + return 0; +} + +int survive_usb_send_magic(struct SurviveContext * ctx, int turnon ) +{ + int r; + + + if( turnon ) { //Magic from vl_magic.h, originally copywritten under LGPL. // * Copyright (C) 2013 Fredrik Hultin // * Copyright (C) 2013 Jakob Bornecrantz - +#if 0 static uint8_t vive_magic_power_on[] = { 0x04, 0x78, 0x29, 0x38, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00, 0x01, 0xa8, 0x0d, 0x76, 0x00, 0x40, 0xfc, 0x01, 0x05, 0xfa, 0xec, 0xd1, 0x6d, 0x00, @@ -246,15 +256,14 @@ int survive_usb_init( struct SurviveContext * ctx ) 0x01, 0x05, 0x2c, 0xb0, 0x2e, 0x65, 0x7a, 0x0d, 0x76, 0x00, 0x68, 0x54, 0x72, 0x00, 0x18, 0x54, 0x72, 0x00, 0x00, 0x6a, 0x72, 0x00, 0x00, 0x00, 0x00, }; - - //From actual use. ((THIS SEEMS TO BREAK IT)) -/* +#else + //From actual steam. static uint8_t vive_magic_power_on[64] = { 0x04, 0x78, 0x29, 0x38, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x7a, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; -*/ +#endif r = update_feature_report( ctx->udev[USB_DEV_HMD], 0, vive_magic_power_on, sizeof( vive_magic_power_on ) ); SV_INFO( "UCR: %d", r ); if( r != sizeof( vive_magic_power_on ) ) return 5; @@ -274,17 +283,38 @@ int survive_usb_init( struct SurviveContext * ctx ) usleep( 1000 ); } #endif + SV_INFO( "Powered unit on." ); } -#endif + else + { - SV_INFO( "Powered unit on." ); + static uint8_t vive_magic_power_off1[] = { + 0x04, 0x78, 0x29, 0x38, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, + 0x30, 0x05, 0x77, 0x00, 0x30, 0x05, 0x77, 0x00, 0x6c, 0x4d, 0x37, 0x65, 0x40, + 0xf9, 0x33, 0x00, 0x04, 0xf8, 0xa3, 0x04, 0x04, 0x00, 0x00, 0x00, 0x70, 0xb0, + 0x72, 0x00, 0xf4, 0xf7, 0xa3, 0x04, 0x7c, 0xf8, 0x33, 0x00, 0x0c, 0xf8, 0xa3, + 0x04, 0x0a, 0x6e, 0x29, 0x65, 0x24, 0xf9, 0x33, 0x00, 0x00, 0x00, 0x00, + }; + static uint8_t vive_magic_power_off2[] = { + 0x04, 0x78, 0x29, 0x38, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, + 0x30, 0x05, 0x77, 0x00, 0xe4, 0xf7, 0x33, 0x00, 0xe4, 0xf7, 0x33, 0x00, 0x60, + 0x6e, 0x72, 0x00, 0xb4, 0xf7, 0x33, 0x00, 0x04, 0x00, 0x00, 0x00, 0x70, 0xb0, + 0x72, 0x00, 0x90, 0xf7, 0x33, 0x00, 0x7c, 0xf8, 0x33, 0x00, 0xd0, 0xf7, 0x33, + 0x00, 0x3c, 0x68, 0x29, 0x65, 0x24, 0xf9, 0x33, 0x00, 0x00, 0x00, 0x00, + }; +// r = update_feature_report( ctx->udev[USB_DEV_HMD], 0, vive_magic_power_off1, sizeof( vive_magic_power_off1 ) ); +// SV_INFO( "UCR: %d", r ); +// if( r != sizeof( vive_magic_power_off1 ) ) return 5; - //libUSB initialized. Continue. - return 0; -} + r = update_feature_report( ctx->udev[USB_DEV_HMD], 0, vive_magic_power_off2, sizeof( vive_magic_power_off2 ) ); + SV_INFO( "UCR: %d", r ); + if( r != sizeof( vive_magic_power_off2 ) ) return 5; + } + +} void survive_usb_close( struct SurviveContext * t ) { -- cgit v1.2.3 From c36b220308c08147a6c995fbb6082aa5ab0a0a21 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Thu, 15 Dec 2016 22:36:16 -0500 Subject: Get working plane tester --- include/survive.h | 1 + test.c | 13 ++- tools/planetest2/Makefile | 2 +- tools/planetest2/camfind.c | 277 +++++++++++++++++++++++++++------------------ 4 files changed, 178 insertions(+), 115 deletions(-) diff --git a/include/survive.h b/include/survive.h index 7ae1402..637bd09 100644 --- a/include/survive.h +++ b/include/survive.h @@ -11,6 +11,7 @@ int survive_poll(); int survive_simple_inflate( struct SurviveContext * ctx, const char * input, int inlen, char * output, int outlen ); +int survive_usb_send_magic(struct SurviveContext * ctx, int on ); #endif diff --git a/test.c b/test.c index 5eff185..1f6778a 100644 --- a/test.c +++ b/test.c @@ -7,6 +7,7 @@ #include #include +struct SurviveContext * ctx; void survivefault( struct SurviveContext * ctx, const char * fault ) { @@ -22,6 +23,16 @@ void survivenote( struct SurviveContext * ctx, const char * fault ) void HandleKey( int keycode, int bDown ) { + if( !bDown ) return; + + if( keycode == 'O' || keycode == 'o' ) + { + survive_usb_send_magic(ctx,1); + } + if( keycode == 'F' || keycode == 'f' ) + { + survive_usb_send_magic(ctx,0); + } } void HandleButton( int x, int y, int button, int bDown ) @@ -91,7 +102,7 @@ int main() printf( "%02x ", output[i] ); } return 0;*/ - struct SurviveContext * ctx = survive_init( &survivefault, &survivenote ); + ctx = survive_init( &survivefault, &survivenote ); CNFGBGColor = 0x000000; CNFGDialogColor = 0x444444; diff --git a/tools/planetest2/Makefile b/tools/planetest2/Makefile index dd19480..b48b099 100644 --- a/tools/planetest2/Makefile +++ b/tools/planetest2/Makefile @@ -1,6 +1,6 @@ all : camfind -CFLAGS:=-g -O4 -DFLT=float -ffast-math -I../../redist -flto +CFLAGS:=-g -O4 -DFLT=float -I../../redist -flto LDFLAGS:=$(CFLAGS) -lm camfind : camfind.o ../../redist/linmath.c diff --git a/tools/planetest2/camfind.c b/tools/planetest2/camfind.c index 54fd8a1..6caee1a 100644 --- a/tools/planetest2/camfind.c +++ b/tools/planetest2/camfind.c @@ -18,8 +18,6 @@ int LoadData( char Camera ); //Values used for RunTest() FLT LighthousePos[3] = { 0, 0, 0 }; FLT LighthouseQuat[4] = { 1, 0, 0, 0 }; -FLT BarrelRotate[4]; //XXX TODO: Concatenate these. - FLT RunOpti( int print ); FLT RunTest( int print ); @@ -41,6 +39,17 @@ int main() FLT bestxyz[3]; memcpy( bestxyz, LighthousePos, sizeof( LighthousePos ) ); + if( 0 ) + { + LighthousePos[0] = .0531311; + LighthousePos[1] = 1.2911; + LighthousePos[2] = 2.902; + RunOpti(1); + FLT ft = RunTest(1); + printf( "Final RMS: %f\n", ft ); + return 0; + } + //STAGE1 1: Detemine vectoral position from lighthouse to target. Does not determine lighthouse-target distance. //This also is constantly optimizing the lighthouse quaternion for optimal spotting. FLT fullrange = 5; //Maximum search space for positions. (Relative to HMD) @@ -52,10 +61,14 @@ int main() FLT bestxyzrunning[3]; FLT beste = 1e20; - FLT splits = 2; - if( cycle == 0 ) splits = 25; - //XXX TODO: Switch to polar coordinate search + FLT splits = 4; + if( cycle == 0 ) splits = 24; + if( cycle == 1 ) splits = 13; + if( cycle == 2 ) splits = 10; + if( cycle == 3 ) splits = 8; + if( cycle == 4 ) splits = 5; + for( dz = 0; dz <= fullrange; dz += fullrange/splits ) for( dy = -fullrange; dy <= fullrange; dy += fullrange/splits ) for( dx = -fullrange; dx <= fullrange; dx += fullrange/splits ) @@ -70,8 +83,6 @@ int main() //Try refining the search for the best orientation several times. ft = RunOpti(0); if( ft < beste ) { beste = ft; memcpy( bestxyzrunning, LighthousePos, sizeof( LighthousePos ) ); } - - //printf( " %f %f %f %f\n", LighthousePos[0], LighthousePos[1], LighthousePos[2], ft ); } memcpy( bestxyz, bestxyzrunning, sizeof( bestxyz ) ); @@ -81,7 +92,7 @@ int main() } //Every cycle, tighten up the search area. - fullrange *= 0.6; + fullrange *= 0.25; } //Use bestxyz @@ -90,29 +101,7 @@ int main() //Optimize the quaternion for lighthouse rotation RunOpti(1); - //STAGE 2: Determine optimal distance from target - { - FLT dist = 0.1; - FLT best_dist = 0; - FLT best_err = 1e20; - for( ; dist < 10; dist+=0.01 ) - { - FLT nrmvect[3]; - normalize3d( nrmvect, bestxyz ); - scale3d( LighthousePos, nrmvect, dist ); - FLT res = RunTest( 0 ); - if( res < best_err ) - { - best_dist = dist; - best_err = res; - } - } - - //Apply the best res. - normalize3d( LighthousePos, bestxyz ); - scale3d( LighthousePos, LighthousePos, best_dist ); - printf( "Best distance: %f\n", best_dist ); - } + printf( "Best Quat: %f %f %f %f\n", PFFOUR( LighthouseQuat ) ); //Print out plane accuracies with these settings. FLT ft = RunTest(1); @@ -121,97 +110,131 @@ int main() FLT RunOpti( int print ) { - int i; + int i, p; FLT UsToTarget[3]; + FLT LastUsToTarget[3]; + FLT mux = .9; + quatsetnone( LighthouseQuat ); - { - //XXX TODO: We should use several points to determine initial rotation optimization to object. - //By using only one point the "best" we could be rotating somewhere wrong. We do use - //several points for the rotate-about-this-vector rotation in stage 2. + int first = 1, second = 0; - //Find out where our ray shoots forth from. - FLT ax = hmd_point_angles[best_hmd_target*2+0]; - FLT ay = hmd_point_angles[best_hmd_target*2+1]; + //First check to see if this is a valid viewpoint. - //NOTE: Inputs may never be output with cross product. - //Create a fictitious normalized ray. Imagine the lighthouse is pointed - //straight in the +z direction, this is the lighthouse ray to the point. - FLT RayShootOut[3] = { sin(ax), sin(ay), 0 }; - RayShootOut[2] = sqrt( 1 - (RayShootOut[0]*RayShootOut[0] + RayShootOut[1]*RayShootOut[1]) ); - - //Find a ray from us to the target point. - sub3d( UsToTarget, &hmd_points[best_hmd_target*3], LighthousePos ); - normalize3d( UsToTarget, UsToTarget ); - - FLT AxisToRotate[3]; - cross3d( AxisToRotate, RayShootOut, UsToTarget ); - //Rotate the lighthouse around this axis to point at the HMD. - - FLT RotateAmount = -acos( dot3d( RayShootOut, UsToTarget ) ); //XXX TODO How to determine if negative. - quatfromaxisangle( LighthouseQuat, AxisToRotate, RotateAmount ); //Tested, working! + for( p = 0; p < 32; p++ ) + { + if( hmd_point_counts[p*2+0] < MIN_HITS_FOR_VALID || hmd_point_counts[p*2+1] < MIN_HITS_FOR_VALID ) continue; + FLT me_to_dot[3]; + sub3d( me_to_dot, LighthousePos, &hmd_points[p*3] ); + float dot = dot3d( &hmd_norms[p*3], me_to_dot ); + if( dot < -.01 ) { return 1000; } } - //Now our lighthouse's ray is pointed at the HMD's dot, but, we need to - //rotate the lighthouse such that it is oriented optimally. We do this - //by finding what the optimal rotation aroud our face would be for all - //remaining points. - FLT rotate_radians = 0; - int points_found_to_rotate = 0; + int iters = 6; - float rots[PTS]; - for( i = 0; i < PTS; i++ ) + for( i = 0; i < iters; i++ ) { - if( i == best_hmd_target ) continue; - int xhits = hmd_point_counts[i*2+0]; - int yhits = hmd_point_counts[i*2+1]; - int xyhits = (xhits Date: Thu, 15 Dec 2016 22:57:51 -0500 Subject: Remove old planetest --- tools/planetest/Makefile | 10 -- tools/planetest/camfind.c | 385 ---------------------------------------------- tools/planetest/linmath.c | 326 --------------------------------------- tools/planetest/linmath.h | 59 ------- 4 files changed, 780 deletions(-) delete mode 100644 tools/planetest/Makefile delete mode 100644 tools/planetest/camfind.c delete mode 100644 tools/planetest/linmath.c delete mode 100644 tools/planetest/linmath.h diff --git a/tools/planetest/Makefile b/tools/planetest/Makefile deleted file mode 100644 index 37f02d7..0000000 --- a/tools/planetest/Makefile +++ /dev/null @@ -1,10 +0,0 @@ -all : camfind - -CFLAGS:=-g -O4 -DFLT=float -ffast-math -LDFLAGS:=$(CFLAGS) -lm - -camfind : camfind.o linmath.o - gcc -o $@ $^ $(LDFLAGS) - -clean : - rm -rf *.o *~ camfind diff --git a/tools/planetest/camfind.c b/tools/planetest/camfind.c deleted file mode 100644 index 1e9b2b9..0000000 --- a/tools/planetest/camfind.c +++ /dev/null @@ -1,385 +0,0 @@ -#include -#include -#include "linmath.h" -#include -#include - -#define PTS 32 -#define MAX_CHECKS 20000 - -FLT hmd_points[PTS*3]; -FLT hmd_norms[PTS*3]; -FLT hmd_point_angles[PTS*2]; -int hmd_point_counts[PTS*2]; -int LoadData( char Camera ); - -//Values used for RunTest() -FLT LighthousePos[3] = { 0, 0, 0 }; -FLT LighthouseQuat[4] = { 1, 0, 0, 0 }; - -//Actual values -FLT xyzypr[6] = { 0, 2, 2, 0, 0, 0 }; - - -FLT RunOpti( int axis ); -FLT RunTest( int print ); -void PrintOpti(); - -int main() -{ - int i; - - //Load either 'L' (LH1) or 'R' (LH2) data. - if( LoadData( 'R' ) ) return 5; - - int opti = 0; - int cycle = 0; - int axis = 0; - - FLT dx, dy, dz; - - FLT bestxyzypr[6]; - memcpy( bestxyzypr, xyzypr, sizeof( xyzypr ) ); - - FLT fullrange = 5; //Maximum search space for positions. (Relative to HMD) - for( cycle = 0; cycle < 20; cycle ++ ) - { - - //Adjust position, one axis at a time, over and over until we zero in. -#define FULLSEARCH - -#ifndef FULLSEARCH - for( axis = 0; axis < 3; axis++ ) -#endif - { - FLT bestxyzyprrunning[6]; - FLT beste = 1e20; - - //Try a bunch of points in this axis. -#ifdef FULLSEARCH - for( dz = -fullrange; dz <= fullrange; dz += fullrange/3 ) - for( dy = -fullrange; dy <= fullrange; dy += fullrange/3 ) - for( dx = -fullrange; dx <= fullrange; dx += fullrange/3 ) - { -#else - for( dx = -fullrange; dx <= fullrange; dx += fullrange/5 ) - { -#endif - //Specificially adjust one axis at a time, searching for the best. - memcpy( xyzypr, bestxyzypr, sizeof( xyzypr ) ); -#ifdef FULLSEARCH - xyzypr[0] += dx; - xyzypr[1] += dy; - xyzypr[2] += dz; -#else - xyzypr[axis] += dx; -#endif - //if( axis == 2 && xyzypr[2] < 0 ) continue; - - xyzypr[3] = 0; - xyzypr[4] = 0; - xyzypr[5] = 0; - FLT ft; - int reopt = 0; - - //Try refining the search for the best orientation several times. - for( reopt = 0; reopt < 4; reopt++ ) - { - //XXX This search mechanism is insufficient :( - //Search through each axis every time. - for( opti = 3; opti < 6; opti++ ) - { - ft = RunOpti( opti ); - } - if( ft < beste ) { beste = ft; memcpy( bestxyzyprrunning, xyzypr, sizeof( xyzypr ) ); } - } - //printf( " %f %f %f %f\n", xyzypr[0], xyzypr[1], xyzypr[2], ft ); -#ifndef FULLSEARCH - memcpy( bestxyzypr, bestxyzyprrunning, sizeof( bestxyzypr ) ); -#endif - } -#ifdef FULLSEARCH - memcpy( bestxyzypr, bestxyzyprrunning, sizeof( bestxyzypr ) ); -#endif - - //Print out the quality of the lock this time. - FLT dist = sqrt(bestxyzypr[0]*bestxyzypr[0] + bestxyzypr[1]*bestxyzypr[1] + bestxyzypr[2]*bestxyzypr[2]); - printf( "%f %f %f (%f) %f %f %f = %f\n", bestxyzypr[0], bestxyzypr[1], bestxyzypr[2], dist, bestxyzypr[3], bestxyzypr[4], bestxyzypr[5], beste ); - } -/* - if( bestxyzypr[2] < 0 ) - { - bestxyzypr[0] *= -1; - bestxyzypr[1] *= -1; - bestxyzypr[2] *= -1; - } -*/ - //Every cycle, tighten up the search area. - fullrange *= 0.6; - } - - //Use bestxyzypr - memcpy( xyzypr, bestxyzypr, sizeof( xyzypr ) ); - - - //Print out plane accuracies with these settings. - PrintOpti(); -} - -void PrintOpti() -{ - FLT xyzyprchange[6]; - memcpy( xyzyprchange, xyzypr, sizeof( xyzypr ) ); - quatfromeuler( LighthouseQuat, &xyzyprchange[3] ); - memcpy( LighthousePos, &xyzyprchange[0], sizeof( LighthousePos ) ); - FLT ft = RunTest(1); - printf( "Final RMS: %f\n", ft ); -} - - -FLT RunOpti( int axis ) -{ - FLT xyzyprchange[6]; - memcpy( xyzyprchange, xyzypr, sizeof( xyzypr ) ); - int i; - FLT minv = 1e20; - FLT fv = -3.3; - FLT bv = 0; - - - //Coarse linear search first, try to figure out about where - for( i = 0; i < 33*2; i++ ) - { - xyzyprchange[axis] = fv; - quatfromeuler( LighthouseQuat, &xyzyprchange[3] ); - memcpy( LighthousePos, &xyzyprchange[0], sizeof( LighthousePos ) ); - FLT ft = RunTest(0); - //printf( " %d / %f = %f\n", ft, fv ); - if( ft < minv ) { minv = ft; bv = fv; } - fv += 0.1; - } - - xyzyprchange[axis] = bv; -#if 1 - //Now, do a more precise binary-ish search. - FLT jumpamount = 0.15; - for( i = 0; i < 20; i++ ) - { - FLT orig = xyzyprchange[axis]; - - minv = 1e20; - - //When searching, consider 'less' 'this' and 'more' - - fv = xyzyprchange[axis] = orig; - quatfromeuler( LighthouseQuat, &xyzyprchange[3] ); - memcpy( LighthousePos, &xyzyprchange[0], sizeof( LighthousePos ) ); - FLT ft = RunTest(0); - if( ft < minv ) { minv = ft; bv = fv; } - - fv = xyzyprchange[axis] = orig + jumpamount; - quatfromeuler( LighthouseQuat, &xyzyprchange[3] ); - memcpy( LighthousePos, &xyzyprchange[0], sizeof( LighthousePos ) ); - ft = RunTest(0); - if( ft < minv ) { minv = ft; bv = fv; } - - fv = xyzyprchange[axis] = orig - jumpamount; - quatfromeuler( LighthouseQuat, &xyzyprchange[3] ); - memcpy( LighthousePos, &xyzyprchange[0], sizeof( LighthousePos ) ); - ft = RunTest(0); - if( ft < minv ) { minv = ft; bv = fv; } - - xyzyprchange[axis] = bv; - - //Tighten up search area. Doing by 0.5 can cause unusual instabilities. - jumpamount *= 0.6; - } -#endif - xyzypr[axis] = bv; - return minv; -} - - -FLT RunTest( int print ) -{ - int k; - FLT totprob = 0.0; - int ict = 0; - for( k = 0; k < 64; k++ ) - { - if( hmd_point_counts[k] == 0 ) continue; - int axis = k%2; - int pt = k/2; - FLT angle = hmd_point_angles[k]; - if( print ) printf( "%d %d : angle: %f / ", axis, pt, angle ); - - //XXX TODO: This is critical. We need to properly define the planes. - FLT plane_normal[3] = { 0, 0, 0 }; - if( axis == 0 ) - { - plane_normal[1] = cos(angle); - plane_normal[2] =-sin(angle); - } - else - { - plane_normal[0] =-cos(angle); - plane_normal[2] =-sin(angle); - } - - quatrotatevector( plane_normal, LighthouseQuat, plane_normal ); //Rotate plane normal by concatenated rotation. - - //plane_normal is our normal / LighthousePos is our point. - FLT w0[] = { hmd_points[pt*3+0], hmd_points[pt*3+1], hmd_points[pt*3+2] }; - -//May be able to remove this. -// FLT w0[] = { hmd_points[pt*3+0], hmd_points[pt*3+2],-hmd_points[pt*3+1] }; //XXX WRONG Why does this produce the right answers? - - //FLT w0[] = { 0, 0, 0 }; - FLT d = -(plane_normal[0] * LighthousePos[0] + plane_normal[1] * LighthousePos[1] + plane_normal[2] * LighthousePos[2]); - FLT D = plane_normal[0] * w0[0] + plane_normal[1] * w0[1] + plane_normal[2] * w0[2] + d; - //Point line distance assuming ||normal|| = 1. - - FLT delta[] = { LighthousePos[0]-hmd_points[pt*3+0],LighthousePos[1]-hmd_points[pt*3+1],LighthousePos[2]-hmd_points[pt*3+2] }; - FLT dot = delta[0] * hmd_norms[pt*3+0] + delta[1] * hmd_norms[pt*3+1] + delta[2] * hmd_norms[pt*3+2]; -// if( dot < -0.04 ) totprob+=10000000000; - if( print ) printf( " %f %f N:%f\n", d, D, dot ); - totprob += (D*D); //Calculate RMS distance of incorrectitude. - - ict++; - } - return sqrt(totprob/ict); -} - - -int LoadData( char Camera ) -{ - int calpts[MAX_CHECKS*4]; //X (0) or Y (1), ID, offset - int calptscount; - - FILE * f = fopen( "correct_hmd_points.csv", "r" ); - int pt = 0; - if( !f ) { fprintf( stderr, "error: can't open hmd points.\n" ); return -5; } - while(!feof(f) && !ferror(f) && pt < PTS) - { - float fa, fb, fc; - int r = fscanf( f,"%g,%g,%g\n", &fa, &fb, &fc ); - hmd_points[pt*3+0] = fa; - hmd_points[pt*3+1] = fb; - hmd_points[pt*3+2] = fc; - pt++; - if( r != 3 ) - { - fprintf( stderr, "Not enough entries on line %d\n", pt ); - return -8; - } - } - if( pt < PTS ) - { - fprintf( stderr, "Not enough points.\n" ); - return -9; - } - fclose( f ); - printf( "Loaded %d points\n", pt ); - - - f = fopen( "hmd_normals.csv", "r" ); - int nrm = 0; - if( !f ) { fprintf( stderr, "error: can't open hmd points.\n" ); return -5; } - while(!feof(f) && !ferror(f) && nrm < PTS) - { - float fa, fb, fc; - int r = fscanf( f,"%g,%g,%g\n", &fa, &fb, &fc ); - hmd_norms[nrm*3+0] = fa; - hmd_norms[nrm*3+1] = fb; - hmd_norms[nrm*3+2] = fc; - nrm++; - if( r != 3 ) - { - fprintf( stderr, "Not enough entries on line %d\n", nrm ); - return -8; - } - } - if( nrm < PTS ) - { - fprintf( stderr, "Not enough points.\n" ); - return -9; - } - if( nrm != pt ) - { - fprintf( stderr, "point/normal counts disagree.\n" ); - return -9; - } - fclose( f ); - printf( "Loaded %d norms\n", nrm ); - - - int xck = 0; - f = fopen( "third_test_with_time_lengths.csv", "r" ); - if( !f ) { fprintf( stderr, "Error: can't open two lighthouses test data.\n" ); return -11; } - while( !feof(f) && !ferror(f) ) - { - char * lineptr; - size_t n; - lineptr = 0; - n = 0; - ssize_t r = getline( &lineptr, &n, f ); - char lf[10]; - char xory[10]; - char dev[10]; - int timestamp; - int sensorid; - int offset; - int code; - int length; - int rk = sscanf( lineptr, "%9s %9s %9s %d %d %d %d %d\n", lf, xory, dev, ×tamp, &sensorid, &code, &offset, &length ); - if( lf[0] == Camera && rk == 8 ) - { - //calpts - if( xory[0] == 'X' ) - { - calpts[xck*3+0] = 0; - } - else if( xory[0] == 'Y' ) - { - calpts[xck*3+0] = 1; - } - else - { - printf( "Confusing line\n" ); - continue; - } - - calpts[xck*3+1] = sensorid; - calpts[xck*3+2] = offset; - xck++; - } - if( lineptr ) free( lineptr ); - } - printf( "Cal points: %d\n", xck ); - calptscount = xck; - - int i; - for( i = 0; i < calptscount; i++ ) - { - int isy = calpts[i*3+0]; - int pt = calpts[i*3+1]; - int ofs = calpts[i*3+2]; - hmd_point_counts[pt*2+isy]++; - hmd_point_angles[pt*2+isy]+=ofs; - } - for( i = 0; i < 32; i++ ) - { - if( hmd_point_counts[i*2+0] < 100 ) hmd_point_counts[i*2+0] = 0; - if( hmd_point_counts[i*2+1] < 100 ) hmd_point_counts[i*2+1] = 0; - - hmd_point_angles[i*2+0]/=hmd_point_counts[i*2+0]; - hmd_point_angles[i*2+1]/=hmd_point_counts[i*2+1]; - hmd_point_angles[i*2+0] = (hmd_point_angles[i*2+0] - 200000) / 200000 * 3.1415926535/2; // Is it really 800,000 ticks per revolution? - hmd_point_angles[i*2+1] = (hmd_point_angles[i*2+1] - 200000) / 200000 * 3.1415926535/2; // Is it really 800,000 ticks per revolution? - } - - - - - return 0; -} diff --git a/tools/planetest/linmath.c b/tools/planetest/linmath.c deleted file mode 100644 index 88643b4..0000000 --- a/tools/planetest/linmath.c +++ /dev/null @@ -1,326 +0,0 @@ -//Copyright 2013 <>< C. N. Lohr. This file licensed under the terms of the MIT license. - -#include "linmath.h" -#include - -void cross3d( FLT * out, const FLT * a, const FLT * b ) -{ - out[0] = a[1]*b[2] - a[2]*b[1]; - out[1] = a[2]*b[0] - a[0]*b[2]; - out[2] = a[0]*b[1] - a[1]*b[0]; -} - -void sub3d( FLT * out, const FLT * a, const FLT * b ) -{ - out[0] = a[0] - b[0]; - out[1] = a[1] - b[1]; - out[2] = a[2] - b[2]; -} - -void add3d( FLT * out, const FLT * a, const FLT * b ) -{ - out[0] = a[0] + b[0]; - out[1] = a[1] + b[1]; - out[2] = a[2] + b[2]; -} - -void scale3d( FLT * out, const FLT * a, FLT scalar ) -{ - out[0] = a[0] * scalar; - out[1] = a[1] * scalar; - out[2] = a[2] * scalar; -} - -void normalize3d( FLT * out, const FLT * in ) -{ - FLT r = 1./sqrtf( in[0] * in[0] + in[1] * in[1] + in[2] * in[2] ); - out[0] = in[0] * r; - out[1] = in[1] * r; - out[2] = in[2] * r; -} - -FLT dot3d( const FLT * a, const FLT * b ) -{ - return a[0] * b[0] + a[1] * b[1] + a[2] * b[2]; -} - -int compare3d( const FLT * a, const FLT * b, FLT epsilon ) -{ - if( !a || !b ) return 0; - if( a[2] - b[2] > epsilon ) return 1; - if( b[2] - a[2] > epsilon ) return -1; - if( a[1] - b[1] > epsilon ) return 1; - if( b[1] - a[1] > epsilon ) return -1; - if( a[0] - b[0] > epsilon ) return 1; - if( b[0] - a[0] > epsilon ) return -1; - return 0; -} - -void copy3d( FLT * out, const FLT * in ) -{ - out[0] = in[0]; - out[1] = in[1]; - out[2] = in[2]; -} - - - -/////////////////////////////////////QUATERNIONS////////////////////////////////////////// -//Originally from Mercury (Copyright (C) 2009 by Joshua Allen, Charles Lohr, Adam Lowman) -//Under the mit/X11 license. - - - - -void quatsetnone( FLT * q ) -{ - q[0] = 0; q[1] = 0; q[2] = 0; q[3] = 1; -} - -void quatcopy( FLT * qout, const FLT * qin ) -{ - qout[0] = qin[0]; - qout[1] = qin[1]; - qout[2] = qin[2]; - qout[3] = qin[3]; -} - -void quatfromeuler( FLT * q, const FLT * euler ) -{ - FLT X = euler[0]/2.0f; //roll - FLT Y = euler[1]/2.0f; //pitch - FLT Z = euler[2]/2.0f; //yaw - - FLT cx = cosf(X); - FLT sx = sinf(X); - FLT cy = cosf(Y); - FLT sy = sinf(Y); - FLT cz = cosf(Z); - FLT sz = sinf(Z); - - //Correct according to - //http://en.wikipedia.org/wiki/Conversion_between_MQuaternions_and_Euler_angles - q[0] = cx*cy*cz+sx*sy*sz;//q1 - q[1] = sx*cy*cz-cx*sy*sz;//q2 - q[2] = cx*sy*cz+sx*cy*sz;//q3 - q[3] = cx*cy*sz-sx*sy*cz;//q4 - quatnormalize( q, q ); -} - -void quattoeuler( FLT * euler, const FLT * q ) -{ - //According to http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles (Oct 26, 2009) - euler[0] = atan2( 2 * (q[0]*q[1] + q[2]*q[3]), 1 - 2 * (q[1]*q[1] + q[2]*q[2] ) ); - euler[1] = asin( 2 * (q[0] *q[2] - q[3]*q[1] ) ); - euler[2] = atan2( 2 * (q[0]*q[3] + q[1]*q[2]), 1 - 2 * (q[2]*q[2] + q[3]*q[3] ) ); -} - -void quatfromaxisangle( FLT * q, const FLT * axis, FLT radians ) -{ - FLT v[3]; - normalize3d( v, axis ); - - FLT sn = sin(radians/2.0f); - q[0] = cos(radians/2.0f); - q[1] = sn * v[0]; - q[2] = sn * v[1]; - q[3] = sn * v[2]; - - quatnormalize( q, q ); -} - -FLT quatmagnitude( const FLT * q ) -{ - return sqrt((q[0]*q[0])+(q[1]*q[1])+(q[2]*q[2])+(q[3]*q[3])); -} - -FLT quatinvsqmagnitude( const FLT * q ) -{ - return 1./((q[0]*q[0])+(q[1]*q[1])+(q[2]*q[2])+(q[3]*q[3])); -} - - -void quatnormalize( FLT * qout, const FLT * qin ) -{ - FLT imag = quatinvsqmagnitude( qin ); - quatscale( qout, qin, imag ); -} - -void quattomatrix( FLT * matrix44, const FLT * qin ) -{ - FLT q[4]; - quatnormalize( q, qin ); - - //Reduced calulation for speed - FLT xx = 2*q[0]*q[0]; - FLT xy = 2*q[0]*q[1]; - FLT xz = 2*q[0]*q[2]; - FLT xw = 2*q[0]*q[3]; - - FLT yy = 2*q[1]*q[1]; - FLT yz = 2*q[1]*q[2]; - FLT yw = 2*q[1]*q[3]; - - FLT zz = 2*q[2]*q[2]; - FLT zw = 2*q[2]*q[3]; - - //opengl major - matrix44[0] = 1-yy-zz; - matrix44[1] = xy-zw; - matrix44[2] = xz+yw; - matrix44[3] = 0; - - matrix44[4] = xy+zw; - matrix44[5] = 1-xx-zz; - matrix44[6] = yz-xw; - matrix44[7] = 0; - - matrix44[8] = xz-yw; - matrix44[9] = yz+xw; - matrix44[10] = 1-xx-yy; - matrix44[11] = 0; - - matrix44[12] = 0; - matrix44[13] = 0; - matrix44[14] = 0; - matrix44[15] = 1; -} - -void quatgetconjugate( FLT * qout, const FLT * qin ) -{ - qout[0] = qin[0]; - qout[1] = -qin[1]; - qout[2] = -qin[2]; - qout[3] = -qin[3]; -} - -void quatgetreciprocal( FLT * qout, const FLT * qin ) -{ - FLT m = quatinvsqmagnitude(qin); - quatgetconjugate( qout, qin ); - quatscale( qout, qout, m ); -} - -void quatsub( FLT * qout, const FLT * a, const FLT * b ) -{ - qout[0] = a[0] - b[0]; - qout[1] = a[1] - b[1]; - qout[2] = a[2] - b[2]; - qout[3] = a[3] - b[3]; -} - -void quatadd( FLT * qout, const FLT * a, const FLT * b ) -{ - qout[0] = a[0] + b[0]; - qout[1] = a[1] + b[1]; - qout[2] = a[2] + b[2]; - qout[3] = a[3] + b[3]; -} - -void quatrotateabout( FLT * qout, const FLT * a, const FLT * b ) -{ - FLT q1[4]; - FLT q2[4]; - - quatnormalize( q1, a ); - quatnormalize( q2, b ); - - qout[0] = (q1[0]*q2[0])-(q1[1]*q2[1])-(q1[2]*q2[2])-(q1[3]*q2[3]); - qout[1] = (q1[0]*q2[1])+(q1[1]*q2[0])+(q1[2]*q2[3])-(q1[3]*q2[2]); - qout[2] = (q1[0]*q2[2])-(q1[1]*q2[3])+(q1[2]*q2[0])+(q1[3]*q2[1]); - qout[3] = (q1[0]*q2[3])+(q1[1]*q2[2])-(q1[2]*q2[1])+(q1[3]*q2[0]); -} - -void quatscale( FLT * qout, const FLT * qin, FLT s ) -{ - qout[0] = qin[0] * s; - qout[1] = qin[1] * s; - qout[2] = qin[2] * s; - qout[3] = qin[3] * s; -} - - -FLT quatinnerproduct( const FLT * qa, const FLT * qb ) -{ - return (qa[0]*qb[0])+(qa[1]*qb[1])+(qa[2]*qb[2])+(qa[3]*qb[3]); -} - -void quatouterproduct( FLT * outvec3, FLT * qa, FLT * qb ) -{ - outvec3[0] = (qa[0]*qb[1])-(qa[1]*qb[0])-(qa[2]*qb[3])+(qa[3]*qb[2]); - outvec3[1] = (qa[0]*qb[2])+(qa[1]*qb[3])-(qa[2]*qb[0])-(qa[3]*qb[1]); - outvec3[2] = (qa[0]*qb[3])-(qa[1]*qb[2])+(qa[2]*qb[1])-(qa[3]*qb[0]); -} - -void quatevenproduct( FLT * q, FLT * qa, FLT * qb ) -{ - q[0] = (qa[0]*qb[0])-(qa[1]*qb[1])-(qa[2]*qb[2])-(qa[3]*qb[3]); - q[1] = (qa[0]*qb[1])+(qa[1]*qb[0]); - q[2] = (qa[0]*qb[2])+(qa[2]*qb[0]); - q[3] = (qa[0]*qb[3])+(qa[3]*qb[0]); -} - -void quatoddproduct( FLT * outvec3, FLT * qa, FLT * qb ) -{ - outvec3[0] = (qa[2]*qb[3])-(qa[3]*qb[2]); - outvec3[1] = (qa[3]*qb[1])-(qa[1]*qb[3]); - outvec3[2] = (qa[1]*qb[2])-(qa[2]*qb[1]); -} - -void quatslerp( FLT * q, const FLT * qa, const FLT * qb, FLT t ) -{ - FLT an[4]; - FLT bn[4]; - quatnormalize( an, qa ); - quatnormalize( bn, qb ); - FLT cosTheta = quatinnerproduct(an,bn); - FLT sinTheta; - - //Careful: If cosTheta is exactly one, or even if it's infinitesimally over, it'll - // cause SQRT to produce not a number, and screw everything up. - if ( 1 - (cosTheta*cosTheta) <= 0 ) - sinTheta = 0; - else - sinTheta = sqrt(1 - (cosTheta*cosTheta)); - - FLT Theta = acos(cosTheta); //Theta is half the angle between the 2 MQuaternions - - if(fabs(Theta) < DEFAULT_EPSILON ) - quatcopy( q, qa ); - else if(fabs(sinTheta) < DEFAULT_EPSILON ) - { - quatadd( q, qa, qb ); - quatscale( q, q, 0.5 ); - } - else - { - FLT aside[4]; - FLT bside[4]; - quatscale( bside, qb, sin( t * Theta ) ); - quatscale( aside, qa, sin((1-t)*Theta) ); - quatadd( q, aside, bside ); - quatscale( q, q, 1./sinTheta ); - } -} - -void quatrotatevector( FLT * vec3out, const FLT * quat, const FLT * vec3in ) -{ - FLT tquat[4]; - FLT vquat[4]; - FLT qrecp[4]; - vquat[0] = 0; - vquat[1] = vec3in[0]; - vquat[2] = vec3in[1]; - vquat[3] = vec3in[2]; - - quatrotateabout( tquat, quat, vquat ); - quatgetreciprocal( qrecp, quat ); - quatrotateabout( vquat, tquat, qrecp ); - - vec3out[0] = vquat[1]; - vec3out[1] = vquat[2]; - vec3out[2] = vquat[3]; -} - - - diff --git a/tools/planetest/linmath.h b/tools/planetest/linmath.h deleted file mode 100644 index 881ca70..0000000 --- a/tools/planetest/linmath.h +++ /dev/null @@ -1,59 +0,0 @@ -//Copyright 2013 <>< C. N. Lohr. This file licensed under the terms of the MIT license. - -#ifndef _LINMATH_H -#define _LINMATH_H - -//Yes, I know it's kind of arbitrary. -#define DEFAULT_EPSILON 0.001 - - -//NOTE: Inputs may never be output with cross product. -void cross3d( FLT * out, const FLT * a, const FLT * b ); - -void sub3d( FLT * out, const FLT * a, const FLT * b ); - -void add3d( FLT * out, const FLT * a, const FLT * b ); - -void scale3d( FLT * out, const FLT * a, FLT scalar ); - -void normalize3d( FLT * out, const FLT * in ); - -FLT dot3d( const FLT * a, const FLT * b ); - -//Returns 0 if equal. If either argument is null, 0 will ALWAYS be returned. -int compare3d( const FLT * a, const FLT * b, FLT epsilon ); - -void copy3d( FLT * out, const FLT * in ); - - - - -//Quaternion things... - -void quatsetnone( FLT * q ); -void quatcopy( FLT * qout, const FLT * qin ); -void quatfromeuler( FLT * q, const FLT * euler ); -void quattoeuler( FLT * euler, const FLT * q ); -void quatfromaxisangle( FLT * q, const FLT * axis, FLT radians ); -FLT quatmagnitude( const FLT * q ); -FLT quatinvsqmagnitude( const FLT * q ); -void quatnormalize( FLT * qout, const FLT * qin ); //Safe for in to be same as out. -void quattomatrix( FLT * matrix44, const FLT * q ); -void quatgetconjugate( FLT * qout, const FLT * qin ); -void quatgetreciprocal( FLT * qout, const FLT * qin ); -void quatsub( FLT * qout, const FLT * a, const FLT * b ); -void quatadd( FLT * qout, const FLT * a, const FLT * b ); -void quatrotateabout( FLT * qout, const FLT * a, const FLT * b ); //same as quat multiply, not piecewise multiply. -void quatscale( FLT * qout, const FLT * qin, FLT s ); -FLT quatinnerproduct( const FLT * qa, const FLT * qb ); -void quatouterproduct( FLT * outvec3, FLT * qa, FLT * qb ); -void quatevenproduct( FLT * q, FLT * qa, FLT * qb ); -void quatoddproduct( FLT * outvec3, FLT * qa, FLT * qb ); -void quatslerp( FLT * q, const FLT * qa, const FLT * qb, FLT t ); -void quatrotatevector( FLT * vec3out, const FLT * quat, const FLT * vec3in ); - - -#endif - - - -- cgit v1.2.3 From 898a9a5f242a1691e1c34062208b48cb0682b5d9 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Fri, 16 Dec 2016 00:41:28 -0500 Subject: Remove dependency on Xinerama Improve camfinder a little Add data_recorder.c Closes #1 making libsurvive much more pluggable. --- Makefile | 10 +-- data_recorder.c | 160 +++++++++++++++++++++++++++++++++++++++++++++ include/survive.h | 42 +++++++++++- redist/XDriver.c | 2 +- src/survive.c | 58 ++++++++++++++-- src/survive_data.c | 29 ++------ src/survive_internal.h | 37 +++-------- src/survive_process.c | 54 ++------------- test.c | 106 +++--------------------------- tools/planetest2/camfind.c | 27 ++------ 10 files changed, 293 insertions(+), 232 deletions(-) create mode 100644 data_recorder.c diff --git a/Makefile b/Makefile index 29c05a1..d688287 100644 --- a/Makefile +++ b/Makefile @@ -1,10 +1,12 @@ -all : test +all : data_recorder test CFLAGS:=-Iinclude -fPIC -g -Os -Iredist -LDFLAGS:=-lpthread -lusb-1.0 -lz -lX11 -lXinerama -DEBUGSTUFF:=redist/os_generic.o redist/DrawFunctions.o redist/XDriver.o +LDFLAGS:=-lpthread -lusb-1.0 -lz -lX11 -test : test.c lib/libsurvive.so +test : test.c lib/libsurvive.so redist/os_generic.o + gcc -o $@ $^ $(LDFLAGS) $(CFLAGS) + +data_recorder : data_recorder.c lib/libsurvive.so redist/os_generic.o redist/DrawFunctions.o redist/XDriver.o gcc -o $@ $^ $(LDFLAGS) $(CFLAGS) lib/libsurvive.so : src/survive.o src/survive_usb.o src/survive_data.o src/survive_process.o redist/jsmn.o $(DEBUGSTUFF) diff --git a/data_recorder.c b/data_recorder.c new file mode 100644 index 0000000..24c8bc1 --- /dev/null +++ b/data_recorder.c @@ -0,0 +1,160 @@ +//Data recorder mod with GUI showing light positions. + +#include +#include +#include +#include +#include +#include +#include +#include + +struct SurviveContext * ctx; + +void HandleKey( int keycode, int bDown ) +{ + if( !bDown ) return; + + if( keycode == 'O' || keycode == 'o' ) + { + survive_usb_send_magic(ctx,1); + } + if( keycode == 'F' || keycode == 'f' ) + { + survive_usb_send_magic(ctx,0); + } +} + +void HandleButton( int x, int y, int button, int bDown ) +{ +} + +void HandleMotion( int x, int y, int mask ) +{ +} + +int bufferpts[32*2]; +char buffermts[32*128]; +int buffertimeto[32]; + +void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ) +{ + if( acode == -1 ) return; + + if( acode == 0 || acode == 2 ) //data = 0 + { + printf( "L X %s %d %d %d %d %d\n", so->codename, timecode, sensor_id, acode, timeinsweep, length ); + if( strcmp( so->codename, "HED" ) == 0 ) + { + bufferpts[sensor_id*2+0] = (timeinsweep-100000)/500; + buffertimeto[sensor_id] = 0; + } + } + if( acode == 1 || acode == 3 ) //data = 1 + { + printf( "L Y %s %d %d %d %d %d\n", so->codename, timecode, sensor_id, acode, timeinsweep, length ); + if( strcmp( so->codename, "HED" ) == 0 ) + { + bufferpts[sensor_id*2+1] = (timeinsweep-100000)/500; + buffertimeto[sensor_id] = 0; + } + } + + + if( acode == 4 || acode == 6 ) //data = 0 + { + printf( "R X %s %d %d %d %d %d\n", so->codename, timecode, sensor_id, acode, timeinsweep, length ); + if( strcmp( so->codename, "HED" ) == 0 ) + { + bufferpts[sensor_id*2+0] = (timeinsweep-100000)/500; + buffertimeto[sensor_id] = 0; + } + } + if( acode == 5 || acode == 7 ) //data = 1 + { + printf( "R Y %s %d %d %d %d %d\n", so->codename, timecode, sensor_id, acode, timeinsweep, length ); + if( strcmp( so->codename, "HED" ) == 0 ) + { + bufferpts[sensor_id*2+1] = (timeinsweep-100000)/500; + buffertimeto[sensor_id] = 0; + } + } + +} + +void my_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id ) +{ + //if( so->codename[0] == 'H' ) + if( 1 ) + { + printf( "I %s %d %d %d %d %d %d %d %d\n", so->codename, timecode, accelgyro[0], accelgyro[1], accelgyro[2], accelgyro[3], accelgyro[4], accelgyro[5], id ); + } +} + + + + +void * GuiThread( void * v ) +{ + short screenx, screeny; + while(1) + { + CNFGHandleInput(); + CNFGClearFrame(); + CNFGColor( 0xFFFFFF ); + CNFGGetDimensions( &screenx, &screeny ); + + int i; + for( i = 0; i < 32; i++ ) + { + if( buffertimeto[i] < 5 ) + { + uint32_t color = i * 3231349; + uint8_t r = color & 0xff; + uint8_t g = (color>>8) & 0xff; + uint8_t b = (color>>16) & 0xff; + r = (r * (5-buffertimeto[i])) / 5 ; + g = (g * (5-buffertimeto[i])) / 5 ; + b = (b * (5-buffertimeto[i])) / 5 ; + CNFGColor( (b<<16) | (g<<8) | r ); + CNFGTackRectangle( bufferpts[i*2+0], bufferpts[i*2+1], bufferpts[i*2+0] + 5, bufferpts[i*2+1] + 5 ); + CNFGPenX = bufferpts[i*2+0]; CNFGPenY = bufferpts[i*2+1]; + CNFGDrawText( buffermts, 2 ); + buffertimeto[i]++; + } + } + + + CNFGSwapBuffers(); + OGUSleep( 10000 ); + } +} + + + +int main() +{ + ctx = survive_init( ); + + survive_install_light_fn( ctx, my_light_process ); + survive_install_imu_fn( ctx, my_imu_process ); + + + CNFGBGColor = 0x000000; + CNFGDialogColor = 0x444444; + CNFGSetup( "Survive GUI Debug", 640, 480 ); + OGCreateThread( GuiThread, 0 ); + + + if( !ctx ) + { + fprintf( stderr, "Fatal. Could not start\n" ); + return 1; + } + + while(survive_poll(ctx) == 0) + { + //Do stuff. + } +} + diff --git a/include/survive.h b/include/survive.h index 637bd09..61a6620 100644 --- a/include/survive.h +++ b/include/survive.h @@ -1,17 +1,53 @@ #ifndef _SURVIVE_H #define _SURVIVE_H +#include + +#define SV_FLOAT double + struct SurviveContext; -struct SurviveContext * survive_init( void(*faultfunction)( struct SurviveContext * ctx, const char * fault ), - void(*notefunction)( struct SurviveContext * ctx, const char * note ) ); +//DANGER: This structure may be redefined + +struct SurviveObject +{ + struct SurviveContext * ctx; + char codename[4]; + int16_t buttonmask; + int16_t axis1; + int16_t axis2; + int16_t axis3; + int8_t charge; + int8_t charging:1; + int8_t ison:1; + int sensors; + + int nr_locations; + SV_FLOAT * sensor_locations; +}; + +typedef void (*text_feedback_fnptr)( struct SurviveContext * ctx, const char * fault ); +typedef void (*light_process_func)( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ); +typedef void (*imu_process_func)( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id ); + +struct SurviveContext * survive_init(); + +//For any of these, you may pass in 0 for the function pointer to use default behavior. +void survive_install_info_fn( struct SurviveContext * ctx, text_feedback_fnptr fbp ); +void survive_install_error_fn( struct SurviveContext * ctx, text_feedback_fnptr fbp ); +void survive_install_light_fn( struct SurviveContext * ctx, light_process_func fbp ); +void survive_install_imu_fn( struct SurviveContext * ctx, imu_process_func fbp ); void survive_close( struct SurviveContext * ctx ); int survive_poll(); + +//Utilitiy functions. int survive_simple_inflate( struct SurviveContext * ctx, const char * input, int inlen, char * output, int outlen ); -int survive_usb_send_magic(struct SurviveContext * ctx, int on ); + +//TODO: Need to make this do haptic responses for hands. +int survive_usb_send_magic( struct SurviveContext * ctx, int on ); #endif diff --git a/redist/XDriver.c b/redist/XDriver.c index 942fe0f..507ca95 100644 --- a/redist/XDriver.c +++ b/redist/XDriver.c @@ -2,7 +2,7 @@ //portions from //http://www.xmission.com/~georgeps/documentation/tutorials/Xlib_Beginner.html -#define HAS_XINERAMA +//#define HAS_XINERAMA #include "DrawFunctions.h" diff --git a/src/survive.c b/src/survive.c index edcbb86..773f02a 100644 --- a/src/survive.c +++ b/src/survive.c @@ -17,13 +17,29 @@ static int jsoneq(const char *json, jsmntok_t *tok, const char *s) { return -1; } -struct SurviveContext * survive_init( void(*ff)( struct SurviveContext * ctx, const char * fault ), void(*notefunction)( struct SurviveContext * ctx, const char * note ) ) + +static void survivefault( struct SurviveContext * ctx, const char * fault ) +{ + fprintf( stderr, "Error: %s\n", fault ); + exit( -1 ); +} + +static void survivenote( struct SurviveContext * ctx, const char * fault ) +{ + fprintf( stderr, "Info: %s\n", fault ); +} + + +struct SurviveContext * survive_init() { int r = 0; - struct SurviveContext * ctx = calloc( 1, sizeof( struct SurviveContext ) ); + struct SurviveContext * ctx = calloc( 1, sizeof( struct SurviveContext ) ); + + ctx->faultfunction = survivefault; + ctx->notefunction = survivenote; - ctx->faultfunction = ff; - ctx->notefunction = notefunction; + ctx->lightproc = survive_default_light_process; + ctx->imuproc = survive_default_imu_process; ctx->headset.sensors = 32; ctx->headset.ctx = ctx; @@ -44,8 +60,6 @@ struct SurviveContext * survive_init( void(*ff)( struct SurviveContext * ctx, co return 0; } - -#if 1 //Next, pull out the config stuff. { char * ct0conf = 0; @@ -123,12 +137,42 @@ struct SurviveContext * survive_init( void(*ff)( struct SurviveContext * ctx, co } } -#endif //ctx->headset->photos = malloc( ctx->headset->sensors * sizeof(struct SurvivePhoto) ); return ctx; } +void survive_install_info_fn( struct SurviveContext * ctx, text_feedback_fnptr fbp ) +{ + if( fbp ) + ctx->notefunction = fbp; + else + ctx->notefunction = survivenote; +} + +void survive_install_error_fn( struct SurviveContext * ctx, text_feedback_fnptr fbp ) +{ + if( fbp ) + ctx->faultfunction = fbp; + else + ctx->faultfunction = survivefault; +} + +void survive_install_light_fn( struct SurviveContext * ctx, light_process_func fbp ) +{ + if( fbp ) + ctx->lightproc = fbp; + else + ctx->lightproc = survive_default_light_process; +} + +void survive_install_imu_fn( struct SurviveContext * ctx, imu_process_func fbp ) +{ + if( fbp ) + ctx->imuproc = fbp; + else + ctx->imuproc = survive_default_imu_process; +} void survive_close( struct SurviveContext * ctx ) { diff --git a/src/survive_data.c b/src/survive_data.c index c84ab09..88c9e7d 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -47,7 +47,7 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement * ct->total_photo_time = 0; ct->total_photos = 0; ct->total_pulsecode_time = 0; - survive_light_process( so, le->sensor_id, -1, 0, le->timestamp, deltat ); + ct->lightproc( so, le->sensor_id, -1, 0, le->timestamp, deltat ); } } else @@ -73,7 +73,7 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement * if( offset_from < 380000 ) { - survive_light_process( so, le->sensor_id, acode, offset_from, le->timestamp, le->length ); + ct->lightproc( so, le->sensor_id, acode, offset_from, le->timestamp, le->length ); } } else @@ -158,7 +158,7 @@ static void handle_watchman( struct SurviveObject * w, uint8_t * readdata ) if( ( ( type & 0xe8 ) == 0xe8 ) || doimu ) //Hmm, this looks kind of yucky... we can get e8's that are accelgyro's but, cleared by first propset. { propset |= 2; - survive_imu_process( w, (int16_t *)&readdata[1], (time1<<24)|(time2<<16)|readdata[0], 0 ); + w->ctx->imuproc( w, (int16_t *)&readdata[1], (time1<<24)|(time2<<16)|readdata[0], 0 ); int16_t * k = (int16_t *)readdata+1; //printf( "Match8 %d %d %d %d %d %3d %3d\n", qty, k[0], k[1], k[2], k[3], k[4], k[5] ); readdata += 13; qty -= 13; @@ -178,16 +178,6 @@ static void handle_watchman( struct SurviveObject * w, uint8_t * readdata ) readdata--; *readdata = type; -#if 1 - // good good maybe? probably wrong - printf( "POST %d: %4d (%02x%02x) - ", propset, qty, time1, time2 ); - for( i = 0; i < qty + 4; i++ ) - { - printf( "%02x ", readdata[i] ); - } - printf("\n"); -#endif - uint8_t * end = &readdata[qty]; uint32_t mytime = (end[1] << 0)|(end[2] << 8)|(end[3] << 16); @@ -289,7 +279,8 @@ static void handle_watchman( struct SurviveObject * w, uint8_t * readdata ) int i; for( i = lese-1; i >= 0; i-- ) { - printf( "%d: %d [%d]\n", les[i].sensor_id, les[i].length, les[i].timestamp ); + //printf( "%d: %d [%d]\n", les[i].sensor_id, les[i].length, les[i].timestamp ); + handle_lightcap( w, &les[i] ); } return; @@ -345,14 +336,6 @@ void survive_data_cb( struct SurviveUSBInterface * si ) //printf( "%d -> ", size ); for( i = 0; i < 3; i++ ) { -/* - for( i = 0; i < 17; i++ ) - { - printf( "%02x ", readdata[i] ); - } - printf( "\n" ); -*/ - //handle_lightdata( (struct LightpulseStructure *)readdata ); int16_t * acceldata = (int16_t*)readdata; readdata += 12; uint32_t timecode = POP4; @@ -363,7 +346,7 @@ void survive_data_cb( struct SurviveUSBInterface * si ) if( cd > 0 ) { ctx->oldcode = code; - survive_imu_process( &ctx->headset, acceldata, timecode, code ); + ctx->imuproc( &ctx->headset, acceldata, timecode, code ); } } diff --git a/src/survive_internal.h b/src/survive_internal.h index 40659e8..ac378a2 100644 --- a/src/survive_internal.h +++ b/src/survive_internal.h @@ -19,6 +19,7 @@ #include #include #include +#include #define SV_INFO( x... ) { char stbuff[1024]; sprintf( stbuff, x ); ctx->notefunction( ctx, stbuff ); } #define SV_ERROR( x... ) { char stbuff[1024]; sprintf( stbuff, x ); ctx->faultfunction( ctx, stbuff ); } @@ -26,8 +27,6 @@ //XXX TODO This one needs to be rewritten. #define SV_KILL() exit(0) -#define SV_FLOAT double - #define USB_DEV_HMD 0 #define USB_DEV_LIGHTHOUSE 1 #define USB_DEV_WATCHMAN1 2 @@ -61,33 +60,20 @@ struct SurviveUSBInterface const char * hname; //human-readable names }; - -struct SurviveObject -{ - struct SurviveContext * ctx; - char codename[4]; - int16_t buttonmask; - int16_t axis1; - int16_t axis2; - int16_t axis3; - int8_t charge; - int8_t charging:1; - int8_t ison:1; - int sensors; - - int nr_locations; - SV_FLOAT * sensor_locations; -}; +//This is defined in survive.h +struct SurviveObject; struct SurviveContext { //USB Subsystem struct libusb_context* usbctx; - void(*faultfunction)( struct SurviveContext * ctx, const char * fault ); - void(*notefunction)( struct SurviveContext * ctx, const char * fault ); struct libusb_device_handle * udev[MAX_USB_DEVS]; struct SurviveUSBInterface uiface[MAX_INTERFACES]; + text_feedback_fnptr faultfunction; + text_feedback_fnptr notefunction; + light_process_func lightproc; + imu_process_func imuproc; //Flood info, for calculating which laser is currently sweeping. int8_t oldcode; @@ -113,13 +99,8 @@ int survive_get_config( char ** config, struct SurviveContext * ctx, int devno, void survive_data_cb( struct SurviveUSBInterface * si ); //Accept higher-level data. -void survive_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ); -void survive_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id ); - - -//Util -int survive_simple_inflate( struct SurviveContext * ctx, const char * input, int inlen, char * output, int outlen ); - +void survive_default_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ); +void survive_default_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id ); #endif diff --git a/src/survive_process.c b/src/survive_process.c index 046971f..d3a8c4a 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -8,56 +8,14 @@ int bufferpts[32*2]; char buffermts[32*128]; int buffertimeto[32]; -void survive_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ) +void survive_default_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ) { - if( acode == -1 ) return; - - if( acode == 0 || acode == 2 ) //data = 0 - { - printf( "L X %s %d %d %d %d %d\n", so->codename, timecode, sensor_id, acode, timeinsweep, length ); - bufferpts[sensor_id*2+0] = (timeinsweep-100000)/500; - buffertimeto[sensor_id] = 0; - //printf( "X: %d\n",bufferpts[sensor_id*2+0] ); - //480-(timeinsweep)/1000; // Full scan - } - if( acode == 1 || acode == 3 ) //data = 1 - { - printf( "L Y %s %d %d %d %d %d\n", so->codename, timecode, sensor_id, acode, timeinsweep, length ); - bufferpts[sensor_id*2+1] = (timeinsweep-100000)/500; - //printf( "Y: %d\n",bufferpts[sensor_id*2+1] ); - buffertimeto[sensor_id] = 0; - - //480-(timeinsweep)/1000; //Full scan - } - - - if( acode == 4 || acode == 6 ) //data = 0 - { - printf( "R X %s %d %d %d %d %d\n", so->codename, timecode, sensor_id, acode, timeinsweep, length ); - bufferpts[sensor_id*2+0] = (timeinsweep-100000)/500; - buffertimeto[sensor_id] = 0; - //printf( "X: %d\n",bufferpts[sensor_id*2+0] ); - //480-(timeinsweep)/1000; // Full scan - } - if( acode == 5 || acode == 7 ) //data = 1 - { - printf( "R Y %s %d %d %d %d %d\n", so->codename, timecode, sensor_id, acode, timeinsweep, length ); - bufferpts[sensor_id*2+1] = (timeinsweep-100000)/500; - //printf( "Y: %d\n",bufferpts[sensor_id*2+1] ); - buffertimeto[sensor_id] = 0; - - //480-(timeinsweep)/1000; //Full scan - } - - - //timeinsweep = 200,000 1/48,000,000ths of a second is "center-of-image" + //TODO: Writeme! } -void survive_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id ) +void survive_default_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id ) { - //if( so->codename[0] == 'H' ) - if( 1 ) - { - printf( "I %s %d %d %d %d %d %d %d %d\n", so->codename, timecode, accelgyro[0], accelgyro[1], accelgyro[2], accelgyro[3], accelgyro[4], accelgyro[5], id ); - } + //TODO: Writeme! } + + diff --git a/test.c b/test.c index 1f6778a..537b2e5 100644 --- a/test.c +++ b/test.c @@ -9,106 +9,12 @@ struct SurviveContext * ctx; -void survivefault( struct SurviveContext * ctx, const char * fault ) -{ - fprintf( stderr, "Error: %s\n", fault ); - exit( -1 ); -} - -void survivenote( struct SurviveContext * ctx, const char * fault ) -{ - fprintf( stderr, "Info: %s\n", fault ); -} - - -void HandleKey( int keycode, int bDown ) -{ - if( !bDown ) return; - - if( keycode == 'O' || keycode == 'o' ) - { - survive_usb_send_magic(ctx,1); - } - if( keycode == 'F' || keycode == 'f' ) - { - survive_usb_send_magic(ctx,0); - } -} - -void HandleButton( int x, int y, int button, int bDown ) -{ -} - -void HandleMotion( int x, int y, int mask ) -{ -} - -extern int bufferpts[32*2]; -extern char buffermts[32*128]; -extern int buffertimeto[32]; - -void * GuiThread( void * v ) -{ - short screenx, screeny; - while(1) - { - CNFGHandleInput(); - CNFGClearFrame(); - CNFGColor( 0xFFFFFF ); - CNFGGetDimensions( &screenx, &screeny ); - - int i; - for( i = 0; i < 32; i++ ) - { - if( buffertimeto[i] < 5 ) - { - uint32_t color = i * 3231349; - uint8_t r = color & 0xff; - uint8_t g = (color>>8) & 0xff; - uint8_t b = (color>>16) & 0xff; - r = (r * (5-buffertimeto[i])) / 5 ; - g = (g * (5-buffertimeto[i])) / 5 ; - b = (b * (5-buffertimeto[i])) / 5 ; - CNFGColor( (b<<16) | (g<<8) | r ); - CNFGTackRectangle( bufferpts[i*2+0], bufferpts[i*2+1], bufferpts[i*2+0] + 5, bufferpts[i*2+1] + 5 ); - CNFGPenX = bufferpts[i*2+0]; CNFGPenY = bufferpts[i*2+1]; - CNFGDrawText( buffermts, 2 ); - buffertimeto[i]++; - } - } - - - CNFGSwapBuffers(); - OGUSleep( 10000 ); - } -} - - - int main() { -/* int i; - uint8_t input[] = { 0x7a, 0x01, 0x48, 0xc4, 0x1e, 0x1a, 0xfe, 0x6f, 0x6a, 0xf7, 0x25, 0x34 }; -// uint8_t input[] = { 0x1f, 0x8b, 0x08, 0x00, 0xc2, 0x45, 0x43, 0x58, 0x00, 0x03, 0xcb, 0xc8, 0xe4, 0x02, 0x00, 0x7a, 0x7a, 0x6f, 0xed, 0x03, 0x00, 0x00, 0x00 }; -// uint8_t input[] = { 0x78, 0xda, 0xcb, 0xc8, 0x04, 0x00, 0x01, 0x3b, 0x00, 0xd2 }; - - uint8_t output[1024]; - - int r = survive_simple_inflate( 0, input, sizeof( input ), output, sizeof(output) ); + int magicon = 0; + double Start = OGGetAbsoluteTime(); - printf( "%d: ", r ); - for( i = 0 ;i < r; i++ ) - { - printf( "%02x ", output[i] ); - } - return 0;*/ - ctx = survive_init( &survivefault, &survivenote ); - - CNFGBGColor = 0x000000; - CNFGDialogColor = 0x444444; - CNFGSetup( "Survive GUI Debug", 640, 480 ); - OGCreateThread( GuiThread, 0 ); - + ctx = survive_init( ); if( !ctx ) { @@ -118,6 +24,12 @@ int main() while(survive_poll(ctx) == 0) { + double Now = OGGetAbsoluteTime(); + if( Now > (Start+1) && !magicon ) + { + survive_usb_send_magic(ctx,1); + magicon = 1; + } //Do stuff. } } diff --git a/tools/planetest2/camfind.c b/tools/planetest2/camfind.c index 6caee1a..c987e46 100644 --- a/tools/planetest2/camfind.c +++ b/tools/planetest2/camfind.c @@ -5,7 +5,7 @@ #include #define PTS 32 -#define MAX_CHECKS 20000 +#define MAX_CHECKS 30000 #define MIN_HITS_FOR_VALID 10 FLT hmd_points[PTS*3]; @@ -28,7 +28,7 @@ int main() int i; //Load either 'L' (LH1) or 'R' (LH2) data. - if( LoadData( 'R' ) ) return 5; + if( LoadData( 'L' ) ) return 5; int opti = 0; int cycle = 0; @@ -39,17 +39,6 @@ int main() FLT bestxyz[3]; memcpy( bestxyz, LighthousePos, sizeof( LighthousePos ) ); - if( 0 ) - { - LighthousePos[0] = .0531311; - LighthousePos[1] = 1.2911; - LighthousePos[2] = 2.902; - RunOpti(1); - FLT ft = RunTest(1); - printf( "Final RMS: %f\n", ft ); - return 0; - } - //STAGE1 1: Detemine vectoral position from lighthouse to target. Does not determine lighthouse-target distance. //This also is constantly optimizing the lighthouse quaternion for optimal spotting. FLT fullrange = 5; //Maximum search space for positions. (Relative to HMD) @@ -231,7 +220,7 @@ FLT RunOpti( int print ) cross3d( xproduct, UsToTarget, RayShootOut ); FLT dist = magnitude3d( xproduct ); errorsq += dist*dist; - if( print ) printf( "%f (%d) ", dist, p ); + if( print ) printf( "%f (%d(%d/%d))\n", dist, p, hmd_point_counts[p*2+0], hmd_point_counts[p*2+1] ); } if( print ) printf( " = %f\n", sqrt( errorsq ) ); return sqrt(errorsq); @@ -270,11 +259,6 @@ FLT RunTest( int print ) //plane_normal is our normal / LighthousePos is our point. FLT w0[] = { hmd_points[pt*3+0], hmd_points[pt*3+1], hmd_points[pt*3+2] }; - -//May be able to remove this. -// FLT w0[] = { hmd_points[pt*3+0], hmd_points[pt*3+2],-hmd_points[pt*3+1] }; //XXX WRONG Why does this produce the right answers? - - //FLT w0[] = { 0, 0, 0 }; FLT d = -(plane_normal[0] * LighthousePos[0] + plane_normal[1] * LighthousePos[1] + plane_normal[2] * LighthousePos[2]); FLT D = plane_normal[0] * w0[0] + plane_normal[1] * w0[1] + plane_normal[2] * w0[2] + d; //Point line distance assuming ||normal|| = 1. @@ -291,8 +275,9 @@ FLT RunTest( int print ) if( print ) { int p; - printf( "Imagespace comparison:\n" ); + printf( "POS: %f %f %f %f\n", PFFOUR(LighthousePos ) ); printf( "QUAT: %f %f %f %f\n", PFFOUR(LighthouseQuat ) ); + printf( "Imagespace comparison:\n" ); for( p = 0; p < 32; p++ ) { if( hmd_point_counts[p*2+0] < MIN_HITS_FOR_VALID || hmd_point_counts[p*2+1] < MIN_HITS_FOR_VALID ) continue; @@ -381,7 +366,7 @@ int LoadData( char Camera ) int xck = 0; - f = fopen( "third_test_with_time_lengths.csv", "r" ); + f = fopen( "testfive.csv", "r" ); if( !f ) { fprintf( stderr, "Error: can't open two lighthouses test data.\n" ); return -11; } while( !feof(f) && !ferror(f) ) { -- cgit v1.2.3