update with another test.

6 files changed, 99 insertions, 26 deletions

diff --git a/redist/linmath.c b/redist/linmath.c
index 37d8e7a..41fa18f 100644
--- a/redist/linmath.c
+++ b/redist/linmath.c
@@ -516,3 +516,27 @@ void matrix44copy(FLT * mout, const FLT * minm )
 	memcpy( mout, minm, sizeof( FLT ) * 16 );
 }
 
+void matrix44transpose(FLT * mout, const FLT * minm )
+{
+	mout[0] = minm[0];
+	mout[1] = minm[4];
+	mout[2] = minm[8];
+	mout[3] = minm[12];
+
+	mout[4] = minm[1];
+	mout[5] = minm[5];
+	mout[6] = minm[9];
+	mout[7] = minm[13];
+
+	mout[8] = minm[2];
+	mout[9] = minm[6];
+	mout[10] = minm[10];
+	mout[11] = minm[14];
+
+	mout[12] = minm[3];
+	mout[13] = minm[7];
+	mout[14] = minm[11];
+	mout[15] = minm[15];
+
+}
+
diff --git a/redist/linmath.h b/redist/linmath.h
index aeaca46..66a38ed 100644
--- a/redist/linmath.h
+++ b/redist/linmath.h
@@ -110,7 +110,7 @@ Matrix3x3 inverseM33(const Matrix3x3 mat);
 
 
 void matrix44copy(FLT * mout, const FLT * minm );
-
+void matrix44transpose(FLT * mout, const FLT * minm );
 
 
 #endif
