1 files changed, 159 insertions, 9 deletions

diff --git a/src/survive_cal.c b/src/survive_cal.c
index ad518b3..1a3be2d 100755
--- a/src/survive_cal.c
+++ b/src/survive_cal.c
@@ -14,6 +14,7 @@
 #include <string.h>
 #include <sys/stat.h>
 #include <sys/types.h>
+#include <linmath.h>
 
 #include "survive_config.h"
 
@@ -22,11 +23,16 @@
 #define NEEDED_TIMES_OF_COMMON 5
 #define DRPTS_NEEDED_FOR_AVG ((int)(DRPTS*3/4))
 
+
+
+
 static void handle_calibration( struct SurviveCalData *cd );
 static void reset_calibration( struct SurviveCalData * cd );
 
 void ootx_packet_clbk_d(ootx_decoder_context *ct, ootx_packet* packet)
 {
+	static uint8_t lighthouses_completed = 0;
+
 	SurviveContext * ctx = (SurviveContext*)(ct->user);
 	SurviveCalData * cd = ctx->calptr;
 	int id = ct->user1;
@@ -53,7 +59,11 @@ void ootx_packet_clbk_d(ootx_decoder_context *ct, ootx_packet* packet)
 	b->OOTXSet = 1;
 
 	config_set_lighthouse(ctx->lh_config,b,id);
-//	config_save("config.json");
+	lighthouses_completed++;
+
+	if (lighthouses_completed >= NUM_LIGHTHOUSES) {
+		config_save(ctx, "config.json");
+	}
 }
 
 int survive_cal_get_status( struct SurviveContext * ctx, char * description, int description_length )
@@ -115,6 +125,23 @@ void survive_cal_install( struct SurviveContext * ctx )
 		SV_INFO( "HMD not found, calibrating using Tracker" );
 	}
 
+
+
+	const char * DriverName;
+	const char * PreferredPoser = config_read_str( ctx->global_config_values, "ConfigPoser", "PoserDaveOrtho" );
+	PoserCB PreferredPoserCB = 0;
+	const char * FirstPoser = 0;
+	printf( "Available posers:\n" );
+	i = 0;	
+	while( ( DriverName = GetDriverNameMatching( "Poser", i++ ) ) )
+	{
+		PoserCB p = GetDriver( DriverName );
+		if( !PreferredPoserCB ) PreferredPoserCB = p;
+		int ThisPoser = strcmp( DriverName, PreferredPoser ) == 0;
+		if( ThisPoser ) PreferredPoserCB = p;
+	}
+	cd->ConfigPoserFn = PreferredPoserCB;
+	printf( "Got config poser: %p\n", cd->ConfigPoserFn );
 	ootx_packet_clbk = ootx_packet_clbk_d;
 
 	ctx->calptr = cd;
@@ -207,7 +234,7 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin
 
 		if( cd->peak_counts >= PTS_BEFORE_COMMON )
 		{
-			int tfc = cd->times_found_common;
+/*			int tfc = cd->times_found_common;
 			if( cd->found_common )
 			{
 				if( tfc >= NEEDED_TIMES_OF_COMMON )
@@ -229,7 +256,12 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin
 				SV_INFO( "Stage 2 bad - redoing. %d %d %d", cd->peak_counts, cd->found_common, tfc );
 				reset_calibration( cd );
 				cd->times_found_common = 0;
-			}
+			}*/
+
+			SV_INFO( "Stage 2 moving to stage 3. %d %d", cd->peak_counts, cd->found_common );
+			reset_calibration( cd );
+			cd->stage = 3;
+			cd->found_common = 1;
 		}			
 
 		break;
@@ -287,7 +319,7 @@ static void handle_calibration( struct SurviveCalData *cd )
 	for( axis = 0; axis < 2; axis++ )
 	{
 		int dpmax = cd->all_counts[sen][lh][axis];
-		if( dpmax < 50 ) continue;
+		if( dpmax < MIN_PTS_BEFORE_CAL ) continue;
 		int i;
 
 		FLT sumsweepangle = 0;
@@ -416,8 +448,10 @@ static void handle_calibration( struct SurviveCalData *cd )
 	}
 	fclose( hists );
 	fclose( ptinfo );
