diff options
Diffstat (limited to 'src/poser_daveortho.c')
-rw-r--r-- | src/poser_daveortho.c | 254 |
1 files changed, 133 insertions, 121 deletions
diff --git a/src/poser_daveortho.c b/src/poser_daveortho.c index e2e8d2e..769ce81 100644 --- a/src/poser_daveortho.c +++ b/src/poser_daveortho.c @@ -8,8 +8,7 @@ #include <dclapack.h> #include <linmath.h> -//Dave talks about this poser here: https://www.youtube.com/watch?v=nSbEltdH9vM&feature=youtu.be&t=2h29m47s - +// 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 @@ -22,8 +21,8 @@ void OrthoSolve( typedef struct { int nextaxis; - float lengths[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; - float angles[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; + float lengths[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; + float angles[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; //Stuff } DummyData; @@ -33,7 +32,8 @@ int PoserDaveOrtho( SurviveObject * so, PoserData * pd ) SurviveContext * ctx = so->ctx; DummyData * dd = so->PoserData; - if( !dd ) so->PoserData = dd = calloc( sizeof( DummyData ), 1 ); + if (!dd) + so->PoserData = dd = calloc(sizeof(DummyData), 1); switch( pt ) { @@ -46,80 +46,84 @@ int PoserDaveOrtho( SurviveObject * so, PoserData * pd ) case POSERDATA_LIGHT: { PoserDataLight * l = (PoserDataLight*)pd; - if( l->length > dd->lengths[l->sensor_id][l->lh][dd->nextaxis] ) - { + 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; + dd->angles[l->sensor_id][l->lh][dd->nextaxis] = l->angle; } break; } - case POSERDATA_SYNC: - { - PoserDataLight * l = (PoserDataLight*)pd; + case POSERDATA_SYNC: { + PoserDataLight *l = (PoserDataLight *)pd; int lhid = l->lh; dd->nextaxis = l->acode & 1; - if( dd->nextaxis == 0 && so->codename[0] == 'H' ) - { + 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 ) - { + 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]; + 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 ); + 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 ); + 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. + // Now, we need to solve where we are as a function of where + // the lighthouses are. - SurvivePose objpose; + 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] ); + // matrix44transpose( MT, &tOut[0][0] ); + matrix44copy(&MT[0][0], &tOut[0][0]); - quatfrommatrix( objpose.Rot, &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( objpose.Rot, objpose.Rot ); + // 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); - 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] ); + 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 ); + // 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; } @@ -127,8 +131,9 @@ int PoserDaveOrtho( SurviveObject * so, PoserData * pd ) { PoserDataFullScene * fs = (PoserDataFullScene*)pd; int LH_ID; - printf( "PDFS\n" ); + printf("PDFS\n"); + SurvivePose alignLh0ToXAxis = {}; for( LH_ID = 0; LH_ID < 2; LH_ID++ ) { int i; @@ -155,9 +160,9 @@ 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] }; - SurvivePose objpose; + // 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]; @@ -167,23 +172,23 @@ int PoserDaveOrtho( SurviveObject * so, PoserData * pd ) //matrix44transpose( MT, &tOut[0][0] ); matrix44copy( &MT[0][0], &tOut[0][0] ); - quatfrommatrix( objpose.Rot, &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( 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] ) ); - } + 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] = objpose.Pos[0]; @@ -195,13 +200,14 @@ int PoserDaveOrtho( SurviveObject * so, PoserData * pd ) 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] ); + 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, &poseout); - 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] ); + 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; @@ -240,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 (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; +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 @@ -360,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; //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); + 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} }; @@ -402,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 == 0 && 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; } @@ -531,8 +540,9 @@ 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, and put into correct "world"-space. I.e. where is the object, relative to the lighthouse. - //------------------- +// 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. @@ -542,56 +552,60 @@ PRINT(ab,2,1); 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; + /* 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; #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 //------------------- - 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... + 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 @@ -617,12 +631,11 @@ PRINT(ab,2,1); // PRINT_MAT(T,4,4); - //Fix handedness for rotations... +// Fix handedness for rotations... // T[1][0]*=-1; // T[1][1]*=-1; // T[1][2]*=-1; - // PRINT_MAT(T,4,4); #if 0 @@ -689,6 +702,5 @@ PRINT(ab,2,1); // 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 - } |