aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--include/libsurvive/poser.h6
-rw-r--r--include/libsurvive/survive.h8
-rw-r--r--include/libsurvive/survive_types.h2
-rw-r--r--simple_pose_test.c12
-rw-r--r--src/poser.c26
-rw-r--r--src/poser_daveortho.c2
-rw-r--r--src/poser_epnp.c6
-rw-r--r--src/poser_sba.c6
-rw-r--r--src/poser_turveytori.c4
-rw-r--r--src/survive.c8
-rw-r--r--src/survive_process.c6
-rw-r--r--test.c8
-rw-r--r--winbuild/test/test.vcxproj2
13 files changed, 58 insertions, 38 deletions
diff --git a/include/libsurvive/poser.h b/include/libsurvive/poser.h
index dcd0e93..4cddf89 100644
--- a/include/libsurvive/poser.h
+++ b/include/libsurvive/poser.h
@@ -18,14 +18,14 @@ typedef enum PoserType_t {
POSERDATA_SYNC, // Sync pulse.
} PoserType;
-typedef void (*poser_raw_pose_func)(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose, void *user);
+typedef void (*poser_pose_func)(SurviveObject *so, uint32_t lighthouse, SurvivePose *pose, void *user);
typedef void (*poser_lighthouse_pose_func)(SurviveObject *so, uint8_t lighthouse, SurvivePose *lighthouse_pose,
SurvivePose *object_pose, void *user);
typedef struct
{
PoserType pt;
- poser_raw_pose_func rawposeproc;
+ poser_pose_func poseproc;
poser_lighthouse_pose_func lighthouseposeproc;
void *userdata;
} PoserData;
@@ -41,7 +41,7 @@ typedef struct
* @param pose The actual object pose. This is in world space, not in LH space. It must represent a transformation from
* object space of the SO to global space.
*/
-void PoserData_poser_raw_pose_func(PoserData *poser_data, SurviveObject *so, uint8_t lighthouse, SurvivePose *pose);
+void PoserData_poser_pose_func(PoserData *poser_data, SurviveObject *so, SurvivePose *pose);
/**
* Meant to be used by individual posers to report back their findings on the pose of a lighthouse.
diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h
index 93ac9b7..0558dc8 100644
--- a/include/libsurvive/survive.h
+++ b/include/libsurvive/survive.h
@@ -75,6 +75,7 @@ struct SurviveObject {
// Pose Information, also "poser" field.
FLT PoseConfidence; // 0..1
SurvivePose OutPose; // Final pose? (some day, one can dream!)
+ uint32_t OutPose_timecode;
SurvivePose FromLHPose[NUM_LIGHTHOUSES]; // Filled out by poser, contains computed position from each lighthouse.
void *PoserData; // Initialized to zero, configured by poser, can be anything the poser wants.
PoserCB PoserFn;
@@ -114,6 +115,7 @@ struct SurviveObject {
haptic_func haptic;
SurviveSensorActivations activations;
+ void* user_ptr;
// Debug
int tsl;
};
@@ -206,7 +208,7 @@ struct SurviveContext {
imu_process_func imuproc;
angle_process_func angleproc;
button_process_func buttonproc;
- raw_pose_func rawposeproc;
+ pose_func poseproc;
lighthouse_pose_func lighthouseposeproc;
htc_config_func configfunction;
handle_lightcap_func lightcapfunction;
@@ -268,7 +270,7 @@ SURVIVE_EXPORT void survive_install_light_fn(SurviveContext *ctx, light_process_
SURVIVE_EXPORT void survive_install_imu_fn(SurviveContext *ctx, imu_process_func fbp);
SURVIVE_EXPORT void survive_install_angle_fn(SurviveContext *ctx, angle_process_func fbp);
SURVIVE_EXPORT void survive_install_button_fn(SurviveContext *ctx, button_process_func fbp);
-SURVIVE_EXPORT void survive_install_raw_pose_fn(SurviveContext *ctx, raw_pose_func fbp);
+SURVIVE_EXPORT void survive_install_pose_fn(SurviveContext *ctx, pose_func fbp);
SURVIVE_EXPORT void survive_install_lighthouse_pose_fn(SurviveContext *ctx, lighthouse_pose_func fbp);
SURVIVE_EXPORT int survive_startup(SurviveContext *ctx);
SURVIVE_EXPORT int survive_poll(SurviveContext *ctx);
@@ -310,7 +312,7 @@ SURVIVE_EXPORT void survive_default_angle_process(SurviveObject *so, int sensor_
SURVIVE_EXPORT void survive_default_button_process(SurviveObject *so, uint8_t eventType, uint8_t buttonId,
uint8_t axis1Id, uint16_t axis1Val, uint8_t axis2Id,
uint16_t axis2Val);
-SURVIVE_EXPORT void survive_default_raw_pose_process(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose);
+SURVIVE_EXPORT void survive_default_raw_pose_process(SurviveObject *so, uint32_t timecode, SurvivePose *pose);
SURVIVE_EXPORT void survive_default_lighthouse_pose_process(SurviveContext *ctx, uint8_t lighthouse,
SurvivePose *lh_pose, SurvivePose *obj_pose);
SURVIVE_EXPORT int survive_default_htc_config_process(SurviveObject *so, char *ct0conf, int len);
diff --git a/include/libsurvive/survive_types.h b/include/libsurvive/survive_types.h
index 7a7dbf1..edce3e9 100644
--- a/include/libsurvive/survive_types.h
+++ b/include/libsurvive/survive_types.h
@@ -49,7 +49,7 @@ typedef void (*light_process_func)( SurviveObject * so, int sensor_id, int acode
typedef void (*imu_process_func)( SurviveObject * so, int mask, FLT * accelgyro, uint32_t timecode, int id );
typedef void (*angle_process_func)( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh);
typedef void(*button_process_func)(SurviveObject * so, uint8_t eventType, uint8_t buttonId, uint8_t axis1Id, uint16_t axis1Val, uint8_t axis2Id, uint16_t axis2Val);
-typedef void (*raw_pose_func)(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose);
+typedef void (*pose_func)(SurviveObject *so, uint32_t timecode, SurvivePose *pose);
typedef void (*lighthouse_pose_func)(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *lighthouse_pose, SurvivePose *object_pose);
// For lightcap, etc. Don't change this structure at all. Regular vive is dependent on it being exactly as-is.
diff --git a/simple_pose_test.c b/simple_pose_test.c
index 0772abb..1696366 100644
--- a/simple_pose_test.c
+++ b/simple_pose_test.c
@@ -42,10 +42,10 @@ void HandleDestroy()
FLT hpos[3];
FLT hpos2[3];
-void testprog_raw_pose_process(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose) {
- survive_default_raw_pose_process(so, lighthouse, pose );
+void testprog_raw_pose_process(SurviveObject *so, uint32_t timecode, SurvivePose *pose) {
+ survive_default_raw_pose_process(so, timecode, pose );
- if( lighthouse != 0 || strcmp( so->codename, "HMD" ) != 0 )
+ if( strcmp( so->codename, "HMD" ) != 0 )
return;
// print the pose;
@@ -64,7 +64,7 @@ void testprog_raw_pose_process(SurviveObject *so, uint8_t lighthouse, SurvivePos
hy = pos[1];
hz = pos[2];*/
- printf("Pose: [%1.1x][%s][% 08.8f,% 08.8f,% 08.8f] [ang:%08.2f %08.2f %08.2f %08.2f]\n", lighthouse, so->codename,
+ printf("Pose: [%u][%s][% 08.8f,% 08.8f,% 08.8f] [ang:%08.2f %08.2f %08.2f %08.2f]\n", timecode, so->codename,
pose->Pos[0], pose->Pos[1], pose->Pos[2], pose->Rot[0], pose->Rot[1], pose->Rot[2], pose->Rot[3]);
hpos[0] = pose->Pos[0];
@@ -163,9 +163,9 @@ int main( int argc, char ** argv )
}
//survive_install_button_fn(ctx, testprog_button_process);
- survive_install_raw_pose_fn(ctx, testprog_raw_pose_process);
+ survive_install_pose_fn(ctx, testprog_raw_pose_process);
//survive_install_imu_fn(ctx, testprog_imu_process);
- survive_install_raw_pose_fn(ctx, testprog_raw_pose_process);
+ survive_install_pose_fn(ctx, testprog_raw_pose_process);
//survive_install_angle_fn(ctx, testprog_angle_process );
ctx->bsd[0].PositionSet = ctx->bsd[1].PositionSet = 1;
diff --git a/src/poser.c b/src/poser.c
index 9a0de24..20334f7 100644
--- a/src/poser.c
+++ b/src/poser.c
@@ -7,11 +7,29 @@
#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);
+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->rawposeproc(so, lighthouse, pose);
+ so->ctx->poseproc(so, PoserData_timecode(poser_data), pose);
}
}
diff --git a/src/poser_daveortho.c b/src/poser_daveortho.c
index 9cdab45..330e7e8 100644
--- a/src/poser_daveortho.c
+++ b/src/poser_daveortho.c
@@ -107,7 +107,7 @@ int PoserDaveOrtho( SurviveObject * so, PoserData * pd )
SurvivePose obj2world;
ApplyPoseToPose(&obj2world, &lh2world, &objpose);
- PoserData_poser_raw_pose_func(pd, so, lhid, &obj2world);
+ PoserData_poser_pose_func(pd, so, &obj2world);
if (0) {
fprintf(stderr,"INQUAT: %f %f %f %f = %f [%f %f %f]\n", objpose.Rot[0], objpose.Rot[1], objpose.Rot[2],
diff --git a/src/poser_epnp.c b/src/poser_epnp.c
index c05450a..eaa1659 100644
--- a/src/poser_epnp.c
+++ b/src/poser_epnp.c
@@ -164,7 +164,7 @@ int PoserEPNP(SurviveObject *so, PoserData *pd) {
if (winnerTakesAll) {
int winner = meas[0] > meas[1] ? 0 : 1;
- PoserData_poser_raw_pose_func(pd, so, winner, &posers[winner]);
+ PoserData_poser_pose_func(pd, so, &posers[winner]);
} else {
double a, b;
a = meas[0] * meas[0];
@@ -175,11 +175,11 @@ int PoserEPNP(SurviveObject *so, PoserData *pd) {
interpolate.Pos[i] = (posers[0].Pos[i] * a + posers[1].Pos[i] * b) / (t);
}
quatslerp(interpolate.Rot, posers[0].Rot, posers[1].Rot, b / (t));
- PoserData_poser_raw_pose_func(pd, so, lightData->lh, &interpolate);
+ PoserData_poser_pose_func(pd, so, &interpolate);
}
} else {
if (meas[lightData->lh])
- PoserData_poser_raw_pose_func(pd, so, lightData->lh, &posers[lightData->lh]);
+ PoserData_poser_pose_func(pd, so, &posers[lightData->lh]);
}
return 0;
}
diff --git a/src/poser_sba.c b/src/poser_sba.c
index bd7d520..e74bb20 100644
--- a/src/poser_sba.c
+++ b/src/poser_sba.c
@@ -129,7 +129,7 @@ typedef struct {
SurvivePose poses;
} sba_set_position_t;
-static void sba_set_position(SurviveObject *so, uint8_t lighthouse, SurvivePose *new_pose, void *_user) {
+static void sba_set_position(SurviveObject *so, uint32_t timecode, SurvivePose *new_pose, void *_user) {
sba_set_position_t *user = _user;
assert(user->hasInfo == false);
user->hasInfo = 1;
@@ -220,7 +220,7 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive
PoserData hdr = pdl->hdr;
memset(&pdl->hdr, 0, sizeof(pdl->hdr)); // Clear callback functions
pdl->hdr.pt = hdr.pt;
- pdl->hdr.rawposeproc = sba_set_position;
+ pdl->hdr.poseproc = sba_set_position;
sba_set_position_t locations = {0};
pdl->hdr.userdata = &locations;
@@ -278,7 +278,7 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive
if (status > 0 && (info[1] / meas_size * 2) < d->max_error) {
d->failures_to_reset_cntr = d->failures_to_reset;
quatnormalize(soLocation.Rot, soLocation.Rot);
- PoserData_poser_raw_pose_func(&pdl->hdr, so, 1, &soLocation);
+ PoserData_poser_pose_func(&pdl->hdr, so, &soLocation);
}
{
diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c
index 4628207..243051e 100644
--- a/src/poser_turveytori.c
+++ b/src/poser_turveytori.c
@@ -1631,9 +1631,9 @@ static void QuickPose(SurviveObject *so, PoserData *pd, SurvivePose *additionalT
SolveForLighthouse(&pose.Pos[0], &pose.Rot[0], to, so, pd, 0, additionalTx, lh, 0);
//printf("P&O: [% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]);
- if (so->ctx->rawposeproc)
+ if (so->ctx->poseproc)
{
- so->ctx->rawposeproc(so, lh, &pose);
+ so->ctx->poseproc(so, lh, &pose);
}
if (ttDebug) printf("!\n");
diff --git a/src/survive.c b/src/survive.c
index f74de85..f6f98fc 100644
--- a/src/survive.c
+++ b/src/survive.c
@@ -217,7 +217,7 @@ SurviveContext *survive_init_internal(int argc, char *const *argv) {
ctx->angleproc = survive_default_angle_process;
ctx->lighthouseposeproc = survive_default_lighthouse_pose_process;
ctx->configfunction = survive_default_htc_config_process;
- ctx->rawposeproc = survive_default_raw_pose_process;
+ ctx->poseproc = survive_default_raw_pose_process;
ctx->calibration_config = survive_calibration_config_ctor();
ctx->calibration_config.use_flag = (enum SurviveCalFlag)survive_configi(ctx, "bsd-cal", SC_GET, SVCal_All);
@@ -381,11 +381,11 @@ void survive_install_button_fn(SurviveContext *ctx, button_process_func fbp) {
ctx->buttonproc = survive_default_button_process;
}
-void survive_install_raw_pose_fn(SurviveContext *ctx, raw_pose_func fbp) {
+void survive_install_pose_fn(SurviveContext *ctx, pose_func fbp) {
if (fbp)
- ctx->rawposeproc = fbp;
+ ctx->poseproc = fbp;
else
- ctx->rawposeproc = survive_default_raw_pose_process;
+ ctx->poseproc = survive_default_raw_pose_process;
}
void survive_install_lighthouse_pose_fn(SurviveContext *ctx, lighthouse_pose_func fbp) {
diff --git a/src/survive_process.c b/src/survive_process.c
index 62459f2..e013327 100644
--- a/src/survive_process.c
+++ b/src/survive_process.c
@@ -139,12 +139,12 @@ void survive_default_button_process(SurviveObject * so, uint8_t eventType, uint8
//}
}
-void survive_default_raw_pose_process(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose) {
+void survive_default_raw_pose_process(SurviveObject *so, uint32_t timecode, SurvivePose *pose) {
// print the pose;
//printf("Pose: [%1.1x][%s][% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", lighthouse, so->codename, pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]);
so->OutPose = *pose;
- so->FromLHPose[lighthouse] = *pose;
- survive_recording_raw_pose_process(so, lighthouse, pose);
+ so->OutPose_timecode = timecode;
+ survive_recording_raw_pose_process(so, timecode, pose);
}
void survive_default_lighthouse_pose_process(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *lighthouse_pose,
diff --git a/test.c b/test.c
index ad7aaa0..9fda5b1 100644
--- a/test.c
+++ b/test.c
@@ -91,11 +91,11 @@ void testprog_lighthouse_process(SurviveContext *ctx, uint8_t lighthouse, Surviv
pose->Pos[1], pose->Pos[2], pose->Rot[0], pose->Rot[1], pose->Rot[2], pose->Rot[3]);
}
-void testprog_raw_pose_process(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose) {
- survive_default_raw_pose_process(so, lighthouse, pose);
+void testprog_raw_pose_process(SurviveObject *so, uint32_t timecode, SurvivePose *pose) {
+ survive_default_raw_pose_process(so, timecode, pose);
// print the pose;
- printf("Pose: [%1.1x][%s][% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", lighthouse, so->codename,
+ printf("Pose: [%u][%s][% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", timecode, so->codename,
pose->Pos[0], pose->Pos[1], pose->Pos[2], pose->Rot[0], pose->Rot[1], pose->Rot[2], pose->Rot[3]);
}
@@ -147,7 +147,7 @@ int main( int argc, char ** argv )
}
survive_install_button_fn(ctx, testprog_button_process);
- survive_install_raw_pose_fn(ctx, testprog_raw_pose_process);
+ survive_install_pose_fn(ctx, testprog_raw_pose_process);
survive_install_imu_fn(ctx, testprog_imu_process);
survive_install_lighthouse_pose_fn(ctx, testprog_lighthouse_process);
diff --git a/winbuild/test/test.vcxproj b/winbuild/test/test.vcxproj
index 5ceff3d..8df96d3 100644
--- a/winbuild/test/test.vcxproj
+++ b/winbuild/test/test.vcxproj
@@ -95,7 +95,7 @@
</ClCompile>
<Link>
<SubSystem>Console</SubSystem>
- <AdditionalDependencies>setupapi.lib;dbghelp.lib;kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;libblas.lib;liblapacke.lib;liblapack.lib;%(AdditionalDependencies)</AdditionalDependencies>
+ <AdditionalDependencies>setupapi.lib;dbghelp.lib;kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies)</AdditionalDependencies>
<GenerateDebugInformation>true</GenerateDebugInformation>
<LinkTimeCodeGeneration>UseFastLinkTimeCodeGeneration</LinkTimeCodeGeneration>
</Link>