-
+/*
 	//Comb through data and make sure we still have a sensor on a WM that 
+	//We don't do this anymore.
+
 	int bcp_senid = 0;
 	int bcp_count = 0;
 	for( sen = 0; sen < MAX_SENSORS_TO_CAL; sen++ )
@@ -440,13 +474,129 @@ static void handle_calibration( struct SurviveCalData *cd )
 		reset_calibration( cd );
 		return;
 	}
-
 	cd->senid_of_checkpt = bcp_senid;
+*/
+	
+	int i, j;
+	PoserDataFullScene fsd;
+	fsd.pt = POSERDATA_FULL_SCENE;
+	for( j = 0; j < NUM_LIGHTHOUSES; j++ )
+	for( i = 0; i < SENSORS_PER_OBJECT; i++ )
+	{
+		int gotdata = 0;
+
+		int dataindex = i*(2*NUM_LIGHTHOUSES)+j*2+0;
 
-	if( survive_cal_lhfind( cd ) == 0 )
+		if( cd->ctsweeps[dataindex+0] < DRPTS_NEEDED_FOR_AVG ||
+			cd->ctsweeps[dataindex+1] < DRPTS_NEEDED_FOR_AVG )
+		{
+			fsd.lengths[i][j][0] = -1;
+			fsd.lengths[i][j][1] = -1;
+			continue;
+		}
+		fsd.lengths[i][j][0] = cd->avglens[dataindex+0];
+		fsd.lengths[i][j][1] = cd->avglens[dataindex+1];
+		fsd.angles[i][j][0] = cd->avgsweeps[dataindex+0];
+		fsd.angles[i][j][1] = cd->avgsweeps[dataindex+1];
+	}
+
+	cd->ConfigPoserFn( cd->hmd, (PoserData*)&fsd );
+	if( 1 )
 	{
-		SV_INFO( "Stage 4 succeeded." );
-		cd->stage = 5;
+		static int notfirstcal = 0;
+		SV_INFO( "Stage 4 succeeded. Inverting %d", notfirstcal );
+
+		if( !notfirstcal )
+		{
+			//  XXX This part is /all/ wrong.
+			//  XXX This part is /all/ wrong.
+			//  XXX This part is /all/ wrong.
+
+			//OK! We've arrived.  Now, we have to get the LH's pose from.
+			int lh;
+			for( lh = 0; lh < NUM_LIGHTHOUSES; lh++ )
+			{
+				SurvivePose * objfromlh = &cd->hmd->FromLHPose[lh];
+				SurvivePose * lhp = &ctx->bsd[lh].Pose;
+
+
+/*				lhp->Pos[0] = objfromlh->Pos[0];
+				lhp->Pos[1] = objfromlh->Pos[1];
+				lhp->Pos[2] = objfromlh->Pos[2];*/
+
+				lhp->Rot[0] =-objfromlh->Rot[0];
+				lhp->Rot[1] = objfromlh->Rot[1];
+				lhp->Rot[2] = objfromlh->Rot[2];
+				lhp->Rot[3] = objfromlh->Rot[3];
+
+				quatrotatevector( lhp->Pos, lhp->Rot, objfromlh->Pos );
+
+				//Write lhp from the inverse of objfromlh
+				//quatrotatevector( lhp->Pos, lhp->Rot, lhp->Pos );
+
+
+				fprintf( stderr, "%f, %f, %f\n", objfromlh->Pos[0], objfromlh->Pos[1], objfromlh->Pos[2] );
+				fprintf( stderr, "%f, %f, %f, %f\n", objfromlh->Rot[0], objfromlh->Rot[1], objfromlh->Rot[2], objfromlh->Rot[3] );
+
+				/*
+					-0.204066 3.238746 -0.856369
+					0.812203 -0.264897 0.505599 0.120520
+					-0.204066 3.238746 -0.856369
+
+					0.020036 3.162476 -0.117896
+					-0.322354 0.450869 0.346281 0.756898
+					0.020036 3.162476 -0.117896
+				*/
+
+				/* Facing up, moved -x 1m.
+					====> 0.446818 -0.309120 -0.747630 ====> -0.222356 -0.701865 -0.558656
+					====> -0.341064 0.099785 0.887015 ====> 0.619095 0.727263 0.029786 
+						IN PLACE, but rotated 90 * up.
+					====> 0.374516 -0.370583 -0.606996 ====> -0.120238 -0.670330 -0.426896
+					====> -0.231758 0.070437 0.765982 ====> 0.497615 0.625761 0.078759 
+				*/
+
+				printf( "\n" );
+
+			}
+			notfirstcal = 1;
+		}
+		else
+		{
+			for( lh = 0; lh < NUM_LIGHTHOUSES; lh++ )
+			{
+				SurvivePose * objfromlh = &cd->hmd->FromLHPose[lh];
+				SurvivePose * lhp = &ctx->bsd[lh].Pose;
+
+				FLT pos[3];
+				quatrotatevector( pos, lhp->Rot, objfromlh->Pos );
+
+				pos[0] -= lhp->Pos[0];
+				pos[1] -= lhp->Pos[1];
+				pos[2] -= lhp->Pos[2];
+
+				//FLT rot[4] = { 
+				//	[0],
+				//	lhp->Rot[1],
+				//	lhp->Rot[2],
+				//	lhp->Rot[3] };
+				//quatrotatevector( pos, lhp->Rot, pos );
+
+				fprintf( stderr, "%f, %f, %f\n", objfromlh->Pos[0], objfromlh->Pos[1], objfromlh->Pos[2] );
+				fprintf( stderr, "%f, %f, %f, %f\n", objfromlh->Rot[0], objfromlh->Rot[1], objfromlh->Rot[2], objfromlh->Rot[3] );
+				fprintf( stderr, "%f, %f, %f\n", lhp->Pos[0], lhp->Pos[1], lhp->Pos[2] );
+				fprintf( stderr, "%f, %f, %f, %f\n", lhp->Rot[0], lhp->Rot[1], lhp->Rot[2], lhp->Rot[3] );
+
+				fprintf( stderr, "====> %f %f %f\n",
+					pos[0], pos[1], pos[2] );
+					
+			}
+		}
+		fprintf( stderr, "\n" );
+
+
+		reset_calibration( cd );
+//		cd->stage = 5;
 	}
 	else
 	{