diff --git a/redist/lintest.c b/redist/lintest.c
index a767670..933d2e5 100644
--- a/redist/lintest.c
+++ b/redist/lintest.c
@@ -3,7 +3,51 @@
 
 int main()
 {
-	
+
+#ifdef NONTRANSPOSED_DAVE
+	FLT pLH1[3] = {-0.275796, 3.105430, -0.868643};
+	FLT qLH1[4] = {0.803743, -0.347009, 0.475276, 0.087718};
+	FLT pNLH1[3] = { 0.122180, 2.772487, -1.511608 };  //1M +x
+	FLT qNLH1[4] = { 0.796702, -0.389329, 0.457024, -0.069414 };
+
+	FLT pLH2[3] = {0.120546, 3.252532, -0.098940};
+	FLT qLH2[4] = {-0.220026, 0.460836, 0.328534, 0.794534 };
+	FLT pNLH2[3] = {-0.183505, 3.356293, 0.695688, };
+	FLT qNLH2[4] = {-0.237438, 0.405213, 0.270438, 0.840410 };
+#else
+
+	FLT pLH1[3] = {-0.321299, 3.130532, -0.786460};
+	FLT qLH1[4] = {0.794180, 0.336117, -0.485668, -0.142934};
+	FLT pNLH1[3] = { 0.113572, 2.791495, -1.495652 };  //1M +x
+	FLT qNLH1[4] = { 0.807419, 0.372818, -0.451339, 0.073308 };
+
+	FLT pLH2[3] = {0.153580, 3.251673, -0.190491};
+	FLT qLH2[4] = {0.217017, 0.482214, 0.306568, 0.791448 };
+	FLT pNLH2[3] = {-0.175330, 3.351943, 0.669623 };
+	FLT qNLH2[4] = {0.257241, 0.394159, 0.292555, 0.832392 };
+#endif
+
+	FLT pOut1[3];
+	FLT pOut2[3];
+
+//	qLH1[1]*=-1;
+//	qLH2[0]*=-1;
+
+	sub3d( pOut1, pLH1, pNLH1 );
+	sub3d( pOut2, pLH2, pNLH2 );
+
+	printf( "%f %f %f\n", PFTHREE( pOut1 ) );
+	printf( "%f %f %f\n", PFTHREE( pOut2 ) );
+
+	quatrotatevector( pOut1, qLH1, pOut1 );
+	quatrotatevector( pOut2, qLH2, pOut2 );
+
+	printf( "%f %f %f\n", PFTHREE( pOut1 ) );
+	printf( "%f %f %f\n", PFTHREE( pOut2 ) );
+
+	return -1;
+
+/*
 	FLT e[3] = { 1,1,3.14 };
 	FLT q[4];
 	FLT m[16];
@@ -39,7 +83,7 @@ int main()
 	quatrotatevector( p, q, p );
 	printf( "%f %f %f\n", PFTHREE( p ) );
 
-
+*/
 	//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 );
diff --git a/src/poser_daveortho.c b/src/poser_daveortho.c
index 80ddd90..beba7ad 100644
--- a/src/poser_daveortho.c
+++ b/src/poser_daveortho.c
@@ -74,13 +74,13 @@ int PoserDaveOrtho( SurviveObject * so, PoserData * pd )
 			FLT S_out[2][SENSORS_PER_OBJECT];
 			OrthoSolve( tOut, S_out, S_in, X_in, max_hits );
 
-			//Now, we need to solve where we are as a function of where
+						//Now, we need to solve where we are as a function of where
 			//the lighthouses are.
 			FLT quat[4];
 			FLT posoff[3] = { tOut[0][3], tOut[1][3], tOut[2][3] };
 			FLT MT[4][4];
-			//matrix44transpose( MT, &T[0][0] );
-			matrix44copy( &MT[0][0], &tOut[0][0] );
+			matrix44transpose( MT, &tOut[0][0] );
+			//matrix44copy( &MT[0][0], &tOut[0][0] );
 
 			quatfrommatrix( quat, &MT[0][0] );
 			//printf( "QUAT: %f %f %f %f = %f\n", quat[0], quat[1], quat[2], quat[3], quatmagnitude(quat) );
diff --git a/src/survive_cal.c b/src/survive_cal.c
index fd2f806..59dd919 100644
--- a/src/survive_cal.c
+++ b/src/survive_cal.c
@@ -307,7 +307,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;
@@ -512,27 +512,34 @@ static void handle_calibration( struct SurviveCalData *cd )
 				lhp->Pos[1] = objfromlh->Pos[1];
 				lhp->Pos[2] = objfromlh->Pos[2];
 
-				lhp->Rot[0] = objfromlh->Rot[0]*-1;
+				lhp->Rot[0] = objfromlh->Rot[0];
 				lhp->Rot[1] = objfromlh->Rot[1];
 				lhp->Rot[2] = objfromlh->Rot[2];
 				lhp->Rot[3] = objfromlh->Rot[3];
 
 				//Write lhp from the inverse of objfromlh
-				quatrotatevector( lhp->Pos, lhp->Rot, lhp->Pos );
+				//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] );
 				fprintf( stderr, "%f %f %f\n", lhp->Pos[0], lhp->Pos[1], lhp->Pos[2] );
 				/*
-					-0.074179 2.793859 0.519508
-					-0.092802 0.087361 0.872115 0.472409
-					0.379494 1.906039 2.074617
+					-0.204066 3.238746 -0.856369
+					0.812203 -0.264897 0.505599 0.120520
+					-0.204066 3.238746 -0.856369
 
-					0.695987 3.346798 -0.776169
-					-0.258207 0.966008 -0.008604 0.009116
-					0.614156 -2.521008 2.356952
+					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" );
@@ -551,21 +558,17 @@ static void handle_calibration( struct SurviveCalData *cd )
 					objfromlh->Pos[1],
 					objfromlh->Pos[2] };
 
-				FLT rot[4] = { objfromlh->Rot[0]*-1,
-					objfromlh->Rot[1],
-					objfromlh->Rot[2],
-					objfromlh->Rot[3] };
-
-				fprintf( stderr, "====> %f %f %f ",
-					pos[0], pos[1], pos[2] );
-
-				quatrotatevector( pos, rot, pos );
 				pos[0] -= lhp->Pos[0];
 				pos[1] -= lhp->Pos[1];
 				pos[2] -= lhp->Pos[2];
 
+				FLT rot[4] = { 
+					lhp->Rot[0],
+					lhp->Rot[1],
+					lhp->Rot[2],
+					lhp->Rot[3] };
 
-				quatrotatevector( lhp->Pos, objfromlh->Rot, lhp->Pos );
+				quatrotatevector( pos, rot, pos );
 
 				fprintf( stderr, "====> %f %f %f ",
 					pos[0], pos[1], pos[2] );
diff --git a/src/survive_cal.h b/src/survive_cal.h
index d21ba36..ce61962 100644
--- a/src/survive_cal.h
+++ b/src/survive_cal.h
@@ -33,7 +33,9 @@ void survive_cal_light( SurviveObject * so, int sensor_id, int acode, int timein
 void survive_cal_angle( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle );
 
 #define MAX_SENSORS_TO_CAL 96
-#define DRPTS 128
+
+#define MIN_PTS_BEFORE_CAL 24
+#define DRPTS 32
 #define MAX_CAL_PT_DAT (MAX_SENSORS_TO_CAL*NUM_LIGHTHOUSES*2)
 struct SurviveCalData
 {