From 905e2c5bcf06537d278d69e3226c2adffeeb9020 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Wed, 21 Mar 2018 03:45:06 -0400 Subject: Fix dave's ortho for use with the rest of the cal system. --- src/poser_daveortho.c | 47 +++++++++++++++-------------------------------- 1 file changed, 15 insertions(+), 32 deletions(-) (limited to 'src') diff --git a/src/poser_daveortho.c b/src/poser_daveortho.c index 769ce81..a7ff691 100644 --- a/src/poser_daveortho.c +++ b/src/poser_daveortho.c @@ -100,24 +100,19 @@ int PoserDaveOrtho( SurviveObject * so, PoserData * pd ) // 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; + quatnormalize(objpose.Rot, objpose.Rot); SurvivePose lh2world = so->ctx->bsd[lhid].Pose; - SurvivePose world2obj, obj2world; - ApplyPoseToPose(world2obj.Pos, lh2world.Pos, pose2lh.Pos); - InvertPose(obj2world.Pos, world2obj.Pos); + + SurvivePose obj2world; + ApplyPoseToPose(obj2world.Pos, lh2world.Pos, objpose.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], + fprintf(stderr,"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]); + // fprintf(stderr,"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]); } } } @@ -131,7 +126,6 @@ int PoserDaveOrtho( SurviveObject * so, PoserData * pd ) { PoserDataFullScene * fs = (PoserDataFullScene*)pd; int LH_ID; - printf("PDFS\n"); SurvivePose alignLh0ToXAxis = {}; for( LH_ID = 0; LH_ID < 2; LH_ID++ ) @@ -206,6 +200,7 @@ int PoserDaveOrtho( SurviveObject * so, PoserData * pd ) 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]); } @@ -601,26 +596,14 @@ PRINT(ab,2,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... +///XXX TODO: Characterize how important these last two steps are. The matrix definitely is not normalized. +#if 1 + cross3d(&T[1][0], &T[2][0], &T[0][0] ); //Renormalize rotations... + cross3d(&T[0][0], &T[1][0], &T[2][0] ); //Renormalize rotations... -#if 0 -// matrix44transpose(T2, T); //Transpose so we are - matrix44copy((FLT*)T2,(FLT*)T); - 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] ); + normalize3d( &T[0][0], &T[0][0] ); + normalize3d( &T[1][0], &T[1][0] ); + normalize3d( &T[2][0], &T[2][0] ); #endif // printf( " In Axis on headset \n" ); -- cgit v1.2.3