diff options
Diffstat (limited to 'src/poser.c')
| -rw-r--r-- | src/poser.c | 18 |
1 files changed, 16 insertions, 2 deletions
diff --git a/src/poser.c b/src/poser.c index 3fb4fe8..9a0de24 100644 --- a/src/poser.c +++ b/src/poser.c @@ -1,8 +1,12 @@ #include "math.h" #include <linmath.h> #include <stdint.h> +#include <stdio.h> #include <survive.h> +#define _USE_MATH_DEFINES // for C +#include <math.h> + void PoserData_poser_raw_pose_func(PoserData *poser_data, SurviveObject *so, uint8_t lighthouse, SurvivePose *pose) { if (poser_data->rawposeproc) { poser_data->rawposeproc(so, lighthouse, pose, poser_data->userdata); @@ -18,6 +22,12 @@ void PoserData_lighthouse_pose_func(PoserData *poser_data, SurviveObject *so, ui } else { const FLT up[3] = {0, 0, 1}; + if (quatmagnitude(lighthouse_pose->Rot) == 0) { + SurviveContext *ctx = so->ctx; + SV_INFO("Pose func called with invalid pose."); + return; + } + // Assume that the space solved for is valid but completely arbitrary. We are going to do a few things: // a) Using the gyro data, normalize it so that gravity is pushing straight down along Z // c) Assume the object is at origin @@ -44,8 +54,12 @@ void PoserData_lighthouse_pose_func(PoserData *poser_data, SurviveObject *so, ui ApplyPoseToPose(&lighthouse2obj, &arb2object, &lighthouse2arb); // Now find the space with the same origin, but rotated so that gravity is up - SurvivePose lighthouse2objUp = {}, object2objUp = {}; - quatfrom2vectors(object2objUp.Rot, so->activations.accel, up); + SurvivePose lighthouse2objUp = {0}, object2objUp = {0}; + if (quatmagnitude(so->activations.accel)) { + quatfrom2vectors(object2objUp.Rot, so->activations.accel, up); + } else { + object2objUp.Rot[0] = 1.0; + } // Calculate the pose of the lighthouse in this space ApplyPoseToPose(&lighthouse2objUp, &object2objUp, &lighthouse2obj); |