1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
|
#include "math.h"
#include <linmath.h>
#include <stdint.h>
#include <stdio.h>
#include <survive.h>
#define _USE_MATH_DEFINES // for C
#include <math.h>
static uint32_t PoserData_timecode(PoserData *poser_data) {
switch (poser_data->pt) {
case POSERDATA_LIGHT: {
PoserDataLight *lightData = (PoserDataLight *)poser_data;
return lightData->timecode;
}
case POSERDATA_FULL_SCENE: {
PoserDataFullScene *pdfs = (PoserDataFullScene *)(poser_data);
return -1;
}
case POSERDATA_IMU: {
PoserDataIMU *imuData = (PoserDataIMU *)poser_data;
return imuData->timecode;
}
}
return -1;
}
void PoserData_poser_pose_func(PoserData *poser_data, SurviveObject *so, SurvivePose *pose) {
if (poser_data->poseproc) {
poser_data->poseproc(so, PoserData_timecode(poser_data), pose, poser_data->userdata);
} else {
so->ctx->poseproc(so, PoserData_timecode(poser_data), pose);
}
}
void PoserData_lighthouse_pose_func(PoserData *poser_data, SurviveObject *so, uint8_t lighthouse,
SurvivePose *objUp2world, SurvivePose *lighthouse_pose, SurvivePose *object_pose) {
if (poser_data->lighthouseposeproc) {
poser_data->lighthouseposeproc(so, lighthouse, lighthouse_pose, object_pose, poser_data->userdata);
} 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
// b) Place the first lighthouse on the X axis by rotating around Z
//
// This calibration setup has the benefit that as long as the user is calibrating on the same flat surface,
// calibration results will be roughly identical between all posers no matter the orientation the object is
// lying
// in.
//
// We might want to go a step further and affix the first lighthouse in a given pose that preserves up so that
// it doesn't matter where on that surface the object is.
SurvivePose object2arb = {.Rot = {1.}};
if (object_pose)
object2arb = *object_pose;
SurvivePose lighthouse2arb = *lighthouse_pose;
// Start by just moving from whatever arbitrary space into object space.
SurvivePose arb2object;
InvertPose(&arb2object, &object2arb);
SurvivePose lighthouse2obj;
ApplyPoseToPose(&lighthouse2obj, &arb2object, &lighthouse2arb);
// Now find the space with the same origin, but rotated so that gravity is 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);
// Purposefully only set this once. It should only depend on the first (calculated) lighthouse
if (quatmagnitude(objUp2world->Rot) == 0) {
// Find what angle we need to rotate about Z by to get to 90 degrees.
FLT ang = atan2(lighthouse2objUp.Pos[1], lighthouse2objUp.Pos[0]);
FLT euler[3] = {0, 0, M_PI / 2. - ang};
quatfromeuler(objUp2world->Rot, euler);
}
// Find find the poses that map to the above
SurvivePose obj2world, lighthouse2world;
ApplyPoseToPose(&obj2world, objUp2world, &object2objUp);
ApplyPoseToPose(&lighthouse2world, objUp2world, &lighthouse2objUp);
so->ctx->lighthouseposeproc(so->ctx, lighthouse, &lighthouse2world, &obj2world);
}
}
|
