From a1717cad950d9f867fbe8aa8a6ab6cf2d8704a64 Mon Sep 17 00:00:00 2001 From: dpeter99 Date: Sun, 1 Apr 2018 17:36:50 +0200 Subject: Removed the manual .def file --- winbuild/libsurvive/libsurvive.def | 30 ------------------------------ 1 file changed, 30 deletions(-) delete mode 100644 winbuild/libsurvive/libsurvive.def (limited to 'winbuild') diff --git a/winbuild/libsurvive/libsurvive.def b/winbuild/libsurvive/libsurvive.def deleted file mode 100644 index 3a50ce6..0000000 --- a/winbuild/libsurvive/libsurvive.def +++ /dev/null @@ -1,30 +0,0 @@ -LIBRARY LIBSURVIVE -EXPORTS - survive_verify_FLT_size - survive_init_internal - survive_startup - survive_poll - survive_close - survive_configi - survive_configs - survive_install_light_fn - survive_install_imu_fn - survive_install_angle_fn - survive_install_htc_config_fn - survive_send_magic - survive_cal_install - survive_cal_get_status - survive_default_light_process - survive_default_imu_process - survive_default_angle_process - survive_install_button_fn - survive_install_raw_pose_fn - survive_install_lighthouse_pose_fn - survive_get_so_by_name - survive_haptic - survive_default_button_process - survive_default_raw_pose_process - survive_default_lighthouse_pose_process - quatnormalize - quatrotatevector - \ No newline at end of file -- cgit v1.3.1 From 0eea189db82fc72463a83a678d9b8d609fd09c6e Mon Sep 17 00:00:00 2001 From: dpeter99 Date: Sun, 1 Apr 2018 18:58:19 +0200 Subject: Added the API file from Unity --- bindings/cs/libsurvive.net/LibSurViveAPI.cs | 342 ++++++++++++++++++++++++++++ bindings/cs/libsurvive.net/cfunctions.cs | 35 ++- winbuild/libsurvive/libsurvive.vcxproj | 3 +- 3 files changed, 361 insertions(+), 19 deletions(-) create mode 100644 bindings/cs/libsurvive.net/LibSurViveAPI.cs (limited to 'winbuild') diff --git a/bindings/cs/libsurvive.net/LibSurViveAPI.cs b/bindings/cs/libsurvive.net/LibSurViveAPI.cs new file mode 100644 index 0000000..48336e8 --- /dev/null +++ b/bindings/cs/libsurvive.net/LibSurViveAPI.cs @@ -0,0 +1,342 @@ +//using UnityEngine; +using System.Collections; +using System.Collections.Generic; + +using libsurvive; +using System; +using System.Threading; + +public class LibSurViveAPI +{ + + public struct SetupConfigs + { + public string playbaskFile; + public int playbackFactor; + public Disambiguator disambiguator; + public Poser poser; + public BoolConfig calibrate; + public string configFile; + } + + public enum Disambiguator + { + StateBased, + Charles, + Turvey, + Default + } + + public enum Poser + { + CharlesSlow, + DaveOrtho, + Dummy, + EPNP, + SBA + } + + public enum BoolConfig + { + Yes, + No, + Default + } + + + private static LibSurViveAPI _instance; + public static LibSurViveAPI instance + { + get + { + if (_instance == null) + { + _instance = new LibSurViveAPI(); + } + + return _instance; + } + } + + LibSurViveAPI() + { + CreateContext(); + + CreateTread(); + + } + + ~LibSurViveAPI() + { + running = false; + } + + Thread internalPollTread; + + private void CreateTread() + { + internalPollTread = new Thread(InternalPoll); + internalPollTread.Start(); + } + + bool running = true; + + void InternalPoll() + { + while (running) + { + cfunctions.Survive_poll(context); + } + } + + //private SurviveContext _context; + public IntPtr context; + + light_process_func light_Process_Func; + raw_pose_func raw_Pose_Func; + lighthouse_pose_func lighthouse_Pose_Func; + angle_process_func angle_Process_Func; + button_process_func button_Process_Func; + htc_config_func htc_Config_Func; + imu_process_func imu_Process_Func; + text_feedback_func error_func; + text_feedback_func info_func; + + internal void CreateContext() + { + //Debug.Log("Start Init"); + + SetupConfigs configs = new SetupConfigs + { + playbaskFile = "P:/c/libsurvive-data/lightcap-reformat/lightcap-reformat.log", + configFile = "survive_conf.json", + playbackFactor = 1 + }; + + string[] args = CreateStartParameters(configs); + + //string[] vs = new[] { "--playback", "P:/c/libsurvive-data/lightcap-reformat/lightcap-reformat.log", "--disambiguator", "StateBased", "--calibrate" }; + + context = cfunctions.Survive_init_internal(args.Length, args); + + if (context == IntPtr.Zero) + { + throw new Exception("There was a problem initializing the lib!"); + } + + light_Process_Func = LightEvent; + raw_Pose_Func = PoseEvent; + lighthouse_Pose_Func = LightHouseEvent; + angle_Process_Func = AngleEvent; + button_Process_Func = ButtonEvent; + htc_Config_Func = HTCConfigEvent; + imu_Process_Func = IMUEvent; + error_func = ErrorEvent; + info_func = InfoEvent; + + cfunctions.Survive_install_raw_pose_fn(context, raw_Pose_Func); + cfunctions.Survive_install_light_fn(context, light_Process_Func); + cfunctions.Survive_install_lighthouse_pose_fn(context, lighthouse_Pose_Func); + cfunctions.Survive_install_angle_fn(context, angle_Process_Func); + cfunctions.Survive_install_button_fn(context, button_Process_Func); + cfunctions.Survive_install_htc_config_fn(context, htc_Config_Func); + cfunctions.Survive_install_imu_fn(context, imu_Process_Func); + cfunctions.Survive_install_error_fn(context, error_func); + cfunctions.Survive_install_info_fn(context, info_func); + + //Debug.Log("Finished Init"); + + //Debug.Log("Start Startup"); + + //Debug.LogError("ASD"); + + int a = 0; + try + { + a = cfunctions.Survive_startup(context); + } + catch (Exception) + { + throw; + } + + if (a != 0) + { + throw new Exception("Error in startup"); + } + + //Debug.Log("Finished Startup"); + + } + + + static public string[] CreateStartParameters(SetupConfigs configs) + { + List args = new List(); + + args.Add("unity"); + + if (configs.playbaskFile != "") + { + args.AddRange(new[] { "--playback", configs.playbaskFile }); + args.AddRange(new[] { "--playback-factor", configs.playbackFactor.ToString() }); + } + + if(configs.disambiguator != Disambiguator.Default) + args.AddRange(new[] { "--disambiguator", Enum.GetName(typeof(Disambiguator), configs.disambiguator) }); + + if (configs.calibrate != BoolConfig.Default) + args.Add(configs.calibrate == BoolConfig.Yes ? "--calibrate" : "--no-calibrate"); + + if (configs.configFile != "") + args.AddRange(new[] { "-c", configs.configFile }); + + //args.AddRange(new[] { "--disambiguator", Enum.GetName(typeof(Poser), disambiguator) }); + + + + return args.ToArray(); + } + + + + + + virtual protected void InfoEvent(IntPtr ctx, string fault) + { + //Debug.Log(fault); + } + + virtual protected void ErrorEvent(IntPtr ctx, string fault) + { + //Debug.LogError(fault); + } + + virtual protected void IMUEvent(IntPtr so, int mask, double[] accelgyro, uint timecode, int id) + { + cfunctions.Survive_default_imu_process(so, mask, accelgyro, timecode, id); + } + + virtual protected int HTCConfigEvent(IntPtr so, string ct0conf, int len) + { + return cfunctions.Survive_default_htc_config_process(so, ct0conf, len); + } + + virtual protected void ButtonEvent(IntPtr so, byte eventType, byte buttonId, byte axis1Id, ushort axis1Val, byte axis2Id, ushort axis2Val) + { + cfunctions.Survive_default_button_process(so, eventType, buttonId, axis1Id, axis1Val, axis2Id, axis2Val); + } + + virtual protected void AngleEvent(IntPtr so, int sensor_id, int acode, uint timecode, double length, double angle, uint lh) + { + cfunctions.Survive_default_angle_process(so, sensor_id, acode, timecode, length, angle, lh); + + //Debug.Log("AngleEvent"); + } + + protected void LightHouseEvent(IntPtr ctx, byte lighthouse, SurvivePose lighthouse_pose, SurvivePose object_pose) + { + cfunctions.Survive_default_lighthouse_pose_process(ctx, lighthouse, lighthouse_pose, object_pose); + + //Debug.Log("LightHouseEvent"); + } + + virtual protected void LightEvent(IntPtr so, int sensor_id, int acode, int timeinsweep, UInt32 timecode, UInt32 length, UInt32 lighthouse) + { + cfunctions.Survive_default_light_process(so, sensor_id, acode, timeinsweep, timecode, length, lighthouse); + + //Console.WriteLine("LightEvent"); + //Debug.Log("LightEvent"); + } + + virtual protected void PoseEvent(IntPtr so, byte lighthouse, SurvivePose pose) + { + cfunctions.Survive_default_raw_pose_process(so, lighthouse, pose); + + //vaDebug.Log("PoseEvent"); + + //poseUpdate(-1, new Vector3((float)pose.Pos[0], (float)pose.Pos[1], (float)pose.Pos[2]), new Quaternion((float)pose.Rot[0], (float)pose.Rot[1], (float)pose.Rot[2], (float)pose.Rot[3])); + + /* + string a = cfunctions.Survive_object_codename(so); + if (updates.ContainsKey(a)) + { + Vector3 pos = new Vector3((float)pose.Pos[0], (float)pose.Pos[1], (float)pose.Pos[2]); + Quaternion rot = new Quaternion((float)pose.Rot[0], (float)pose.Rot[1], (float)pose.Rot[2], (float)pose.Rot[3]); + updates[a](pos, rot); + } + */ + } + + + public delegate void PoseUpdate(SurviveVector3 pos, SurviveQuaternion quat); + + public Dictionary updates = new Dictionary(); + + public void AddCalback(string ID, PoseUpdate update) + { + if (!updates.ContainsKey(ID)) + { + updates.Add(ID, update); + } + else + { + updates[ID] += update; + } + } + + /* + public void Poll() + { + cfunctions.Survive_poll(context); + } + */ + + public SurviveObject GetSurviveObjectByName(string name) + { + if (name == "") + { + throw new Exception("Empty string is not accepted"); + } + + if (context == IntPtr.Zero) + throw new Exception("The context hasn't been initialsied yet"); + + return new SurviveObject( cfunctions.Survive_get_so_by_name(context, name)); + } +} + +public class SurviveObject +{ + public SurvivePose pose { get; private set; } + + public int charge { get; private set; } + public bool charging { get; private set; } + + + public SurviveObject(IntPtr ptr) + { + if (ptr == IntPtr.Zero) + { + throw new Exception("Can't create SurviveObject with 0 pointer"); + } + + //pose = cfunctions.Survive_object_pose(ptr); + charge = cfunctions.Survive_object_charge(ptr); + charging = cfunctions.Survive_object_charging(ptr); + } +} + +public class SurviveVector3 +{ + float x; + float y; + float z; +} + +public class SurviveQuaternion +{ + float x; + float y; + float z; +} diff --git a/bindings/cs/libsurvive.net/cfunctions.cs b/bindings/cs/libsurvive.net/cfunctions.cs index 67812f5..ab5f7fd 100644 --- a/bindings/cs/libsurvive.net/cfunctions.cs +++ b/bindings/cs/libsurvive.net/cfunctions.cs @@ -12,23 +12,16 @@ namespace libsurvive [StructLayout(LayoutKind.Sequential)] public class SurvivePose { - [MarshalAs(UnmanagedType.ByValArray, SizeConst = 3)] + [MarshalAs(UnmanagedType.ByValArray, SizeConst = 3)] public double[] Pos; // Position in the form xyz [MarshalAs(UnmanagedType.ByValArray, SizeConst = 4)] - public double[] Rot; // Quaternion in the form wxyz - } - - [StructLayout(LayoutKind.Sequential)] - public class SurviveObject - { - [MarshalAs(UnmanagedType.ByValArray, SizeConst = 4)] - public char[] codename; //3 letters, null-terminated. Currently HMD, WM0, WM1. + public double[] Rot; // Quaternion in the form wxyz } class cfunctions { -//#pragma warning disable IDE1006 // Naming Styles - [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall, EntryPoint = "survive_init_internal") ] + //#pragma warning disable IDE1006 // Naming Styles + [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall, EntryPoint = "survive_init_internal")] public static extern SurviveContextPtr Survive_init_internal(int argc, string[] args); [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall, EntryPoint = "survive_close")] @@ -95,26 +88,32 @@ namespace libsurvive [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall, EntryPoint = "survive_object_codename")] - public static extern char Survive_object_codename(SurviveObjectPtr so); + public static extern string Survive_object_codename(SurviveObjectPtr so); [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall, EntryPoint = "survive_object_drivername")] - public static extern char Survive_object_drivername(SurviveObject so); + public static extern char Survive_object_drivername(SurviveObjectPtr so); [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall, EntryPoint = "survive_object_charge")] - public static extern byte Survive_object_charge(SurviveObject so); + public static extern byte Survive_object_charge(SurviveObjectPtr so); [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall, EntryPoint = "survive_object_charging")] - public static extern bool Survive_object_charging(SurviveObject so); + 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(SurviveObject so); + public static extern SurvivePose Survive_object_pose(SurviveObjectPtr so); + [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall, EntryPoint = "survive_object_sensor_locations")] - public static extern int Survive_object_sensor_locations(SurviveObjectPtr so); + public static extern double[] Survive_object_sensor_locations(SurviveObjectPtr so); [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall, EntryPoint = "survive_object_sensor_normals")] - public static extern int Survive_object_sensor_normals(SurviveObjectPtr so); + public static extern double[] Survive_object_sensor_normals(SurviveObjectPtr so); + + + + [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall, EntryPoint = "survive_get_so_by_name")] + public static extern IntPtr Survive_get_so_by_name(IntPtr ctx, string name); //#pragma warning restore IDE1006 // Naming Styles } diff --git a/winbuild/libsurvive/libsurvive.vcxproj b/winbuild/libsurvive/libsurvive.vcxproj index 5caa159..aefdc00 100644 --- a/winbuild/libsurvive/libsurvive.vcxproj +++ b/winbuild/libsurvive/libsurvive.vcxproj @@ -91,7 +91,8 @@ true $(IntDir)Test.txt true - $(MSBuildProjectDirectory)\libsurvive.def + + -- cgit v1.3.1 From a87071facfd896a7d9cd2939d69a8a7b80db8779 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Mon, 2 Apr 2018 00:13:42 -0600 Subject: Removed def file from proj --- winbuild/libsurvive/libsurvive.vcxproj | 1 - winbuild/libsurvive/libsurvive.vcxproj.filters | 1 - 2 files changed, 2 deletions(-) (limited to 'winbuild') diff --git a/winbuild/libsurvive/libsurvive.vcxproj b/winbuild/libsurvive/libsurvive.vcxproj index aefdc00..4b76c99 100644 --- a/winbuild/libsurvive/libsurvive.vcxproj +++ b/winbuild/libsurvive/libsurvive.vcxproj @@ -214,7 +214,6 @@ - Designer diff --git a/winbuild/libsurvive/libsurvive.vcxproj.filters b/winbuild/libsurvive/libsurvive.vcxproj.filters index ffddee9..1f98c68 100644 --- a/winbuild/libsurvive/libsurvive.vcxproj.filters +++ b/winbuild/libsurvive/libsurvive.vcxproj.filters @@ -190,7 +190,6 @@ - \ No newline at end of file -- 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 'winbuild') 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