Merge branch 'master' into standard_lh_calib

1 files changed, 220 insertions, 73 deletions

diff --git a/src/poser_daveortho.c b/src/poser_daveortho.c
index 2bf57c6..769ce81 100644
--- a/src/poser_daveortho.c
+++ b/src/poser_daveortho.c
@@ -8,7 +8,7 @@
 #include <dclapack.h>
 #include <linmath.h>
 
-static int LH_ID;
+// Dave talks about this poser here: https://www.youtube.com/watch?v=nSbEltdH9vM&feature=youtu.be&t=2h29m47s
 
 void OrthoSolve(
     FLT T[4][4],               // OUTPUT: 4x4 transformation matrix
@@ -20,7 +20,9 @@ void OrthoSolve(
 
 typedef struct
 {
-	int something;
+	int nextaxis;
+	float lengths[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2];
+	float angles[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2];
 	//Stuff
 } DummyData;
 
@@ -30,7 +32,8 @@ int PoserDaveOrtho( SurviveObject * so, PoserData * pd )
 	SurviveContext * ctx = so->ctx;
 	DummyData * dd = so->PoserData;
 
-	if( !dd ) so->PoserData = dd = malloc( sizeof( DummyData ) );
+	if (!dd)
+		so->PoserData = dd = calloc(sizeof(DummyData), 1);
 
 	switch( pt )
 	{
@@ -43,13 +46,94 @@ int PoserDaveOrtho( SurviveObject * so, PoserData * pd )
 	case POSERDATA_LIGHT:
 	{
 		PoserDataLight * l = (PoserDataLight*)pd;
-		//printf( "LIG:%s %d @ %f rad, %f s (AC %d) (TC %d)\n", so->codename, l->sensor_id, l->angle, l->length, l->acode, l->timecode );
+		if (l->length > dd->lengths[l->sensor_id][l->lh][dd->nextaxis]) {
+			dd->lengths[l->sensor_id][l->lh][dd->nextaxis] = l->length;
+			dd->angles[l->sensor_id][l->lh][dd->nextaxis] = l->angle;
+		}
+		break;
+	}
+	case POSERDATA_SYNC: {
+		PoserDataLight *l = (PoserDataLight *)pd;
+		int lhid = l->lh;
+
+		dd->nextaxis = l->acode & 1;
+
+		if (dd->nextaxis == 0) {
+			int i;
+			int max_hits = 0;
+			FLT S_in[2][SENSORS_PER_OBJECT];
+			FLT X_in[3][SENSORS_PER_OBJECT];
+			for (i = 0; i < SENSORS_PER_OBJECT; i++) {
+				// Load all our valid points into something the LHFinder can use.
+				if (dd->lengths[i][lhid][0] > 0 && dd->lengths[i][lhid][1] > 0) {
+					S_in[0][max_hits] = dd->angles[i][lhid][0];
+					S_in[1][max_hits] = dd->angles[i][lhid][1];
+					dd->lengths[i][lhid][0] = -1;
+					dd->lengths[i][lhid][1] = -1;
+					X_in[0][max_hits] = so->sensor_locations[i * 3 + 0];
+					X_in[1][max_hits] = so->sensor_locations[i * 3 + 1];
+					X_in[2][max_hits] = so->sensor_locations[i * 3 + 2];
+					max_hits++;
+				}
+			}
+			if (max_hits > 2) {
+				// if( lhid == 0 ) { printf( "\033[H\033[2J" ); }
+				// printf( "%d\n", max_hits );
+				FLT tOut[4][4];
+				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
+				// the lighthouses are.
+
+				SurvivePose objpose;
+				FLT MT[4][4];
+
+				objpose.Pos[0] = tOut[0][3];
+				objpose.Pos[1] = tOut[1][3];
+				objpose.Pos[2] = tOut[2][3];
+
+				// matrix44transpose( MT, &tOut[0][0] );
+				matrix44copy(&MT[0][0], &tOut[0][0]);
+
+				quatfrommatrix(objpose.Rot, &MT[0][0]);
+
+				// printf( "QUAT: %f %f %f %f = %f\n", quat[0], quat[1], quat[2], quat[3], quatmagnitude(quat) );
+				// quat[2] -= 0.005; //fixes up lh0 in test data set.
+				quatnormalize(objpose.Rot, objpose.Rot);
+
+				SurvivePose poseout;
+				InvertPose(poseout.Pos, objpose.Pos);
+				SurvivePose pose2lh = objpose;
+				// SurvivePose pose2lh = poseout;
+
+				SurvivePose lh2world = so->ctx->bsd[lhid].Pose;
+				SurvivePose world2obj, obj2world;
+				ApplyPoseToPose(world2obj.Pos, lh2world.Pos, pose2lh.Pos);
+				InvertPose(obj2world.Pos, world2obj.Pos);
+				PoserData_poser_raw_pose_func(pd, so, lhid, &obj2world);
+
+				if (0) {
+					printf("INQUAT: %f %f %f %f = %f [%f %f %f]\n", objpose.Rot[0], objpose.Rot[1], objpose.Rot[2],
+						   objpose.Rot[3], quatmagnitude(objpose.Rot), objpose.Pos[0], objpose.Pos[1], objpose.Pos[2]);
+					printf("OUQUAT: %f %f %f %f = %f [%f %f %f]\n", poseout.Rot[0], poseout.Rot[1], poseout.Rot[2],
+						   poseout.Rot[3], quatmagnitude(poseout.Rot), poseout.Pos[0], poseout.Pos[1], poseout.Pos[2]);
+				}
+			}
+		}
+
+		// if( so->codename[0] == 'H' ) printf( "LIG:%s %d @ %d rad, %f s (AX %d) (TC %d)\n", so->codename,
+		// l->sensor_id, l->lh, l->length, dd->nextaxis, l->timecode );
+
 		break;
 	}
 	case POSERDATA_FULL_SCENE:
 	{
 		PoserDataFullScene * fs = (PoserDataFullScene*)pd;
+		int LH_ID;
+		printf("PDFS\n");
 
+		SurvivePose alignLh0ToXAxis = {};
 		for( LH_ID = 0; LH_ID < 2; LH_ID++ )
 		{
 			int i;
@@ -76,38 +160,54 @@ int PoserDaveOrtho( SurviveObject * so, PoserData * pd )
 
 						//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 quat[4];
+			// FLT posoff[3] = { tOut[0][3], tOut[1][3], tOut[2][3] };
+			SurvivePose objpose;
 			FLT MT[4][4];
 
+			objpose.Pos[0] = tOut[0][3];
+			objpose.Pos[1] = tOut[1][3];
+			objpose.Pos[2] = tOut[2][3];
+
 			//matrix44transpose( MT, &tOut[0][0] );
 			matrix44copy( &MT[0][0], &tOut[0][0] );
 
-			quatfrommatrix( quat, &MT[0][0] );
-
+			quatfrommatrix(objpose.Rot, &MT[0][0]);
 
 			//printf( "QUAT: %f %f %f %f = %f\n", quat[0], quat[1], quat[2], quat[3], quatmagnitude(quat) );
 			//quat[2] -= 0.005; //fixes up lh0 in test data set.
-			quatnormalize( quat, quat );
-			printf( "QUAT: %f %f %f %f = %f [%f %f %f]\n", quat[0], quat[1], quat[2], quat[3], quatmagnitude(quat), posoff[0], posoff[1], posoff[2] );
-			
-			for( i = 0; i < max_hits;i++ )
-			{
-				FLT pt[3] = { X_in[0][i], X_in[1][i], X_in[2][i] };
-				quatrotatevector( pt, quat, pt );
-				add3d( pt, pt, posoff );
-				printf( "OUT %f %f %f ANGLE %f %f AOUT %f %f\n", 
-					pt[0], pt[1], pt[2],
-					S_in[0][i], S_in[1][i], atan2( pt[0], pt[1] ), atan2( pt[2], pt[1] ) );
+			quatnormalize(objpose.Rot, objpose.Rot);
+			printf("QUAT: %f %f %f %f = %f [%f %f %f]\n", objpose.Rot[0], objpose.Rot[1], objpose.Rot[2],
+				   objpose.Rot[3], quatmagnitude(objpose.Rot), objpose.Pos[0], objpose.Pos[1], objpose.Pos[2]);
+
+			if (0)
+				;
+			for (i = 0; i < max_hits; i++) {
+				FLT pt[3] = {X_in[0][i], X_in[1][i], X_in[2][i]};
+				quatrotatevector(pt, objpose.Rot, pt);
+				add3d(pt, pt, objpose.Pos);
+				printf("OUT %f %f %f ANGLE %f %f AOUT %f %f\n", pt[0], pt[1], pt[2], S_in[0][i], S_in[1][i],
+					   atan2(pt[0], pt[1]), atan2(pt[2], pt[1]));
 			}
 
-			so->FromLHPose[LH_ID].Pos[0] = posoff[0];
-			so->FromLHPose[LH_ID].Pos[1] = posoff[1];
-			so->FromLHPose[LH_ID].Pos[2] = posoff[2];
-			so->FromLHPose[LH_ID].Rot[0] = quat[0];
-			so->FromLHPose[LH_ID].Rot[1] = quat[1];
-			so->FromLHPose[LH_ID].Rot[2] = quat[2];
-			so->FromLHPose[LH_ID].Rot[3] = quat[3];
+			/*
+				so->FromLHPose[LH_ID].Pos[0] = objpose.Pos[0];
+				so->FromLHPose[LH_ID].Pos[1] = objpose.Pos[1];
+				so->FromLHPose[LH_ID].Pos[2] = objpose.Pos[2];
+				so->FromLHPose[LH_ID].Rot[0] = objpose.Rot[0];
+				so->FromLHPose[LH_ID].Rot[1] = objpose.Rot[1];
+				so->FromLHPose[LH_ID].Rot[2] = objpose.Rot[2];
+				so->FromLHPose[LH_ID].Rot[3] = objpose.Rot[3];
+			*/
+
+			SurvivePose poseout;
+			InvertPose(poseout.Pos, objpose.Pos);
+			printf("INQUAT: %f %f %f %f = %f [%f %f %f]\n", objpose.Rot[0], objpose.Rot[1], objpose.Rot[2],
+				   objpose.Rot[3], quatmagnitude(objpose.Rot), objpose.Pos[0], objpose.Pos[1], objpose.Pos[2]);
+
+			PoserData_lighthouse_pose_func(&fs->hdr, so, LH_ID, &alignLh0ToXAxis, &poseout, 0);
+			printf("OUQUAT: %f %f %f %f = %f [%f %f %f]\n", poseout.Rot[0], poseout.Rot[1], poseout.Rot[2],
+				   poseout.Rot[3], quatmagnitude(poseout.Rot), poseout.Pos[0], poseout.Pos[1], poseout.Pos[2]);
 		}
 
 		break;
@@ -146,14 +246,12 @@ REGISTER_LINKTIME( PoserDaveOrtho );
     oy=(c)*(x)-(a)*(z); \
     oz=(a)*(y)-(b)*(x); }
 
-void OrthoSolve(
-    FLT T[4][4],               // OUTPUT: 4x4 transformation matrix
-    FLT S_out[2][SENSORS_PER_OBJECT],  // OUTPUT:  array of screenspace points
-    FLT S_in[2][SENSORS_PER_OBJECT],   // INPUT:  array of screenspace points
-    FLT X_in[3][SENSORS_PER_OBJECT],   // INPUT:  array of offsets
-    int nPoints)
-{
-    int i,j,k;
+void OrthoSolve(FLT T[4][4],					  // OUTPUT: 4x4 transformation matrix
+				FLT S_out[2][SENSORS_PER_OBJECT], // OUTPUT:  array of screenspace points  (May not be populated!!!)
+				FLT S_in[2][SENSORS_PER_OBJECT],  // INPUT:  array of screenspace points
+				FLT X_in[3][SENSORS_PER_OBJECT],  // INPUT:  array of offsets
+				int nPoints) {
+	int i,j,k;
     FLT R[3][3];           // OUTPUT: 3x3 rotation matrix
     FLT trans[3];          // INPUT:  x,y,z translation vector
 
@@ -266,11 +364,11 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len);
 */
 //    FLT ydist1 = 1.0 /  uhat_len; //0.25*PI / uhat_len;
 //    FLT ydist2 = 1.0 /  rhat_len; //0.25*PI / rhat_len;
-    FLT ydist  = 1.0 / urhat_len;
-    printf("ydist %f\n", ydist);
-//    printf("ydist1 %f ydist2 %f ydist %f\n", ydist1, ydist2, ydist);
+	FLT ydist = 1.0 / urhat_len; // TRICKY XXX Dave operates with "y forward" for some reason...
+	//    printf("ydist %f\n", ydist);
+	//    printf("ydist1 %f ydist2 %f ydist %f\n", ydist1, ydist2, ydist);
 
-    //--------------------
+	//--------------------
     // Rescale the axies to be of the proper length
     //--------------------
     FLT x[3][1] = { {M[0][0]*ydist}, {0.0}, {M[1][0]*ydist} };
@@ -308,9 +406,14 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len);
 //                printf("err %f hand %f\n", err, hand);
                 
                 // If we are the best right-handed frame so far
-                //if (hand > 0 && err < bestErr) { x[1][0]=x_y; y[1][0]=y_y; z[1][0]=z_y; bestErr=err; }
-				if ( i == 0 && j == 1 && k == 0) { x[1][0]=x_y; y[1][0]=y_y; z[1][0]=z_y; bestErr=err; }
-                z_y = -z_y;
+				if (hand > 0 && err < bestErr) {
+					x[1][0] = x_y;
+					y[1][0] = y_y;
+					z[1][0] = z_y;
+					bestErr = err;
+				}
+				// if ( i == 0 && j == 0 && k == 0) { x[1][0]=x_y; y[1][0]=y_y; z[1][0]=z_y; bestErr=err; }
+				z_y = -z_y;
             }
             y_y = -y_y;
         }
@@ -437,60 +540,104 @@ PRINT(ab,2,1);
     FLT transdist = sqrt( trans[0]*trans[0] + trans[1]*trans[1] + trans[2]*trans[2] );
 
     //-------------------
-    // Pack into the 4x4 transformation matrix
-    //-------------------
+// Pack into the 4x4 transformation matrix, and put into correct "world"-space. I.e. where is the object, relative to
+// the lighthouse.
+//-------------------
+
+#if 0
+	//Don't do any transformation to the correct space.
     T[0][0]=R[0][0]; T[0][1]=R[0][1]; T[0][2]=R[0][2]; T[0][3]=trans[0];
     T[1][0]=R[1][0]; T[1][1]=R[1][1]; T[1][2]=R[1][2]; T[1][3]=trans[1];
     T[2][0]=R[2][0]; T[2][1]=R[2][1]; T[2][2]=R[2][2]; T[2][3]=trans[2];
     T[3][0]=0.0;     T[3][1]=0.0;     T[3][2]=0.0;     T[3][3]=1.0;
+#else
+
+	/* WRONG: This transposes.
+		T[0][0]=-R[0][0]; T[0][1]=-R[0][1]; T[0][2]=-R[0][2]; T[0][3]=-trans[0];
+		T[2][0]= R[1][0]; T[2][1]= R[1][1]; T[2][2]= R[1][2]; T[2][3]=-trans[1];
+		T[1][0]= R[2][0]; T[1][1]= R[2][1]; T[1][2]= R[2][2]; T[1][3]=trans[2];
+		T[3][0]=0.0;     T[3][1]=0.0;     T[3][2]=0.0;     T[3][3]=1.0;
+	*/
+
+	// XXX XXX XXX FOR ANYONE LOOKING TO CONTROL THE COORDINATE FRAME, THESE ARE THE LINES!!!
+
+	/** This is probably all wonky and backwards  but "looks" right except with multiple transforms, perhaps it's
+	   transposed.
+		T[0][0]=-R[0][0]; T[0][1]= R[0][1]; T[0][2]= R[0][2]; T[0][3]=-trans[0];
+		T[2][0]=-R[1][0]; T[2][1]=-R[1][1]; T[2][2]= R[1][2]; T[2][3]=-trans[1];//This Z axis has problems with getting
+	   magnitudes. We can re=make it from the other two axes.
+		T[1][0]=-R[2][0]; T[1][1]= R[2][1]; T[1][2]=-R[2][2]; T[1][3]= trans[2];
+		T[3][0]=0.0;     T[3][1]=0.0;     T[3][2]=0.0;     T[3][3]=1.0;
+	*/
+	T[0][0] = -R[0][0];
+	T[0][1] = -R[0][1];
+	T[0][2] = -R[0][2];
+	T[0][3] = -trans[0];
+	T[1][0] = R[2][0];
+	T[1][1] = R[2][1];
+	T[1][2] = R[2][2];
+	T[2][3] = -trans[1];
+	T[2][0] = R[1][0];
+	T[2][1] = R[1][1];
+	T[2][2] = R[1][2];
+	T[1][3] = trans[2];
+	T[3][0] = 0.0;
+	T[3][1] = 0.0;
+	T[3][2] = 0.0;
+	T[3][3] = 1.0;
 
-
-	FLT T2[4][4];
+#endif
 
 	//-------------------
 	// Orthogonalize the matrix
 	//-------------------
-	FLT temp[4][4];
-	FLT quat[4], quatNorm[4];
-	FLT euler[3];
-
+	// FLT temp[4][4];
+	// FLT quat[4], quatNorm[4];
+	// FLT euler[3];
 
 	//-------------------
 	// Orthogonalize the matrix
 	//-------------------
-	PRINT_MAT(T,4,4);
 
-#if 1
+	cross3d(&T[2][0], &T[0][0], &T[1][0]); // Generate Z from X/Y because Z is kinda rekt.
+
+// cross3d( &T[1][0], &T[2][0], &T[0][0] );	//Renormalize rotations...
+// cross3d( &T[0][0], &T[1][0], &T[2][0] );	//Renormalize rotations...
+
+// XXX XXX TODO
+// We could further normalize things...
+
+#if 0
 //	matrix44transpose(T2, T);  //Transpose so we are
 	matrix44copy((FLT*)T2,(FLT*)T);
-	cross3d( &T2[1][0], &T2[0][0], &T2[2][0] );
-	cross3d( &T2[2][0], &T2[1][0], &T2[0][0] ); //Replace axes in-place.
-	matrix44copy((FLT*)T,(FLT*)T2);
+	cross3d( &T2[1][0], &T2[0][0], &T2[2][0] ); //Replace axes in-place.
+	cross3d( &T2[2][0], &T2[1][0], &T2[0][0] ); 
+//	matrix44copy((FLT*)T,(FLT*)T2);
 //	matrix44transpose(T, T2);
-
+	normalize3d( &T[0][0], &T2[0][0] );
+	normalize3d( &T[1][0], &T2[1][0] );
+	normalize3d( &T[2][0], &T2[2][0] );
+#else
+//	normalize3d( &T[0][0], &T[0][0] );
+//	normalize3d( &T[1][0], &T[1][0] );
+//	normalize3d( &T[2][0], &T[2][0] );
 #endif
 
-	normalize3d( &T[0][0], &T[0][0] );
-	normalize3d( &T[1][0], &T[1][0] );
-	normalize3d( &T[2][0], &T[2][0] );
-	//Change handedness
+//	printf( "      In Axis on headset      \n" );
+//	printf( "    x                  y                   z\n" );
+//	PRINT_MAT(T,4,4);
 
-	T[1][0]*=-1;
-	T[1][1]*=-1;
-	T[1][2]*=-1;
+//	PRINT_MAT(T,4,4);
 
-/*
-	//Check Orthogonality.  Yep. It's orthogonal.
-	FLT tmp[3];
-	cross3d( tmp, &T[0][0], &T[1][0] );
-	printf( "M3: %f\n", magnitude3d( tmp ) );
-	cross3d( tmp, &T[2][0], &T[1][0] );
-	printf( "M3: %f\n", magnitude3d( tmp ) );
-	cross3d( tmp, &T[2][0], &T[0][0] );
-	printf( "M3: %f\n", magnitude3d( tmp ) );
-*/
+//	PRINT_MAT(T,4,4);
+
+// Fix handedness for rotations...
+//	T[1][0]*=-1;
+//	T[1][1]*=-1;
+//	T[1][2]*=-1;
 
 //	PRINT_MAT(T,4,4);
+#if 0
 
 #if 1
 
@@ -554,6 +701,6 @@ PRINT(ab,2,1);
         //S_out[1][i] = Tz;
 //        printf("point %i Txyz %f %f %f in %f %f out %f %f morph %f %f\n", i, Tx,Ty,Tz, S_in[0][i], S_in[1][i], S_out[0][i], S_out[1][i], S_morph[0][i], S_morph[1][i]);
     }
-
+#endif
 }