aboutsummaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
authorCharles Lohr <lohr85@gmail.com>2018-04-07 18:33:30 +0000
committerCharles Lohr <lohr85@gmail.com>2018-04-07 18:33:30 +0000
commit6445b2067a0f0dead657a0b54c0765b4edb4d5b8 (patch)
tree4b499c8bc74e537ca52d46e451d75c7f9b3010a5 /src
parent864388d05fc56e9e11fe870d4b5b501a378e9186 (diff)
downloadlibsurvive-6445b2067a0f0dead657a0b54c0765b4edb4d5b8.tar.gz
libsurvive-6445b2067a0f0dead657a0b54c0765b4edb4d5b8.tar.bz2
apply a small drag from the accelerometer to correct "up" vector.
Diffstat (limited to 'src')
-rw-r--r--src/poser_charlesrefine.c26
1 files changed, 25 insertions, 1 deletions
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);