diff options
| -rw-r--r-- | calibrate.c | 10 | ||||
| -rw-r--r-- | redist/lintest.c | 16 | ||||
| -rw-r--r-- | src/survive.c | 2 | ||||
| -rw-r--r-- | src/survive_cal_lhfind.c | 13 | ||||
| -rw-r--r-- | src/survive_data.c | 2 |
5 files changed, 35 insertions, 8 deletions
diff --git a/calibrate.c b/calibrate.c index 82da7a7..be506a1 100644 --- a/calibrate.c +++ b/calibrate.c @@ -13,6 +13,7 @@ #include "src/survive_config.h" struct SurviveContext * ctx; +int quit = 0; void HandleKey( int keycode, int bDown ) { @@ -26,6 +27,10 @@ void HandleKey( int keycode, int bDown ) { survive_send_magic(ctx,0,0,0); } + if( keycode == 'Q' || keycode == 'q' ) + { + quit = 1; + } } void HandleButton( int x, int y, int button, int bDown ) @@ -163,10 +168,13 @@ int main() return 1; } - while(survive_poll(ctx) == 0) + while(survive_poll(ctx) == 0 && !quit) { //Do stuff. } + + survive_close( ctx ); + printf( "Returned\n" ); } diff --git a/redist/lintest.c b/redist/lintest.c index 0a268ff..377824e 100644 --- a/redist/lintest.c +++ b/redist/lintest.c @@ -7,6 +7,16 @@ int main() FLT e[3] = { 1,1,3.14 }; FLT q[4]; FLT m[16]; + FLT pt[3] = { 1, 1, 1 }; + + q[0] = 0; + q[1] = 0; + q[2] = 0; + q[3] = 1; + + quatrotatevector( pt, q, pt ); + printf( "%f %f %f\n", PFTHREE( pt ) ); + printf( "\n" ); quatfromeuler( q, e ); printf( "%f %f %f %f\n\n", PFFOUR( q ) ); @@ -32,9 +42,9 @@ int main() //Try setting up a pose. - FLT mypose[7] = { 0, 0, 10, q[0], q[1], q[2], q[3] ); - ApplyPoseToPoint( FLT * pout, const FLT * pin, const FLT * pose ); -void InvertPose( FLT * poseout, const FLT * pose ); +// FLT mypose[7] = { 0, 0, 10, q[0], q[1], q[2], q[3] ); +// ApplyPoseToPoint( FLT * pout, const FLT * pin, const FLT * pose ); +//void InvertPose( FLT * poseout, const FLT * pose ); } diff --git a/src/survive.c b/src/survive.c index 09eb432..6d49d55 100644 --- a/src/survive.c +++ b/src/survive.c @@ -53,7 +53,7 @@ SurviveContext * survive_init( int headless ) } i = 0; - const char * PreferredPoser = config_read_str( ctx->global_config_values, "DefualtPoser", "PoserDummy" ); + const char * PreferredPoser = config_read_str( ctx->global_config_values, "DefaultPoser", "PoserDummy" ); PoserCB PreferredPoserCB = 0; const char * FirstPoser = 0; printf( "Available posers:\n" ); diff --git a/src/survive_cal_lhfind.c b/src/survive_cal_lhfind.c index a1bb2cc..cc32154 100644 --- a/src/survive_cal_lhfind.c +++ b/src/survive_cal_lhfind.c @@ -129,13 +129,15 @@ int survive_cal_lhfind( SurviveCalData * cd ) fullrange *= 0.25; } - if( beste > 0.01 ) + if( beste > 0.1 ) { //Error too high SV_ERROR( "LH: %d / Best E %f Error too high\n", lh, beste ); return -1; } + RunOpti(cd, lh, 1, LighthousePos, LighthouseQuat); + cd->ctx->bsd[lh].PositionSet = 1; copy3d( cd->ctx->bsd[lh].Pose.Pos, LighthousePos ); quatcopy( cd->ctx->bsd[lh].Pose.Rot, LighthouseQuat ); @@ -198,6 +200,7 @@ static FLT RunOpti( SurviveCalData * cd, int lh, int print, FLT * LighthousePos, RayShootOut[2] = sqrt( 1 - (RayShootOut[0]*RayShootOut[0] + RayShootOut[1]*RayShootOut[1]) ); FLT RayShootOutWorld[3]; + quatnormalize( LighthouseQuat, LighthouseQuat ); //Rotate that ray by the current rotation estimation. quatrotatevector( RayShootOutWorld, LighthouseQuat, RayShootOut ); @@ -207,6 +210,7 @@ static FLT RunOpti( SurviveCalData * cd, int lh, int print, FLT * LighthousePos, normalize3d( UsToTarget, UsToTarget ); FLT RotatedLastUs[3]; + quatnormalize( LighthouseQuat, LighthouseQuat ); quatrotatevector( RotatedLastUs, LighthouseQuat, LastUsToTarget ); //Rotate the lighthouse around this axis to point at the HMD. @@ -221,6 +225,7 @@ static FLT RunOpti( SurviveCalData * cd, int lh, int print, FLT * LighthousePos, //Don't need to worry about being negative, cross product will fix it. FLT RotateAmount = anglebetween3d( RayShootOutWorld, UsToTarget ); quatfromaxisangle( ConcatQuat, AxisToRotate, RotateAmount ); + quatnormalize( ConcatQuat, ConcatQuat ); } else { @@ -244,9 +249,13 @@ static FLT RunOpti( SurviveCalData * cd, int lh, int print, FLT * LighthousePos, FLT RotateAmount = anglebetween3d( Actual, Target ) * mux; //printf( "FA: %f (O:%f)\n", acos( dot3d( Actual, Target ) ), RotateAmount ); quatfromaxisangle( ConcatQuat, AxisToRotate, RotateAmount ); + quatnormalize( ConcatQuat, ConcatQuat ); } - quatrotateabout( LighthouseQuat, ConcatQuat, LighthouseQuat ); //Chekcked. This appears to be + + quatnormalize( ConcatQuat, ConcatQuat ); + quatnormalize( LighthouseQuat, LighthouseQuat ); + quatrotateabout( LighthouseQuat, ConcatQuat, LighthouseQuat ); //Checked. This appears to be mux = mux * 0.94; if( second ) { second = 0; } diff --git a/src/survive_data.c b/src/survive_data.c index 75edfb3..e5f703a 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -12,7 +12,7 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) //int32_t deltat = (uint32_t)le->timestamp - (uint32_t)so->last_master_time; //if( so->codename[0] != 'H' ) - printf( "*** %s %d %d %d %d %d\n", so->codename, le->sensor_id, le->type, le->length, le->timestamp, le->timestamp-so->tsl ); +// printf( "*** %s %d %d %d %d %d\n", so->codename, le->sensor_id, le->type, le->length, le->timestamp, le->timestamp-so->tsl ); if( le->sensor_id > SENSORS_PER_OBJECT ) { |