From ee12999bcb71f9b4b3c48bd0c1874f7ac251f355 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Tue, 20 Mar 2018 21:34:42 -0400 Subject: significant improvements to Orthodave. --- src/poser_daveortho.c | 138 ++++++++++++++++++++++++++++++++++++++++---------- src/survive_process.c | 18 ++++++- 2 files changed, 129 insertions(+), 27 deletions(-) (limited to 'src') diff --git a/src/poser_daveortho.c b/src/poser_daveortho.c index 2bf57c6..0e88a4e 100644 --- a/src/poser_daveortho.c +++ b/src/poser_daveortho.c @@ -8,6 +8,8 @@ #include #include +//Dave talks about this poser here: https://www.youtube.com/watch?v=nSbEltdH9vM&feature=youtu.be&t=2h29m47s + static int LH_ID; void OrthoSolve( @@ -20,7 +22,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 +34,7 @@ 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,7 +47,77 @@ 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 ); + 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 && l->lh == 0 && so->codename[0] == 'H' ) + { + 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 ) + { + 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. + FLT quat[4]; + FLT posoff[3] = { tOut[0][3], tOut[1][3], tOut[2][3] }; + FLT MT[4][4]; + + //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) ); + //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] ); + +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, 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] ) ); + } + } + } + + //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: @@ -148,7 +222,7 @@ REGISTER_LINKTIME( PoserDaveOrtho ); void OrthoSolve( FLT T[4][4], // OUTPUT: 4x4 transformation matrix - FLT S_out[2][SENSORS_PER_OBJECT], // OUTPUT: array of screenspace points + 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) @@ -437,12 +511,26 @@ 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 + + //XXX XXX XXX FOR ANYONE LOOKING TO CONTROL THE COORDINATE FRAME, THESE ARE THE LINES!!! + 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; + +#endif + + FLT T2[4][4]; @@ -450,47 +538,44 @@ PRINT(ab,2,1); //------------------- // 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); + //PRINT_MAT(T,4,4); #if 1 // 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] ); +// 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] ); -#endif +#else normalize3d( &T[0][0], &T[0][0] ); normalize3d( &T[1][0], &T[1][0] ); normalize3d( &T[2][0], &T[2][0] ); - //Change handedness +#endif - T[1][0]*=-1; - T[1][1]*=-1; - T[1][2]*=-1; + //Fix handedness for rotations... + T[0][0]*=-1; + T[0][1]*=-1; + T[0][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); +#if 0 #if 1 @@ -554,6 +639,7 @@ 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 } diff --git a/src/survive_process.c b/src/survive_process.c index b697f4a..f0928b4 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -19,7 +19,23 @@ void survive_default_light_process( SurviveObject * so, int sensor_id, int acode } //We don't use sync times, yet. - if( acode < -1 ) return; + if( sensor_id <= -1 ) + { + if( so->PoserFn ) + { + PoserDataLight l = { + .hdr = { .pt = POSERDATA_SYNC, }, + .sensor_id = sensor_id, + .acode = acode, + .timecode = timecode, + .length = length, + .angle = 0, + .lh = lh, + }; + so->PoserFn( so, (PoserData *)&l ); + } + return; + } if( base_station > NUM_LIGHTHOUSES ) return; -- cgit v1.2.3