From ee3160032cd6a609a490be011019b323fe1c0c02 Mon Sep 17 00:00:00 2001 From: dpeter99 Date: Sun, 1 Apr 2018 17:36:08 +0200 Subject: Added new accesors (WIP) --- include/libsurvive/survive.h | 7 +++++++ 1 file changed, 7 insertions(+) (limited to 'include') diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index 7248b1c..8a4183e 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -120,6 +120,13 @@ struct SurviveObject { // These exports are mostly for language binding against SURVIVE_EXPORT const char *survive_object_codename(SurviveObject *so); + +SURVIVE_EXPORT const char *survive_object_drivername(SurviveObject *so); +SURVIVE_EXPORT const int8_t *survive_object_charge(SurviveObject *so); +SURVIVE_EXPORT const bool *survive_object_charging(SurviveObject *so); + +SURVIVE_EXPORT const SurvivePose *survive_object_pose(SurviveObject *so); + SURVIVE_EXPORT int8_t survive_object_sensor_ct(SurviveObject *so); SURVIVE_EXPORT const FLT *survive_object_sensor_locations(SurviveObject *so); SURVIVE_EXPORT const FLT *survive_object_sensor_normals(SurviveObject *so); -- cgit v1.3.1 From 7770495e7f14fe5094b31df5f5976c34fd68a0da Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sun, 1 Apr 2018 11:48:56 -0600 Subject: Fixed marshalling --- bindings/cs/Demo/Program.cs | 11 ++++------- bindings/cs/libsurvive.net/LibSurViveAPI.cs | 7 +++++-- bindings/cs/libsurvive.net/cfunctions.cs | 2 +- include/libsurvive/survive.h | 4 ++-- src/survive.c | 4 ++-- 5 files changed, 14 insertions(+), 14 deletions(-) (limited to 'include') diff --git a/bindings/cs/Demo/Program.cs b/bindings/cs/Demo/Program.cs index aa98b6c..53c2307 100644 --- a/bindings/cs/Demo/Program.cs +++ b/bindings/cs/Demo/Program.cs @@ -12,13 +12,10 @@ namespace Demo { static void Main(string[] args) { - SurviveContext context = new SurviveContext(args); - context.AddPoseUpdateCallback(HMDUpdate, -1); - - - while (context.Poll() == 0) { - } - + LibSurViveAPI api = LibSurViveAPI.instance; + + var so = api.GetSurviveObjectByName("HMD"); + } public static void HMDUpdate(int ObjectID, Vector3 pos) diff --git a/bindings/cs/libsurvive.net/LibSurViveAPI.cs b/bindings/cs/libsurvive.net/LibSurViveAPI.cs index 48336e8..aa2fdae 100644 --- a/bindings/cs/libsurvive.net/LibSurViveAPI.cs +++ b/bindings/cs/libsurvive.net/LibSurViveAPI.cs @@ -5,6 +5,7 @@ using System.Collections.Generic; using libsurvive; using System; using System.Threading; +using System.Runtime.InteropServices; public class LibSurViveAPI { @@ -108,7 +109,7 @@ public class LibSurViveAPI SetupConfigs configs = new SetupConfigs { - playbaskFile = "P:/c/libsurvive-data/lightcap-reformat/lightcap-reformat.log", + playbaskFile = @"C:\Users\justin\source\libsurvive-win-update\bindings\cs\Demo\lightcap-reformat.log", configFile = "survive_conf.json", playbackFactor = 1 }; @@ -320,8 +321,10 @@ public class SurviveObject { throw new Exception("Can't create SurviveObject with 0 pointer"); } + var ptr1 = cfunctions.Survive_object_pose(ptr); + pose = (SurvivePose)Marshal.PtrToStructure(ptr1, typeof(SurvivePose)); - //pose = cfunctions.Survive_object_pose(ptr); + //pose = charge = cfunctions.Survive_object_charge(ptr); charging = cfunctions.Survive_object_charging(ptr); } diff --git a/bindings/cs/libsurvive.net/cfunctions.cs b/bindings/cs/libsurvive.net/cfunctions.cs index ab5f7fd..ce46d18 100644 --- a/bindings/cs/libsurvive.net/cfunctions.cs +++ b/bindings/cs/libsurvive.net/cfunctions.cs @@ -101,7 +101,7 @@ namespace libsurvive public static extern bool Survive_object_charging(SurviveObjectPtr so); [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall, EntryPoint = "survive_object_pose")] - public static extern SurvivePose Survive_object_pose(SurviveObjectPtr so); + public static extern IntPtr Survive_object_pose(SurviveObjectPtr so); [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall, EntryPoint = "survive_object_sensor_locations")] diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index 8a4183e..b829a52 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -122,8 +122,8 @@ struct SurviveObject { SURVIVE_EXPORT const char *survive_object_codename(SurviveObject *so); SURVIVE_EXPORT const char *survive_object_drivername(SurviveObject *so); -SURVIVE_EXPORT const int8_t *survive_object_charge(SurviveObject *so); -SURVIVE_EXPORT const bool *survive_object_charging(SurviveObject *so); +SURVIVE_EXPORT const int8_t survive_object_charge(SurviveObject *so); +SURVIVE_EXPORT const bool survive_object_charging(SurviveObject *so); SURVIVE_EXPORT const SurvivePose *survive_object_pose(SurviveObject *so); diff --git a/src/survive.c b/src/survive.c index e4015de..f74de85 100644 --- a/src/survive.c +++ b/src/survive.c @@ -567,8 +567,8 @@ int survive_simple_inflate(struct SurviveContext *ctx, const char *input, int in const char *survive_object_codename(SurviveObject *so) { return so->codename; } const char *survive_object_drivername(SurviveObject *so) { return so->drivername; } -const int8_t *survive_object_charge(SurviveObject *so) { return so->charge; } -const bool *survive_object_charging(SurviveObject *so) { return so->charging; } +const int8_t survive_object_charge(SurviveObject *so) { return so->charge; } +const bool survive_object_charging(SurviveObject *so) { return so->charging; } const SurvivePose *survive_object_pose(SurviveObject *so) { return &so->OutPose; } -- cgit v1.3.1 From 0f5bf3c2f9fd7481d6b3c7c5364a08fe3ecdb2fb Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Mon, 2 Apr 2018 16:38:04 -0600 Subject: Added documentation about writing a poser --- README.md | 16 ++++++----- docs/writing_a_poser.md | 66 ++++++++++++++++++++++++++++++++++++++++++++++ include/libsurvive/poser.h | 29 +++++++++++++++++++- 3 files changed, 104 insertions(+), 7 deletions(-) create mode 100644 docs/writing_a_poser.md (limited to 'include') diff --git a/README.md b/README.md index 4b18c43..8a28bb0 100644 --- a/README.md +++ b/README.md @@ -118,9 +118,10 @@ Poser | [poser_dummy.c](src/poser_dummy.c) | Template for posers | [@cnlohr](htt Poser | [poser_octavioradii.c](src/poser_octavioradii.c) | A potentially very fast poser that works by finding the best fit of the distances from the lighthouse to each sensor that matches the known distances between sensors, given the known angles of a lighthouse sweep. Incomplete- distances appear to be found correctly, but more work needed to turn this into a pose. | [@mwturvey](https://github.com/mwturvey) and [@octavio2895](https://github.com/octavio2895) Poser | [poser_turveytori.c](src/poser_turveytori.c) | A moderately fast, fairly high precision poser that works by determine the angle at the lighthouse between many sets of two sensors. Using the inscirbed angle theorom, each set defines a torus of possible locations of the lighthouse. Multiple sets define multiple tori, and this poser finds most likely location of the lighthouse using least-squares distance. Best suited for calibration, but is can be used for real-time tracking on a powerful system. | [@mwturvey](https://github.com/mwturvey) Poser | [poser_epnp.c](src/poser_epnp.c) | Reasonably fast and accurate calibration and tracker that uses the [EPNP algorithm](https://en.wikipedia.org/wiki/Perspective-n-Point#EPnP) to solve the perspective and points problem. Suitable for fast tracking, but does best with >5-6 sensor readings. | [@jdavidberger](https://github.com/jdavidberger) -Poser | [poser_sba.c](src/poser_sba.c) | Reasonably fast and accurate calibration and tracker but is dependent on a 'seed' poser to give it an initial estimate. This then performs [bundle adjustment](https://en.wikipedia.org/wiki/Bundle_adjustment) to minimize reprojection error given both ligthhouse readings. This has the benefit of greatly increasing accuracy by incorporating all the light data that is available. Set 'SBASeedPoser' config option to specify the seed poser; default is EPNP. | [@jdavidberger](https://github.com/jdavidberger) -Disambiguator | [survive_data.c](src/survive_data.c) (currently #ifdefed out) | The old disambiguator - very fast, but slightly buggy. | [@cnlohr](https://github.com/cnlohr) -Disambiguator | [survive_data.c](src/survive_data.c) (current disambiguator) | More complicated but much more robust disambiguator | [@mwturvey](https://github.com/mwturvey) +Poser | [poser_sba.c](src/poser_sba.c) (default) | Reasonably fast and accurate calibration and tracker but is dependent on a 'seed' poser to give it an initial estimate. This then performs [bundle adjustment](https://en.wikipedia.org/wiki/Bundle_adjustment) to minimize reprojection error given both ligthhouse readings. This has the benefit of greatly increasing accuracy by incorporating all the light data that is available. Set 'SBASeedPoser' config option to specify the seed poser; default is EPNP. | [@jdavidberger](https://github.com/jdavidberger) +Disambiguator | [survive_data.c](src/survive_charlesbiguator.c) | The old disambiguator - very fast, but slightly buggy. | [@cnlohr](https://github.com/cnlohr) +Disambiguator | [survive_data.c](src/survive_turveybiguator.c) (default) | More complicated but much more robust disambiguator | [@mwturvey](https://github.com/mwturvey) +Disambiguator | [survive_data.c](src/survive_statebased_disambiguator.c) | A fast disambiguator that was times the state shifts between pulses. Experimental. Made to allow tracking very close to the lighthouse | [@jdavidberger](https://github.com/jdavidberger) Dismabiguator | superceded disambiguator | A more sophisticated disambiguator, development abandoned. Removed from tree. | [@jpicht](https://github.com/jpicht) Driver | [survive_vive.c](src/survive_vive.c) | Driver for HTC Vive HMD, Watchmen (wired+wireless) and Tracker | [@cnlohr](https://github.com/cnlohr) and [@mwturvey](https://github.com/mwturvey) OOTX Decoder | [ootx_decoder.c](src/ootx_decoder.c) | The system that takes the pulse-codes from the sync pulses from the lighthouses and get [OOTX Data](https://github.com/nairol/LighthouseRedox/blob/master/docs/Light%20Emissions.md) | [@axlecrusher](https://github.com/axlecrusher) @@ -131,7 +132,7 @@ Component Type | Pluggability method --- | --- Driver | Dynamically loadable runtime, can co-exist with other drivers. Poser | Selectable by configuration at runtime -Disambiguator | Selectable by #define +Disambiguator | Selectable by configuration at runtime OOTX Decoder | Not Pluggable @@ -146,7 +147,7 @@ To support the Vive on HDMI, you either need a newer version of HDMI, or you nee ## General Information -The default configuration of libsurvive requires both basestations and both controllers to be active, but currently libsurvive can not make proper use of both basestations at the same time - it outputs a different pose for each basestation and does not fuse the data for one more accurate pose. This will hopefully be implemented soon. For now it's a good idea to change the configuration to only require one basestation. +The default configuration of libsurvive requires both basestations and both controllers to be active. Here is an example of a default configuration file that libsurvive will create as `config.json` in the current working directory when any libsurvive client is executed: @@ -172,7 +173,7 @@ Here is an example of a default configuration file that libsurvive will create a } ``` -To make libsurvive calibrate and run with one basestation, `lighthousecount` needs to be changed to `1`. +To make libsurvive calibrate and run with one basestation, `lighthousecount` needs to be changed to `1`. You can also pass in `-l 1` as command line arguments. It may be annoying to always require the controllers for calibration. To make libsurvive calibrate by using the HMD, `RequiredTrackersForCal` needs to be changed to the magic string `HMD`. The strings for the controllers are `WM0` and `WM1`, short for "Watchman". Other possible values are `WW0` (Wired Watchman) for a controller directly connected with USB or `TR0` for a Vive tracker directly connected with USB (When connected wirelessly, the tracker uses the dongles, so uses `WM0` or `WM1`). @@ -225,6 +226,9 @@ Sometimes libsurvive goes very quickly through these steps and fills in all pose [Here is a short demo video how successfuly running ./test should look like](https://haagch.frickel.club/Peek%202018-02-21%2023-23.webm). +If there is already calibration data present, the library will use it. Pass `--calibrate` to force a new calibration pass. +Conversely, if there isn't calibration data the library will auto-calibrate. Pass `--no-calibrate` to disable this calibration. + ## Using libsurvive in your own application Example code for libsurvive can be found in [test.c](https://github.com/cnlohr/libsurvive/blob/master/test.c). [calibrate.c](https://github.com/cnlohr/libsurvive/blob/master/calibrate.c) may contain some interesting code too. diff --git a/docs/writing_a_poser.md b/docs/writing_a_poser.md new file mode 100644 index 0000000..ac450e3 --- /dev/null +++ b/docs/writing_a_poser.md @@ -0,0 +1,66 @@ +# How to write a poser + +Posers in libsurvive serve two related purposes: + +1) If the lighthouse position isn't known, solve for them. +2) If the lighthouse position is known, solve for the position of objects in the scene. + +To do this, a poser is given a variety of different signals about the current state of the input. + +# Registering a Poser + +In an effort to decouple posers from the underlying mechanics, posers are registered through a link-time +constructor. Make a function in the following form: + +``` +#include "survive.h" + +int PoserMyPoser(SurviveObject *so, PoserData *pd) { + switch (pd->pt) { + case POSERDATA_LIGHT: { + PoserDataLight *lightData = (PoserDataLight *)pd; + return -1; + } + case POSERDATA_FULL_SCENE: { + PoserDataFullScene pdfs = (PoserDataFullScene *)(pd); + return -1; + } + case POSERDATA_IMU: { + PoserDataIMU *imuData = (PoserDataIMU *)pd; + return -1; + } + return -1; +} + +REGISTER_LINKTIME(PoserMyPoser); + +``` + +Note that the prefix of the function MUST be "Poser" for it to work. + +Now compile it into the shared object, and it will be one of the poser options available. + +## Input to a poser + +`PoserData` is a tagged union that contains the new input to the poser. See above for how to get the concrete +types for each type of event. + +If you don't handle a given input, return -1. If you do, return a 0. If you have a more specific +error number, you can return that as well. + +## Output from a poser + +To report an objects location for any one of the events above, call the function + +`PoserData_poser_raw_pose_func`. See the documentation in poser.h for more info. + +To report an initial solution to the lighthouse solution, call the function `PoserData_lighthouse_pose_func`. Again, +see documention in poser.h + +## Calibration data + +Note that by default, no angle data is presented in a calibrated fashion. Depending on your use case, you +may want to treat the data by grouping X, Y pairs and trying to undistort them. You can do this with the library +function `survive_apply_bsd_calibration` from `survive_reproject.h`. Please understand though that this is, at best, an +approximation and can be far off if the angle values are too spread out in time. + diff --git a/include/libsurvive/poser.h b/include/libsurvive/poser.h index b0b1a7b..dcd0e93 100644 --- a/include/libsurvive/poser.h +++ b/include/libsurvive/poser.h @@ -30,9 +30,36 @@ typedef struct void *userdata; } PoserData; +/** + * Meant to be used by individual posers to report back their findings on the pose of an object back to the invoker of + * the call. + * + * @param poser_data the data pointer passed into the poser function invocation + * @param so The survive object which we are giving a solution for. + * @param lighthouse @deprecated The lighthouse which observed that position. Make it -1 if it was a combination of + * lighthouses. Will be removed in the future. + * @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); + +/** + * Meant to be used by individual posers to report back their findings on the pose of a lighthouse. + * + * Note that you are free to assume the position of the lighthouse and solve for the object or vice versa. Most solvers + * assume that the object is at 0,0,0 but this isn't a hard requirement. + * + * @param poser_data the data pointer passed into the poser function invocation + * @param so The survive object which gave us the info for the solution + * @param lighthouse The lighthouse which to solve for + * @param objUp2world For use when solving for both ligthhouse positions. For the first invocation of this function, + * pass in a zero-inited SurvivePose. This function will set that to a relative transform to normalize the space. + * pass the same pose in again for the second lighthouse to get accurate results. + * @param lighthouse_pose This is the assumed or derived position of the given lighthouse. + * @param object_pose This is the assumed or derived position of the tracked object. + */ void PoserData_lighthouse_pose_func(PoserData *poser_data, SurviveObject *so, uint8_t lighthouse, - /* OUTPARAM */ SurvivePose *objUp2world, SurvivePose *lighthouse_poses, + /* OUTPARAM */ SurvivePose *objUp2world, SurvivePose *lighthouse_pose, SurvivePose *object_pose); typedef struct PoserDataIMU { -- cgit v1.3.1 From d3d159772f1684082d7a545fbb3028d98d803308 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Tue, 3 Apr 2018 11:45:37 -0600 Subject: Added necessary exports for winbuild --- include/libsurvive/survive.h | 4 +-- redist/linmath.h | 83 ++++++++++++++++++++++++-------------------- 2 files changed, 47 insertions(+), 40 deletions(-) (limited to 'include') diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index b829a52..93ac9b7 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -239,7 +239,7 @@ struct SurviveContext { struct survive_calibration_config calibration_config; }; -void survive_verify_FLT_size( +SURVIVE_EXPORT void survive_verify_FLT_size( uint32_t user_size); // Baked in size of FLT to verify users of the library have the correct setting. SURVIVE_EXPORT SurviveContext *survive_init_internal(int argc, char *const *argv); @@ -278,7 +278,7 @@ SURVIVE_EXPORT SurviveObject *survive_get_so_by_name(SurviveContext *ctx, const // Utilitiy functions. int survive_simple_inflate(SurviveContext *ctx, const char *input, int inlen, char *output, int outlen); -int survive_send_magic(SurviveContext *ctx, int magic_code, void *data, int datalen); +SURVIVE_EXPORT int survive_send_magic(SurviveContext *ctx, int magic_code, void *data, int datalen); // These functions search both the stored-general and temporary sections for a parameter and return it. #define SC_GET 0 // Get, only. diff --git a/redist/linmath.h b/redist/linmath.h index 76e1322..094c230 100644 --- a/redist/linmath.h +++ b/redist/linmath.h @@ -7,6 +7,13 @@ extern "C" { #endif + +#ifdef _WIN32 +#define LINMATH_EXPORT __declspec(dllexport) +#else +#define LINMATH_EXPORT __attribute__((visibility("default"))) +#endif + // Yes, I know it's kind of arbitrary. #define DEFAULT_EPSILON 0.001 @@ -64,85 +71,85 @@ extern LinmathQuat LinmathQuat_Identity; extern LinmathPose LinmathPose_Identity; // NOTE: Inputs may never be output with cross product. -void cross3d(FLT *out, const FLT *a, const FLT *b); +LINMATH_EXPORT void cross3d(FLT *out, const FLT *a, const FLT *b); -void sub3d(FLT *out, const FLT *a, const FLT *b); +LINMATH_EXPORT void sub3d(FLT *out, const FLT *a, const FLT *b); -void add3d(FLT *out, const FLT *a, const FLT *b); +LINMATH_EXPORT void add3d(FLT *out, const FLT *a, const FLT *b); -void scale3d(FLT *out, const FLT *a, FLT scalar); +LINMATH_EXPORT void scale3d(FLT *out, const FLT *a, FLT scalar); -void normalize3d(FLT *out, const FLT *in); +LINMATH_EXPORT void normalize3d(FLT *out, const FLT *in); FLT dot3d(const FLT *a, const FLT *b); // Returns 0 if equal. If either argument is null, 0 will ALWAYS be returned. int compare3d(const FLT *a, const FLT *b, FLT epsilon); -void copy3d(FLT *out, const FLT *in); +LINMATH_EXPORT void copy3d(FLT *out, const FLT *in); FLT magnitude3d(const FLT *a); FLT dist3d(const FLT *a, const FLT *b); FLT anglebetween3d(FLT *a, FLT *b); -void rotatearoundaxis(FLT *outvec3, FLT *invec3, FLT *axis, FLT angle); -void angleaxisfrom2vect(FLT *angle, FLT *axis, FLT *src, FLT *dest); -void axisanglefromquat(FLT *angle, FLT *axis, LinmathQuat quat); +LINMATH_EXPORT void rotatearoundaxis(FLT *outvec3, FLT *invec3, FLT *axis, FLT angle); +LINMATH_EXPORT void angleaxisfrom2vect(FLT *angle, FLT *axis, FLT *src, FLT *dest); +LINMATH_EXPORT void axisanglefromquat(FLT *angle, FLT *axis, LinmathQuat quat); // Quaternion things... typedef FLT LinmathEulerAngle[3]; -void quatsetnone(LinmathQuat q); -void quatcopy(LinmathQuat q, const LinmathQuat qin); -void quatfromeuler(LinmathQuat q, const LinmathEulerAngle euler); -void quattoeuler(LinmathEulerAngle euler, const LinmathQuat q); -void quatfromaxisangle(LinmathQuat q, const FLT *axis, FLT radians); +LINMATH_EXPORT void quatsetnone(LinmathQuat q); +LINMATH_EXPORT void quatcopy(LinmathQuat q, const LinmathQuat qin); +LINMATH_EXPORT void quatfromeuler(LinmathQuat q, const LinmathEulerAngle euler); +LINMATH_EXPORT void quattoeuler(LinmathEulerAngle euler, const LinmathQuat q); +LINMATH_EXPORT void quatfromaxisangle(LinmathQuat q, const FLT *axis, FLT radians); FLT quatmagnitude(const LinmathQuat q); FLT quatinvsqmagnitude(const LinmathQuat q); -void quatnormalize(LinmathQuat qout, const LinmathQuat qin); // Safe for in to be same as out. -void quattomatrix(FLT *matrix44, const LinmathQuat q); -void quatfrommatrix(LinmathQuat q, const FLT *matrix44); -void quatfrommatrix33(LinmathQuat q, const FLT *matrix33); -void quatgetconjugate(LinmathQuat qout, const LinmathQuat qin); -void quatgetreciprocal(LinmathQuat qout, const LinmathQuat qin); -void quatsub(LinmathQuat qout, const LinmathQuat a, const LinmathQuat b); -void quatadd(LinmathQuat qout, const LinmathQuat a, const LinmathQuat b); -void quatrotateabout(LinmathQuat qout, const LinmathQuat a, +LINMATH_EXPORT void quatnormalize(LinmathQuat qout, const LinmathQuat qin); // Safe for in to be same as out. +LINMATH_EXPORT void quattomatrix(FLT *matrix44, const LinmathQuat q); +LINMATH_EXPORT void quatfrommatrix(LinmathQuat q, const FLT *matrix44); +LINMATH_EXPORT void quatfrommatrix33(LinmathQuat q, const FLT *matrix33); +LINMATH_EXPORT void quatgetconjugate(LinmathQuat qout, const LinmathQuat qin); +LINMATH_EXPORT void quatgetreciprocal(LinmathQuat qout, const LinmathQuat qin); +LINMATH_EXPORT void quatsub(LinmathQuat qout, const LinmathQuat a, const LinmathQuat b); +LINMATH_EXPORT void quatadd(LinmathQuat qout, const LinmathQuat a, const LinmathQuat b); +LINMATH_EXPORT void quatrotateabout(LinmathQuat qout, const LinmathQuat a, const LinmathQuat b); // same as quat multiply, not piecewise multiply. -void quatscale(LinmathQuat qout, const LinmathQuat qin, FLT s); +LINMATH_EXPORT void quatscale(LinmathQuat qout, const LinmathQuat qin, FLT s); FLT quatinnerproduct(const LinmathQuat qa, const LinmathQuat qb); -void quatouterproduct(FLT *outvec3, LinmathQuat qa, LinmathQuat qb); -void quatevenproduct(LinmathQuat q, LinmathQuat qa, LinmathQuat qb); -void quatoddproduct(FLT *outvec3, LinmathQuat qa, LinmathQuat qb); -void quatslerp(LinmathQuat q, const LinmathQuat qa, const LinmathQuat qb, FLT t); -void quatrotatevector(FLT *vec3out, const LinmathQuat quat, const FLT *vec3in); -void quatfrom2vectors(LinmathQuat q, const FLT *src, const FLT *dest); +LINMATH_EXPORT void quatouterproduct(FLT *outvec3, LinmathQuat qa, LinmathQuat qb); +LINMATH_EXPORT void quatevenproduct(LinmathQuat q, LinmathQuat qa, LinmathQuat qb); +LINMATH_EXPORT void quatoddproduct(FLT *outvec3, LinmathQuat qa, LinmathQuat qb); +LINMATH_EXPORT void quatslerp(LinmathQuat q, const LinmathQuat qa, const LinmathQuat qb, FLT t); +LINMATH_EXPORT void quatrotatevector(FLT *vec3out, const LinmathQuat quat, const FLT *vec3in); +LINMATH_EXPORT void quatfrom2vectors(LinmathQuat q, const FLT *src, const FLT *dest); // This is the quat equivalent of 'pout = pose * pin' if pose were a 4x4 matrix in homogenous space -void ApplyPoseToPoint(LinmathPoint3d pout, const LinmathPose *pose, const LinmathPoint3d pin); +LINMATH_EXPORT void ApplyPoseToPoint(LinmathPoint3d pout, const LinmathPose *pose, const LinmathPoint3d pin); // This is the quat equivalent of 'pout = lhs_pose * rhs_pose' if poses were a 4x4 matrix in homogenous space -void ApplyPoseToPose(LinmathPose *pout, const LinmathPose *lhs_pose, const LinmathPose *rhs_pose); +LINMATH_EXPORT void ApplyPoseToPose(LinmathPose *pout, const LinmathPose *lhs_pose, const LinmathPose *rhs_pose); // This is the quat equivlant of 'pose_in^-1'; so that ApplyPoseToPose(..., InvertPose(..., pose_in), pose_in) == // Identity ( [0, 0, 0], [1, 0, 0, 0] ) // by definition. -void InvertPose(LinmathPose *poseout, const LinmathPose *pose_in); +LINMATH_EXPORT void InvertPose(LinmathPose *poseout, const LinmathPose *pose_in); -void PoseToMatrix(FLT *mat44, const LinmathPose *pose_in); +LINMATH_EXPORT void PoseToMatrix(FLT *mat44, const LinmathPose *pose_in); // Matrix Stuff typedef struct { FLT val[3][3]; // row, column } Matrix3x3; -void rotate_vec(FLT *out, const FLT *in, Matrix3x3 rot); -void rotation_between_vecs_to_m3(Matrix3x3 *m, const FLT v1[3], const FLT v2[3]); +LINMATH_EXPORT void rotate_vec(FLT *out, const FLT *in, Matrix3x3 rot); +LINMATH_EXPORT void rotation_between_vecs_to_m3(Matrix3x3 *m, const FLT v1[3], const FLT v2[3]); Matrix3x3 inverseM33(const Matrix3x3 mat); -void matrix44copy(FLT *mout, const FLT *minm); -void matrix44transpose(FLT *mout, const FLT *minm); +LINMATH_EXPORT void matrix44copy(FLT *mout, const FLT *minm); +LINMATH_EXPORT void matrix44transpose(FLT *mout, const FLT *minm); #ifdef __cplusplus } -- cgit v1.3.1 From 18b20af7195b94889924156de2b4aa704b2c7391 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Tue, 3 Apr 2018 15:11:12 -0600 Subject: Refactor pose function to get timecode and not lh --- include/libsurvive/poser.h | 6 +++--- include/libsurvive/survive.h | 8 +++++--- include/libsurvive/survive_types.h | 2 +- simple_pose_test.c | 12 ++++++------ src/poser.c | 26 ++++++++++++++++++++++---- src/poser_daveortho.c | 2 +- src/poser_epnp.c | 6 +++--- src/poser_sba.c | 6 +++--- src/poser_turveytori.c | 4 ++-- src/survive.c | 8 ++++---- src/survive_process.c | 6 +++--- test.c | 8 ++++---- winbuild/test/test.vcxproj | 2 +- 13 files changed, 58 insertions(+), 38 deletions(-) (limited to 'include') 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 -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 @@ Console - 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) + 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) true UseFastLinkTimeCodeGeneration -- cgit v1.3.1