From bd7afcf2d1cdc49fc0bf7593cba12d576b227461 Mon Sep 17 00:00:00 2001 From: Charles Lohr Date: Wed, 4 Apr 2018 03:34:25 +0000 Subject: Add gyro support (Still need madgwick's) --- src/poser_charlesrefine.c | 15 +++++++++++++++ 1 file changed, 15 insertions(+) (limited to 'src/poser_charlesrefine.c') diff --git a/src/poser_charlesrefine.c b/src/poser_charlesrefine.c index 61bd575..9aa44d9 100644 --- a/src/poser_charlesrefine.c +++ b/src/poser_charlesrefine.c @@ -37,6 +37,21 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { case POSERDATA_IMU: { // Really should use this... PoserDataIMU *imuData = (PoserDataIMU *)pd; + + + //TODO: Actually do Madgwick's algorithm + LinmathQuat applymotion; + const SurvivePose * object_pose = &so->OutPose; + imuData->gyro[0] *= -0.001; + imuData->gyro[1] *= -0.001; + imuData->gyro[2] *= 0.001; + quatfromeuler( applymotion, imuData->gyro ); + //printf( "%f %f %f\n", imuData->gyro [0], imuData->gyro [1], imuData->gyro [2] ); + SurvivePose object_pose_out; + quatrotateabout(object_pose_out.Rot, object_pose->Rot, applymotion ); + copy3d( object_pose_out.Pos, object_pose->Pos ); + PoserData_poser_raw_pose_func(pd, so, 0, &object_pose_out); + return 0; } case POSERDATA_LIGHT: { -- cgit v1.2.3