From 6445b2067a0f0dead657a0b54c0765b4edb4d5b8 Mon Sep 17 00:00:00 2001 From: Charles Lohr Date: Sat, 7 Apr 2018 18:33:30 +0000 Subject: apply a small drag from the accelerometer to correct "up" vector. --- src/poser_charlesrefine.c | 26 +++++++++++++++++++++++++- 1 file changed, 25 insertions(+), 1 deletion(-) (limited to 'src/poser_charlesrefine.c') diff --git a/src/poser_charlesrefine.c b/src/poser_charlesrefine.c index cbc47c8..ce14f70 100644 --- a/src/poser_charlesrefine.c +++ b/src/poser_charlesrefine.c @@ -40,6 +40,7 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { so->PoserData = dd = calloc(sizeof(CharlesPoserData), 1); SurvivePose object_pose_out; memcpy(&object_pose_out, &LinmathPose_Identity, sizeof(LinmathPose_Identity)); + memcpy(&dd->InteralPoseUsedForCalc, &LinmathPose_Identity, sizeof(LinmathPose_Identity)); so->PoseConfidence = 1.0; PoserData_poser_pose_func(pd, so, &object_pose_out); } @@ -58,8 +59,31 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { imuData->gyro[2] *= 1./240.; quatfromeuler( applymotion, imuData->gyro ); //printf( "%f %f %f\n", imuData->gyro [0], imuData->gyro [1], imuData->gyro [2] ); + + LinmathQuat InvertQuat; + quatgetreciprocal(InvertQuat, object_pose->Rot); + + //Apply a tiny tug to re-right headset based on the gravity vector. + LinmathVec3d reright = { 0, 0, 1 }; + LinmathVec3d normup; + normalize3d( normup, imuData->accel ); + LinmathVec3d correct_diff; + quatrotatevector(reright, InvertQuat, reright); + sub3d( correct_diff, normup, reright ); + scale3d( correct_diff, correct_diff, -0.001 ); //This is the coefficient applying the drag. + add3d( correct_diff, correct_diff, reright ); + LinmathQuat reright_quat; + normalize3d( correct_diff, correct_diff ); + quatfrom2vectors( reright_quat, reright, correct_diff ); + + + SurvivePose object_pose_out; - quatrotateabout(object_pose_out.Rot, object_pose->Rot, applymotion ); + quatrotateabout(object_pose_out.Rot, object_pose->Rot, applymotion ); //Contribution from Gyro + quatrotateabout(object_pose_out.Rot, object_pose_out.Rot, reright_quat); //Contribution from Accelerometer + quatnormalize(object_pose_out.Rot, object_pose_out.Rot); + + copy3d( object_pose_out.Pos, object_pose->Pos ); PoserData_poser_pose_func(pd, so, &object_pose_out); quatcopy( dd->InteralPoseUsedForCalc.Rot, object_pose_out.Rot); -- cgit v1.2.3