diff options
48 files changed, 6123 insertions, 738 deletions
@@ -14,31 +14,33 @@ UNAME=$(shell uname) # Mac OSX ifeq ($(UNAME), Darwin) + CFLAGS:=$(CFLAGS) -DRASTERIZER -DHIDAPI -I/usr/local/include -x objective-c LDFLAGS:=$(LDFLAGS) -framework OpenGL -framework Cocoa -framework IOKit -#DRAWFUNCTIONS=redist/DrawFunctions.c redist/RawDrawNull.c -#GRAPHICS_LOFI:=redist/DrawFunctions.o redist/RawDrawNull.o -DRAWFUNCTIONS=redist/DrawFunctions.c redist/CocoaDriver.m redist/RawDrawRasterizer.c -GRAPHICS_LOFI:=redist/DrawFunctions.o redist/CocoaDriver.o redist/RawDrawRasterizer.o +DRAWFUNCTIONS=redist/CNFGFunctions.c redist/CocoaDriver.m +GRAPHICS_LOFI:=redist/CNFGFunctions.o redist/CocoaDriver.o # Linux / FreeBSD else LDFLAGS:=$(LDFLAGS) -lX11 -DRAWFUNCTIONS=redist/DrawFunctions.c redist/XDriver.c -GRAPHICS_LOFI:=redist/DrawFunctions.o redist/XDriver.o +DRAWFUNCTIONS=redist/CNFGFunctions.c redist/CNFGXDriver.c +GRAPHICS_LOFI:=redist/CNFGFunctions.o redist/CNFGXDriver.o endif -POSERS:=src/poser_dummy.o src/poser_daveortho.o src/poser_charlesslow.o +POSERS:=src/poser_dummy.o src/poser_daveortho.o src/poser_charlesslow.o src/poser_octavioradii.o src/poser_turveytori.o REDISTS:=redist/json_helpers.o redist/linmath.o redist/jsmn.o redist/os_generic.o ifeq ($(UNAME), Darwin) REDISTS:=$(REDISTS) redist/hid-osx.c endif LIBSURVIVE_CORE:=src/survive.o src/survive_usb.o src/survive_data.o src/survive_process.o src/ootx_decoder.o src/survive_driverman.o src/survive_vive.o src/survive_config.o src/survive_cal.o -LIBSURVIVE_CORE:=$(LIBSURVIVE_CORE) -LIBSURVIVE_O:=$(POSERS) $(REDISTS) $(LIBSURVIVE_CORE) -LIBSURVIVE_C:=$(LIBSURVIVE_O:.o=.c) + + +#If you want to use HIDAPI on Linux. +#CFLAGS:=$(CFLAGS) -DHIDAPI +#REDISTS:=$(REDISTS) redist/hid-linux.o +#LDFLAGS:=$(LDFLAGS) -ludev #Useful Preprocessor Directives: # -DUSE_DOUBLE = use double instead of float for most operations. @@ -50,6 +52,11 @@ LIBSURVIVE_C:=$(LIBSURVIVE_O:.o=.c) + +LIBSURVIVE_CORE:=$(LIBSURVIVE_CORE) +LIBSURVIVE_O:=$(POSERS) $(REDISTS) $(LIBSURVIVE_CORE) +LIBSURVIVE_C:=$(LIBSURVIVE_O:.o=.c) + # unused: redist/crc32.c testCocoa : testCocoa.c $(DRAWFUNCTIONS) @@ -58,10 +65,10 @@ testCocoa : testCocoa.c $(DRAWFUNCTIONS) test : test.c ./lib/libsurvive.so redist/os_generic.o $(CC) -o $@ $^ $(LDFLAGS) $(CFLAGS) -data_recorder : data_recorder.c ./lib/libsurvive.so redist/os_generic.c $(GRAPHICS_LOFI) +data_recorder : data_recorder.c ./lib/libsurvive.so redist/os_generic.c $(DRAWFUNCTIONS) $(CC) -o $@ $^ $(LDFLAGS) $(CFLAGS) -calibrate : calibrate.c ./lib/libsurvive.so redist/os_generic.c $(GRAPHICS_LOFI) +calibrate : calibrate.c ./lib/libsurvive.so redist/os_generic.c $(DRAWFUNCTIONS) $(CC) -o $@ $^ $(LDFLAGS) $(CFLAGS) calibrate_client : calibrate_client.c ./lib/libsurvive.so redist/os_generic.c $(GRAPHICS_LOFI) @@ -1,8 +1,10 @@ # libsurvive -**WARNING PROJECT NOT YET IN EXPERIMENTAL PHASE** + -Discord: https://discordapp.com/invite/7QbCAGS +An Open-Source tool for working with lighthouse-based tracking data, including support for the HTC Vive, which is still in the experimental phase. + +Most of the development is discussed on Discord. Join the chat and discussion here: https://discordapp.com/invite/7QbCAGS ## Livestream collection | Note | Youtube URL | Run time | @@ -15,6 +17,10 @@ Discord: https://discordapp.com/invite/7QbCAGS | Fifth livestream | https://www.youtube.com/watch?v=hHt3twW5_fI | 3:13:38 | | Sixth livestream | https://www.youtube.com/watch?v=JsfkNRFkFM4 | 3:44:49 | | Seventh livestream | https://www.youtube.com/watch?v=EKSHvO3QSWY | 1:17:21 | +| Eighth livestream | https://www.youtube.com/watch?v=nSbEltdH9vM | 6:06:36 | +| Ninth livestream | https://www.youtube.com/watch?v=60sGTd8T-KY | 3:28:44 | +| Ninth B livestream | https://www.youtube.com/watch?v=IIYj1Ig_gz8 | 2:25:33 | +| Tenth livestream | https://www.youtube.com/watch?v=boXRdXca6Qc | ?:??:?? | Notes from second livestream trying to reverse engineer the watchman protocol: https://gist.github.com/cnlohr/581c433f36f4249f8bbc9c2b6450ef0e @@ -24,6 +30,22 @@ Please see the issues for what help needs to be done now! HackADay article and video with Dr. Yates on how they made the Vive a thing. http://hackaday.com/2016/12/21/alan-yates-why-valves-lighthouse-cant-work/ + +## Nomenclature + +* WRT = With Respect To +* PoV / POV = Point of View (typically WRT to a LH, sometimes (though rarely) a sensor) +* LH = Lighthouse = Base Station = A device that produces a 1.8 MHz modulated sync pulse in IR and then sweeps the scene with laser planes. +* Sync Pulse = A pulse of modulated IR data sent from a ligthhouse, typically by the floodlight aspect of a lighthouse. +* Sweep Pulse = The evenlope created by a laser sweeping over a light sensor. +* OOTX = Omnidirectional Optical Transmitter = Data encoded in the sync pulses of the LHs. +* HMD = Headset = Main sensor receiver with a visual display for a human. +* WM = Watchman = Controller = The HTC Vive controller. +* TR = Tracker = Official HTC Tracker. +* LightcapElement = A single pulse of light, including a timestamp, source sensor and length of pulse. +* Disambiguator = System that accepts lightcap elements and pulls out OOTX data and relative sweep times of sweep pulses. +* Poser = Device to convert series of angles from a LH's PoV + ## Getting things working There are two things you should consider doing to your system before running libsurvive. @@ -40,38 +62,76 @@ I say "high-performance" really this project is based tightly off of OSVR-Vive-L 2. Put it under an open-source instead of a force-source license. (GPL to MIT/X11) 3. Write it in C. 4. Avoid extra layers where convenient. -5. (long shot) Make the vive vivable for use with Intel Integrated Graphics systems. - +5. (long shot) Make the vive viable for use with Intel Integrated Graphics systems. [It works with HD4000 using DisplayPort. See "Intel Integrated Graphics" section below.] -Will ~~I~~ we succeed? Probably not. - -Definitely going to try! +Will ~~I~~ we succeed? Probably not. ~~Definitely going to try!~~ Though it's looking like we might. ## External dependencies -* libUSB +* libUSB (Linux) or hidapi (Win, OSX; included in redist) * pthread -* libX11 (where applicable) -* zlib (may use puff.c if needed) - -If I ever get to video output... OpenGL. +* libX11 (Linux) or Native (win32) or OpenGL (OSX) +* zlib (Linux) or puff.c (win32, included in redist) ## Architecture -There is an internal representation and an external representation. These lines may get blurred. Internal representation lives in .h files in the ```src/``` folder. External lives in ```include/``` folder. +<TABLE><TR><TH>Description</TH><TH>Diagram</TH> +</TR><TR> +<TD WIDTH=50%> + +### Layout + +In the src/ folder you'll find most of the internal code that is part of libsurvive. The redist/ folder contains code that libsurvive uses that was copied from other projects. Libsurvive links to other libraries, but very few. You'll find that most of the functionality lies within libsurvive or in the redist folder. For the user-facing headers you can find them in the include/ folder. + +### Logical Data Flow + +There are device drivers, such as survive_vive.c which connect to physical devices, via libUSB, hidapi or another method and collect raw IMU and lightcap data. Lightcap data is specically a sensor ID, light pulse length (in ticks) and the time of the light pulse (in ticks). The driver also must provide locations of the sensors to populate the SurviveObject structures of whatever sensors that driver is responsible for. + +Once this data is collected, the light pulses are disambiguated (see survive_data.c) into OOTX sync pulses (id -1, -2 depending on lighthouse) as well as sweep pulses which provide the time of a sweep pulse passing the sensor. This is passed off to "lightproc." The default behavior for lightproc can be found in survive.c. The user may override this, however, if they have interest in the raw pulse information for whatever reason. The default behavior for lightproc determines the time delta between the sync pulse and the sweep pulse time and derives angle. The derivation for the angle is simply calculated by the time difference between the sync pulse and the sweep pulse. It then calls "angleproc" (not implemented yet: Using OOTX data from lighthouses to correct and tweak angles) + +Angleproc may also be overridden by the user for similar purposes to for "angleproc" which passes its information off to a calibrator (if running) as well as to whatever posers are enabled. The posers will take this data and determine position from it. + +</TD> +<TD WIDTH=50%><img src=https://raw.githubusercontent.com/cnlohr/libsurvive/master/useful_files/FunctionalSystem.png width=400></TD> +</TR> +</TABLE> + +## Lists of components + +Component Type | Component | Description | Authors +--- | --- | --- | --- +Poser | [poser_charlesslow.c](src/poser_charlesslow.c) | A very slow, but exhaustive poser system. Calibration only. | [@cnlohr](https://github.com/cnlohr) +Poser | [poser_daveortho.c](src/poser_daveortho.c) | A very fast system using orthograpic view and affine transformations. Calibration only (for now) | [@ultramn](https://github.com/ultramn) +Poser | [poser_dummy.c](src/poser_dummy.c) | Template for posers | [@cnlohr](https://github.com/cnlohr) +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) +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) +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) + +## Component Pluggability Matrix + +Component Type | Pluggability method +--- | --- +Driver | Dynamically loadable runtime, can co-exist with other drivers. +Poser | Selectable by configuration at runtime +Disambiguator | Selectable by #define +OOTX Decoder | Not Pluggable + -It is written in some fairly stout "layers" which are basically just function calls: -| Layer | Description | Status | -| ------- | ------------- | -------- | -| survive_usb.c | Data is taken in at "survive_usb.c" from libusb. | Done | -| survive_data.c | Raw HID messages are processed into logical "light" "analog" and "imu" messages. | Mostly done, Missing light data from controllers, and lighthouse data. | -| survive_process.c | Process the high-level data into solutions for | Not yet started. Will be done by ultramn | +## Intel Integrated Graphics -I may or may not read data from the Vive regarding configuration. If I do, it would be added to the survive_usb.c +The limiting factor for Vive viability on a given computer is the maximum available pixel clock frequency, and frequency limitations of the HDMI port, and HDMI and DisplayPort video cables. DisplayPort can support higher frequencies than HDMI, on Ivy Bridge HD4000 graphics. In fact, the Vive works with HD4000 graphics using DisplayPort, with native EDID resolution (2160x1200@90Hz). +To support the Vive on HDMI, you either need a newer version of HDMI, or you need to define a custom resolution that respects pixel clock and video port limits, and is also accepted and displayed by the Vive. So far, we have not had success using custom resolutions on linux or on Windows. Windows imposes additional limitations in the form of restriction of WHQL certified drivers forbidden from using custom display resolutions (only allowing those defined by EDID in the monitor). Intel has released uncertified beta drivers for Haswell and newer processors, which should be able to support custom resolutions for the Vive (untested at this time). +## Addendum and notes +Thanks to Mr. Faul for our logo! +Special thanks to @nairol for an extreme amount of detail in reverse engineering the existing HTC Vive system on his https://github.com/nairol/LighthouseRedox project. diff --git a/calibrate.c b/calibrate.c index 28e920a..9a43696 100644 --- a/calibrate.c +++ b/calibrate.c @@ -7,7 +7,7 @@ #include <string.h> #include <os_generic.h> #include "src/survive_cal.h" -#include <DrawFunctions.h> +#include <CNFGFunctions.h> #include "src/survive_config.h" @@ -40,6 +40,10 @@ void HandleMotion( int x, int y, int mask ) { } +void HandleDestroy() +{ +} + //int bufferpts[32*2*3][2]; int bufferpts[32*2*3][2]; @@ -47,38 +51,37 @@ int bufferpts[32*2*3][2]; char buffermts[32*128*3]; int buffertimeto[32*3][2]; -void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ) +void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lh) { // if( timeinsweep < 0 ) return; - survive_default_light_process( so, sensor_id, acode, timeinsweep, timecode, length ); + survive_default_light_process( so, sensor_id, acode, timeinsweep, timecode, length, lh); if( sensor_id < 0 ) return; - if( acode == -1 ) return; + if( acode < 0 ) return; //return; int jumpoffset = sensor_id; - if( strcmp( so->codename, "WM0" ) == 0 ) jumpoffset += 32; + if( strcmp( so->codename, "WM0" ) == 0 || strcmp( so->codename, "WW0" ) == 0) jumpoffset += 32; else if( strcmp( so->codename, "WM1" ) == 0 ) jumpoffset += 64; - - if( acode == 0 || acode == 2 ) //data = 0 + if( acode % 2 == 0 && lh == 0) //data = 0 { - bufferpts[jumpoffset*2+0][0] = (timeinsweep-100000)/500; + bufferpts[jumpoffset*2+0][0] = (timeinsweep-100000)/300; buffertimeto[jumpoffset][0] = 0; } - if( acode == 1 || acode == 3 ) //data = 1 + if( acode % 2 == 1 && lh == 0 ) //data = 1 { - bufferpts[jumpoffset*2+1][0] = (timeinsweep-100000)/500; + bufferpts[jumpoffset*2+1][0] = (timeinsweep-100000)/300; buffertimeto[jumpoffset][0] = 0; } - if( acode == 4 || acode == 6 ) //data = 0 + if( acode % 2 == 0 && lh == 1 ) //data = 0 { - bufferpts[jumpoffset*2+0][1] = (timeinsweep-100000)/500; + bufferpts[jumpoffset*2+0][1] = (timeinsweep-100000)/300; buffertimeto[jumpoffset][1] = 0; } - if( acode == 5 || acode == 7 ) //data = 1 + if( acode % 2 == 1 && lh == 1 ) //data = 1 { - bufferpts[jumpoffset*2+1][1] = (timeinsweep-100000)/500; + bufferpts[jumpoffset*2+1][1] = (timeinsweep-100000)/300; buffertimeto[jumpoffset][1] = 0; } } @@ -95,9 +98,9 @@ void my_imu_process( struct SurviveObject * so, int mask, FLT * accelgyro, uint3 } -void my_angle_process( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle ) +void my_angle_process( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh) { - survive_default_angle_process( so, sensor_id, acode, timecode, length, angle ); + survive_default_angle_process( so, sensor_id, acode, timecode, length, angle, lh ); } char* sensor_name[32]; @@ -128,13 +131,16 @@ void * GuiThread( void * jnk ) uint8_t g = 0x00; uint8_t b = 0xff; - if (nn==0) b = 0; //lighthouse B - if (nn==1) r = 0; //lighthouse C + if (nn==0) b = 0; //lighthouse B, red, master + if (nn==1) r = 0; //lighthouse C, blue, slave // r = (r * (5-buffertimeto[i][nn])) / 5 ; // g = (g * (5-buffertimeto[i][nn])) / 5 ; // b = (b * (5-buffertimeto[i][nn])) / 5 ; CNFGColor( (b<<16) | (g<<8) | r ); + + if (bufferpts[i*2+0][nn] == 0 || bufferpts[i*2+1][nn]==0) continue; //do not draw if aither coordinate is 0 + CNFGTackRectangle( bufferpts[i*2+0][nn], bufferpts[i*2+1][nn], bufferpts[i*2+0][nn] + 5, bufferpts[i*2+1][nn] + 5 ); CNFGPenX = bufferpts[i*2+0][nn]; CNFGPenY = bufferpts[i*2+1][nn]; CNFGDrawText( buffermts, 2 ); diff --git a/calibrate_client.c b/calibrate_client.c index f28520b..abfbabc 100644 --- a/calibrate_client.c +++ b/calibrate_client.c @@ -8,7 +8,7 @@ #include <string.h> #include <os_generic.h> #include "src/survive_cal.h" -#include <DrawFunctions.h> +#include <CNFGFunctions.h> #include "src/survive_config.h" @@ -35,14 +35,17 @@ void HandleButton( int x, int y, int button, int bDown ) void HandleMotion( int x, int y, int mask ) { } +void HandleDestroy() +{ +} int bufferpts[32*2*3]; char buffermts[32*128*3]; int buffertimeto[32*3]; -void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ) +void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lh) { - survive_default_light_process( so, sensor_id, acode, timeinsweep, timecode, length ); + survive_default_light_process( so, sensor_id, acode, timeinsweep, timecode, length, lh); if( acode == -1 ) return; //return; @@ -87,9 +90,9 @@ void my_imu_process( struct SurviveObject * so, int mask, FLT * accelgyro, uint3 } -void my_angle_process( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle ) +void my_angle_process( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh ) { - survive_default_angle_process( so, sensor_id, acode, timecode, length, angle ); + survive_default_angle_process( so, sensor_id, acode, timecode, length, angle, lh ); } @@ -157,7 +160,7 @@ int main() // config_save("config.json"); */ - + ctx = survive_init( 1 ); survive_install_light_fn( ctx, my_light_process ); @@ -216,9 +219,12 @@ int main() so = wm0; if( strcmp( dev, "WM1" ) == 0 ) so = wm1; + uint32_t lh = 0; + if (lineptr[0] == 'r') + lh = 1; if( so ) - my_light_process( so, sensor_id, acode, timeinsweep, timecode, length ); + my_light_process( so, sensor_id, acode, timeinsweep, timecode, length, lh ); break; } diff --git a/data_recorder.c b/data_recorder.c index 19bf712..085b67d 100644 --- a/data_recorder.c +++ b/data_recorder.c @@ -1,13 +1,15 @@ //Data recorder mod with GUI showing light positions. +#ifdef __linux__ #include <unistd.h> +#endif #include <stdio.h> #include <stdlib.h> #include <stdint.h> #include <survive.h> #include <string.h> #include <os_generic.h> -#include <DrawFunctions.h> +#include <CNFGFunctions.h> struct SurviveContext * ctx; @@ -33,13 +35,17 @@ void HandleMotion( int x, int y, int mask ) { } +void HandleDestroy() +{ +} + int bufferpts[32*2*3]; char buffermts[32*128*3]; int buffertimeto[32*3]; -void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ) +void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lh) { - survive_default_light_process( so, sensor_id, acode, timeinsweep, timecode, length ); + survive_default_light_process( so, sensor_id, acode, timeinsweep, timecode, length, lh); if( acode == -1 ) return; //return; diff --git a/dave/AffineSolve b/dave/AffineSolve Binary files differindex 98a9590..bd93cbd 100755 --- a/dave/AffineSolve +++ b/dave/AffineSolve diff --git a/dave/AffineSolve.c b/dave/AffineSolve.c index 4fba56b..685062e 100644 --- a/dave/AffineSolve.c +++ b/dave/AffineSolve.c @@ -11,10 +11,13 @@ #include <math.h> #include "dclapack.h" #include <linmath.h> +#define RegisterDriver(a,b) +#include "poser_daveortho.c" #define LH_ID 0 #define NUM_HMD 32 +#define INDIR "full_test_triangle_on_floor/" -#define MAX_POINTS 128 +#define MAX_POINTS SENSORS_PER_OBJECT //#define _ABS(a) ( (a)<=0 ? -(a) : (a) ) #define _SIGN(a) ( (a)<=0 ? -1.0f : 1.0f ) #define RANDF ( (float)rand() / (float)RAND_MAX ) @@ -31,7 +34,7 @@ float hmd_pos[NUM_HMD][3]; void ReadHmdPoints() { int i; - FILE *fin = fopen("HMD_points.csv","r"); + FILE *fin = fopen(INDIR "HMD_points.csv","r"); if (fin==NULL) { printf("ERROR: could not open HMD_points.csv for reading\n"); exit(1); @@ -52,7 +55,7 @@ void ReadPtinfo() for (i=0; i<NUM_HMD; i++) { hmd_angle[i][0]=-9999.0; hmd_angle[i][1]=-9999.0; } // Read ptinfo.csv - FILE *fin = fopen("ptinfo.csv", "r"); + FILE *fin = fopen(INDIR "ptinfo.csv", "r"); if (fin==NULL) { printf("ERROR: could not open ptinfo.csv for reading\n"); exit(1); } while (!feof(fin)) { @@ -74,6 +77,350 @@ void ReadPtinfo() fclose(fin); } +void AffineSolve( + float T[4][4], // OUTPUT: transform + float O[MAX_POINTS][4], // INPUT: points, offsets + float N[MAX_POINTS][3], // INPUT: plane normals + float D[MAX_POINTS], // INPUT: plane offsets + int nPoints, int nIter, + float stepSizeRot, float stepSizePos, float falloff, int constrain) +{ + int i,j,k,iter; + //T[3][3] = 1.0f; + + printf("iter x y z error\n"); + + float gradDot = 1.0; + float prevGradDot = 1.0; + float de_dT[3][4]; // the gradient + float conj[3][4]; // the conjugate + float errorSq=0.0; + for (iter=0; iter<nIter; iter++) + { + //---------------------------------- + // Calculate the gradient direction + //---------------------------------- + errorSq = 0.0; + memset(de_dT, 0, 3*4*sizeof(float)); + for (i=0; i<nPoints; i++) + { + // What is the plane deviation error + float Ei = -D[i]; + for (j=0; j<3; j++) { + float Tj_oi = 0.0f; + for (k=0; k<4; k++) { + Tj_oi += T[j][k] * O[i][k]; + } + Ei += N[i][j] * Tj_oi; + } +// printf("E[%d] %f\n", i, Ei); + + // Figure out contribution to the error + for (j=0; j<3; j++) { + for (k=0; k<4; k++) { + de_dT[j][k] += N[i][j] * O[i][k] * Ei; + } + } + + errorSq += Ei*Ei; + } + +// printf("%d %f %f %f %f\n", iter, T[0][3], T[1][3], T[2][3], sqrt(errorSq)); +//exit(1); + // Constrain the gradient (such that dot products are zero) + if (constrain) + { + float T0T1 = 0.0, T1T2 = 0.0, T2T0 = 0.0; + for (k=0; k<3; k++) { + T0T1 += T[0][k] * T[1][k]; + T1T2 += T[1][k] * T[2][k]; + T2T0 += T[2][k] * T[0][k]; + } +// printf("T0T1 %f T1T2 %f T2T0 %f\n", T0T1, T1T2, T2T0); + for (k=0; k<3; k++) { + de_dT[0][k] += ORTHOG_PENALTY * 2.0 * T0T1 * T[1][k]; + de_dT[0][k] += ORTHOG_PENALTY * 2.0 * T2T0 * T[2][k]; + de_dT[1][k] += ORTHOG_PENALTY * 2.0 * T1T2 * T[2][k]; + de_dT[1][k] += ORTHOG_PENALTY * 2.0 * T0T1 * T[0][k]; + de_dT[2][k] += ORTHOG_PENALTY * 2.0 * T1T2 * T[1][k]; + de_dT[2][k] += ORTHOG_PENALTY * 2.0 * T2T0 * T[0][k]; + } + } + + // Calculate the gradient dot product + // (used by conjugate gradient method) + prevGradDot = gradDot; + gradDot = 0.0; + for (j=0; j<3; j++) { + for (k=0; k<4; k++) { + gradDot += de_dT[j][k] * de_dT[j][k]; + } + } + +// printf("Iter %d error %f gradDot %f prevGradDot %f\n", iter, sqrt(errorSq), gradDot, prevGradDot); + + //---------------------------------- + // Calculate the conjugate direction + //---------------------------------- +// if (iter==0) { + // First iteration, just use the gradient + for (j=0; j<3; j++) { + for (k=0; k<4; k++) { + conj[j][k] = -de_dT[j][k]; + } + } +/* } else { + // Calculate "beta" for Fletcher Reeves method + float beta = gradDot / prevGradDot; +//printf("gradDot %f prevGradDot %f beta %f\n", gradDot, prevGradDot, beta); + + // Update the conjugate + for (j=0; j<3; j++) { + for (k=0; k<4; k++) { + conj[j][k] = beta*conj[j][k] - de_dT[j][k]; + } + } + } +*/ + +// PRINT_MAT(de_dT,4,4); +// exit(1); + + //---------------------------------- + // How large is the gradient ? + //---------------------------------- + + double gradSizeRot = 0.0; + double gradSizePos = 0.0; + for (j=0; j<3; j++) { + for (k=0; k<3; k++) { + gradSizeRot += _ABS(conj[j][k]); + } + gradSizePos += _ABS(conj[j][k]); + } + if (gradSizeRot <= TOO_SMALL && gradSizePos <= TOO_SMALL) { break; } // Quit, we've totally converged + + //---------------------------------- + // Descend in the gradient direction + //---------------------------------- + if (gradSizeRot > TOO_SMALL) { + float scaleRot = stepSizeRot / gradSizeRot; + for (j=0; j<3; j++) { + for (k=0; k<3; k++) { + T[j][k] += scaleRot * conj[j][k]; + } + } + stepSizeRot *= falloff; + } + + if (gradSizePos > TOO_SMALL) { + float scalePos = stepSizePos / gradSizePos; + for (j=0; j<3; j++) { + T[j][3] += scalePos * conj[j][3]; + } + stepSizePos *= falloff; + } + + // Constrain the gradient (such that scaling is one) + if (constrain) + { + // Measure the scales + float len[3] = {0.0, 0.0, 0.0}; + for (j=0; j<3; j++) { + double lenSq = 0.0; + for (k=0; k<3; k++) { lenSq += (double)T[j][k] * (double)T[j][k]; } + len[j] = sqrt(lenSq); + } + + // How far off is the scale? + float xzLen = 0.5 * (len[0] + len[2]); + if (xzLen > TOO_SMALL) { + float inv_xzLen = 1.0 / xzLen; + for (j=0; j<3; j++) { + T[3][j] *= inv_xzLen; + } + } + + // Rescale the thing + for (j=0; j<3; j++) + { + if (len[j] > TOO_SMALL) { + float inv_len = 1.0 / len[j]; + for (k=0; k<3; k++) { T[j][k] *= inv_len; } + } + } + } + } + float dist = sqrt(T[0][3]*T[0][3] + T[1][3]*T[1][3] + T[2][3]*T[2][3]); + printf("AffineSolve: pos: %f %f %f dist: %f\n", T[0][3], T[1][3], T[2][3], dist); +} + +int main() +{ + int i,j,k,sen,axis; + + // Read the data files + ReadHmdPoints(); + ReadPtinfo(); + + //------------------------- + // Package the lighthouse data for "AffineSolve" + //------------------------- + + // Data for the "iterative" affine solve formula + float Tcalc[4][4]; + float O[MAX_POINTS][4]; + float N[MAX_POINTS][3]; + float D[MAX_POINTS]; + int nPlanes = 0; + + for (sen=0; sen<NUM_HMD; sen++) + { + for (axis=0; axis<2; axis++) + { + if (hmd_angle[sen][axis] != -9999.0) + { + // Set the offset + O[nPlanes][0] = hmd_pos[sen][0]; + O[nPlanes][1] = hmd_pos[sen][1]; + O[nPlanes][2] = hmd_pos[sen][2]; + O[nPlanes][3] = 1.0; + + // Calculate the plane equation + if (axis == 0) { // Horizontal + N[nPlanes][0] = -cos(hmd_angle[sen][axis]); + N[nPlanes][1] = -sin(hmd_angle[sen][axis]); + N[nPlanes][2] = 0.0; + D[nPlanes] = 0.0; + } else { // Vertical + N[nPlanes][0] = 0.0; + N[nPlanes][1] = -sin(hmd_angle[sen][axis]); + N[nPlanes][2] = cos(hmd_angle[sen][axis]); + D[nPlanes] = 0.0; + } + + printf("plane %d O %.3f %.3f %.3f %.3f N %.3f %.3f %.3f D %.3f\n", + nPlanes, + O[nPlanes][0], O[nPlanes][1], O[nPlanes][2], O[nPlanes][3], + N[nPlanes][0], N[nPlanes][1], N[nPlanes][2], + D[nPlanes]); + nPlanes++; + } + } + } + + + printf("nPlanes %d\n", nPlanes); + + //} + + PRINT_MAT(Tcalc,4,4); + + + //-------------------------------------------------- + // Package the data for "OrthoSolve" + //-------------------------------------------------- + + // Data for the "fake" ortho solve formula + float Tortho[4][4]; // OUTPUT: 4x4 transformation matrix + FLOAT S_out[2][MAX_POINTS]; // INPUT: array of screenspace points + FLOAT S_in[2][MAX_POINTS]; // INPUT: array of screenspace points + FLOAT X_in[3][MAX_POINTS]; // INPUT: array of offsets + int nPoints=0; + + // Transform into the "OrthoSolve" format + for (sen=0; sen<NUM_HMD; sen++) + { + if (hmd_angle[sen][0] != -9999.0 && hmd_angle[sen][1] != -9999.0) + { + S_in[0][nPoints] = hmd_angle[sen][0]; + S_in[1][nPoints] = hmd_angle[sen][1]; + X_in[0][nPoints] = hmd_pos[sen][0]; + X_in[1][nPoints] = hmd_pos[sen][1]; + X_in[2][nPoints] = hmd_pos[sen][2]; + nPoints++; + } + } + printf("OrthoSolve nPoints %d\n", nPoints); + + //-------------------------------------------------- + // Run the "OrthoSolve" and then the "AffineSolve" + //-------------------------------------------------- + + int loop; + for (loop=0; loop<1; loop++) + { + // Run OrthoSolve + OrthoSolve( + Tortho, // OUTPUT: 4x4 transformation matrix + S_out, // OUTPUT: array of output screenspace points + S_in, // INPUT: array of screenspace points + X_in, // INPUT: array of offsets + nPoints); + printf( "POS: %f %f %f\n", Tortho[0][3], Tortho[1][3], Tortho[2][3]); + + // Come up with rotation and transposed version of Tortho + FLT TorthoTr[4][4], Rortho[4][4], RorthoTr[4][4]; + TRANSP(Tortho,TorthoTr,4,4); + memcpy(Rortho,Tortho,4*4*sizeof(FLT)); + Rortho[3][0]=0.0; Rortho[3][1]=0.0; Rortho[3][2]=0.0; + TRANSP(Rortho,RorthoTr,4,4); + PRINT(Tortho,4,4); + PRINT(Rortho,4,4); + + // Print out some quaternions + FLT Tquat[4], TquatTr[4], Rquat[4], RquatTr[4]; + quatfrommatrix(Tquat, &Tortho[0][0] ); + quatfrommatrix(TquatTr, &TorthoTr[0][0] ); + quatfrommatrix(Rquat, &Rortho[0][0] ); + quatfrommatrix(RquatTr, &RorthoTr[0][0] ); + printf( "Tquat : %f %f %f %f = %f\n", Tquat [0], Tquat [1], Tquat [2], Tquat [3], quatmagnitude(Tquat)); + printf( "TquatTr: %f %f %f %f = %f\n", TquatTr[0], TquatTr[1], TquatTr[2], TquatTr[3], quatmagnitude(TquatTr)); + printf( "Rquat : %f %f %f %f = %f\n", Rquat [0], Rquat [1], Rquat [2], Rquat [3], quatmagnitude(Rquat)); + printf( "RquatTr: %f %f %f %f = %f\n", RquatTr[0], RquatTr[1], RquatTr[2], RquatTr[3], quatmagnitude(RquatTr)); + + // Flip y and z axies + FLT T2[4][4] = { + { Tortho[0][0], -Tortho[0][2], -Tortho[0][1], 0.0 }, + { Tortho[1][0], -Tortho[1][2], -Tortho[1][1], 0.0 }, + { Tortho[2][0], -Tortho[2][2], -Tortho[2][1], 0.0 }, + { 0.0, 0.0, 0.0, 1.0 } }; +PRINT(T2,4,4); + + // Print out the quaternions + FLT T2quat[4]; + quatfrommatrix(T2quat, &T2[0][0] ); + printf( "T2quat : %f %f %f %f = %f\n", T2quat [0], T2quat [1], T2quat [2], T2quat [3], quatmagnitude(T2quat)); + } + + // Run the calculation for Tcalc + //int run; + //for (run=0; run<100; run++) { +/* + // Initialize Tcalc to the identity matrix + memcpy(Tcalc, Tortho, 4*4*sizeof(float)); + //memset(Tcalc, 0, 4*4*sizeof(float)); + //for (i=0; i<4; i++) { Tcalc[i][i] = 1.0f; } + + // Solve it! + AffineSolve( + Tcalc, // OUTPUT: transform + O, // INPUT: points, offsets + N, // INPUT: plane normals + D, // INPUT: plane offsets + nPlanes, NITER, + STEP_SIZE_ROT, STEP_SIZE_POS, FALLOFF, + 1); +*/ + // insert code here... + return 0; +} + + + + + +#if 0 #define PRINT_MAT(A,M,N) { \ int m,n; \ printf(#A "\n"); \ @@ -433,308 +780,4 @@ PRINT(ab,2,1); // printf("xbar %f %f %f\n", xbar[0], xbar[1], xbar[2]); // printf("trans %f %f %f dist: %f\n", trans[0], trans[1], trans[2], transdist); } - -void AffineSolve( - float T[4][4], // OUTPUT: transform - float O[MAX_POINTS][4], // INPUT: points, offsets - float N[MAX_POINTS][3], // INPUT: plane normals - float D[MAX_POINTS], // INPUT: plane offsets - int nPoints, int nIter, - float stepSizeRot, float stepSizePos, float falloff, int constrain) -{ - int i,j,k,iter; - //T[3][3] = 1.0f; - - printf("iter x y z error\n"); - - float gradDot = 1.0; - float prevGradDot = 1.0; - float de_dT[3][4]; // the gradient - float conj[3][4]; // the conjugate - float errorSq=0.0; - for (iter=0; iter<nIter; iter++) - { - //---------------------------------- - // Calculate the gradient direction - //---------------------------------- - errorSq = 0.0; - memset(de_dT, 0, 3*4*sizeof(float)); - for (i=0; i<nPoints; i++) - { - // What is the plane deviation error - float Ei = -D[i]; - for (j=0; j<3; j++) { - float Tj_oi = 0.0f; - for (k=0; k<4; k++) { - Tj_oi += T[j][k] * O[i][k]; - } - Ei += N[i][j] * Tj_oi; - } -// printf("E[%d] %f\n", i, Ei); - - // Figure out contribution to the error - for (j=0; j<3; j++) { - for (k=0; k<4; k++) { - de_dT[j][k] += N[i][j] * O[i][k] * Ei; - } - } - - errorSq += Ei*Ei; - } - -// printf("%d %f %f %f %f\n", iter, T[0][3], T[1][3], T[2][3], sqrt(errorSq)); -//exit(1); - // Constrain the gradient (such that dot products are zero) - if (constrain) - { - float T0T1 = 0.0, T1T2 = 0.0, T2T0 = 0.0; - for (k=0; k<3; k++) { - T0T1 += T[0][k] * T[1][k]; - T1T2 += T[1][k] * T[2][k]; - T2T0 += T[2][k] * T[0][k]; - } -// printf("T0T1 %f T1T2 %f T2T0 %f\n", T0T1, T1T2, T2T0); - for (k=0; k<3; k++) { - de_dT[0][k] += ORTHOG_PENALTY * 2.0 * T0T1 * T[1][k]; - de_dT[0][k] += ORTHOG_PENALTY * 2.0 * T2T0 * T[2][k]; - de_dT[1][k] += ORTHOG_PENALTY * 2.0 * T1T2 * T[2][k]; - de_dT[1][k] += ORTHOG_PENALTY * 2.0 * T0T1 * T[0][k]; - de_dT[2][k] += ORTHOG_PENALTY * 2.0 * T1T2 * T[1][k]; - de_dT[2][k] += ORTHOG_PENALTY * 2.0 * T2T0 * T[0][k]; - } - } - - // Calculate the gradient dot product - // (used by conjugate gradient method) - prevGradDot = gradDot; - gradDot = 0.0; - for (j=0; j<3; j++) { - for (k=0; k<4; k++) { - gradDot += de_dT[j][k] * de_dT[j][k]; - } - } - -// printf("Iter %d error %f gradDot %f prevGradDot %f\n", iter, sqrt(errorSq), gradDot, prevGradDot); - - //---------------------------------- - // Calculate the conjugate direction - //---------------------------------- -// if (iter==0) { - // First iteration, just use the gradient - for (j=0; j<3; j++) { - for (k=0; k<4; k++) { - conj[j][k] = -de_dT[j][k]; - } - } -/* } else { - // Calculate "beta" for Fletcher Reeves method - float beta = gradDot / prevGradDot; -//printf("gradDot %f prevGradDot %f beta %f\n", gradDot, prevGradDot, beta); - - // Update the conjugate - for (j=0; j<3; j++) { - for (k=0; k<4; k++) { - conj[j][k] = beta*conj[j][k] - de_dT[j][k]; - } - } - } -*/ - -// PRINT_MAT(de_dT,4,4); -// exit(1); - - //---------------------------------- - // How large is the gradient ? - //---------------------------------- - - double gradSizeRot = 0.0; - double gradSizePos = 0.0; - for (j=0; j<3; j++) { - for (k=0; k<3; k++) { - gradSizeRot += _ABS(conj[j][k]); - } - gradSizePos += _ABS(conj[j][k]); - } - if (gradSizeRot <= TOO_SMALL && gradSizePos <= TOO_SMALL) { break; } // Quit, we've totally converged - - //---------------------------------- - // Descend in the gradient direction - //---------------------------------- - if (gradSizeRot > TOO_SMALL) { - float scaleRot = stepSizeRot / gradSizeRot; - for (j=0; j<3; j++) { - for (k=0; k<3; k++) { - T[j][k] += scaleRot * conj[j][k]; - } - } - stepSizeRot *= falloff; - } - - if (gradSizePos > TOO_SMALL) { - float scalePos = stepSizePos / gradSizePos; - for (j=0; j<3; j++) { - T[j][3] += scalePos * conj[j][3]; - } - stepSizePos *= falloff; - } - - // Constrain the gradient (such that scaling is one) - if (constrain) - { - // Measure the scales - float len[3] = {0.0, 0.0, 0.0}; - for (j=0; j<3; j++) { - double lenSq = 0.0; - for (k=0; k<3; k++) { lenSq += (double)T[j][k] * (double)T[j][k]; } - len[j] = sqrt(lenSq); - } - - // How far off is the scale? - float xzLen = 0.5 * (len[0] + len[2]); - if (xzLen > TOO_SMALL) { - float inv_xzLen = 1.0 / xzLen; - for (j=0; j<3; j++) { - T[3][j] *= inv_xzLen; - } - } - - // Rescale the thing - for (j=0; j<3; j++) - { - if (len[j] > TOO_SMALL) { - float inv_len = 1.0 / len[j]; - for (k=0; k<3; k++) { T[j][k] *= inv_len; } - } - } - } - } - float dist = sqrt(T[0][3]*T[0][3] + T[1][3]*T[1][3] + T[2][3]*T[2][3]); - printf("AffineSolve: pos: %f %f %f dist: %f\n", T[0][3], T[1][3], T[2][3], dist); -} - -int main() -{ - int i,j,k,sen,axis; - - // Read the data files - ReadHmdPoints(); - ReadPtinfo(); - - //------------------------- - // Package the lighthouse data for "AffineSolve" - //------------------------- - - // Data for the "iterative" affine solve formula - float Tcalc[4][4]; - float O[MAX_POINTS][4]; - float N[MAX_POINTS][3]; - float D[MAX_POINTS]; - int nPlanes = 0; - - for (sen=0; sen<NUM_HMD; sen++) - { - for (axis=0; axis<2; axis++) - { - if (hmd_angle[sen][axis] != -9999.0) - { - // Set the offset - O[nPlanes][0] = hmd_pos[sen][0]; - O[nPlanes][1] = hmd_pos[sen][1]; - O[nPlanes][2] = hmd_pos[sen][2]; - O[nPlanes][3] = 1.0; - - // Calculate the plane equation - if (axis == 0) { // Horizontal - N[nPlanes][0] = -cos(hmd_angle[sen][axis]); - N[nPlanes][1] = -sin(hmd_angle[sen][axis]); - N[nPlanes][2] = 0.0; - D[nPlanes] = 0.0; - } else { // Vertical - N[nPlanes][0] = 0.0; - N[nPlanes][1] = -sin(hmd_angle[sen][axis]); - N[nPlanes][2] = cos(hmd_angle[sen][axis]); - D[nPlanes] = 0.0; - } - - printf("plane %d O %.3f %.3f %.3f %.3f N %.3f %.3f %.3f D %.3f\n", - nPlanes, - O[nPlanes][0], O[nPlanes][1], O[nPlanes][2], O[nPlanes][3], - N[nPlanes][0], N[nPlanes][1], N[nPlanes][2], - D[nPlanes]); - nPlanes++; - } - } - } - - - printf("nPlanes %d\n", nPlanes); - - //} - - PRINT_MAT(Tcalc,4,4); - - - //-------------------------------------------------- - // Package the data for "OrthoSolve" - //-------------------------------------------------- - - // Data for the "fake" ortho solve formula - float Tortho[4][4]; // OUTPUT: 4x4 transformation matrix - FLOAT S_out[2][MAX_POINTS]; // INPUT: array of screenspace points - FLOAT S_in[2][MAX_POINTS]; // INPUT: array of screenspace points - FLOAT X_in[3][MAX_POINTS]; // INPUT: array of offsets - int nPoints=0; - - // Transform into the "OrthoSolve" format - for (sen=0; sen<NUM_HMD; sen++) - { - if (hmd_angle[sen][0] != -9999.0 && hmd_angle[sen][1] != -9999.0) - { - S_in[0][nPoints] = hmd_angle[sen][0]; - S_in[1][nPoints] = hmd_angle[sen][1]; - X_in[0][nPoints] = hmd_pos[sen][0]; - X_in[1][nPoints] = hmd_pos[sen][1]; - X_in[2][nPoints] = hmd_pos[sen][2]; - nPoints++; - } - } - printf("OrthoSolve nPoints %d\n", nPoints); - - //-------------------------------------------------- - // Run the "OrthoSolve" and then the "AffineSolve" - //-------------------------------------------------- - - int loop; - for (loop=0; loop<1; loop++) - { - // Run OrthoSolve - OrthoSolve( - Tortho, // OUTPUT: 4x4 transformation matrix - S_out, // OUTPUT: array of output screenspace points - S_in, // INPUT: array of screenspace points - X_in, // INPUT: array of offsets - nPoints); - } - - // Run the calculation for Tcalc - //int run; - //for (run=0; run<100; run++) { -/* - // Initialize Tcalc to the identity matrix - memcpy(Tcalc, Tortho, 4*4*sizeof(float)); - //memset(Tcalc, 0, 4*4*sizeof(float)); - //for (i=0; i<4; i++) { Tcalc[i][i] = 1.0f; } - - // Solve it! - AffineSolve( - Tcalc, // OUTPUT: transform - O, // INPUT: points, offsets - N, // INPUT: plane normals - D, // INPUT: plane offsets - nPlanes, NITER, - STEP_SIZE_ROT, STEP_SIZE_POS, FALLOFF, - 1); -*/ - // insert code here... - return 0; -} +#endif diff --git a/dave/Makefile b/dave/Makefile index bf62837..330bda3 100644 --- a/dave/Makefile +++ b/dave/Makefile @@ -1,7 +1,21 @@ +UNAME:=$(shell uname) + +CFLAGS:= -lm -I../redist -I../src -I../include/libsurvive + +ifeq ($(UNAME), Linux) +CFLAGS:= $(CFLAGS) -lGL -lGLU -lglut -lX11 -I../../redist -DLINUX -lm -lpthread -DLINUX +endif + +# Darwin is Mac OSX !! +ifeq ($(UNAME), Darwin) +CFLAGS:= $(CFLAGS) -I../../redist -w -framework OpenGL -framework GLUT +endif + + all: # gcc -O3 -o kalman_filter kalman_filter.c main.c - gcc -O3 -o dclapack_test dclapack_test.c -lm - gcc -O0 -g -o AffineSolve AffineSolve.c -lm -I../redist ../redist/linmath.c #-Wall - + gcc -O3 -o dclapack_test dclapack_test.c $(CFLAGS) + gcc -O3 -o AffineSolve AffineSolve.c $(CFLAGS) ../redist/linmath.c #-Wall + gcc -O3 -o OrthoPlot OrthoPlot.c fileutil.c ../redist/linmath.c ../redist/os_generic.c $(CFLAGS) clean: rm -f kalman_filter dclapack_test diff --git a/dave/OrthoPlot.c b/dave/OrthoPlot.c new file mode 100644 index 0000000..222828c --- /dev/null +++ b/dave/OrthoPlot.c @@ -0,0 +1,451 @@ +/* + When creating your project, uncheck OWL, + uncheck Class Library, select Static + instead of Dynamic and change the target + model to Console from GUI. + Also link glut.lib to your project once its done. + */ + +#include <stdio.h> // Standard Header For Most Programs +#include <stdlib.h> +#include <string.h> +#include <math.h> +#include "os_generic.h" +#include "linmath.h" +#include "fileutil.h" + +#ifdef __APPLE__ +#include <OpenGL/gl.h> // The GL Header File +#include <GLUT/glut.h> // The GL Utility Toolkit (Glut) Header +#else +#include <GL/gl.h> +#include <GL/glu.h> +#endif +#ifdef __linux__ +#include <GL/freeglut.h> +#endif + +#define RegisterDriver(a,b) +#include "poser_daveortho.c" + + +// Required to set up a window +#define WIDTH 1280 +#define HEIGHT 1280 +#define FULLSCREEN 0 +int keys[256]; // Regular keyboard keys +int sp_keys[256]; // Special keyboard keycodes (GLUT specific) + +#define LH_ID 0 +#define NUM_HMD 32 +#define INDIR "dave/full_test_triangle_on_floor/" +#define MAX_POINTS SENSORS_PER_OBJECT + +#define PI 3.1415926535897932386264 +#define MOVESPEED 1.0 +#define ROTSPEED 5.0 + +// View space +float posx=0.0f; +float posy=0.0f; +float posz=0.0f; +float rotx=0.0f; +float roty=0.0f; +float rotz=0.0f; + +// Data for the "fake" ortho solve formula +float Tortho[4][4]; // OUTPUT: 4x4 transformation matrix +FLOAT S_out[2][MAX_POINTS]; // INPUT: array of screenspace points +FLOAT S_in[2][MAX_POINTS]; // INPUT: array of screenspace points +FLOAT X_in[3][MAX_POINTS]; // INPUT: array of offsets +int nPoints=0; + +//-------------------------------------------------------------------- +// +//-------------------------------------------------------------------- + +void DrawGrid( + float minX, float maxX, + float minY, float maxY, + float minZ, float maxZ, + float stepX, float stepY, float stepZ); + +void DrawCoordinateSystem( + float x, float y, float z, + float qx, float qy, float qz, float qr); + + +float hmd_pos[NUM_HMD][3]; +void ReadHmdPoints() +{ + int i; + FILE *fin = fopen(INDIR "HMD_points.csv","r"); + if (fin==NULL) { + printf("ERROR: could not open " INDIR "HMD_points.csv for reading\n"); + exit(1); + } + + for (i=0; i<NUM_HMD; i++) { + fscanf(fin, "%f %f %f", &(hmd_pos[i][0]), &(hmd_pos[i][1]), &(hmd_pos[i][2])); + } + + fclose(fin); +} + +float hmd_angle[NUM_HMD][2]; +void ReadPtinfo() +{ + // Initialize to -9999 + int i; + for (i=0; i<NUM_HMD; i++) { hmd_angle[i][0]=-9999.0; hmd_angle[i][1]=-9999.0; } + + // Read ptinfo.csv + FILE *fin = fopen(INDIR "ptinfo.csv", "r"); + if (fin==NULL) { printf("ERROR: could not open ptinfo.csv for reading\n"); exit(1); } + while (!feof(fin)) + { + // Read the angle + int sen,lh,axis,count; + float angle, avglen, stddevang, stddevlen; + float max_outlier_length, max_outlier_angle; + int rt = fscanf( fin, "%d %d %d %d %f %f %f %f %f %f\n", + &sen, &lh, &axis, &count, + &angle, &avglen, &stddevang, &stddevlen, + &max_outlier_length, &max_outlier_angle); + if (rt != 10) { break; } + + // If it's valid, store in the result + if (lh == LH_ID && sen < NUM_HMD) { + hmd_angle[sen][axis] = angle; + } + } + fclose(fin); +} + + +//-------------------------------------------------------------------- +// +//-------------------------------------------------------------------- + +/* + * init() is called at program startup + */ +void init() +{ + int i,j,k,sen,axis; + + // Read the data files + ReadHmdPoints(); + ReadPtinfo(); + + //-------------------------------------------------- + // Package the data for "OrthoSolve" + //-------------------------------------------------- + + // Transform into the "OrthoSolve" format + for (sen=0; sen<NUM_HMD; sen++) + { + if (hmd_angle[sen][0] != -9999.0 && hmd_angle[sen][1] != -9999.0) + { + S_in[0][nPoints] = hmd_angle[sen][0]; + S_in[1][nPoints] = hmd_angle[sen][1]; + X_in[0][nPoints] = hmd_pos[sen][0]; + X_in[1][nPoints] = hmd_pos[sen][1]; + X_in[2][nPoints] = hmd_pos[sen][2]; + nPoints++; + } + } + printf("OrthoSolve nPoints %d\n", nPoints); + //-------------------------------------------------- + // Run the "OrthoSolve" and then the "AffineSolve" + //-------------------------------------------------- + + // Run OrthoSolve + OrthoSolve( + Tortho, // OUTPUT: 4x4 transformation matrix + S_out, // OUTPUT: array of output screenspace points + S_in, // INPUT: array of screenspace points + X_in, // INPUT: array of offsets + nPoints); + //printf( "POS: %f %f %f\n", Tortho[0][3], Tortho[1][3], Tortho[2][3]); + + //-------------------------------------------------- + // Spawn a thread to read the HMD angles + //-------------------------------------------------- + OGCreateThread(ThreadReadHmtAngles,0); + + //-------------------------------------------------- + // Initialize OpenGL + //-------------------------------------------------- + glShadeModel(GL_SMOOTH); // Enable Smooth Shading + glClearColor(0.0f, 0.0f, 0.0f, 1.0f); // Black Background + glClearDepth(1.0f); // Depth Buffer Setup + glEnable(GL_DEPTH_TEST); // Enables Depth Testing + glDepthFunc(GL_LEQUAL); // The Type Of Depth Testing To Do + glEnable ( GL_COLOR_MATERIAL ); + glDisable(GL_CULL_FACE); + glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST); +} + +/* + * draw() is called once every frame + */ +void draw() +{ + int i,j; + + //------------------------ + // Check for keyboard input + //------------------------ + if (keys['w'] || keys['W']) { + posz += MOVESPEED; + } + if (keys['s'] || keys['S']) { + posz -= MOVESPEED; + } + if (keys['a'] || keys['A']) { + roty += ROTSPEED; + } + if (keys['d'] || keys['D']) { + roty -= ROTSPEED; + } + if (keys[27]) { + exit(0); + } + + //------------------------ + // Update the scene + //------------------------ + + //------------------------ + // Draw using OpenGL + //------------------------ + + // Clear the screen + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); // Clear Screen And Depth Buffer + + // Translate and rotate the camera + glLoadIdentity(); // Reset The Current Modelview Matrix +// glTranslatef(-posx,-posy,posz); // Move Left 1.5 Units And Into The + + // Bouning box around the points (in radians) + float x0=-45.0 * (PI/180.0); + float x1= 45.0 * (PI/180.0); + float y0=-45.0 * (PI/180.0); + float y1= 45.0 * (PI/180.0); + + + //------------------------ + // Read the angles from stdin + //------------------------ + FLT hmdAngles[4][NUM_HMD]; + + // Read the hmd angles + OGLockMutex(read_mutex); + for (i=0; i<4; i++) { + for (j=0; j<NUM_HMD; j++) { + hmdAngles[i][j] = read_hmdAngles[i][j]; + } + } + OGUnlockMutex(read_mutex); + + // Draw the hmd bearing angles + nPoints=0; + for (i=0; i<NUM_HMD; i++) { + const double dist=10.0; + + int sweepx = (LH_ID==0) ? SWEEP_LX : SWEEP_RX; + int sweepy = (LH_ID==0) ? SWEEP_LY : SWEEP_RY; + + // If the left lighthouse sees it + if (read_hmdAngleViewed[sweepx][i] >= read_frameno-6 && + read_hmdAngleViewed[sweepy][i] >= read_frameno-6 && + hmdAngles[sweepx][i]!=-9999.0 && hmdAngles[sweepy][i]!=-9999.0) + { + S_in[0][nPoints] = hmdAngles[sweepy][i]; + S_in[1][nPoints] = hmdAngles[sweepx][i]; + X_in[0][nPoints] = hmd_pos[i][0]; + X_in[1][nPoints] = hmd_pos[i][1]; + X_in[2][nPoints] = hmd_pos[i][2]; +printf("i %d S %f %f X %f %f %f frno %d %d currfr %d\n", + i, S_in[0][nPoints], S_in[1][nPoints], + X_in[0][nPoints], X_in[1][nPoints], X_in[2][nPoints], + read_hmdAngleViewed[sweepx][i], read_hmdAngleViewed[sweepy][i], read_frameno); + nPoints++; + } + } + + read_frameno++; + + //-------------------------------------------------- + // Run the "OrthoSolve" and then the "AffineSolve" + //-------------------------------------------------- + + // Run OrthoSolve + OrthoSolve( + Tortho, // OUTPUT: 4x4 transformation matrix + S_out, // OUTPUT: array of output screenspace points + S_in, // INPUT: array of screenspace points + X_in, // INPUT: array of offsets + nPoints); + printf( "POS: %f %f %f\n", Tortho[0][3], Tortho[1][3], Tortho[2][3]); + + + //------------------------ + // Draw the inputs + //------------------------ + glPointSize(3.0); + + // Draw the input points + glColor3f(1.0,0.5,0.5); + glBegin(GL_POINTS); + for (i=0; i<nPoints; i++) { + glVertex2f( (S_in[0][i]-x0)/(x1-x0), (S_in[1][i]-y0)/(y1-y0) ); + } + glEnd(); + + // Draw the output points + glColor3f(0.5,0.5,1.0); + glBegin(GL_POINTS); + for (i=0; i<nPoints; i++) { + glVertex2f( (S_out[0][i]-x0)/(x1-x0), (S_out[1][i]-y0)/(y1-y0) ); + } + glEnd(); + + + // + // We're Done + // + glutSwapBuffers ( ); // Swap The Buffers To Not Be Left With A Clear Screen +} + +/* + * resize() is called when we change the screen size + */ +void resize(int width, int height) // Resize And Initialize The GL Window +{ + glViewport(0,0,width,height); // Reset The Current Viewport + + glMatrixMode(GL_PROJECTION); // Select The Projection Matrix + glLoadIdentity(); // Reset The Projection Matrix + + // Uncomment For a 3D perspective + //gluPerspective(45.0f,(float)width/(float)height,0.1f,1000.0f); + // Uncomment for a 2D perspective + glOrtho(0.0, 1.0, 0.0, 1.0, -10.0, 10.0); + + glMatrixMode(GL_MODELVIEW); // Select The Modelview Matrix + glLoadIdentity(); // Reset The Modelview Matrix +} + +/* + * These functions are called whenever a key is pressed + */ +void keyboardDown ( unsigned char key, int x, int y ) // Create Keyboard Function +{ + keys[key] = 1; +} +void keyboardUp ( unsigned char key, int x, int y ) +{ + keys[key] = 0; +} + +void specialKeyDown ( int key, int x, int y ) // Create Special Function (required for arrow keys) +{ + if (key<256) { + sp_keys[key] = 1; + } +} +void specialKeyUp (int key, int x, int y) +{ + if (key<256) { + sp_keys[key] = 0; + } +} + +int main ( int argc, char** argv ) // Create Main Function For Bringing It All Together +{ + glutInit ( &argc, argv ); // Erm Just Write It =) + glutInitDisplayMode ( GLUT_RGB | GLUT_DEPTH | GLUT_DOUBLE ); // Display Mode + glutInitWindowSize ( WIDTH, HEIGHT ); // If glutFullScreen wasn't called this is the window size + glutCreateWindow ( "OpenGL" ); // Window Title (argv[0] for current directory as title) + if (FULLSCREEN) { + glutFullScreen ( ); // Put Into Full Screen + } + glutDisplayFunc ( draw ); // Matching Earlier Functions To Their Counterparts + glutIdleFunc ( draw ); + glutReshapeFunc ( resize ); + glutKeyboardFunc ( keyboardDown ); + glutKeyboardUpFunc ( keyboardUp ); + glutSpecialFunc ( specialKeyDown ); + glutSpecialUpFunc ( specialKeyUp ); + init (); + glutMainLoop ( ); // Initialize The Main Loop + return 0; +} + + + +void DrawGrid( + float minX, float maxX, + float minY, float maxY, + float minZ, float maxZ, + float stepX, float stepY, float stepZ) +{ + float x,y,z; + + glBegin(GL_LINES); + + // X grid stripes + for (y=minY; y<maxY; y+=stepY) { + for (z=minZ; z<maxZ; z+=stepZ) { + glVertex3f(minX, y, z); + glVertex3f(maxX, y, z); + } + } + + // Y grid stripes + for (x=minX; x<maxX; x+=stepX) { + for (z=minZ; z<maxZ; z+=stepZ) { + glVertex3f(x, minY, z); + glVertex3f(x, maxY, z); + } + } + + // Z grid stripes + for (y=minY; y<maxY; y+=stepY) { + for (x=minX; x<maxX; x+=stepX) { + glVertex3f(x, y, minZ); + glVertex3f(x, y, maxZ); + } + } + + glEnd(); +} + + +void DrawCoordinateSystem( + float x, float y, float z, + float qx, float qy, float qz, float qr) +{ + FLT i0[3],j0[3],k0[3]; + FLT i[3],j[3],k[3]; + FLT q[4]; + + i0[0]=1.0; i0[1]=0.0; i0[2]=0.0; + j0[0]=0.0; j0[1]=1.0; j0[2]=0.0; + k0[0]=0.0; k0[1]=0.0; k0[2]=1.0; + q [0]=qr; q [1]=qx; q [2]=qy; q [3]=qz; + + quatrotatevector(i, q, i0); + quatrotatevector(j, q, j0); + quatrotatevector(k, q, k0); + + glBegin(GL_LINES); + glColor3f(1, 0, 0); glVertex3f(x,z,y); glVertex3f(x+i[0],z+i[2],y+i[1]); + glColor3f(0, 1, 0); glVertex3f(x,z,y); glVertex3f(x+j[0],z+j[2],y+j[1]); + glColor3f(0, 0, 1); glVertex3f(x,z,y); glVertex3f(x+k[0],z+k[2],y+k[1]); + glEnd(); +} + + diff --git a/dave/dclapack_test.c b/dave/dclapack_test.c index b0667ef..3f48b08 100644 --- a/dave/dclapack_test.c +++ b/dave/dclapack_test.c @@ -1,6 +1,6 @@ #define FLOAT float #define ORDER 50 -#include "dclapack.h" +#include "../redist/dclapack.h" #include <stdio.h> #include <stdlib.h> diff --git a/dave/fileutil.c b/dave/fileutil.c new file mode 100644 index 0000000..04dc241 --- /dev/null +++ b/dave/fileutil.c @@ -0,0 +1,133 @@ +#include "fileutil.h" +#include <stdio.h> +#include <stdlib.h> + +#define PI 3.14159265358979323846264 + + +og_mutex_t read_mutex; +og_thread_t read_thread; +double read_hmdAngles[NUM_SWEEP][NUM_HMD]; +int read_hmdAngleViewed[NUM_SWEEP][NUM_HMD]; +int read_frameno=0; + +static FILE *fopen_orDie(const char *path, const char *flag) +{ + FILE *f = fopen(path, flag); + if (f == NULL) { + printf("ERROR could not oepn %s for %s\n", path, flag); + exit(1); + } + return f; +} + +static void SeekToken(FILE *f, const char *keyword) +{ + char token[4096]; + do { + fscanf(f, "%s", token); + } while( strcmp(token,keyword)!=0 && !feof(f) ); +} + +void LoadLighthousePos( + const char *path, + float *x, float *y, float *z, + float *qi, float *qj, float *qk, float *qreal) +{ + FILE *f = fopen_orDie(path,"r"); + SeekToken(f, "POS:"); + fscanf(f, "%f %f %f\n", x, y, z); + SeekToken(f, "QUAT:"); + fscanf(f, "%f %f %f %f\n", qreal, qi, qj, qk); + fclose (f); +} + +void LoadHmdProcessedDataAngles( + const char *path, + double angles[NUM_SWEEP][NUM_HMD]) +{ + int i,j; + char type[256]; + char sweep[256]; + int id; + int nSweep; + double ang; + double d1,d2,d3; // revisit these numbers later + + // Initialize all of the angles to -9999 + for (i=0; i<NUM_SWEEP; i++) { + for (j=0; j<NUM_HMD; j++) { + angles[i][j] = -9999.0; // Initially no value + } + } + + FILE *f = fopen_orDie(path, "r"); + + while (!feof(f)) + { + // Read the line from the file + int rt=fscanf(f, "%s %s %d %d %lf %lf %lf %lf", + &type, &sweep, &id, &nSweep, &ang, + &d1,&d2,&d3); + + if (rt<8) { break; } + + // Only hmd points + if ( strcmp(type,"HMD")!=0 ) { continue; } + + // Which sweep is it? + int sweepId=-1; + if ( strcmp(sweep,"LX")==0 ) { sweepId=SWEEP_LX; } + else if ( strcmp(sweep,"LY")==0 ) { sweepId=SWEEP_LY; } + else if ( strcmp(sweep,"RX")==0 ) { sweepId=SWEEP_RX; } + else if ( strcmp(sweep,"RY")==0 ) { sweepId=SWEEP_RY; } + else { continue; } + + // Convert the angle from ticks to radians + angles[sweepId][id] = (PI / 400000.0) * ( ang-200000.0 ); + } + + fclose(f); +} + +void *ThreadReadHmtAngles(void *junk) +{ + char house[256]; + char xy[256]; + char hmd[256]; + double ts; + int id; + int syncid; + int timeInSweep; + int length; + //lighthouse sweep hmdOrwmd timestamp id syncid timeinsweep length + + while(1) { + int rt=scanf("%s %s %s %lf %d %d %d %d", house, xy, hmd, &ts, &id, &syncid, &timeInSweep, &length); + if (rt==8) + { + //int rt=scanf("%s %s %s %lf %d %d %d %d", house, xy, hmd, &ts, &id, &syncid, &timeInSweep, &length); + //printf( "%s %s %s %f %d %d %d %d\n", house, xy, hmd, ts,id, syncid, timeInSweep, length ); + + if( id < 0 ) continue; + int sweepId=0; + if ( house[0]=='R' ) { sweepId+=2; } + if ( xy[0] =='Y' ) { sweepId++; } + double angle = (PI / 400000.0) * ( (double)timeInSweep-200000.0 ); + + if ( strcmp(hmd,"HMD")==0 ) { id += 0; } + else if ( strcmp(hmd,"WM0")==0 ) { id += 32; } + else if ( strcmp(hmd,"WM1")==0 ) { id += 56; } + else { continue; } + + if ( id<0 || id >NUM_HMD) { continue; } + + OGLockMutex (read_mutex); + read_hmdAngles[sweepId][id] = angle; + OGUnlockMutex(read_mutex); + read_hmdAngleViewed[sweepId][id] = read_frameno; + } + } +} + + diff --git a/dave/fileutil.h b/dave/fileutil.h new file mode 100644 index 0000000..e5da244 --- /dev/null +++ b/dave/fileutil.h @@ -0,0 +1,35 @@ +#ifndef _fileutil_h_ +#define _fileutil_h_ + +#include <pthread.h> +#include "os_generic.h" + +void LoadLighthousePos( + const char *path, + float *x, float *y, float *z, + float *qi, float *qj, float *qk, float *qreal); + + +// first 32 are hmd, next 24 wm0 next 24 wm1 +#define NUM_HMD 80 +#define NUM_SWEEP 4 +#define SWEEP_LX 0 +#define SWEEP_LY 1 +#define SWEEP_RX 2 +#define SWEEP_RY 3 +void LoadHmdProcessedDataAngles( + const char *path, + double angle[NUM_SWEEP][NUM_HMD]); + + +extern og_mutex_t read_mutex; +extern og_thread_t read_thread; +extern double read_hmdAngles[NUM_SWEEP][NUM_HMD]; +extern int read_hmdAngleViewed[NUM_SWEEP][NUM_HMD]; +extern int read_frameno; +void *ThreadReadHmtAngles(void *junk); + + +#endif // __fileutil_h_ + + diff --git a/include/libsurvive/poser.h b/include/libsurvive/poser.h index 98c926e..582590e 100644 --- a/include/libsurvive/poser.h +++ b/include/libsurvive/poser.h @@ -3,6 +3,11 @@ #include "survive_types.h" + +#ifdef __cplusplus +extern "C" { +#endif + typedef enum PoserType_t { POSERDATA_NONE = 0, @@ -32,7 +37,8 @@ typedef struct { PoserType pt; int sensor_id; - int acode; //OOTX Code associated with this sweep. base_station = acode >> 2; axis = acode & 1; + int acode; //OOTX Code associated with this sweep. bit 1 indicates vertical(1) or horizontal(0) sweep + int lh; //Lighthouse making this sweep uint32_t timecode; //In object-local ticks. FLT length; //In seconds FLT angle; //In radians from center of lighthouse. @@ -45,6 +51,7 @@ typedef struct //If "lengths[...]" < 0, means not a valid piece of sweep information. FLT lengths[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; FLT angles [SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; //2 Axes (Angles in LH space) + FLT synctimes[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES]; PoserDataIMU lastimu; } PoserDataFullScene; @@ -53,4 +60,8 @@ typedef struct typedef int (*PoserCB)( SurviveObject * so, PoserData * pd ); +#ifdef __cplusplus +}; +#endif + #endif diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index e04586c..1532d62 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -5,6 +5,10 @@ #include "survive_types.h" #include "poser.h" +#ifdef __cplusplus +extern "C" { +#endif + //DANGER: This structure may be redefined. Note that it is logically split into 64-bit chunks //for optimization on 32- and 64-bit systems. @@ -32,13 +36,13 @@ struct SurviveObject PoserCB PoserFn; //Device-specific information about the location of the sensors. This data will be used by the poser. - int8_t nr_locations; - FLT * sensor_locations; - FLT * sensor_normals; + int8_t nr_locations; // sensor count + FLT * sensor_locations; // size is nr_locations*3. Contains x,y,z values for each sensor + FLT * sensor_normals;// size is nrlocations*3. cointains normal vector for each sensor //Timing sensitive data (mostly for disambiguation) int32_t timebase_hz; //48,000,000 for normal vive hardware. (checked) - int32_t timecenter_ticks; //200,000 for normal vive hardware. (checked) + int32_t timecenter_ticks; //200,000 for normal vive hardware. (checked) (This doubles-up as 2x this = full sweep length) int32_t pulsedist_max_ticks; //500,000 for normal vive hardware. (guessed) int32_t pulselength_min_sync; //2,200 for normal vive hardware. (guessed) int32_t pulse_in_clear_time; //35,000 for normal vive hardware. (guessed) @@ -47,6 +51,7 @@ struct SurviveObject int32_t pulse_synctime_slack; //5,000 for normal vive hardware. (guessed) //Flood info, for calculating which laser is currently sweeping. + void * disambiguator_data; int8_t oldcode; int8_t sync_set_number; //0 = master, 1 = slave, -1 = fault. int8_t did_handle_ootx; //If unset, will send lightcap data for sync pulses next time a sensor is hit. @@ -57,6 +62,12 @@ struct SurviveObject uint32_t last_lighttime; //May be a 24- or 32- bit number depending on what device. + FLT* acc_bias; // size is FLT*3. contains x,y,z + FLT* acc_scale; // size is FLT*3. contains x,y,z + FLT* gyro_bias; // size is FLT*3. contains x,y,z + FLT* gyro_scale; // size is FLT*3. contains x,y,z + + //Debug int tsl; }; @@ -115,7 +126,7 @@ void survive_install_imu_fn( SurviveContext * ctx, imu_process_func fbp ); void survive_install_angle_fn( SurviveContext * ctx, angle_process_func fbp ); void survive_close( SurviveContext * ctx ); -int survive_poll(); +int survive_poll( SurviveContext * ctx ); SurviveObject * survive_get_so_by_name( SurviveContext * ctx, const char * name ); @@ -127,11 +138,14 @@ int survive_send_magic( SurviveContext * ctx, int magic_code, void * data, int d //Install the calibrator. void survive_cal_install( SurviveContext * ctx ); //XXX This will be removed if not already done so. +// Read back a human-readable string description of the calibration status +int survive_cal_get_status( struct SurviveContext * ctx, char * description, int description_length ); + //Call these from your callback if overridden. //Accept higher-level data. -void survive_default_light_process( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ); +void survive_default_light_process( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length , uint32_t lh); void survive_default_imu_process( SurviveObject * so, int mode, FLT * accelgyro, uint32_t timecode, int id ); -void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle ); +void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh ); ////////////////////// Survive Drivers //////////////////////////// @@ -173,5 +187,9 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ); #define SV_ERROR( ... ) { char stbuff[1024]; sprintf( stbuff, __VA_ARGS__ ); ctx->faultfunction( ctx, stbuff ); } #define SV_KILL() exit(0) //XXX This should likely be re-defined. +#ifdef __cplusplus +}; +#endif + #endif diff --git a/include/libsurvive/survive_types.h b/include/libsurvive/survive_types.h index 1600e11..224719e 100644 --- a/include/libsurvive/survive_types.h +++ b/include/libsurvive/survive_types.h @@ -1,6 +1,11 @@ #ifndef _SURVIVE_TYPES_H #define _SURVIVE_TYPES_H +#ifdef __cplusplus +extern "C" { +#endif + + #ifndef FLT #ifdef USE_DOUBLE #define FLT double @@ -28,9 +33,9 @@ typedef struct BaseStationData BaseStationData; typedef struct SurviveCalData SurviveCalData; //XXX Warning: This may be removed. Check at a later time for its defunctness. typedef void (*text_feedback_func)( SurviveContext * ctx, const char * fault ); -typedef void (*light_process_func)( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ); +typedef void (*light_process_func)( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lighthouse); 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 ); +typedef void (*angle_process_func)( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh); //Device drivers (prefix your drivers with "DriverReg") i.e. @@ -39,6 +44,9 @@ typedef int (*DeviceDriver)( SurviveContext * ctx ); typedef int (*DeviceDriverCb)( struct SurviveContext * ctx, void * driver ); typedef int (*DeviceDriverMagicCb)( struct SurviveContext * ctx, void * driver, int magic_code, void * data, int datalen ); +#ifdef __cplusplus +}; +#endif #endif diff --git a/redist/DrawFunctions.c b/redist/CNFGFunctions.c index f4f27d2..25e9298 100644 --- a/redist/DrawFunctions.c +++ b/redist/CNFGFunctions.c @@ -21,14 +21,13 @@ ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -#include "DrawFunctions.h" +#include "CNFGFunctions.h" #include <stdio.h> int CNFGPenX, CNFGPenY; uint32_t CNFGBGColor; uint32_t CNFGLastColor; uint32_t CNFGDialogColor; //background for boxes - const unsigned short FontCharMap[256] = { 65535, 0, 10, 20, 32, 44, 56, 68, 70, 65535, 65535, 80, 92, 65535, 104, 114, 126, 132, 138, 148, 156, 166, 180, 188, 200, 206, 212, 218, 224, 228, 238, 244, @@ -68,7 +67,7 @@ const unsigned char FontCharData[1902] = { 0x23, 0x14, 0x14, 0x03, 0x10, 0x94, 0x00, 0x01, 0x23, 0x24, 0x04, 0x03, 0x03, 0x21, 0x21, 0xa0, 0x21, 0x10, 0x10, 0x01, 0x01, 0x12, 0x12, 0x03, 0x03, 0x14, 0x14, 0x23, 0x02, 0xa4, 0x10, 0x91, 0x10, 0x01, 0x01, 0x03, 0x03, 0x94, 0x10, 0x21, 0x21, 0x23, 0x23, 0x94, 0x01, 0x23, 0x11, 0x13, - 0x21, 0x03, 0x02, 0xa2, 0x02, 0x22, 0x11, 0x93, 0x31, 0xc0, 0x03, 0xa1, 0x00, 0x20, 0x20, 0x24, + 0x21, 0x03, 0x02, 0xa2, 0x02, 0x22, 0x11, 0x93, 0x04, 0x93, 0x03, 0xa1, 0x00, 0x20, 0x20, 0x24, 0x24, 0x04, 0x04, 0x00, 0x12, 0x92, 0x01, 0x10, 0x10, 0x14, 0x04, 0xa4, 0x01, 0x10, 0x10, 0x21, 0x21, 0x22, 0x22, 0x04, 0x04, 0xa4, 0x00, 0x20, 0x20, 0x24, 0x24, 0x04, 0x12, 0xa2, 0x00, 0x02, 0x02, 0x22, 0x20, 0xa4, 0x20, 0x00, 0x00, 0x02, 0x02, 0x22, 0x22, 0x24, 0x24, 0x84, 0x20, 0x02, @@ -208,11 +207,7 @@ void CNFGDrawText( const char * text, int scale ) int x2 = (int)((((*(lmap+1)) & 0x70)>>4)*scale + iox); int y2 = (int)(((*(lmap+1)) & 0x0f)*scale + ioy); lmap++; - if(x1 == x2 && y1 == y2){ - CNFGTackPixel( x1, y1 ); - } else { - CNFGTackSegment( x1, y1, x2, y2 ); - } + CNFGTackSegment( x1, y1, x2, y2 ); bQuit = *lmap & 0x80; lmap++; } while( !bQuit ); @@ -275,3 +270,89 @@ void CNFGDrawTextbox( int x, int y, const char * text, int textsize ) CNFGPenY = y + textsize; CNFGDrawText( text, textsize ); } + + +#ifdef CNFGOGL + +#ifdef _MSC_VER +#include <windows.h> +#pragma comment( lib, "OpenGL32.lib" ) +#endif +#include <GL/gl.h> + +uint32_t CNFGColor( uint32_t RGB ) +{ + unsigned char red = RGB & 0xFF; + unsigned char grn = ( RGB >> 8 ) & 0xFF; + unsigned char blu = ( RGB >> 16 ) & 0xFF; + glColor3ub( red, grn, blu ); +} + +void CNFGClearFrame() +{ + short w, h; + unsigned char red = CNFGBGColor & 0xFF; + unsigned char grn = ( CNFGBGColor >> 8 ) & 0xFF; + unsigned char blu = ( CNFGBGColor >> 16 ) & 0xFF; + glClearColor( red/255.0, grn/255.0, blu/255.0, 1.0 ); + glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT ); + CNFGGetDimensions( &w, &h ); + glMatrixMode( GL_PROJECTION ); + glLoadIdentity(); + glViewport( 0, 0, w, h ); + glOrtho( 0, w, h, 0, 1, -1 ); + glMatrixMode( GL_MODELVIEW ); + glLoadIdentity(); +} + + +void CNFGTackSegment( short x1, short y1, short x2, short y2 ) +{ + if( x1 == x2 && y1 == y2 ) + { + glBegin( GL_POINTS ); + glVertex2f( x1+.5, y1+.5 ); + glEnd(); + } + else + { + glBegin( GL_LINES ); + glVertex2f( x1+.5, y1+.5 ); + glVertex2f( x2+.5, y2+.5 ); + glEnd(); + } +} + +void CNFGTackPixel( short x1, short y1 ) +{ + glBegin( GL_POINTS ); + glVertex2f( x1, y1 ); + glEnd(); +} + +void CNFGTackRectangle( short x1, short y1, short x2, short y2 ) +{ + glBegin( GL_QUADS ); + glVertex2f( x1, y1 ); + glVertex2f( x2, y1 ); + glVertex2f( x2, y2 ); + glVertex2f( x1, y2 ); + glEnd(); +} + +void CNFGTackPoly( RDPoint * points, int verts ) +{ + int i; + glBegin( GL_TRIANGLE_FAN ); + glVertex2f( points[0].x, points[0].y ); + for( i = 1; i < verts; i++ ) + { + glVertex2f( points[i].x, points[i].y ); + } + glEnd(); +} + +void CNFGInternalResize( short x, short y ) { } + + +#endif diff --git a/redist/DrawFunctions.h b/redist/CNFGFunctions.h index 542fcf9..179a20b 100644 --- a/redist/DrawFunctions.h +++ b/redist/CNFGFunctions.h @@ -1,4 +1,4 @@ -//Copyright (c) 2011 <>< Charles Lohr - Under the MIT/x11 or NewBSD License you choose. +//Copyright (c) 2011, 2017 <>< Charles Lohr - Under the MIT/x11 or NewBSD License you choose. #ifndef _DRAWFUCNTIONS_H #define _DRAWFUCNTIONS_H @@ -34,7 +34,6 @@ void CNFGClearFrame(); void CNFGSwapBuffers(); void CNFGGetDimensions( short * x, short * y ); -void CNFGTearDown(); void CNFGSetup( const char * WindowName, int w, int h ); void CNFGSetupFullscreen( const char * WindowName, int screen_number ); void CNFGHandleInput(); @@ -44,8 +43,18 @@ void CNFGHandleInput(); void HandleKey( int keycode, int bDown ); void HandleButton( int x, int y, int button, int bDown ); void HandleMotion( int x, int y, int mask ); +void HandleDestroy(); +//Internal function for resizing rasterizer for rasterizer-mode. +void CNFGInternalResize( short x, short y ); //don't call this. + +//Not available on all systems. Use The OGL portion with care. +#ifdef CNFGOGL +void CNFGSetVSync( int vson ); +void * CNFGGetExtension( const char * extname ); +#endif + #ifdef __cplusplus }; #endif diff --git a/redist/RawDrawNull.c b/redist/CNFGNullDriver.c index 34346cc..c35884a 100644 --- a/redist/RawDrawNull.c +++ b/redist/CNFGNullDriver.c @@ -1,6 +1,6 @@ //Copyright (c) 2017 <>< Charles Lohr - Under the MIT/x11 or NewBSD License you choose. -#include "DrawFunctions.h" +#include "CNFGFunctions.h" static int w, h; void CNFGGetDimensions( short * x, short * y ) diff --git a/redist/RawDrawRasterizer.c b/redist/CNFGRasterizer.h index af40588..1b8e2dd 100644 --- a/redist/RawDrawRasterizer.c +++ b/redist/CNFGRasterizer.h @@ -1,6 +1,7 @@ +//Don't call this file yourself. It is intended to be included in any drivers which want to support the rasterizer plugin. + #ifdef RASTERIZER -#include "DrawFunctions.h" -#include <stdio.h> +#include "CNFGFunctions.h" #include <stdlib.h> #include <stdint.h> @@ -8,6 +9,15 @@ static uint32_t * buffer = 0; static short bufferx; static short buffery; + +void CNFGInternalResize( short x, short y ) +{ + bufferx = x; + buffery = y; + if( buffer ) free( buffer ); + buffer = malloc( bufferx * buffery * 4 ); +} + static uint32_t SWAPS( uint32_t r ) { uint32_t ret = (r&0xFF)<<16; @@ -24,11 +34,6 @@ uint32_t CNFGColor( uint32_t RGB ) return CNFGLastColor; } -void CNFGTackPixel( short x, short y ) -{ - buffer[bufferx*y+x] = CNFGLastColor; -} - void CNFGTackSegment( short x1, short y1, short x2, short y2 ) { short tx, ty; @@ -53,11 +58,11 @@ void CNFGTackSegment( short x1, short y1, short x2, short y2 ) for( tx = minx; tx <= maxx; tx++ ) { - thisy += slope; ty = thisy; if( tx < 0 || ty < 0 || ty >= buffery ) continue; if( tx >= bufferx ) break; buffer[ty * bufferx + tx] = CNFGLastColor; + thisy += slope; } } else @@ -71,15 +76,14 @@ void CNFGTackSegment( short x1, short y1, short x2, short y2 ) for( ty = miny; ty <= maxy; ty++ ) { - thisx += slope; tx = thisx; if( ty < 0 || tx < 0 || tx >= bufferx ) continue; if( ty >= buffery ) break; buffer[ty * bufferx + tx] = CNFGLastColor; + thisx += slope; } } } - void CNFGTackRectangle( short x1, short y1, short x2, short y2 ) { short minx = (x1<x2)?x1:x2; @@ -216,7 +220,6 @@ void CNFGClearFrame() { bufferx = x; buffery = y; - printf( "MALLOCING: %d\n", x * y ); buffer = malloc( x * y * 8 ); } @@ -229,9 +232,15 @@ void CNFGClearFrame() } } +void CNFGTackPixel( short x, short y ) +{ + if( x < 0 || y < 0 || x >= bufferx || y >= buffery ) return; + buffer[x+bufferx*y] = CNFGLastColor; +} + void CNFGSwapBuffers() { - CNFGUpdateScreenWithBitmap( buffer, bufferx, buffery ); + CNFGUpdateScreenWithBitmap( (long unsigned int*)buffer, bufferx, buffery ); } diff --git a/redist/WinDriver.c b/redist/CNFGWinDriver.c index 3613150..b1c1eb0 100644 --- a/redist/WinDriver.c +++ b/redist/CNFGWinDriver.c @@ -2,80 +2,73 @@ //Portion from: http://en.wikibooks.org/wiki/Windows_Programming/Window_Creation -#include "DrawFunctions.h" +#include "CNFGFunctions.h" #include <windows.h> #include <stdlib.h> #include <malloc.h> //for alloca +static HBITMAP lsBitmap; static HINSTANCE lhInstance; static HWND lsHWND; -static HDC lsHDC; -static HBITMAP lsBackBitmap; static HDC lsWindowHDC; -static HBRUSH lsHBR; -static HPEN lsHPEN; -static HBRUSH lsClearBrush; -static unsigned int lsLastWidth; -static unsigned int lsLastHeight; - -static void InternalHandleResize() -{ - DeleteObject( lsBackBitmap ); - lsBackBitmap = CreateCompatibleBitmap( lsHDC, lsLastWidth, lsLastHeight ); - SelectObject( lsHDC, lsBackBitmap ); +static HDC lsHDC; -} +#ifdef RASTERIZER +#include "CNFGRasterizer.h" -uint32_t CNFGColor( uint32_t RGB ) +void InternalHandleResize() { - CNFGLastColor = RGB; - - DeleteObject( lsHBR ); - lsHBR = CreateSolidBrush( RGB ); - SelectObject( lsHDC, lsHBR ); + if( lsBitmap ) DeleteObject( lsBitmap ); - DeleteObject( lsHPEN ); - lsHPEN = CreatePen( PS_SOLID, 0, RGB ); - SelectObject( lsHDC, lsHPEN ); - - return RGB; + CNFGInternalResize( bufferx, buffery ); + lsBitmap = CreateBitmap( bufferx, buffery, 1, 32, buffer ); + SelectObject( lsHDC, lsBitmap ); } +#else +static int bufferx, buffery; +static int bufferx, buffery; +static void InternalHandleResize(); +#endif -void CNFGTackSegment( short x1, short y1, short x2, short y2 ) -{ - POINT pt[2] = { {x1, y1}, {x2, y2} }; - Polyline( lsHDC, pt, 2 ); - SetPixel( lsHDC, x1, y1, CNFGLastColor ); - SetPixel( lsHDC, x2, y2, CNFGLastColor ); -} -void CNFGTackRectangle( short x1, short y1, short x2, short y2 ) +#ifdef CNFGOGL +#include <GL/gl.h> +static HGLRC hRC=NULL; +static void InternalHandleResize() { } +void CNFGSwapBuffers() { - RECT r; - if( x1 < x2 ) { r.left = x1; r.right = x2; } - else { r.left = x2; r.right = x1; } - if( y1 < y2 ) { r.top = y1; r.bottom = y2; } - else { r.top = y2; r.bottom = y1; } - FillRect( lsHDC, &r, lsHBR ); + SwapBuffers(lsWindowHDC); } +#endif -void CNFGClearFrame() +void CNFGGetDimensions( short * x, short * y ) { - RECT r = { 0, 0, lsLastWidth, lsLastHeight }; - DeleteObject( lsClearBrush ); - lsClearBrush = CreateSolidBrush( CNFGBGColor ); - SelectObject( lsHDC, lsClearBrush ); - - FillRect( lsHDC, &r, lsClearBrush ); + static int lastx, lasty; + RECT window; + GetClientRect( lsHWND, &window ); + bufferx = ( window.right - window.left); + buffery = ( window.bottom - window.top); + if( bufferx != lastx || buffery != lasty ) + { + lastx = bufferx; + lasty = buffery; + InternalHandleResize(); + } + *x = bufferx; + *y = buffery; } -void CNFGSwapBuffers() + +void CNFGUpdateScreenWithBitmap( unsigned long * data, int w, int h ) { - int thisw, thish; RECT r; - BitBlt( lsWindowHDC, 0, 0, lsLastWidth, lsLastHeight, lsHDC, 0, 0, SRCCOPY ); + + int a = SetBitmapBits(lsBitmap,w*h*4,data); + a = BitBlt(lsWindowHDC, 0, 0, w, h, lsHDC, 0, 0, SRCCOPY); UpdateWindow( lsHWND ); + int thisw, thish; + //Check to see if the window is closed. if( !IsWindow( lsHWND ) ) { @@ -85,36 +78,18 @@ void CNFGSwapBuffers() GetClientRect( lsHWND, &r ); thisw = r.right - r.left; thish = r.bottom - r.top; - if( thisw != lsLastWidth || thish != lsLastHeight ) + if( thisw != bufferx || thish != buffery ) { - lsLastWidth = thisw; - lsLastHeight = thish; + bufferx = thisw; + buffery = thish; InternalHandleResize(); } } -void CNFGTackPoly( RDPoint * points, int verts ) -{ - int i; - POINT * t = (POINT*)alloca( sizeof( POINT ) * verts ); - for( i = 0; i < verts; i++ ) - { - t[i].x = points[i].x; - t[i].y = points[i].y; - } - Polygon( lsHDC, t, verts ); -} - - -void CNFGTackPixel( short x1, short y1 ) -{ - SetPixel( lsHDC, x1, y1, CNFGLastColor ); -} -void CNFGGetDimensions( short * x, short * y ) +void CNFGTearDown() { - *x = lsLastWidth; - *y = lsLastHeight; + PostQuitMessage(0); } //This was from the article @@ -123,17 +98,13 @@ LRESULT CALLBACK MyWndProc(HWND hwnd, UINT msg, WPARAM wParam, LPARAM lParam) switch(msg) { case WM_DESTROY: + HandleDestroy(); CNFGTearDown(); return 0; } return DefWindowProc(hwnd, msg, wParam, lParam); } -void CNFGTearDown() -{ - PostQuitMessage(0); -} - //This was from the article, too... well, mostly. void CNFGSetup( const char * name_of_window, int width, int height ) { @@ -143,8 +114,8 @@ void CNFGSetup( const char * name_of_window, int width, int height ) int w, h, wd, hd; HINSTANCE hInstance = GetModuleHandle(NULL); - lsLastWidth = width; - lsLastHeight = height; + bufferx = width; + buffery = height; wnd.style = CS_HREDRAW | CS_VREDRAW; //we will explain this later wnd.lpfnWndProc = MyWndProc; @@ -167,22 +138,63 @@ void CNFGSetup( const char * name_of_window, int width, int height ) WS_OVERLAPPEDWINDOW, //basic window style CW_USEDEFAULT, CW_USEDEFAULT, //set starting point to default value - lsLastWidth, - lsLastHeight, //set all the dimensions to default value + bufferx, + buffery, //set all the dimensions to default value NULL, //no parent window NULL, //no menu hInstance, NULL); //no parameters to pass - lsWindowHDC = GetDC( lsHWND ); + +#ifdef CNFGOGL + //From NeHe + static PIXELFORMATDESCRIPTOR pfd = + { + sizeof(PIXELFORMATDESCRIPTOR), + 1, + PFD_DRAW_TO_WINDOW | + PFD_SUPPORT_OPENGL | + PFD_DOUBLEBUFFER, + PFD_TYPE_RGBA, + 32, + 0, 0, 0, 0, 0, 0, + 0, + 0, + 0, + 0, 0, 0, 0, + 16, + 0, + 0, + PFD_MAIN_PLANE, + 0, + 0, 0, 0 + }; + GLuint PixelFormat = ChoosePixelFormat( lsWindowHDC, &pfd ); + if( !SetPixelFormat( lsWindowHDC, PixelFormat, &pfd ) ) + { + MessageBox( 0, "Could not create PFD for OpenGL Context\n", 0, 0 ); + exit( -1 ); + } + if (!(hRC=wglCreateContext(lsWindowHDC))) // Are We Able To Get A Rendering Context? + { + MessageBox( 0, "Could not create OpenGL Context\n", 0, 0 ); + exit( -1 ); + } + if(!wglMakeCurrent(lsWindowHDC,hRC)) // Try To Activate The Rendering Context + { + MessageBox( 0, "Could not current OpenGL Context\n", 0, 0 ); + exit( -1 ); + } +#endif + lsHDC = CreateCompatibleDC( lsWindowHDC ); - lsBackBitmap = CreateCompatibleBitmap( lsWindowHDC, lsLastWidth, lsLastHeight ); - SelectObject( lsHDC, lsBackBitmap ); + lsBitmap = CreateCompatibleBitmap( lsWindowHDC, bufferx, buffery ); + SelectObject( lsHDC, lsBitmap ); - lsClearBrush = CreateSolidBrush( CNFGBGColor ); - lsHBR = CreateSolidBrush( 0xFFFFFF ); - lsHPEN = CreatePen( PS_SOLID, 0, 0xFFFFFF ); + //lsClearBrush = CreateSolidBrush( CNFGBGColor ); + //lsHBR = CreateSolidBrush( 0xFFFFFF ); + //lsHPEN = CreatePen( PS_SOLID, 0, 0xFFFFFF ); ShowWindow(lsHWND, 1); //display the window on the screen @@ -193,7 +205,7 @@ void CNFGSetup( const char * name_of_window, int width, int height ) h = ( window.bottom - window.top); wd = w - client.right; hd = h - client.bottom; - MoveWindow( lsHWND, window.left, window.top, lsLastWidth + wd, lsLastHeight + hd, 1 ); + MoveWindow( lsHWND, window.left, window.top, bufferx + wd, buffery + hd, 1 ); InternalHandleResize(); } @@ -220,7 +232,7 @@ void CNFGHandleInput() case WM_MBUTTONUP: HandleButton( (msg.lParam & 0xFFFF), (msg.lParam>>16) & 0xFFFF, 3, 0 ); break; case WM_KEYDOWN: case WM_KEYUP: - HandleKey( tolower( (int)msg.wParam ), (msg.message==WM_KEYDOWN) ); + HandleKey( tolower( (int)(msg.wParam) ), (msg.message==WM_KEYDOWN) ); break; default: DispatchMessage(&msg); @@ -229,3 +241,110 @@ void CNFGHandleInput() } } +#ifndef CNFGOGL + +#ifndef RASTERIZER + +static HBITMAP lsBackBitmap; +static HDC lsWindowHDC; +static HBRUSH lsHBR; +static HPEN lsHPEN; +static HBRUSH lsClearBrush; + +static void InternalHandleResize() +{ + DeleteObject( lsBackBitmap ); + lsBackBitmap = CreateCompatibleBitmap( lsHDC, bufferx, buffery ); + SelectObject( lsHDC, lsBackBitmap ); +} + +uint32_t CNFGColor( uint32_t RGB ) +{ + CNFGLastColor = RGB; + + DeleteObject( lsHBR ); + lsHBR = CreateSolidBrush( RGB ); + SelectObject( lsHDC, lsHBR ); + + DeleteObject( lsHPEN ); + lsHPEN = CreatePen( PS_SOLID, 0, RGB ); + SelectObject( lsHDC, lsHPEN ); + + return RGB; +} + +void CNFGTackSegment( short x1, short y1, short x2, short y2 ) +{ + POINT pt[2] = { {x1, y1}, {x2, y2} }; + Polyline( lsHDC, pt, 2 ); + SetPixel( lsHDC, x1, y1, CNFGLastColor ); + SetPixel( lsHDC, x2, y2, CNFGLastColor ); +} + +void CNFGTackRectangle( short x1, short y1, short x2, short y2 ) +{ + RECT r; + if( x1 < x2 ) { r.left = x1; r.right = x2; } + else { r.left = x2; r.right = x1; } + if( y1 < y2 ) { r.top = y1; r.bottom = y2; } + else { r.top = y2; r.bottom = y1; } + FillRect( lsHDC, &r, lsHBR ); +} + +void CNFGClearFrame() +{ + RECT r = { 0, 0, bufferx, buffery }; + DeleteObject( lsClearBrush ); + lsClearBrush = CreateSolidBrush( CNFGBGColor ); + SelectObject( lsHDC, lsClearBrush ); + FillRect( lsHDC, &r, lsClearBrush); +} + +void CNFGTackPoly( RDPoint * points, int verts ) +{ + int i; + POINT * t = (POINT*)alloca( sizeof( POINT ) * verts ); + for( i = 0; i < verts; i++ ) + { + t[i].x = points[i].x; + t[i].y = points[i].y; + } + Polygon( lsHDC, t, verts ); +} + + +void CNFGTackPixel( short x1, short y1 ) +{ + SetPixel( lsHDC, x1, y1, CNFGLastColor ); +} + +void CNFGSwapBuffers() +{ + int thisw, thish; + + RECT r; + BitBlt( lsWindowHDC, 0, 0, bufferx, buffery, lsHDC, 0, 0, SRCCOPY ); + UpdateWindow( lsHWND ); + //Check to see if the window is closed. + if( !IsWindow( lsHWND ) ) + { + exit( 0 ); + } + + GetClientRect( lsHWND, &r ); + thisw = r.right - r.left; + thish = r.bottom - r.top; + + if( thisw != bufferx || thish != buffery ) + { + bufferx = thisw; + buffery = thish; + InternalHandleResize(); + } +} + +void CNFGInternalResize( short bufferx, short buffery ) { } +#endif + +#endif + diff --git a/redist/XDriver.c b/redist/CNFGXDriver.c index 507ca95..ebaed91 100644 --- a/redist/XDriver.c +++ b/redist/CNFGXDriver.c @@ -1,10 +1,10 @@ -//Copyright (c) 2011 <>< Charles Lohr - Under the MIT/x11 or NewBSD License you choose. +//Copyright (c) 2011, 2017 <>< Charles Lohr - Under the MIT/x11 or NewBSD License you choose. //portions from //http://www.xmission.com/~georgeps/documentation/tutorials/Xlib_Beginner.html //#define HAS_XINERAMA -#include "DrawFunctions.h" +#include "CNFGFunctions.h" #include <X11/Xlib.h> #include <X11/Xutil.h> @@ -25,12 +25,33 @@ Window CNFGWindow; Pixmap CNFGPixmap; GC CNFGGC; GC CNFGWindowGC; +Visual * CNFGVisual; + + +#ifdef CNFGOGL +#include <GL/glx.h> +#include <GL/glxext.h> + +GLXContext CNFGCtx; +void * CNFGGetExtension( const char * extname ) { return glXGetProcAddressARB((const GLubyte *) extname); } +#endif + int FullScreen = 0; void CNFGGetDimensions( short * x, short * y ) { + static int lastx; + static int lasty; + *x = CNFGWinAtt.width; *y = CNFGWinAtt.height; + + if( lastx != *x || lasty != *y ) + { + lastx = *x; + lasty = *y; + CNFGInternalResize( lastx, lasty ); + } } static void InternalLinkScreenAndGo( const char * WindowName ) @@ -76,7 +97,7 @@ void CNFGSetupFullscreen( const char * WindowName, int screen_no ) exit( 1 ); } - Visual * visual = DefaultVisual(CNFGDisplay, screen); + CNFGVisual = DefaultVisual(CNFGDisplay, screen); CNFGWinAtt.depth = DefaultDepth(CNFGDisplay, screen); if (XineramaQueryExtension(CNFGDisplay, &a, &b ) && @@ -107,7 +128,9 @@ void CNFGSetupFullscreen( const char * WindowName, int screen_no ) CNFGWindow = XCreateWindow(CNFGDisplay, XRootWindow(CNFGDisplay, screen), xpos, ypos, CNFGWinAtt.width, CNFGWinAtt.height, - 0, CNFGWinAtt.depth, InputOutput, visual, CWBorderPixel | CWEventMask | CWOverrideRedirect | CWSaveUnder, &setwinattr); + 0, CNFGWinAtt.depth, InputOutput, CNFGVisual, + CWBorderPixel | CWEventMask | CWOverrideRedirect | CWSaveUnder, + &setwinattr); XMapWindow(CNFGDisplay, CNFGWindow); XSetInputFocus( CNFGDisplay, CNFGWindow, RevertToParent, CurrentTime ); @@ -145,7 +168,31 @@ void CNFGSetup( const char * WindowName, int w, int h ) XGetWindowAttributes( CNFGDisplay, RootWindow(CNFGDisplay, 0), &CNFGWinAtt ); int depth = CNFGWinAtt.depth; - CNFGWindow = XCreateWindow(CNFGDisplay, RootWindow(CNFGDisplay, 0), 1, 1, w, h, 0, depth, InputOutput, CopyFromParent, 0, 0 ); + int screen = DefaultScreen(CNFGDisplay); + CNFGVisual = DefaultVisual(CNFGDisplay, screen); + +#ifdef CNFGOGL + int attribs[] = { GLX_RGBA, + GLX_DOUBLEBUFFER, + GLX_RED_SIZE, 1, + GLX_GREEN_SIZE, 1, + GLX_BLUE_SIZE, 1, + GLX_DEPTH_SIZE, 1, + None }; + XVisualInfo * vis = glXChooseVisual(CNFGDisplay, screen, attribs); + CNFGVisual = vis->visual; + depth = vis->depth; + CNFGCtx = glXCreateContext( CNFGDisplay, vis, NULL, True ); +#endif + + XSetWindowAttributes attr; + attr.background_pixel = 0; + attr.border_pixel = 0; + attr.colormap = XCreateColormap( CNFGDisplay, RootWindow(CNFGDisplay, 0), CNFGVisual, AllocNone); + attr.event_mask = StructureNotifyMask | ExposureMask | KeyPressMask; + int mask = CWBackPixel | CWBorderPixel | CWColormap | CWEventMask; + + CNFGWindow = XCreateWindow(CNFGDisplay, RootWindow(CNFGDisplay, 0), 1, 1, w, h, 0, depth, InputOutput, CNFGVisual, mask, &attr ); XMapWindow(CNFGDisplay, CNFGWindow); XFlush(CNFGDisplay); @@ -153,7 +200,12 @@ void CNFGSetup( const char * WindowName, int w, int h ) Atom WM_DELETE_WINDOW = XInternAtom( CNFGDisplay, "WM_DELETE_WINDOW", False ); XSetWMProtocols( CNFGDisplay, CNFGWindow, &WM_DELETE_WINDOW, 1 ); + XSelectInput( CNFGDisplay, CNFGWindow, KeyPressMask | KeyReleaseMask | ButtonPressMask | ButtonReleaseMask | ExposureMask | PointerMotionMask ); + +#ifdef CNFGOGL + glXMakeCurrent( CNFGDisplay, CNFGWindow, CNFGCtx ); +#endif } void CNFGHandleInput() @@ -216,7 +268,6 @@ void CNFGUpdateScreenWithBitmap( unsigned long * data, int w, int h ) if( !xi ) { int screen = DefaultScreen(CNFGDisplay); - Visual * visual = DefaultVisual(CNFGDisplay, screen); depth = DefaultDepth(CNFGDisplay, screen)/8; // xi = XCreateImage(CNFGDisplay, DefaultVisual( CNFGDisplay, DefaultScreen(CNFGDisplay) ), depth*8, ZPixmap, 0, (char*)data, w, h, 32, w*4 ); // lw = w; @@ -226,7 +277,7 @@ void CNFGUpdateScreenWithBitmap( unsigned long * data, int w, int h ) if( lw != w || lh != h ) { if( xi ) free( xi ); - xi = XCreateImage(CNFGDisplay, DefaultVisual( CNFGDisplay, DefaultScreen(CNFGDisplay) ), depth*8, ZPixmap, 0, (char*)data, w, h, 32, w*4 ); + xi = XCreateImage(CNFGDisplay, CNFGVisual, depth*8, ZPixmap, 0, (char*)data, w, h, 32, w*4 ); lw = w; lh = h; } @@ -237,7 +288,25 @@ void CNFGUpdateScreenWithBitmap( unsigned long * data, int w, int h ) } -#ifndef RASTERIZER +#ifdef CNFGOGL + +void CNFGSetVSync( int vson ) +{ + void (*glfn)( int ); + glfn = (void (*)( int ))CNFGGetExtension( "glXSwapIntervalMESA" ); if( glfn ) glfn( vson ); + glfn = (void (*)( int ))CNFGGetExtension( "glXSwapIntervalSGI" ); if( glfn ) glfn( vson ); + glfn = (void (*)( int ))CNFGGetExtension( "glXSwapIntervalEXT" ); if( glfn ) glfn( vson ); +} + +void CNFGSwapBuffers() +{ + glFlush(); + glFinish(); + glXSwapBuffers( CNFGDisplay, CNFGWindow ); +} +#endif + +#if !defined( RASTERIZER ) && !defined( CNFGOGL) uint32_t CNFGColor( uint32_t RGB ) @@ -286,5 +355,10 @@ void CNFGTackPoly( RDPoint * points, int verts ) XFillPolygon(CNFGDisplay, CNFGPixmap, CNFGGC, (XPoint *)points, 3, Convex, CoordModeOrigin ); } +void CNFGInternalResize( short x, short y ) { } + +#else +#include "CNFGRasterizer.h" #endif + diff --git a/redist/json_helpers.c b/redist/json_helpers.c index 0267932..af7ddda 100644 --- a/redist/json_helpers.c +++ b/redist/json_helpers.c @@ -50,44 +50,46 @@ void json_write_float_array(FILE* f, const char* tag, float* v, uint8_t count) { uint8_t i = 0; char * str1 = NULL; char * str2 = NULL; - asprintf(&str1,"\"%s\":[", tag); + if( asprintf(&str1,"\"%s\":[", tag) < 0 ) goto giveup; for (i=0;i<count;++i) { if ( (i+1) < count) { - asprintf(&str2, "%s\"%f\"", str1,v[i]); + if( asprintf(&str2, "%s\"%f\"", str1,v[i]) < 0 ) goto giveup; } else { - asprintf(&str2, "%s\"%f\",", str1,v[i]); + if( asprintf(&str2, "%s\"%f\",", str1,v[i]) < 0 ) goto giveup; } free(str1); str1=str2; str2=NULL; } - asprintf(&str2, "%s]", str1); + if( asprintf(&str2, "%s]", str1) < 0 ) goto giveup; fputs(str2,f); - free(str1); - free(str2); +giveup: + if( str1 ) free(str1); + if( str2 ) free(str2); } void json_write_double_array(FILE* f, const char* tag, double* v, uint8_t count) { uint8_t i = 0; char * str1 = NULL; char * str2 = NULL; - asprintf(&str1,"\"%s\":[", tag); + if( asprintf(&str1,"\"%s\":[", tag) < 0 ) goto giveup; for (i=0;i<count;++i) { if (i<(count-1)) { - asprintf(&str2, "%s\"%f\",", str1,v[i]); + if( asprintf(&str2, "%s\"%f\",", str1,v[i]) < 0 ) goto giveup; } else { - asprintf(&str2, "%s\"%f\"", str1,v[i]); + if( asprintf(&str2, "%s\"%f\"", str1,v[i]) < 0 ) goto giveup; } free(str1); str1=str2; str2=NULL; } - asprintf(&str2, "%s]", str1); + if( asprintf(&str2, "%s]", str1) < 0 ) goto giveup; fputs(str2,f); - free(str1); - free(str2); +giveup: + if( str1 ) free(str1); + if( str2 ) free(str2); } void json_write_uint32(FILE* f, const char* tag, uint32_t v) { @@ -117,8 +119,9 @@ char* load_file_to_mem(const char* path) { fseek( f, 0, SEEK_END ); int len = ftell( f ); fseek( f, 0, SEEK_SET ); - char * JSON_STRING = malloc( len ); - fread( JSON_STRING, len, 1, f ); + char * JSON_STRING = malloc( len + 1); + memset(JSON_STRING,0,len+1); + int i = fread( JSON_STRING, len, 1, f ); i = i; //Ignore return value. fclose( f ); return JSON_STRING; } @@ -173,7 +176,7 @@ void json_load_file(const char* path) { int16_t children = -1; - for (i=0; i<(int)items; i+=2) + for (i=0; i<(unsigned int)items; i+=2) { //increment i on each successful tag + values combination, not individual tokens jsmntok_t* tag_t = tokens+i; @@ -213,3 +216,32 @@ void json_load_file(const char* path) { free(JSON_STRING); } +int parse_float_array(char* str, jsmntok_t* token, FLT** f, uint8_t count) { + uint16_t i = 0; + + if (count==0) return 0; + + if (*f!=NULL) free(*f); + *f = malloc(sizeof(FLT) * count); + + for(i=0;i<count;++i) { + char* end = str + token->end; + char* s = str+token->start; + + #ifdef USE_DOUBLE + (*f)[i] = strtod(s, &end); + #else + (*f)[i] = strtof(s, &end); + #endif + + if (s == end) { + free(*f); + *f=NULL; + return 0; //not a float + } + token++; + } + + + return count; +}
\ No newline at end of file diff --git a/redist/json_helpers.h b/redist/json_helpers.h index 1cccfe3..3ebf66b 100644 --- a/redist/json_helpers.h +++ b/redist/json_helpers.h @@ -4,6 +4,8 @@ #define JSON_HELPERS_H #include <stdint.h> +#include <jsmn.h> +#include "survive_types.h" void json_write_float_array(FILE* f, const char* tag, float* v, uint8_t count); void json_write_double_array(FILE* f, const char* tag, double* v, uint8_t count); @@ -11,6 +13,8 @@ void json_write_uint32(FILE* f, const char* tag, uint32_t v); void json_write_float(FILE* f, const char* tag, float v); void json_write_str(FILE* f, const char* tag, const char* v); +int parse_float_array(char* str, jsmntok_t* token, FLT** values, uint8_t count); + void json_load_file(const char* path); extern void (*json_begin_object)(char* tag); extern void (*json_end_object)(); diff --git a/redist/linmath.c b/redist/linmath.c index eefcd5f..5fefe1e 100644 --- a/redist/linmath.c +++ b/redist/linmath.c @@ -82,6 +82,28 @@ FLT anglebetween3d( FLT * a, FLT * b ) return FLT_ACOS(dot); } +// algorithm found here: http://inside.mines.edu/fs_home/gmurray/ArbitraryAxisRotation/ +void rotatearoundaxis(FLT *outvec3, FLT *invec3, FLT *axis, FLT angle) +{ + // TODO: this really should be external. + normalize3d(axis, axis); + + FLT s = FLT_SIN(angle); + FLT c = FLT_COS(angle); + + FLT u=axis[0]; + FLT v=axis[1]; + FLT w=axis[2]; + + FLT x=invec3[0]; + FLT y=invec3[1]; + FLT z=invec3[2]; + + outvec3[0] = u*(u*x + v*y + w*z)*(1-c) + x*c + (-w*y + v*z)*s; + outvec3[1] = v*(u*x + v*y + w*z)*(1-c) + y*c + ( w*x - u*z)*s; + outvec3[2] = w*(u*x + v*y + w*z)*(1-c) + z*c + (-v*x + u*y)*s; +} + /////////////////////////////////////QUATERNIONS////////////////////////////////////////// //Originally from Mercury (Copyright (C) 2009 by Joshua Allen, Charles Lohr, Adam Lowman) //Under the mit/X11 license. @@ -215,7 +237,7 @@ void quatfrommatrix( FLT * q, const FLT * matrix44 ) q[1] = (matrix44[9] - matrix44[6]) / S; q[2] = (matrix44[2] - matrix44[8]) / S; q[3] = (matrix44[4] - matrix44[1]) / S; - } else if ((matrix44[0] > matrix44[5])&(matrix44[0] > matrix44[10])) { + } else if ((matrix44[0] > matrix44[5])&&(matrix44[0] > matrix44[10])) { FLT S = FLT_SQRT(1.0 + matrix44[0] - matrix44[5] - matrix44[10]) * 2.; // S=4*qx q[0] = (matrix44[9] - matrix44[6]) / S; q[1] = 0.25f * S; @@ -498,17 +520,15 @@ void quatfrom2vectors(FLT *q, const FLT *src, const FLT *dest) FLT invs = 1 / s; FLT c[3]; - //cross3d(c, v0, v1); - cross3d(c, v1, v0); + cross3d(c, v0, v1); - q[0] = c[0] * invs; - q[1] = c[1] * invs; - q[2] = c[2] * invs; - q[3] = s * 0.5f; + q[0] = s * 0.5f; + q[1] = c[0] * invs; + q[2] = c[1] * invs; + q[3] = c[2] * invs; quatnormalize(q, q); } - } void matrix44copy(FLT * mout, const FLT * minm ) diff --git a/redist/linmath.h b/redist/linmath.h index 4e0cb77..6f0bf60 100644 --- a/redist/linmath.h +++ b/redist/linmath.h @@ -70,6 +70,7 @@ FLT magnitude3d(const FLT * a ); FLT anglebetween3d( FLT * a, FLT * b ); +void rotatearoundaxis(FLT *outvec3, FLT *invec3, FLT *axis, FLT angle); //Quaternion things... void quatsetnone( FLT * q ); diff --git a/redist/os_generic.c b/redist/os_generic.c index 1ab4863..3191357 100644 --- a/redist/os_generic.c +++ b/redist/os_generic.c @@ -151,8 +151,9 @@ void OGDeleteSema( og_sema_t os ) #else -#define _GNU_SOURCE - +#ifndef _GNU_SOURCE +# define _GNU_SOURCE +#endif #include <sys/stat.h> #include <stdlib.h> @@ -198,7 +199,7 @@ double OGGetFileTime( const char * file ) og_thread_t OGCreateThread( void * (routine)( void * ), void * parameter ) { - pthread_t * ret = malloc( sizeof( pthread_t ) ); + pthread_t * ret = (pthread_t *)malloc( sizeof( pthread_t ) ); int r = pthread_create( ret, 0, routine, parameter ); if( r ) { @@ -277,7 +278,7 @@ void OGDeleteMutex( og_mutex_t om ) og_sema_t OGCreateSema() { - sem_t * sem = malloc( sizeof( sem_t ) ); + sem_t * sem = (sem_t *)malloc( sizeof( sem_t ) ); sem_init( sem, 0, 0 ); return (og_sema_t)sem; } @@ -285,24 +286,24 @@ og_sema_t OGCreateSema() int OGGetSema( og_sema_t os ) { int valp; - sem_getvalue( os, &valp ); + sem_getvalue( (sem_t *)os, &valp ); return valp; } void OGLockSema( og_sema_t os ) { - sem_wait( os ); + sem_wait( (sem_t *)os ); } void OGUnlockSema( og_sema_t os ) { - sem_post( os ); + sem_post( (sem_t *)os ); } void OGDeleteSema( og_sema_t os ) { - sem_destroy( os ); + sem_destroy( (sem_t *)os ); free(os); } diff --git a/redist/svd.h b/redist/svd.h new file mode 100644 index 0000000..41708da --- /dev/null +++ b/redist/svd.h @@ -0,0 +1,450 @@ +/************************************************************************** +** +** svd3 +** +** Quick singular value decomposition as described by: +** A. McAdams, A. Selle, R. Tamstorf, J. Teran and E. Sifakis, +** "Computing the Singular Value Decomposition of 3x3 matrices +** with minimal branching and elementary floating point operations", +** University of Wisconsin - Madison technical report TR1690, May 2011 +** +** OPTIMIZED CPU VERSION +** Implementation by: Eric Jang +** +** 13 Apr 2014 +** +** This file originally retrieved from: +** https://github.com/ericjang/svd3/blob/master/svd3.h 3/26/2017 +** +** Original licesnse is MIT per: +** https://github.com/ericjang/svd3/blob/master/LICENSE.md +** +** Ported from C++ to C by Mike Turvey. All modifications also released +** under an MIT license. +**************************************************************************/ + + +#ifndef SVD3_H +#define SVD3_H + +#define _gamma 5.828427124 // FOUR_GAMMA_SQUARED = sqrt(8)+3; +#define _cstar 0.923879532 // cos(pi/8) +#define _sstar 0.3826834323 // sin(p/8) +#define EPSILON 1e-6 + +#include <math.h> + +/* This is a novel and fast routine for the reciprocal square root of an +IEEE float (single precision). +http://www.lomont.org/Math/Papers/2003/InvSqrt.pdf +http://playstation2-linux.com/download/p2lsd/fastrsqrt.pdf +http://www.beyond3d.com/content/articles/8/ +*/ +inline float rsqrt(float x) { + // int ihalf = *(int *)&x - 0x00800000; // Alternative to next line, + // float xhalf = *(float *)&ihalf; // for sufficiently large nos. + float xhalf = 0.5f*x; + int i = *(int *)&x; // View x as an int. + // i = 0x5f3759df - (i >> 1); // Initial guess (traditional). + i = 0x5f375a82 - (i >> 1); // Initial guess (slightly better). + x = *(float *)&i; // View i as float. + x = x*(1.5f - xhalf*x*x); // Newton step. + // x = x*(1.5008908 - xhalf*x*x); // Newton step for a balanced error. + return x; +} + +/* This is rsqrt with an additional step of the Newton iteration, for +increased accuracy. The constant 0x5f37599e makes the relative error +range from 0 to -0.00000463. +You can't balance the error by adjusting the constant. */ +inline float rsqrt1(float x) { + float xhalf = 0.5f*x; + int i = *(int *)&x; // View x as an int. + i = 0x5f37599e - (i >> 1); // Initial guess. + x = *(float *)&i; // View i as float. + x = x*(1.5f - xhalf*x*x); // Newton step. + x = x*(1.5f - xhalf*x*x); // Newton step again. + return x; +} + +inline float accurateSqrt(float x) +{ + return x * rsqrt1(x); +} + +inline void condSwap(bool c, float *X, float *Y) +{ + // used in step 2 + float Z = *X; + *X = c ? *Y : *X; + *Y = c ? Z : *Y; +} + +inline void condNegSwap(bool c, float *X, float *Y) +{ + // used in step 2 and 3 + float Z = -*X; + *X = c ? *Y : *X; + *Y = c ? Z : *Y; +} + +// matrix multiplication M = A * B +inline void multAB(float a11, float a12, float a13, + float a21, float a22, float a23, + float a31, float a32, float a33, + // + float b11, float b12, float b13, + float b21, float b22, float b23, + float b31, float b32, float b33, + // + float *m11, float *m12, float *m13, + float *m21, float *m22, float *m23, + float *m31, float *m32, float *m33) +{ + + *m11 = a11*b11 + a12*b21 + a13*b31; *m12 = a11*b12 + a12*b22 + a13*b32; *m13 = a11*b13 + a12*b23 + a13*b33; + *m21 = a21*b11 + a22*b21 + a23*b31; *m22 = a21*b12 + a22*b22 + a23*b32; *m23 = a21*b13 + a22*b23 + a23*b33; + *m31 = a31*b11 + a32*b21 + a33*b31; *m32 = a31*b12 + a32*b22 + a33*b32; *m33 = a31*b13 + a32*b23 + a33*b33; +} + +// matrix multiplication M = Transpose[A] * B +inline void multAtB(float a11, float a12, float a13, + float a21, float a22, float a23, + float a31, float a32, float a33, + // + float b11, float b12, float b13, + float b21, float b22, float b23, + float b31, float b32, float b33, + // + float *m11, float *m12, float *m13, + float *m21, float *m22, float *m23, + float *m31, float *m32, float *m33) +{ + *m11 = a11*b11 + a21*b21 + a31*b31; *m12 = a11*b12 + a21*b22 + a31*b32; *m13 = a11*b13 + a21*b23 + a31*b33; + *m21 = a12*b11 + a22*b21 + a32*b31; *m22 = a12*b12 + a22*b22 + a32*b32; *m23 = a12*b13 + a22*b23 + a32*b33; + *m31 = a13*b11 + a23*b21 + a33*b31; *m32 = a13*b12 + a23*b22 + a33*b32; *m33 = a13*b13 + a23*b23 + a33*b33; +} + +inline void quatToMat3(const float * qV, + float *m11, float *m12, float *m13, + float *m21, float *m22, float *m23, + float *m31, float *m32, float *m33 +) +{ + float w = qV[3]; + float x = qV[0]; + float y = qV[1]; + float z = qV[2]; + + float qxx = x*x; + float qyy = y*y; + float qzz = z*z; + float qxz = x*z; + float qxy = x*y; + float qyz = y*z; + float qwx = w*x; + float qwy = w*y; + float qwz = w*z; + + *m11 = 1 - 2 * (qyy + qzz); *m12 = 2 * (qxy - qwz); *m13 = 2 * (qxz + qwy); + *m21 = 2 * (qxy + qwz); *m22 = 1 - 2 * (qxx + qzz); *m23 = 2 * (qyz - qwx); + *m31 = 2 * (qxz - qwy); *m32 = 2 * (qyz + qwx); *m33 = 1 - 2 * (qxx + qyy); +} + +inline void approximateGivensQuaternion(float a11, float a12, float a22, float *ch, float *sh) +{ + /* + * Given givens angle computed by approximateGivensAngles, + * compute the corresponding rotation quaternion. + */ + *ch = 2 * (a11 - a22); + *sh = a12; + bool b = _gamma* (*sh)*(*sh) < (*ch)*(*ch); + // fast rsqrt function suffices + // rsqrt2 (https://code.google.com/p/lppython/source/browse/algorithm/HDcode/newCode/rsqrt.c?r=26) + // is even faster but results in too much error + float w = rsqrt((*ch)*(*ch) + (*sh)*(*sh)); + *ch = b ? w*(*ch) : (float)_cstar; + *sh = b ? w*(*sh) : (float)_sstar; +} + +inline void jacobiConjugation(const int x, const int y, const int z, + float *s11, + float *s21, float *s22, + float *s31, float *s32, float *s33, + float * qV) +{ + float ch, sh; + approximateGivensQuaternion(*s11, *s21, *s22, &ch, &sh); + + float scale = ch*ch + sh*sh; + float a = (ch*ch - sh*sh) / scale; + float b = (2 * sh*ch) / scale; + + // make temp copy of S + float _s11 = *s11; + float _s21 = *s21; float _s22 = *s22; + float _s31 = *s31; float _s32 = *s32; float _s33 = *s33; + + // perform conjugation S = Q'*S*Q + // Q already implicitly solved from a, b + *s11 = a*(a*_s11 + b*_s21) + b*(a*_s21 + b*_s22); + *s21 = a*(-b*_s11 + a*_s21) + b*(-b*_s21 + a*_s22); *s22 = -b*(-b*_s11 + a*_s21) + a*(-b*_s21 + a*_s22); + *s31 = a*_s31 + b*_s32; *s32 = -b*_s31 + a*_s32; *s33 = _s33; + + // update cumulative rotation qV + float tmp[3]; + tmp[0] = qV[0] * sh; + tmp[1] = qV[1] * sh; + tmp[2] = qV[2] * sh; + sh *= qV[3]; + + qV[0] *= ch; + qV[1] *= ch; + qV[2] *= ch; + qV[3] *= ch; + + // (x,y,z) corresponds to ((0,1,2),(1,2,0),(2,0,1)) + // for (p,q) = ((0,1),(1,2),(0,2)) + qV[z] += sh; + qV[3] -= tmp[z]; // w + qV[x] += tmp[y]; + qV[y] -= tmp[x]; + + // re-arrange matrix for next iteration + _s11 = *s22; + _s21 = *s32; _s22 = *s33; + _s31 = *s21; _s32 = *s31; _s33 = *s11; + *s11 = _s11; + *s21 = _s21; *s22 = _s22; + *s31 = _s31; *s32 = _s32; *s33 = _s33; + +} + +inline float dist2(float x, float y, float z) +{ + return x*x + y*y + z*z; +} + +// finds transformation that diagonalizes a symmetric matrix +inline void jacobiEigenanlysis( // symmetric matrix + float *s11, + float *s21, float *s22, + float *s31, float *s32, float *s33, + // quaternion representation of V + float * qV) +{ + qV[3] = 1; qV[0] = 0; qV[1] = 0; qV[2] = 0; // follow same indexing convention as GLM + for (int i = 0; i<4; i++) + { + // we wish to eliminate the maximum off-diagonal element + // on every iteration, but cycling over all 3 possible rotations + // in fixed order (p,q) = (1,2) , (2,3), (1,3) still retains + // asymptotic convergence + jacobiConjugation(0, 1, 2, s11, s21, s22, s31, s32, s33, qV); // p,q = 0,1 + jacobiConjugation(1, 2, 0, s11, s21, s22, s31, s32, s33, qV); // p,q = 1,2 + jacobiConjugation(2, 0, 1, s11, s21, s22, s31, s32, s33, qV); // p,q = 0,2 + } +} + + +inline void sortSingularValues(// matrix that we want to decompose + float *b11, float *b12, float *b13, + float *b21, float *b22, float *b23, + float *b31, float *b32, float *b33, + // sort V simultaneously + float *v11, float *v12, float *v13, + float *v21, float *v22, float *v23, + float *v31, float *v32, float *v33) +{ + float rho1 = dist2(*b11, *b21, *b31); + float rho2 = dist2(*b12, *b22, *b32); + float rho3 = dist2(*b13, *b23, *b33); + bool c; + c = rho1 < rho2; + condNegSwap(c, b11, b12); condNegSwap(c, v11, v12); + condNegSwap(c, b21, b22); condNegSwap(c, v21, v22); + condNegSwap(c, b31, b32); condNegSwap(c, v31, v32); + condSwap(c, &rho1, &rho2); + c = rho1 < rho3; + condNegSwap(c, b11, b13); condNegSwap(c, v11, v13); + condNegSwap(c, b21, b23); condNegSwap(c, v21, v23); + condNegSwap(c, b31, b33); condNegSwap(c, v31, v33); + condSwap(c, &rho1, &rho3); + c = rho2 < rho3; + condNegSwap(c, b12, b13); condNegSwap(c, v12, v13); + condNegSwap(c, b22, b23); condNegSwap(c, v22, v23); + condNegSwap(c, b32, b33); condNegSwap(c, v32, v33); +} + + +void QRGivensQuaternion(float a1, float a2, float *ch, float *sh) +{ + // a1 = pivot point on diagonal + // a2 = lower triangular entry we want to annihilate + float epsilon = (float)EPSILON; + float rho = accurateSqrt(a1*a1 + a2*a2); + + *sh = rho > epsilon ? a2 : 0; + *ch = fabsf(a1) + fmaxf(rho, epsilon); + bool b = a1 < 0; + condSwap(b, sh, ch); + float w = rsqrt((*ch)*(*ch) + (*sh)*(*sh)); + *ch *= w; + *sh *= w; +} + + +inline void QRDecomposition(// matrix that we want to decompose + float b11, float b12, float b13, + float b21, float b22, float b23, + float b31, float b32, float b33, + // output Q + float *q11, float *q12, float *q13, + float *q21, float *q22, float *q23, + float *q31, float *q32, float *q33, + // output R + float *r11, float *r12, float *r13, + float *r21, float *r22, float *r23, + float *r31, float *r32, float *r33) +{ + float ch1, sh1, ch2, sh2, ch3, sh3; + float a, b; + + // first givens rotation (ch,0,0,sh) + QRGivensQuaternion(b11, b21, &ch1, &sh1); + a = 1 - 2 * sh1*sh1; + b = 2 * ch1*sh1; + // apply B = Q' * B + *r11 = a*b11 + b*b21; *r12 = a*b12 + b*b22; *r13 = a*b13 + b*b23; + *r21 = -b*b11 + a*b21; *r22 = -b*b12 + a*b22; *r23 = -b*b13 + a*b23; + *r31 = b31; *r32 = b32; *r33 = b33; + + // second givens rotation (ch,0,-sh,0) + QRGivensQuaternion(*r11, *r31, &ch2, &sh2); + a = 1 - 2 * sh2*sh2; + b = 2 * ch2*sh2; + // apply B = Q' * B; + b11 = a*(*r11) + b*(*r31); b12 = a*(*r12) + b*(*r32); b13 = a*(*r13) + b*(*r33); + b21 = *r21; b22 = *r22; b23 = *r23; + b31 = -b*(*r11) + a*(*r31); b32 = -b*(*r12) + a*(*r32); b33 = -b*(*r13) + a*(*r33); + + // third givens rotation (ch,sh,0,0) + QRGivensQuaternion(b22, b32, &ch3, &sh3); + a = 1 - 2 * sh3*sh3; + b = 2 * ch3*sh3; + // R is now set to desired value + *r11 = b11; *r12 = b12; *r13 = b13; + *r21 = a*b21 + b*b31; *r22 = a*b22 + b*b32; *r23 = a*b23 + b*b33; + *r31 = -b*b21 + a*b31; *r32 = -b*b22 + a*b32; *r33 = -b*b23 + a*b33; + + // construct the cumulative rotation Q=Q1 * Q2 * Q3 + // the number of floating point operations for three quaternion multiplications + // is more or less comparable to the explicit form of the joined matrix. + // certainly more memory-efficient! + float sh12 = sh1*sh1; + float sh22 = sh2*sh2; + float sh32 = sh3*sh3; + + *q11 = (-1 + 2 * sh12)*(-1 + 2 * sh22); + *q12 = 4 * ch2*ch3*(-1 + 2 * sh12)*sh2*sh3 + 2 * ch1*sh1*(-1 + 2 * sh32); + *q13 = 4 * ch1*ch3*sh1*sh3 - 2 * ch2*(-1 + 2 * sh12)*sh2*(-1 + 2 * sh32); + + *q21 = 2 * ch1*sh1*(1 - 2 * sh22); + *q22 = -8 * ch1*ch2*ch3*sh1*sh2*sh3 + (-1 + 2 * sh12)*(-1 + 2 * sh32); + *q23 = -2 * ch3*sh3 + 4 * sh1*(ch3*sh1*sh3 + ch1*ch2*sh2*(-1 + 2 * sh32)); + + *q31 = 2 * ch2*sh2; + *q32 = 2 * ch3*(1 - 2 * sh22)*sh3; + *q33 = (-1 + 2 * sh22)*(-1 + 2 * sh32); +} + +void svd(// input A + float a11, float a12, float a13, + float a21, float a22, float a23, + float a31, float a32, float a33, + // output U + float *u11, float *u12, float *u13, + float *u21, float *u22, float *u23, + float *u31, float *u32, float *u33, + // output S + float *s11, float *s12, float *s13, + float *s21, float *s22, float *s23, + float *s31, float *s32, float *s33, + // output V + float *v11, float *v12, float *v13, + float *v21, float *v22, float *v23, + float *v31, float *v32, float *v33) +{ + // normal equations matrix + float ATA11, ATA12, ATA13; + float ATA21, ATA22, ATA23; + float ATA31, ATA32, ATA33; + + multAtB(a11, a12, a13, a21, a22, a23, a31, a32, a33, + a11, a12, a13, a21, a22, a23, a31, a32, a33, + &ATA11, &ATA12, &ATA13, &ATA21, &ATA22, &ATA23, &ATA31, &ATA32, &ATA33); + + // symmetric eigenalysis + float qV[4]; + jacobiEigenanlysis(&ATA11, &ATA21, &ATA22, &ATA31, &ATA32, &ATA33, qV); + quatToMat3(qV, v11, v12, v13, v21, v22, v23, v31, v32, v33); + + float b11, b12, b13; + float b21, b22, b23; + float b31, b32, b33; + multAB(a11, a12, a13, a21, a22, a23, a31, a32, a33, + *v11, *v12, *v13, *v21, *v22, *v23, *v31, *v32, *v33, + &b11, &b12, &b13, &b21, &b22, &b23, &b31, &b32, &b33); + + // sort singular values and find V + sortSingularValues(&b11, &b12, &b13, &b21, &b22, &b23, &b31, &b32, &b33, + v11, v12, v13, v21, v22, v23, v31, v32, v33); + + // QR decomposition + QRDecomposition(b11, b12, b13, b21, b22, b23, b31, b32, b33, + u11, u12, u13, u21, u22, u23, u31, u32, u33, + s11, s12, s13, s21, s22, s23, s31, s32, s33 + ); +} + +/// polar decomposition can be reconstructed trivially from SVD result +// A = UP +void pd(float a11, float a12, float a13, + float a21, float a22, float a23, + float a31, float a32, float a33, + // output U + float *u11, float *u12, float *u13, + float *u21, float *u22, float *u23, + float *u31, float *u32, float *u33, + // output P + float *p11, float *p12, float *p13, + float *p21, float *p22, float *p23, + float *p31, float *p32, float *p33) +{ + float w11, w12, w13, w21, w22, w23, w31, w32, w33; + float s11, s12, s13, s21, s22, s23, s31, s32, s33; + float v11, v12, v13, v21, v22, v23, v31, v32, v33; + + svd(a11, a12, a13, a21, a22, a23, a31, a32, a33, + &w11, &w12, &w13, &w21, &w22, &w23, &w31, &w32, &w33, + &s11, &s12, &s13, &s21, &s22, &s23, &s31, &s32, &s33, + &v11, &v12, &v13, &v21, &v22, &v23, &v31, &v32, &v33); + + // P = VSV' + float t11, t12, t13, t21, t22, t23, t31, t32, t33; + multAB(v11, v12, v13, v21, v22, v23, v31, v32, v33, + s11, s12, s13, s21, s22, s23, s31, s32, s33, + &t11, &t12, &t13, &t21, &t22, &t23, &t31, &t32, &t33); + + multAB(t11, t12, t13, t21, t22, t23, t31, t32, t33, + v11, v21, v31, v12, v22, v32, v13, v23, v33, + p11, p12, p13, p21, p22, p23, p31, p32, p33); + + // U = WV' + multAB(w11, w12, w13, w21, w22, w23, w31, w32, w33, + v11, v21, v31, v12, v22, v32, v13, v23, v33, + u11, u12, u13, u21, u22, u23, u31, u32, u33); +} + +#endif
\ No newline at end of file diff --git a/src/poser_daveortho.c b/src/poser_daveortho.c index e81e154..9d02bb3 100644 --- a/src/poser_daveortho.c +++ b/src/poser_daveortho.c @@ -265,7 +265,8 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len); // FLT ydist1 = 1.0 / uhat_len; //0.25*PI / uhat_len; // FLT ydist2 = 1.0 / rhat_len; //0.25*PI / rhat_len; FLT ydist = 1.0 / urhat_len; - //printf("ydist1 %f ydist2 %f ydist %f\n", ydist1, ydist2, ydist); + printf("ydist %f\n", ydist); +// printf("ydist1 %f ydist2 %f ydist %f\n", ydist1, ydist2, ydist); //-------------------- // Rescale the axies to be of the proper length @@ -282,7 +283,7 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len); if( x_y != x_y ) x_y = 0; if( y_y != y_y ) y_y = 0; if( z_y != z_y ) z_y = 0; -/* + // Exhaustively flip the minus sign of the z axis until we find the right one . . . FLT bestErr = 9999.0; FLT xy_dot2 = x[0][0]*y[0][0] + x[2][0]*y[2][0]; @@ -302,7 +303,7 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len); FLT cx,cy,cz; CrossProduct(cx,cy,cz,x[0][0],x_y,x[2][0],y[0][0],y_y,y[2][0]); FLT hand = cx*z[0][0] + cy*z_y + cz*z[2][0]; - printf("err %f hand %f\n", err, hand); +// printf("err %f hand %f\n", err, hand); // If we are the best right-handed frame so far //if (hand > 0 && err < bestErr) { x[1][0]=x_y; y[1][0]=y_y; z[1][0]=z_y; bestErr=err; } @@ -313,9 +314,9 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len); } x_y = -x_y; } - printf("bestErr %f\n", bestErr); -*/ +// printf("bestErr %f\n", bestErr); +/* //------------------------- // A test version of the rescaling to the proper length //------------------------- @@ -338,7 +339,7 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len); if( y_y != y_y ) y_y = 0; if( z_y != z_y ) z_y = 0; - printf( "---> %f %f %f\n", x_y, y_y, z_y ); +// printf( "---> %f %f %f\n", x_y, y_y, z_y ); // Exhaustively flip the minus sign of the z axis until we find the right one . . . FLT bestErr = 9999.0; @@ -359,7 +360,7 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len); FLT cx,cy,cz; CrossProduct(cx,cy,cz,x2[0][0],x_y,x2[2][0],y2[0][0],y_y,y2[2][0]); FLT hand = cx*z2[0][0] + cy*z_y + cz*z2[2][0]; - printf("err %f hand %f\n", err, hand); + //printf("err %f hand %f\n", err, hand); // If we are the best right-handed frame so far if (hand > 0 && err < bestErr) { x2[1][0]=x_y; y2[1][0]=y_y; z2[1][0]=z_y; bestErr=err; } @@ -380,7 +381,7 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len); } } ydist = bestYdist; - +*/ /* for (i=0; i<nPoints; i++) { FLT x1 = x[0][0]*X[0][i] + y[0][0]*X[1][i] + z[0][0]*X[2][i]; @@ -441,7 +442,103 @@ PRINT(ab,2,1); T[2][0]=R[2][0]; T[2][1]=R[2][1]; T[2][2]=R[2][2]; T[2][3]=trans[2]; T[3][0]=0.0; T[3][1]=0.0; T[3][2]=0.0; T[3][3]=1.0; - PRINT_MAT(T,4,4); + + FLT T2[4][4]; + + //------------------- + // Orthogonalize the matrix + //------------------- + FLT temp[4][4]; + FLT quat[4], quatNorm[4]; + FLT euler[3]; + + + //------------------- + // Orthogonalize the matrix + //------------------- + PRINT_MAT(T,4,4); + +#if 1 +// matrix44transpose(T2, T); //Transpose so we are + matrix44copy((FLT*)T2,(FLT*)T); + cross3d( &T2[1][0], &T2[0][0], &T2[2][0] ); + cross3d( &T2[2][0], &T2[1][0], &T2[0][0] ); //Replace axes in-place. + matrix44copy((FLT*)T,(FLT*)T2); +// matrix44transpose(T, T2); + +#endif + + normalize3d( &T[0][0], &T[0][0] ); + normalize3d( &T[1][0], &T[1][0] ); + normalize3d( &T[2][0], &T[2][0] ); + //Change handedness + + T[1][0]*=-1; + T[1][1]*=-1; + T[1][2]*=-1; + +/* + //Check Orthogonality. Yep. It's orthogonal. + FLT tmp[3]; + cross3d( tmp, &T[0][0], &T[1][0] ); + printf( "M3: %f\n", magnitude3d( tmp ) ); + cross3d( tmp, &T[2][0], &T[1][0] ); + printf( "M3: %f\n", magnitude3d( tmp ) ); + cross3d( tmp, &T[2][0], &T[0][0] ); + printf( "M3: %f\n", magnitude3d( tmp ) ); +*/ + +// PRINT_MAT(T,4,4); + +#if 1 + + //matrix44copy(T2,T); + matrix44transpose((FLT*)T2,(FLT*)T); + + quatfrommatrix( quat, &T2[0][0] ); + printf( "QM: %f\n", quatmagnitude( quat ) ); + quatnormalize(quatNorm,quat); + quattoeuler(euler,quatNorm); + quattomatrix( &T2[0][0], quatNorm ); + + PRINT_MAT(T2,4,4); + printf("rot %f %f %f len %f\n", euler[0], euler[1], euler[2], quatmagnitude(quat)); +// PRINT(T,4,4); + +// matrix44copy(temp,T2); + matrix44transpose((FLT*)temp,(FLT*)T2); + + +// matrix44transpose(T2, temp); +// memcpy(T2,temp,16*sizeof(float)); + for (i=0; i<3; i++) { + for (j=0; j<3; j++) { + T[i][j] = temp[i][j]; + } + } + +/* PRINT(T2,4,4); */ +#endif + + T[1][0]*=-1; + T[1][1]*=-1; + T[1][2]*=-1; + + +/* + CrossProduct(T[0][2],T[1][2],T[2][2], T[0][0],T[1][0],T[2][0], T[0][1],T[1][1],T[2][1]); + CrossProduct(T[0][0],T[1][0],T[2][0], T[0][1],T[1][1],T[2][1], T[0][2],T[1][2],T[2][2]); + CrossProduct(T[0][1],T[1][1],T[2][1], T[0][2],T[1][2],T[2][2], T[0][0],T[1][0],T[2][0]); + float xlen = sqrt(T[0][0]*T[0][0] + T[1][0]*T[1][0] + T[2][0]*T[2][0]); + float ylen = sqrt(T[0][1]*T[0][1] + T[1][1]*T[1][1] + T[2][1]*T[2][1]); + float zlen = sqrt(T[0][2]*T[0][2] + T[1][0]*T[1][2] + T[2][2]*T[2][2]); + T[0][0]/=xlen; T[1][0]/=xlen; T[2][0]/=xlen; + T[0][1]/=ylen; T[1][1]/=ylen; T[2][1]/=ylen; + T[0][2]/=zlen; T[1][2]/=zlen; T[2][2]/=zlen; +*/ + + + // PRINT_MAT(T,4,4); //------------------- // Plot the output points //------------------- @@ -453,7 +550,7 @@ PRINT(ab,2,1); S_out[1][i] = atan2(Tz, Ty); // vert //S_out[0][i] = Tx; //S_out[1][i] = Tz; - printf("point %i Txyz %f %f %f in %f %f out %f %f morph %f %f\n", i, Tx,Ty,Tz, S_in[0][i], S_in[1][i], S_out[0][i], S_out[1][i], S_morph[0][i], S_morph[1][i]); +// printf("point %i Txyz %f %f %f in %f %f out %f %f morph %f %f\n", i, Tx,Ty,Tz, S_in[0][i], S_in[1][i], S_out[0][i], S_out[1][i], S_morph[0][i], S_morph[1][i]); } } diff --git a/src/poser_octavioradii.c b/src/poser_octavioradii.c new file mode 100644 index 0000000..0d8674c --- /dev/null +++ b/src/poser_octavioradii.c @@ -0,0 +1,721 @@ +#include <survive.h> +#include <stdio.h> +#include <stdlib.h> + +typedef struct +{ +#define OLD_ANGLES_BUFF_LEN 3 + FLT oldAngles[SENSORS_PER_OBJECT][2][NUM_LIGHTHOUSES][OLD_ANGLES_BUFF_LEN]; // sensor, sweep axis, lighthouse, instance + int angleIndex[NUM_LIGHTHOUSES][2]; // index into circular buffer ahead. separate index for each axis. + int lastAxis[NUM_LIGHTHOUSES]; + + int hitCount[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; +} OctavioRadiiData; + +#include <stdio.h> +#include <stdlib.h> +#include "linmath.h" +#include <string.h> +#include <stdint.h> +#include <math.h> + +#define PTS 32 +#define MAX_CHECKS 40000 +#define MIN_HITS_FOR_VALID 10 + +FLT hmd_points[PTS * 3]; +FLT hmd_norms[PTS * 3]; +FLT hmd_point_angles[PTS * 2]; +int hmd_point_counts[PTS * 2]; +int best_hmd_target = 0; + +//Values used for RunTest() +FLT LighthousePos[3] = { 0, 0, 0 }; +FLT LighthouseQuat[4] = { 1, 0, 0, 0 }; + + +#define MAX_POINT_PAIRS 100 + +typedef struct +{ + FLT x; + FLT y; + FLT z; +} Point; + +typedef struct +{ + Point point; // location of the sensor on the tracked object; + Point normal; // unit vector indicating the normal for the sensor + double theta; // "horizontal" angular measurement from lighthouse radians + double phi; // "vertical" angular measurement from lighthouse in radians. + int id; +} TrackedSensor; + +typedef struct +{ + size_t numSensors; + TrackedSensor sensor[0]; +} TrackedObject; + +typedef struct +{ + unsigned char index1; + unsigned char index2; + FLT KnownDistance; +} PointPair; + +static FLT distance(Point a, Point b) +{ + FLT x = a.x - b.x; + FLT y = a.y - b.y; + FLT z = a.z - b.z; + return FLT_SQRT(x*x + y*y + z*z); +} + +typedef struct +{ + FLT HorizAngle; + FLT VertAngle; +} SensorAngles; + +#define SQUARED(x) ((x)*(x)) + +static FLT calculateFitnessOld(SensorAngles *angles, FLT *radii, PointPair *pairs, size_t numPairs) +{ + FLT fitness = 0; + for (size_t i = 0; i < numPairs; i++) + { + FLT estimatedDistanceBetweenPoints = + SQUARED(radii[pairs[i].index1]) + + SQUARED(radii[pairs[i].index2]) + - 2 * radii[pairs[i].index1] * radii[pairs[i].index2] + * FLT_SIN(angles[pairs[i].index1].HorizAngle) * FLT_SIN(angles[pairs[i].index2].HorizAngle) + * FLT_COS(angles[pairs[i].index1].VertAngle - angles[pairs[i].index2].VertAngle) + + FLT_COS(angles[pairs[i].index1].VertAngle) * FLT_COS(angles[pairs[i].index2].VertAngle); + + fitness += SQUARED(estimatedDistanceBetweenPoints - pairs[i].KnownDistance); + } + + return FLT_SQRT(fitness); +} + +// angles is an array of angles between a sensor pair +// pairs is an array of point pairs +// radii is the guess at the radii of those angles +static FLT calculateFitnessOld2(SensorAngles *angles, FLT *radii, PointPair *pairs, size_t numPairs) +{ + FLT fitness = 0; + for (size_t i = 0; i < numPairs; i++) + { + // These are the vectors that represent the direction for the two points. + // TODO: optimize by precomputing the tangent. + FLT v1[3], v2[3], diff[3]; + + v1[0] = 1; + v2[0] = 1; + v1[1] = tan(angles[pairs[i].index1].HorizAngle); // can be precomputed + v2[1] = tan(angles[pairs[i].index2].HorizAngle); // can be precomputed + v1[2] = tan(angles[pairs[i].index1].VertAngle); // can be precomputed + v2[2] = tan(angles[pairs[i].index2].VertAngle); // can be precomputed + + // Now, normalize the vectors + normalize3d(v1, v1); // can be precomputed + normalize3d(v2, v2); // can be precomputed + + // Now, given the specified radii, find where the new points are + scale3d(v1, v1, radii[pairs[i].index1]); + scale3d(v2, v2, radii[pairs[i].index2]); + + // Cool, now find the vector between these two points + // TODO: optimize the following two funcs into one. + sub3d(diff, v1, v2); + + FLT distance = magnitude3d(diff); + + FLT t1 = magnitude3d(v1); + FLT t2 = magnitude3d(v2); + + + + FLT estimatedDistanceBetweenPoints = + + SQUARED(radii[pairs[i].index1]) + + SQUARED(radii[pairs[i].index2]) + - 2 * radii[pairs[i].index1] * radii[pairs[i].index2] + * FLT_SIN(angles[pairs[i].index1].HorizAngle) * FLT_SIN(angles[pairs[i].index2].HorizAngle) + * FLT_COS(angles[pairs[i].index1].VertAngle - angles[pairs[i].index2].VertAngle) + + FLT_COS(angles[pairs[i].index1].VertAngle) * FLT_COS(angles[pairs[i].index2].VertAngle); + + + //fitness += SQUARED(estimatedDistanceBetweenPoints - pairs[i].KnownDistance); + fitness += SQUARED(distance - pairs[i].KnownDistance); + } + + return FLT_SQRT(fitness); +} + +static FLT angleBetweenSensors(SensorAngles *a, SensorAngles *b) +{ + FLT angle = FLT_ACOS(FLT_COS(a->VertAngle - b->VertAngle)*FLT_COS(a->HorizAngle - b->HorizAngle)); + //FLT angle2 = FLT_ACOS(FLT_COS(b->phi - a->phi)*FLT_COS(b->theta - a->theta)); + + return angle; +} + +// angles is an array of angles between a sensor pair +// pairs is an array of point pairs +// radii is the guess at the radii of those angles +static FLT calculateFitness(SensorAngles *angles, FLT *radii, PointPair *pairs, size_t numPairs) +{ + FLT fitness = 0; + for (size_t i = 0; i < numPairs; i++) + { + + FLT angle = angleBetweenSensors(&angles[pairs[i].index1], &angles[pairs[i].index2]); + + // now we have a side-angle-side triangle, and we need to find the third side. + + // The Law of Cosines says: a^2 = b^2 + c^2 ? 2bc * cosA, + // where A is the angle opposite side a. + + // Transform this to: + // a = sqrt(b^2 + c^2 - 2bc * cosA) and we know the length of the missing side! + + FLT b2 = (SQUARED(radii[pairs[i].index1])); + FLT c2 = (SQUARED(radii[pairs[i].index2])); + FLT bc2 = (2 * radii[pairs[i].index1] * radii[pairs[i].index2]); + FLT cosA = (FLT_COS(angle)); + + FLT angleInDegrees = angle * 180 / LINMATHPI; + + FLT dist = sqrt( (SQUARED(radii[pairs[i].index1])) + + (SQUARED(radii[pairs[i].index2])) - + ( (2 * radii[pairs[i].index1] * radii[pairs[i].index2]) * + (FLT_COS(angle)))); + + + FLT fitnessAdder = SQUARED(dist - pairs[i].KnownDistance); + + if (isnan(fitnessAdder)) + { + int a = 0; + } + + //printf(" %2d %f\n", i, fitnessAdder); + + //fitness += SQUARED(estimatedDistanceBetweenPoints - pairs[i].KnownDistance); + fitness += SQUARED(dist - pairs[i].KnownDistance); + } + + //fitness = 1 / fitness; + return FLT_SQRT(fitness); +} + +#define MAX_RADII 32 + +// note gradientOut will be of the same degree as numRadii +static void getGradient(FLT *gradientOut, SensorAngles *angles, FLT *radii, size_t numRadii, PointPair *pairs, size_t numPairs, const FLT precision) +{ + FLT baseline = calculateFitness(angles, radii, pairs, numPairs); + + for (size_t i = 0; i < numRadii; i++) + { + FLT tmpPlus[MAX_RADII]; + memcpy(tmpPlus, radii, sizeof(*radii) * numRadii); + tmpPlus[i] += precision; + gradientOut[i] = -(calculateFitness(angles, tmpPlus, pairs, numPairs) - baseline); + } + + return; +} + +static void normalizeAndMultiplyVector(FLT *vectorToNormalize, size_t count, FLT desiredMagnitude) +{ + FLT distanceIn = 0; + + for (size_t i = 0; i < count; i++) + { + distanceIn += SQUARED(vectorToNormalize[i]); + } + distanceIn = FLT_SQRT(distanceIn); + + + FLT scale = desiredMagnitude / distanceIn; + + for (size_t i = 0; i < count; i++) + { + vectorToNormalize[i] *= scale; + } + + return; +} + + +static RefineEstimateUsingGradientDescentRadii(FLT *estimateOut, SensorAngles *angles, FLT *initialEstimate, size_t numRadii, PointPair *pairs, size_t numPairs, FILE *logFile) +{ + int i = 0; + FLT lastMatchFitness = calculateFitness(angles, initialEstimate, pairs, numPairs); + if (estimateOut != initialEstimate) + { + memcpy(estimateOut, initialEstimate, sizeof(*estimateOut) * numRadii); + } + + + // The values below are somewhat magic, and definitely tunable + // The initial vlue of g will represent the biggest step that the gradient descent can take at first. + // bigger values may be faster, especially when the initial guess is wildly off. + // The downside to a bigger starting guess is that if we've picked a good guess at the local minima + // if there are other local minima, we may accidentally jump to such a local minima and get stuck there. + // That's fairly unlikely with the lighthouse problem, from expereince. + // The other downside is that if it's too big, we may have to spend a few iterations before it gets down + // to a size that doesn't jump us out of our minima. + // The terminal value of g represents how close we want to get to the local minima before we're "done" + // The change in value of g for each iteration is intentionally very close to 1. + // in fact, it probably could probably be 1 without any issue. The main place where g is decremented + // is in the block below when we've made a jump that results in a worse fitness than we're starting at. + // In those cases, we don't take the jump, and instead lower the value of g and try again. + for (FLT g = 0.4; g > 0.00001; g *= 0.9999) + { + i++; + + + + FLT point1[MAX_RADII]; + memcpy(point1, estimateOut, sizeof(*point1) * numRadii); + + // let's get 3 iterations of gradient descent here. + FLT gradient1[MAX_RADII]; + getGradient(gradient1, angles, point1, numRadii, pairs, numPairs, g / 1000 /*somewhat arbitrary*/); + normalizeAndMultiplyVector(gradient1, numRadii, g); + + FLT point2[MAX_RADII]; + for (size_t i = 0; i < numRadii; i++) + { + point2[i] = point1[i] + gradient1[i]; + } + FLT gradient2[MAX_RADII]; + getGradient(gradient2, angles, point2, numRadii, pairs, numPairs, g / 1000 /*somewhat arbitrary*/); + normalizeAndMultiplyVector(gradient2, numRadii, g); + + FLT point3[MAX_RADII]; + for (size_t i = 0; i < numRadii; i++) + { + point3[i] = point2[i] + gradient2[i]; + } + + // remember that gradient descent has a tendency to zig-zag when it encounters a narrow valley? + // Well, solving the lighthouse problem presents a very narrow valley, and the zig-zag of a basic + // gradient descent is kinda horrible here. Instead, think about the shape that a zig-zagging + // converging gradient descent makes. Instead of using the gradient as the best indicator of + // the direction we should follow, we're looking at one side of the zig-zag pattern, and specifically + // following *that* vector. As it turns out, this works *amazingly* well. + + FLT specialGradient[MAX_RADII]; + for (size_t i = 0; i < numRadii; i++) + { + specialGradient[i] = point3[i] - point1[i]; + } + + // The second parameter to this function is very much a tunable parameter. Different values will result + // in a different number of iterations before we get to the minimum. Numbers between 3-10 seem to work well + // It's not clear what would be optimum here. + normalizeAndMultiplyVector(specialGradient, numRadii, g / 4); + + + FLT point4[MAX_RADII]; + for (size_t i = 0; i < numRadii; i++) + { + point4[i] = point3[i] + specialGradient[i]; + } + + + FLT newMatchFitness = calculateFitness(angles, point4, pairs, numPairs); + + + if (newMatchFitness < lastMatchFitness) + { + //if (logFile) + //{ + // writePoint(logFile, lastPoint.x, lastPoint.y, lastPoint.z, 0xFFFFFF); + //} + + lastMatchFitness = newMatchFitness; + memcpy(estimateOut, point4, sizeof(*estimateOut) * numRadii); + +#ifdef RADII_DEBUG + printf("+ %d %0.9f (%0.9f) \n", i, newMatchFitness, g); +#endif + g = g * 1.05; + } + else + { +//#ifdef RADII_DEBUG + // printf("-"); + //printf("- %d %0.9f (%0.9f) [%0.9f] \n", i, newMatchFitness, g, estimateOut[0]); +//#endif + // if it wasn't a match, back off on the distance we jump + g *= 0.7; + + } + +#ifdef RADII_DEBUG + FLT avg = 0; + FLT diffFromAvg[MAX_RADII]; + + for (size_t m = 0; m < numRadii; m++) + { + avg += estimateOut[m]; + } + avg = avg / numRadii; + + for (size_t m = 0; m < numRadii; m++) + { + diffFromAvg[m] = estimateOut[m] - avg;; + } + printf("[avg:%f] ", avg); + + for (size_t x = 0; x < numRadii; x++) + { + printf("%f, ", diffFromAvg[x]); + //printf("%f, ", estimateOut[x]); + } + printf("\n"); + + +#endif + + + } + printf(" i=%d ", i); +} + +static void SolveForLighthouseRadii(Point *objPosition, FLT *objOrientation, TrackedObject *obj) +{ + FLT estimate[MAX_RADII]; + + for (size_t i = 0; i < MAX_RADII; i++) + { + estimate[i] = 2.38; + } + + + //for (int i=0; i < obj->numSensors; i++) + //{ + // printf("%d, ", obj->sensor[i].id); + //} + + SensorAngles angles[MAX_RADII]; + PointPair pairs[MAX_POINT_PAIRS]; + + size_t pairCount = 0; + + //obj->numSensors = 5; // TODO: HACK!!!! + + for (size_t i = 0; i < obj->numSensors; i++) + { + angles[i].HorizAngle = obj->sensor[i].theta; + angles[i].VertAngle = obj->sensor[i].phi; + } + + for (unsigned char i = 0; i < obj->numSensors - 1; i++) + { + for (unsigned char j = i + 1; j < obj->numSensors; j++) + { + pairs[pairCount].index1 = i; + pairs[pairCount].index2 = j; + pairs[pairCount].KnownDistance = distance(obj->sensor[i].point, obj->sensor[j].point); + pairCount++; + } + } + + + RefineEstimateUsingGradientDescentRadii(estimate, angles, estimate, obj->numSensors, pairs, pairCount, NULL); + + // we should now have an estimate of the radii. + + //for (int i = 0; i < obj->numSensors; i++) + for (int i = 0; i < 1; i++) + { + printf("radius[%d]: %f\n", i, estimate[i]); + } + + // (FLT *estimateOut, SensorAngles *angles, FLT *initialEstimate, size_t numRadii, PointPair *pairs, size_t numPairs, FILE *logFile) + + return; +} + +static void QuickPose(SurviveObject *so) +{ + OctavioRadiiData * td = so->PoserData; + + + //for (int i=0; i < so->nr_locations; i++) + //{ + // FLT x0=td->oldAngles[i][0][0][td->angleIndex[0][0]]; + // FLT y0=td->oldAngles[i][1][0][td->angleIndex[0][1]]; + // //FLT x1=td->oldAngles[i][0][1][td->angleIndex[1][0]]; + // //FLT y1=td->oldAngles[i][1][1][td->angleIndex[1][1]]; + // //printf("%2d: %8.8f, %8.8f %8.8f, %8.8f \n", + // // i, + // // x0, + // // y0, + // // x1, + // // y1 + // // ); + // printf("%2d: %8.8f, %8.8f \n", + // i, + // x0, + // y0 + // ); + //} + //printf("\n"); + + TrackedObject *to; + + to = malloc(sizeof(TrackedObject) + (SENSORS_PER_OBJECT * sizeof(TrackedSensor))); + + { + int sensorCount = 0; + + for (int i = 0; i < so->nr_locations; i++) + { + int lh = 0; + //printf("%d[%d], ",i,td->hitCount[i][lh][0]); + + int angleIndex0 = (td->angleIndex[lh][0] + 1 + OLD_ANGLES_BUFF_LEN) % OLD_ANGLES_BUFF_LEN; + int angleIndex1 = (td->angleIndex[lh][1] + 1 + OLD_ANGLES_BUFF_LEN) % OLD_ANGLES_BUFF_LEN; + if ((td->oldAngles[i][0][lh][angleIndex0] != 0 && td->oldAngles[i][1][lh][angleIndex1] != 0)) + + + { + if (td->hitCount[i][lh][0] > 10 && td->hitCount[i][lh][1] > 10) + { + FLT norm[3] = { so->sensor_normals[i * 3 + 0] , so->sensor_normals[i * 3 + 1] , so->sensor_normals[i * 3 + 2] }; + FLT point[3] = { so->sensor_locations[i * 3 + 0] , so->sensor_locations[i * 3 + 1] , so->sensor_locations[i * 3 + 2] }; + + to->sensor[sensorCount].normal.x = norm[0]; + to->sensor[sensorCount].normal.y = norm[1]; + to->sensor[sensorCount].normal.z = norm[2]; + to->sensor[sensorCount].point.x = point[0]; + to->sensor[sensorCount].point.y = point[1]; + to->sensor[sensorCount].point.z = point[2]; + to->sensor[sensorCount].theta = td->oldAngles[i][0][lh][angleIndex0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal) + to->sensor[sensorCount].phi = td->oldAngles[i][1][lh][angleIndex1] + LINMATHPI / 2; // lighthouse 0, angle 1 (vertical) + to->sensor[sensorCount].id=i; + + + + //printf("%2d: %8.8f, %8.8f \n", + // i, + // to->sensor[sensorCount].theta, + // to->sensor[sensorCount].phi + // ); + + sensorCount++; + } + } + } + //printf("\n"); + to->numSensors = sensorCount; + + if (sensorCount > 4) + { + FLT pos[3]; + FLT orient[4]; + SolveForLighthouseRadii(pos, orient, to); + } + + + } + + + free(to); + +} + + +int PoserOctavioRadii( SurviveObject * so, PoserData * pd ) +{ + PoserType pt = pd->pt; + SurviveContext * ctx = so->ctx; + OctavioRadiiData * dd = so->PoserData; + + if( !dd ) + { + so->PoserData = dd = malloc( sizeof(OctavioRadiiData) ); + memset(dd, 0, sizeof(OctavioRadiiData)); + } + + + switch( pt ) + { + case POSERDATA_IMU: + { + PoserDataIMU * imu = (PoserDataIMU*)pd; + //printf( "IMU:%s (%f %f %f) (%f %f %f)\n", so->codename, imu->accel[0], imu->accel[1], imu->accel[2], imu->gyro[0], imu->gyro[1], imu->gyro[2] ); + break; + } + case POSERDATA_LIGHT: + { + PoserDataLight * l = (PoserDataLight*)pd; + + if (l->lh >= NUM_LIGHTHOUSES || l->lh < 0) + { + // should never happen. Famous last words... + break; + } + int axis = l->acode & 0x1; + + //printf("%d ", l->sensor_id); + + + //printf( "LIG:%s %d @ %f rad, %f s (AC %d) (TC %d)\n", so->codename, l->sensor_id, l->angle, l->length, l->acode, l->timecode ); + if ((dd->lastAxis[l->lh] != (l->acode & 0x1)) ) + { + int lastAxis = dd->lastAxis[l->lh]; + //printf("\n"); + //if (0 == l->lh) + // printf("or[%d,%d] ", l->lh,lastAxis); + + for (int i=0; i < SENSORS_PER_OBJECT; i++) + { + //FLT oldAngles[SENSORS_PER_OBJECT][2][NUM_LIGHTHOUSES][OLD_ANGLES_BUFF_LEN]; // sensor, sweep axis, lighthouse, instance + int index = dd->angleIndex[l->lh][axis]; + if (dd->oldAngles[i][axis][l->lh][dd->angleIndex[l->lh][axis]] != 0) + { + //if (0 == l->lh) + // printf("%d ", i); + + dd->hitCount[i][l->lh][axis]++; + } + else + { + dd->hitCount[i][l->lh][axis] *= 0.5; + } + } + //if (0 == l->lh) + // printf("\n"); + //int foo = l->acode & 0x1; + //printf("%d", foo); + + + //if (axis) + { + if (0 == l->lh && axis) // only once per full cycle... + { + static unsigned int counter = 1; + + counter++; + + // let's just do this occasionally for now... + if (counter % 4 == 0) + QuickPose(so); + } + // axis changed, time to increment the circular buffer index. + + + dd->angleIndex[l->lh][axis]++; + dd->angleIndex[l->lh][axis] = dd->angleIndex[l->lh][axis] % OLD_ANGLES_BUFF_LEN; + + // and clear out the data. + for (int i=0; i < SENSORS_PER_OBJECT; i++) + { + dd->oldAngles[i][axis][l->lh][dd->angleIndex[l->lh][axis]] = 0; + } + + } + dd->lastAxis[l->lh] = axis; + } + + //if (0 == l->lh) + // printf("(%d) ", l->sensor_id); + + //FLT oldAngles[SENSORS_PER_OBJECT][2][NUM_LIGHTHOUSES][OLD_ANGLES_BUFF_LEN]; // sensor, sweep axis, lighthouse, instance + dd->oldAngles[l->sensor_id][axis][l->lh][dd->angleIndex[l->lh][axis]] = l->angle; + break; } + case POSERDATA_FULL_SCENE: + { + TrackedObject *to; + + PoserDataFullScene * fs = (PoserDataFullScene*)pd; + + to = malloc(sizeof(TrackedObject) + (SENSORS_PER_OBJECT * sizeof(TrackedSensor))); + + //FLT lengths[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; + //FLT angles[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; //2 Axes (Angles in LH space) + //FLT synctimes[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES]; + + //to->numSensors = so->nr_locations; + { + int sensorCount = 0; + + for (int i = 0; i < so->nr_locations; i++) + { + if (fs->lengths[i][0][0] != -1 && fs->lengths[i][0][1] != -1) //lh 0 + { + to->sensor[sensorCount].normal.x = so->sensor_normals[i * 3 + 0]; + to->sensor[sensorCount].normal.y = so->sensor_normals[i * 3 + 1]; + to->sensor[sensorCount].normal.z = so->sensor_normals[i * 3 + 2]; + to->sensor[sensorCount].point.x = so->sensor_locations[i * 3 + 0]; + to->sensor[sensorCount].point.y = so->sensor_locations[i * 3 + 1]; + to->sensor[sensorCount].point.z = so->sensor_locations[i * 3 + 2]; + to->sensor[sensorCount].theta = fs->angles[i][0][0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal) + to->sensor[sensorCount].phi = fs->angles[i][0][1] + LINMATHPI / 2; // lighthosue 0, angle 1 (vertical) + to->sensor[sensorCount].id=i; + sensorCount++; + } + } + + to->numSensors = sensorCount; + + Point position; + FLT orientation[4]; + + SolveForLighthouseRadii(&position, orientation, to); + } + { + int sensorCount = 0; + int lh = 1; + + for (int i = 0; i < so->nr_locations; i++) + { + if (fs->lengths[i][lh][0] != -1 && fs->lengths[i][lh][1] != -1) + { + to->sensor[sensorCount].normal.x = so->sensor_normals[i * 3 + 0]; + to->sensor[sensorCount].normal.y = so->sensor_normals[i * 3 + 1]; + to->sensor[sensorCount].normal.z = so->sensor_normals[i * 3 + 2]; + to->sensor[sensorCount].point.x = so->sensor_locations[i * 3 + 0]; + to->sensor[sensorCount].point.y = so->sensor_locations[i * 3 + 1]; + to->sensor[sensorCount].point.z = so->sensor_locations[i * 3 + 2]; + to->sensor[sensorCount].theta = fs->angles[i][lh][0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal) + to->sensor[sensorCount].phi = fs->angles[i][lh][1] + LINMATHPI / 2; // lighthosue 0, angle 1 (vertical) + to->sensor[sensorCount].id=i; + sensorCount++; + } + } + + to->numSensors = sensorCount; + + Point position; + FLT orientation[4]; + + SolveForLighthouseRadii(&position, orientation, to); + } + //printf( "Full scene data.\n" ); + break; + } + case POSERDATA_DISASSOCIATE: + { + free( dd ); + so->PoserData = 0; + //printf( "Need to disassociate.\n" ); + break; + } + } + return 0; +} + + +REGISTER_LINKTIME( PoserOctavioRadii ); + diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c new file mode 100644 index 0000000..7abf5d0 --- /dev/null +++ b/src/poser_turveytori.c @@ -0,0 +1,1642 @@ +#include <survive.h> +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <memory.h> +#include <assert.h> +#include "linmath.h" +#include <stddef.h> +#include <math.h> +#include <stdint.h> +#if defined(__FreeBSD__) || defined(__APPLE__) +#include <stdlib.h> +#else +#include <malloc.h> //for alloca +#endif + + +#define PointToFlts(x) ((FLT*)(x)) + +typedef struct +{ + FLT x; + FLT y; + FLT z; +} Point; + +void writePoint(FILE *file, double x, double y, double z, unsigned int rgb) {} +void updateHeader(FILE * file) {} +void writeAxes(FILE * file) {} +void drawLineBetweenPoints(FILE *file, Point a, Point b, unsigned int color) {} +void writePcdHeader(FILE * file) {} +void writePointCloud(FILE *f, Point *pointCloud, unsigned int Color) {} +void markPointWithStar(FILE *file, Point point, unsigned int color) {} + +typedef struct +{ + Point point; // location of the sensor on the tracked object; + Point normal; // unit vector indicating the normal for the sensor + double theta; // "horizontal" angular measurement from lighthouse radians + double phi; // "vertical" angular measurement from lighthouse in radians. +} TrackedSensor; + +typedef struct +{ + size_t numSensors; + TrackedSensor sensor[0]; +} TrackedObject; + + +#ifndef M_PI +#define M_PI 3.14159265358979323846264338327 +#endif + +#define SQUARED(x) ((x)*(x)) + +typedef union +{ + struct + { + unsigned char Blue; + unsigned char Green; + unsigned char Red; + unsigned char Alpha; + }; + uint32_t long_value; +} RGBValue; + +static RGBValue RED = { .Red = 255,.Green = 0,.Blue = 0,.Alpha = 125 }; +static RGBValue GREEN = { .Red = 0,.Green = 255,.Blue = 0,.Alpha = 125 }; +static RGBValue BLUE = { .Red = 0,.Green = 0,.Blue = 255,.Alpha = 125 }; + +static const double WORLD_BOUNDS = 100; +#define MAX_TRACKED_POINTS 40 + +static const float DefaultPointsPerOuterDiameter = 60; + +typedef struct +{ + FLT down[3]; // populated by the IMU for posing + //Stuff + +#define OLD_ANGLES_BUFF_LEN 3 + FLT oldAngles[SENSORS_PER_OBJECT][2][NUM_LIGHTHOUSES][OLD_ANGLES_BUFF_LEN]; // sensor, sweep axis, lighthouse, instance + int angleIndex[NUM_LIGHTHOUSES][2]; // index into circular buffer ahead. separate index for each axis. + int lastAxis[NUM_LIGHTHOUSES]; + + Point lastLhPos[NUM_LIGHTHOUSES]; + FLT lastLhRotAxisAngle[NUM_LIGHTHOUSES][4]; +} ToriData; + + + + + + + +static FLT distance(Point a, Point b) +{ + FLT x = a.x - b.x; + FLT y = a.y - b.y; + FLT z = a.z - b.z; + return FLT_SQRT(x*x + y*y + z*z); +} + +Matrix3x3 GetRotationMatrixForTorus(Point a, Point b) +{ + Matrix3x3 result; + FLT v1[3] = { 0, 0, 1 }; + FLT v2[3] = { a.x - b.x, a.y - b.y, a.z - b.z }; + + normalize3d(v2, v2); + + rotation_between_vecs_to_m3(&result, v1, v2); + + // Useful for debugging... + //FLT v2b[3]; + //rotate_vec(v2b, v1, result); + + return result; +} + +typedef struct +{ + Point a; + Point b; + FLT angle; + FLT tanAngle; // tangent of angle + Matrix3x3 rotation; + Matrix3x3 invRotation; // inverse of rotation + char ai; + char bi; +} PointsAndAngle; + + +Point RotateAndTranslatePoint(Point p, Matrix3x3 rot, Point newOrigin) +{ + Point q; + + double pf[3] = { p.x, p.y, p.z }; + q.x = rot.val[0][0] * p.x + rot.val[1][0] * p.y + rot.val[2][0] * p.z + newOrigin.x; + q.y = rot.val[0][1] * p.x + rot.val[1][1] * p.y + rot.val[2][1] * p.z + newOrigin.y; + q.z = rot.val[0][2] * p.x + rot.val[1][2] * p.y + rot.val[2][2] * p.z + newOrigin.z; + + return q; +} + +double angleFromPoints(Point p1, Point p2, Point center) +{ + Point v1, v2, v1norm, v2norm; + v1.x = p1.x - center.x; + v1.y = p1.y - center.y; + v1.z = p1.z - center.z; + + v2.x = p2.x - center.x; + v2.y = p2.y - center.y; + v2.z = p2.z - center.z; + + double v1mag = sqrt(v1.x * v1.x + v1.y * v1.y + v1.z * v1.z); + v1norm.x = v1.x / v1mag; + v1norm.y = v1.y / v1mag; + v1norm.z = v1.z / v1mag; + + double v2mag = sqrt(v2.x * v2.x + v2.y * v2.y + v2.z * v2.z); + v2norm.x = v2.x / v2mag; + v2norm.y = v2.y / v2mag; + v2norm.z = v2.z / v2mag; + + double res = v1norm.x * v2norm.x + v1norm.y * v2norm.y + v1norm.z * v2norm.z; + + double angle = acos(res); + + return angle; +} + +Point midpoint(Point a, Point b) +{ + Point m; + m.x = (a.x + b.x) / 2; + m.y = (a.y + b.y) / 2; + m.z = (a.z + b.z) / 2; + + return m; +} + +// What we're doing here is: +// * Given a point in space +// * And points and a lighthouse angle that implicitly define a torus +// * for that torus, what is the toroidal angle of the plane that will go through that point in space +// * and given that toroidal angle, what is the poloidal angle that will be directed toward that point in space? +void estimateToroidalAndPoloidalAngleOfPoint( + PointsAndAngle *pna, + Point point, + double *toroidalSin, + double *toroidalCos, + double *poloidalAngle, + double *poloidalSin) +{ + // We take the inverse of the rotation matrix, and this now defines a rotation matrix that will take us from + // the tracked object coordinate system into the "easy" or "default" coordinate system of the torus. + // Using this will allow us to derive angles much more simply by being in a "friendly" coordinate system. + Matrix3x3 rot = pna->invRotation; + Point origin; + origin.x = 0; + origin.y = 0; + origin.z = 0; + + Point m = midpoint(pna->a, pna->b); + + // in this new coordinate system, we'll rename all of the points we care about to have an "F" after them + // This will be their representation in the "friendly" coordinate system + Point pointF; + + // Okay, I lied a little above. In addition to the rotation matrix that we care about, there was also + // a translation that we did to move the origin. If we're going to get to the "friendly" coordinate system + // of the torus, we need to first undo the translation, then undo the rotation. Below, we're undoing the translation. + pointF.x = point.x - m.x; + pointF.y = point.y - m.y; + pointF.z = point.z - m.z; + + // now we'll undo the rotation part. + pointF = RotateAndTranslatePoint(pointF, rot, origin); + + // hooray, now pointF is in our more-friendly coordinate system. + + // Now, it's time to figure out the toroidal angle to that point. This should be pretty easy. + // We will "flatten" the z dimension to only look at the x and y values. Then, we just need to measure the + // angle between a vector to pointF and a vector along the x axis. + + FLT toroidalHyp = FLT_SQRT(SQUARED(pointF.y) + SQUARED(pointF.x)); + + *toroidalSin = pointF.y / toroidalHyp; + + *toroidalCos = pointF.x / toroidalHyp; + + //*toroidalAngle = atan(pointF.y / pointF.x); + //if (pointF.x < 0) + //{ + // *toroidalAngle += M_PI; + //} + + //assert(*toroidalSin / FLT_SIN(*toroidalAngle) - 1 < 0.000001); + //assert(*toroidalSin / FLT_SIN(*toroidalAngle) - 1 > -0.000001); + + //assert(*toroidalCos / FLT_COS(*toroidalAngle) - 1 < 0.000001); + //assert(*toroidalCos / FLT_COS(*toroidalAngle) - 1 > -0.000001); + + // SCORE!! We've got the toroidal angle. We're half done! + + // Okay, what next...? Now, we will need to rotate the torus *again* to make it easy to + // figure out the poloidal angle. We should rotate the entire torus by the toroidal angle + // so that the point we're focusin on will lie on the x/z plane. We then should translate the + // torus so that the center of the poloidal circle is at the origin. At that point, it will + // be trivial to determine the poloidal angle-- it will be the angle on the xz plane of a + // vector from the origin to the point. + + // okay, instead of rotating the torus & point by the toroidal angle to get the point on + // the xz plane, we're going to take advantage of the radial symmetry of the torus + // (i.e. it's symmetric about the point we'd want to rotate it, so the rotation wouldn't + // change the torus at all). Therefore, we'll leave the torus as is, but we'll rotate the point + // This will only impact the x and y coordinates, and we'll use "G" as the postfix to represent + // this new coordinate system + + Point pointG; + pointG.z = pointF.z; + pointG.y = 0; + pointG.x = sqrt(SQUARED(pointF.x) + SQUARED(pointF.y)); + + // okay, that ended up being easier than I expected. Now that we have the point on the xZ plane, + // our next step will be to shift it down so that the center of the poloidal circle is at the origin. + // As you may have noticed, y has now gone to zero, and from here on out, we can basically treat + // this as a 2D problem. I think we're getting close... + + // I stole these lines from the torus generator. Gonna need the poloidal radius. + double distanceBetweenPoints = distance(pna->a, pna->b); // we don't care about the coordinate system of these points because we're just getting distance. + double toroidalRadius = distanceBetweenPoints / (2 * pna->tanAngle); + double poloidalRadius = sqrt(SQUARED(toroidalRadius) + SQUARED(distanceBetweenPoints / 2)); + + // The center of the polidal circle already lies on the z axis at this point, so we won't shift z at all. + // The shift along the X axis will be the toroidal radius. + + Point pointH; + pointH.z = pointG.z; + pointH.y = pointG.y; + pointH.x = pointG.x - toroidalRadius; + + // Okay, almost there. If we treat pointH as a vector on the XZ plane, if we get its angle, + // that will be the poloidal angle we're looking for. (crosses fingers) + + FLT poloidalHyp = FLT_SQRT(SQUARED(pointH.z) + SQUARED(pointH.x)); + + *poloidalSin = pointH.z / poloidalHyp; + + + *poloidalAngle = atan(pointH.z / pointH.x); + if (pointH.x < 0) + { + *poloidalAngle += M_PI; + } + + //assert(*toroidalSin / FLT_SIN(*toroidalAngle) - 1 < 0.000001); + //assert(*toroidalSin / FLT_SIN(*toroidalAngle) - 1 > -0.000001); + + + + // Wow, that ended up being not so much code, but a lot of interesting trig. + // can't remember the last time I spent so much time working through each line of code. + + return; +} + +#define MAX_POINT_PAIRS 100 + +FLT angleBetweenSensors(TrackedSensor *a, TrackedSensor *b) +{ + FLT angle = FLT_ACOS(FLT_COS(a->phi - b->phi)*FLT_COS(a->theta - b->theta)); + //FLT angle2 = FLT_ACOS(FLT_COS(b->phi - a->phi)*FLT_COS(b->theta - a->theta)); + + return angle; +} + +// This provides a pretty good estimate of the angle above, probably better +// the further away the lighthouse is. But, it's not crazy-precise. +// It's main advantage is speed. +FLT pythAngleBetweenSensors2(TrackedSensor *a, TrackedSensor *b) +{ + FLT p = (a->phi - b->phi); + FLT d = (a->theta - b->theta); + + FLT adjd = FLT_SIN((a->phi + b->phi) / 2); + FLT adjP = FLT_SIN((a->theta + b->theta) / 2); + FLT pythAngle = sqrt(SQUARED(p*adjP) + SQUARED(d*adjd)); + return pythAngle; +} + +Point calculateTorusPointFromAngles(PointsAndAngle *pna, FLT toroidalSin, FLT toroidalCos, FLT poloidalAngle, FLT poloidalSin) +{ + Point result; + + FLT distanceBetweenPoints = distance(pna->a, pna->b); + Point m = midpoint(pna->a, pna->b); + Matrix3x3 rot = pna->rotation; + + FLT toroidalRadius = distanceBetweenPoints / (2 * pna->tanAngle); + FLT poloidalRadius = FLT_SQRT(SQUARED(toroidalRadius) + SQUARED(distanceBetweenPoints / 2)); + + result.x = (toroidalRadius + poloidalRadius*cos(poloidalAngle))*toroidalCos; + result.y = (toroidalRadius + poloidalRadius*cos(poloidalAngle))*toroidalSin; + result.z = poloidalRadius*poloidalSin; + result = RotateAndTranslatePoint(result, rot, m); + + return result; +} + +FLT getPointFitnessForPna(Point pointIn, PointsAndAngle *pna) +{ + + double toroidalSin = 0; + double toroidalCos = 0; + double poloidalAngle = 0; + double poloidalSin = 0; + + estimateToroidalAndPoloidalAngleOfPoint( + pna, + pointIn, + &toroidalSin, + &toroidalCos, + &poloidalAngle, + &poloidalSin); + + Point torusPoint = calculateTorusPointFromAngles(pna, toroidalSin, toroidalCos, poloidalAngle, poloidalSin); + + FLT dist = distance(pointIn, torusPoint); + + // This is some voodoo black magic. This is here to solve the problem that the origin + // (which is near the center of all the tori) erroniously will rank as a good match. + // through a lot of empiracle testing on how to compensate for this, the "fudge factor" + // below ended up being the best fit. As simple as it is, I have a strong suspicion + // that there's some crazy complex thesis-level math that could be used to derive this + // but it works so we'll run with it. + // Note that this may be resulting in a skewing of the found location by several millimeters. + // it is not clear if this is actually removing existing skew (to get a more accurate value) + // or if it is introducing an undesirable skew. + double fudge = FLT_SIN((poloidalAngle - M_PI) / 2); + dist = dist / fudge; + + return dist; +} + +FLT getPointFitness(Point pointIn, PointsAndAngle *pna, size_t pnaCount, int deubgPrint) +{ + FLT fitness; + + FLT resultSum = 0; + FLT *fitnesses = alloca(sizeof(FLT) * pnaCount); + int i=0, j=0; + + FLT worstFitness = 0; + + for (size_t i = 0; i < pnaCount; i++) + { + fitness = getPointFitnessForPna(pointIn, &(pna[i])); + + if (worstFitness < fitness) + { + i = pna[i].ai; + j = pna[i].bi; + worstFitness = fitness; + } + + fitnesses[i] = fitness; + if (deubgPrint) + { + printf(" [%d, %d](%f)\n", pna[i].ai, pna[i].bi, fitness); + } + } + + for (size_t i = 0; i < pnaCount; i++) + { + // TODO: This is an UGLY HACK!!! It is NOT ROBUST and MUST BE BETTER + // Right now, we're just throwing away the single worst fitness value + // this works frequently, but we REALLY need to do a better job of determing + // exactly when we should throw away a bad value. I'm thinking that decision + // alone could be a master's thesis, so lots of work to be done. + // This is just a stupid general approach that helps in a lot of cases, + // but is NOT suitable for long term use. + //if (pna[i].bi != i && pna[i].bi != j && pna[i].ai != i && pna[i].ai != j) + if (fitnesses[i] != worstFitness) + resultSum += SQUARED(fitnesses[i]); + } + return 1 / FLT_SQRT(resultSum); +} + +// TODO: Use a central point instead of separate "minus" points for each axis. This will reduce +// the number of fitness calls by 1/3. +Point getGradient(Point pointIn, PointsAndAngle *pna, size_t pnaCount, FLT precision) +{ + Point result; + + FLT baseFitness = getPointFitness(pointIn, pna, pnaCount, 0); + + Point tmpXplus = pointIn; + Point tmpXminus = pointIn; + tmpXplus.x = pointIn.x + precision; + tmpXminus.x = pointIn.x - precision; + result.x = baseFitness - getPointFitness(tmpXminus, pna, pnaCount, 0); + + Point tmpYplus = pointIn; + Point tmpYminus = pointIn; + tmpYplus.y = pointIn.y + precision; + tmpYminus.y = pointIn.y - precision; + result.y = baseFitness - getPointFitness(tmpYminus, pna, pnaCount, 0); + + Point tmpZplus = pointIn; + Point tmpZminus = pointIn; + tmpZplus.z = pointIn.z + precision; + tmpZminus.z = pointIn.z - precision; + result.z = baseFitness - getPointFitness(tmpZminus, pna, pnaCount, 0); + + return result; +} + +Point getNormalizedAndScaledVector(Point vectorIn, FLT desiredMagnitude) +{ + FLT distanceIn = sqrt(SQUARED(vectorIn.x) + SQUARED(vectorIn.y) + SQUARED(vectorIn.z)); + + FLT scale = desiredMagnitude / distanceIn; + + Point result = vectorIn; + + result.x *= scale; + result.y *= scale; + result.z *= scale; + + return result; +} + +Point getAvgPoints(Point a, Point b) +{ + Point result; + result.x = (a.x + b.x) / 2; + result.y = (a.y + b.y) / 2; + result.z = (a.z + b.z) / 2; + return result; +} + + +// This is modifies the basic gradient descent algorithm to better handle the shallow valley case, +// which appears to be typical of this convergence. +static Point RefineEstimateUsingModifiedGradientDescent1(Point initialEstimate, PointsAndAngle *pna, size_t pnaCount, FILE *logFile) +{ + int i = 0; + FLT lastMatchFitness = getPointFitness(initialEstimate, pna, pnaCount, 0); + Point lastPoint = initialEstimate; + + // The values below are somewhat magic, and definitely tunable + // The initial vlue of g will represent the biggest step that the gradient descent can take at first. + // bigger values may be faster, especially when the initial guess is wildly off. + // The downside to a bigger starting guess is that if we've picked a good guess at the local minima + // if there are other local minima, we may accidentally jump to such a local minima and get stuck there. + // That's fairly unlikely with the lighthouse problem, from expereince. + // The other downside is that if it's too big, we may have to spend a few iterations before it gets down + // to a size that doesn't jump us out of our minima. + // The terminal value of g represents how close we want to get to the local minima before we're "done" + // The change in value of g for each iteration is intentionally very close to 1. + // in fact, it probably could probably be 1 without any issue. The main place where g is decremented + // is in the block below when we've made a jump that results in a worse fitness than we're starting at. + // In those cases, we don't take the jump, and instead lower the value of g and try again. + for (FLT g = 0.2; g > 0.00001; g *= 0.99) + { + i++; + Point point1 = lastPoint; + // let's get 3 iterations of gradient descent here. + Point gradient1 = getGradient(point1, pna, pnaCount, g / 1000 /*somewhat arbitrary*/); + Point gradientN1 = getNormalizedAndScaledVector(gradient1, g); + + Point point2; + point2.x = point1.x + gradientN1.x; + point2.y = point1.y + gradientN1.y; + point2.z = point1.z + gradientN1.z; + + Point gradient2 = getGradient(point2, pna, pnaCount, g / 1000 /*somewhat arbitrary*/); + Point gradientN2 = getNormalizedAndScaledVector(gradient2, g); + + Point point3; + point3.x = point2.x + gradientN2.x; + point3.y = point2.y + gradientN2.y; + point3.z = point2.z + gradientN2.z; + + // remember that gradient descent has a tendency to zig-zag when it encounters a narrow valley? + // Well, solving the lighthouse problem presents a very narrow valley, and the zig-zag of a basic + // gradient descent is kinda horrible here. Instead, think about the shape that a zig-zagging + // converging gradient descent makes. Instead of using the gradient as the best indicator of + // the direction we should follow, we're looking at one side of the zig-zag pattern, and specifically + // following *that* vector. As it turns out, this works *amazingly* well. + + Point specialGradient = { .x = point3.x - point1.x,.y = point3.y - point1.y,.z = point3.y - point1.y }; + + // The second parameter to this function is very much a tunable parameter. Different values will result + // in a different number of iterations before we get to the minimum. Numbers between 3-10 seem to work well + // It's not clear what would be optimum here. + specialGradient = getNormalizedAndScaledVector(specialGradient, g / 4); + + Point point4; + + point4.x = point3.x + specialGradient.x; + point4.y = point3.y + specialGradient.y; + point4.z = point3.z + specialGradient.z; + + FLT newMatchFitness = getPointFitness(point4, pna, pnaCount, 0); + + if (newMatchFitness > lastMatchFitness) + { + if (logFile) + { + writePoint(logFile, lastPoint.x, lastPoint.y, lastPoint.z, 0xFFFFFF); + } + + lastMatchFitness = newMatchFitness; + lastPoint = point4; +#ifdef TORI_DEBUG + printf("+"); +#endif + } + else + { +#ifdef TORI_DEBUG + printf("-"); +#endif + g *= 0.7; + + } + + // from empiracle evidence, we're probably "good enough" at this point. + // So, even though we could still improve, we're likely to be improving + // very slowly, and we should just take what we've got and move on. + // This also seems to happen almost only when data is a little more "dirty" + // because the tracker is being rotated. + if (i > 120) + { + //printf("i got big"); + break; + } + } + printf(" i=%3d ", i); + + return lastPoint; +} + + +// interesting-- this is one place where we could use any sensors that are only hit by +// just an x or y axis to make our estimate better. TODO: bring that data to this fn. +FLT RotationEstimateFitnessOld(Point lhPoint, FLT *quaternion, TrackedObject *obj) +{ + FLT fitness = 0; + for (size_t i = 0; i < obj->numSensors; i++) + { + // first, get the normal of the plane for the horizonal sweep + FLT theta = obj->sensor[i].theta; + // make two vectors that lie on the plane + FLT t1H[3] = { 1, tan(theta-LINMATHPI/2), 0 }; + FLT t2H[3] = { 1, tan(theta-LINMATHPI/2), 1 }; + + FLT tNormH[3]; + + // the normal is the cross of two vectors on the plane. + cross3d(tNormH, t1H, t2H); + + normalize3d(tNormH, tNormH); + + // Now do the same for the vertical sweep + + // first, get the normal of the plane for the horizonal sweep + FLT phi = obj->sensor[i].phi; + // make two vectors that lie on the plane + FLT t1V[3] = { 0, 1, tan(phi-LINMATHPI/2)}; + FLT t2V[3] = { 1, 1, tan(phi-LINMATHPI/2)}; + + FLT tNormV[3]; + + // the normal is the cross of two vectors on the plane. + cross3d(tNormV, t1V, t2V); + + normalize3d(tNormV, tNormV); + + + // First, where is the sensor in the object's reference frame? + FLT sensor_in_obj_reference_frame[3] = {obj->sensor->point.x, obj->sensor->point.y, obj->sensor->point.z}; + // Where is the point, in the reference frame of the lighthouse? + // This has two steps, first we translate from the object's location being the + // origin to the lighthouse being the origin. + // And second, we apply the quaternion to rotate into the proper reference frame for the lighthouse. + + FLT sensor_in_lh_reference_frame[3]; + sub3d(sensor_in_lh_reference_frame, sensor_in_obj_reference_frame, (FLT[3]){lhPoint.x, lhPoint.y, lhPoint.z}); + + quatrotatevector(sensor_in_lh_reference_frame, quaternion, sensor_in_lh_reference_frame); + + // now the we've got the location of the sensor in the lighthouses's reference frame, given lhPoint and quaternion inputs. + + // We need an arbitrary vector from the plane to the point. + // Since the plane goes through the origin, this is trivial. + // The sensor point itself is such a vector! + + // And go calculate the distances! + // TODO: don't need to ABS these because we square them below. + FLT dH = FLT_FABS(dot3d(sensor_in_lh_reference_frame, tNormH)); + FLT dV = FLT_FABS(dot3d(sensor_in_lh_reference_frame, tNormV)); + + + fitness += SQUARED(dH); + fitness += SQUARED(dV); + } + + fitness = FLT_SQRT(fitness); + + return fitness; +} + +FLT RotationEstimateFitnessAxisAngle(Point lh, FLT *AxisAngle, TrackedObject *obj) +{ + // For this fitness calculator, we're going to use the rotation information to figure out where + // we expect to see the tracked object sensors, and we'll do a sum of squares to grade + // the quality of the guess formed by the AxisAngle; + + FLT fitness = 0; + + // for each point in the tracked object + for (int i=0; i< obj->numSensors; i++) + { + + + + // let's see... we need to figure out where this sensor should be in the LH reference frame. + FLT sensorLocation[3] = {obj->sensor[i].point.x-lh.x, obj->sensor[i].point.y-lh.y, obj->sensor[i].point.z-lh.z}; + + // And this puppy needs to be rotated... + + rotatearoundaxis(sensorLocation, sensorLocation, AxisAngle, AxisAngle[3]); + + // Now, the vector indicating the position of the sensor, as seen by the lighthouse is: + FLT realVectFromLh[3] = {1, tan(obj->sensor[i].theta - LINMATHPI/2), tan(obj->sensor[i].phi - LINMATHPI/2)}; + + // and the vector we're calculating given the rotation passed in is the same as the sensor location: + FLT calcVectFromLh[3] = {sensorLocation[0], sensorLocation[1], sensorLocation[2]}; + + FLT angleBetween = anglebetween3d( realVectFromLh, calcVectFromLh ); + + fitness += SQUARED(angleBetween); + } + + return 1/FLT_SQRT(fitness); +} + +// This figures out how far away from the scanned planes each point is, then does a sum of squares +// for the fitness. +// +// interesting-- this is one place where we could use any sensors that are only hit by +// just an x or y axis to make our estimate better. TODO: bring that data to this fn. +FLT RotationEstimateFitnessAxisAngleOriginal(Point lhPoint, FLT *quaternion, TrackedObject *obj) +{ + FLT fitness = 0; + for (size_t i = 0; i < obj->numSensors; i++) + { + // first, get the normal of the plane for the horizonal sweep + FLT theta = obj->sensor[i].theta; + // make two vectors that lie on the plane + FLT t1H[3] = { 1, tan(theta-LINMATHPI/2), 0 }; + FLT t2H[3] = { 1, tan(theta-LINMATHPI/2), 1 }; + + FLT tNormH[3]; + + // the normal is the cross of two vectors on the plane. + cross3d(tNormH, t1H, t2H); + + normalize3d(tNormH, tNormH); + + // Now do the same for the vertical sweep + + // first, get the normal of the plane for the horizonal sweep + FLT phi = obj->sensor[i].phi; + // make two vectors that lie on the plane + FLT t1V[3] = { 0, 1, tan(phi-LINMATHPI/2)}; + FLT t2V[3] = { 1, 1, tan(phi-LINMATHPI/2)}; + + FLT tNormV[3]; + + // the normal is the cross of two vectors on the plane. + cross3d(tNormV, t1V, t2V); + + normalize3d(tNormV, tNormV); + + + // First, where is the sensor in the object's reference frame? + FLT sensor_in_obj_reference_frame[3] = {obj->sensor->point.x, obj->sensor->point.y, obj->sensor->point.z}; + // Where is the point, in the reference frame of the lighthouse? + // This has two steps, first we translate from the object's location being the + // origin to the lighthouse being the origin. + // And second, we apply the quaternion to rotate into the proper reference frame for the lighthouse. + + FLT sensor_in_lh_reference_frame[3]; + sub3d(sensor_in_lh_reference_frame, sensor_in_obj_reference_frame, (FLT[3]){lhPoint.x, lhPoint.y, lhPoint.z}); + + //quatrotatevector(sensor_in_lh_reference_frame, quaternion, sensor_in_lh_reference_frame); + rotatearoundaxis(sensor_in_lh_reference_frame, sensor_in_lh_reference_frame, quaternion, quaternion[3]); + + // now the we've got the location of the sensor in the lighthouses's reference frame, given lhPoint and quaternion inputs. + + // We need an arbitrary vector from the plane to the point. + // Since the plane goes through the origin, this is trivial. + // The sensor point itself is such a vector! + + // And go calculate the distances! + // TODO: don't need to ABS these because we square them below. + FLT dH = FLT_FABS(dot3d(sensor_in_lh_reference_frame, tNormH)); + FLT dV = FLT_FABS(dot3d(sensor_in_lh_reference_frame, tNormV)); + + + fitness += SQUARED(dH); + fitness += SQUARED(dV); + } + + fitness = FLT_SQRT(fitness); + + return 1/fitness; +} + +// interesting-- this is one place where we could use any sensors that are only hit by +// just an x or y axis to make our estimate better. TODO: bring that data to this fn. +FLT RotationEstimateFitnessQuaternion(Point lhPoint, FLT *quaternion, TrackedObject *obj) +{ + FLT fitness = 0; + for (size_t i = 0; i < obj->numSensors; i++) + { + // first, get the normal of the plane for the horizonal sweep + FLT theta = obj->sensor[i].theta; + // make two vectors that lie on the plane + FLT t1H[3] = { 1, tan(theta-LINMATHPI/2), 0 }; + FLT t2H[3] = { 1, tan(theta-LINMATHPI/2), 1 }; + + FLT tNormH[3]; + + // the normal is the cross of two vectors on the plane. + cross3d(tNormH, t1H, t2H); + + normalize3d(tNormH, tNormH); + + // Now do the same for the vertical sweep + + // first, get the normal of the plane for the horizonal sweep + FLT phi = obj->sensor[i].phi; + // make two vectors that lie on the plane + FLT t1V[3] = { 0, 1, tan(phi-LINMATHPI/2)}; + FLT t2V[3] = { 1, 1, tan(phi-LINMATHPI/2)}; + + FLT tNormV[3]; + + // the normal is the cross of two vectors on the plane. + cross3d(tNormV, t1V, t2V); + + normalize3d(tNormV, tNormV); + + + // First, where is the sensor in the object's reference frame? + FLT sensor_in_obj_reference_frame[3] = {obj->sensor->point.x, obj->sensor->point.y, obj->sensor->point.z}; + // Where is the point, in the reference frame of the lighthouse? + // This has two steps, first we translate from the object's location being the + // origin to the lighthouse being the origin. + // And second, we apply the quaternion to rotate into the proper reference frame for the lighthouse. + + FLT sensor_in_lh_reference_frame[3]; + sub3d(sensor_in_lh_reference_frame, sensor_in_obj_reference_frame, (FLT[3]){lhPoint.x, lhPoint.y, lhPoint.z}); + + quatrotatevector(sensor_in_lh_reference_frame, quaternion, sensor_in_lh_reference_frame); + //rotatearoundaxis(sensor_in_lh_reference_frame, sensor_in_lh_reference_frame, quaternion, quaternion[3]); + + // now the we've got the location of the sensor in the lighthouses's reference frame, given lhPoint and quaternion inputs. + + // We need an arbitrary vector from the plane to the point. + // Since the plane goes through the origin, this is trivial. + // The sensor point itself is such a vector! + + // And go calculate the distances! + // TODO: don't need to ABS these because we square them below. + FLT dH = FLT_FABS(dot3d(sensor_in_lh_reference_frame, tNormH)); + FLT dV = FLT_FABS(dot3d(sensor_in_lh_reference_frame, tNormV)); + + + fitness += SQUARED(dH); + fitness += SQUARED(dV); + } + + fitness = FLT_SQRT(fitness); + + return 1/fitness; +} + + +void getRotationGradientQuaternion(FLT *gradientOut, Point lhPoint, FLT *quaternion, TrackedObject *obj, FLT precision) +{ + + FLT baseFitness = RotationEstimateFitnessQuaternion(lhPoint, quaternion, obj); + + FLT tmp0plus[4]; + quatadd(tmp0plus, quaternion, (FLT[4]){precision, 0, 0, 0}); + gradientOut[0] = RotationEstimateFitnessQuaternion(lhPoint, tmp0plus, obj) - baseFitness; + + FLT tmp1plus[4]; + quatadd(tmp1plus, quaternion, (FLT[4]){0, precision, 0, 0}); + gradientOut[1] = RotationEstimateFitnessQuaternion(lhPoint, tmp1plus, obj) - baseFitness; + + FLT tmp2plus[4]; + quatadd(tmp2plus, quaternion, (FLT[4]){0, 0, precision, 0}); + gradientOut[2] = RotationEstimateFitnessQuaternion(lhPoint, tmp2plus, obj) - baseFitness; + + FLT tmp3plus[4]; + quatadd(tmp3plus, quaternion, (FLT[4]){0, 0, 0, precision}); + gradientOut[3] = RotationEstimateFitnessQuaternion(lhPoint, tmp3plus, obj) - baseFitness; + + return; +} + +void getRotationGradientAxisAngle(FLT *gradientOut, Point lhPoint, FLT *quaternion, TrackedObject *obj, FLT precision) +{ + + FLT baseFitness = RotationEstimateFitnessAxisAngle(lhPoint, quaternion, obj); + + FLT tmp0plus[4]; + quatadd(tmp0plus, quaternion, (FLT[4]){precision, 0, 0, 0}); + gradientOut[0] = RotationEstimateFitnessAxisAngle(lhPoint, tmp0plus, obj) - baseFitness; + + FLT tmp1plus[4]; + quatadd(tmp1plus, quaternion, (FLT[4]){0, precision, 0, 0}); + gradientOut[1] = RotationEstimateFitnessAxisAngle(lhPoint, tmp1plus, obj) - baseFitness; + + FLT tmp2plus[4]; + quatadd(tmp2plus, quaternion, (FLT[4]){0, 0, precision, 0}); + gradientOut[2] = RotationEstimateFitnessAxisAngle(lhPoint, tmp2plus, obj) - baseFitness; + + FLT tmp3plus[4]; + quatadd(tmp3plus, quaternion, (FLT[4]){0, 0, 0, precision}); + gradientOut[3] = RotationEstimateFitnessAxisAngle(lhPoint, tmp3plus, obj) - baseFitness; + + return; +} + +//void getNormalizedAndScaledRotationGradient(FLT *vectorToScale, FLT desiredMagnitude) +//{ +// quatnormalize(vectorToScale, vectorToScale); +// quatscale(vectorToScale, vectorToScale, desiredMagnitude); +// return; +//} +void getNormalizedAndScaledRotationGradient(FLT *vectorToScale, FLT desiredMagnitude) +{ + quatnormalize(vectorToScale, vectorToScale); + quatscale(vectorToScale, vectorToScale, desiredMagnitude); + //vectorToScale[3] = desiredMagnitude; + + return; +} + +static void WhereIsTheTrackedObjectAxisAngle(FLT *posOut, FLT *rotation, Point lhPoint) +{ + posOut[0] = -lhPoint.x; + posOut[1] = -lhPoint.y; + posOut[2] = -lhPoint.z; + + rotatearoundaxis(posOut, posOut, rotation, rotation[3]); + + printf("{% 04.4f, % 04.4f, % 04.4f} ", posOut[0], posOut[1], posOut[2]); +} + +static void RefineRotationEstimateAxisAngle(FLT *rotOut, Point lhPoint, FLT *initialEstimate, TrackedObject *obj) +{ + int i = 0; + FLT lastMatchFitness = RotationEstimateFitnessAxisAngle(lhPoint, initialEstimate, obj); + + quatcopy(rotOut, initialEstimate); + + // The values below are somewhat magic, and definitely tunable + // The initial vlue of g will represent the biggest step that the gradient descent can take at first. + // bigger values may be faster, especially when the initial guess is wildly off. + // The downside to a bigger starting guess is that if we've picked a good guess at the local minima + // if there are other local minima, we may accidentally jump to such a local minima and get stuck there. + // That's fairly unlikely with the lighthouse problem, from expereince. + // The other downside is that if it's too big, we may have to spend a few iterations before it gets down + // to a size that doesn't jump us out of our minima. + // The terminal value of g represents how close we want to get to the local minima before we're "done" + // The change in value of g for each iteration is intentionally very close to 1. + // in fact, it probably could probably be 1 without any issue. The main place where g is decremented + // is in the block below when we've made a jump that results in a worse fitness than we're starting at. + // In those cases, we don't take the jump, and instead lower the value of g and try again. + for (FLT g = 0.1; g > 0.000000001 || i > 10000; g *= 0.99) + { + i++; + FLT point1[4]; + quatcopy(point1, rotOut); + // let's get 3 iterations of gradient descent here. + FLT gradient1[4]; + + normalize3d(point1, point1); + + getRotationGradientAxisAngle(gradient1, lhPoint, point1, obj, g/10000); + getNormalizedAndScaledRotationGradient(gradient1,g); + + FLT point2[4]; + quatadd(point2, gradient1, point1); + //quatnormalize(point2,point2); + + normalize3d(point1, point1); + + FLT gradient2[4]; + getRotationGradientAxisAngle(gradient2, lhPoint, point2, obj, g/10000); + getNormalizedAndScaledRotationGradient(gradient2,g); + + FLT point3[4]; + quatadd(point3, gradient2, point2); + + normalize3d(point1, point1); + + //quatnormalize(point3,point3); + + // remember that gradient descent has a tendency to zig-zag when it encounters a narrow valley? + // Well, solving the lighthouse problem presents a very narrow valley, and the zig-zag of a basic + // gradient descent is kinda horrible here. Instead, think about the shape that a zig-zagging + // converging gradient descent makes. Instead of using the gradient as the best indicator of + // the direction we should follow, we're looking at one side of the zig-zag pattern, and specifically + // following *that* vector. As it turns out, this works *amazingly* well. + + FLT specialGradient[4]; + quatsub(specialGradient,point3,point1); + + // The second parameter to this function is very much a tunable parameter. Different values will result + // in a different number of iterations before we get to the minimum. Numbers between 3-10 seem to work well + // It's not clear what would be optimum here. + getNormalizedAndScaledRotationGradient(specialGradient,g/4); + + FLT point4[4]; + quatadd(point4, specialGradient, point3); + //quatnormalize(point4,point4); + normalize3d(point1, point1); + + FLT newMatchFitness = RotationEstimateFitnessAxisAngle(lhPoint, point4, obj); + + if (newMatchFitness > lastMatchFitness) + { + + lastMatchFitness = newMatchFitness; + quatcopy(rotOut, point4); +//#ifdef TORI_DEBUG + //printf("+ %8.8f, (%8.8f, %8.8f, %8.8f) %f\n", newMatchFitness, point4[0], point4[1], point4[2], point4[3]); +//#endif + g *= 1.02; + + } + else + { +//#ifdef TORI_DEBUG + //printf("- , %f\n", point4[3]); +//#endif + g *= 0.7; + + } + + if (i > 998) + { + //printf("Ri got big"); + break; + } + } + printf(" Ri=%d ", i); +} +static void WhereIsTheTrackedObjectQuaternion(FLT *rotation, Point lhPoint) +{ + FLT reverseRotation[4] = {rotation[0], rotation[1], rotation[2], -rotation[3]}; + FLT objPoint[3] = {lhPoint.x, lhPoint.y, lhPoint.z}; + + //rotatearoundaxis(objPoint, objPoint, reverseRotation, reverseRotation[3]); + quatrotatevector(objPoint, rotation, objPoint); + printf("(%f, %f, %f)\n", objPoint[0], objPoint[1], objPoint[2]); +} + + + +static void RefineRotationEstimateQuaternion(FLT *rotOut, Point lhPoint, FLT *initialEstimate, TrackedObject *obj) +{ + int i = 0; + FLT lastMatchFitness = RotationEstimateFitnessQuaternion(lhPoint, initialEstimate, obj); + + quatcopy(rotOut, initialEstimate); + + // The values below are somewhat magic, and definitely tunable + // The initial vlue of g will represent the biggest step that the gradient descent can take at first. + // bigger values may be faster, especially when the initial guess is wildly off. + // The downside to a bigger starting guess is that if we've picked a good guess at the local minima + // if there are other local minima, we may accidentally jump to such a local minima and get stuck there. + // That's fairly unlikely with the lighthouse problem, from expereince. + // The other downside is that if it's too big, we may have to spend a few iterations before it gets down + // to a size that doesn't jump us out of our minima. + // The terminal value of g represents how close we want to get to the local minima before we're "done" + // The change in value of g for each iteration is intentionally very close to 1. + // in fact, it probably could probably be 1 without any issue. The main place where g is decremented + // is in the block below when we've made a jump that results in a worse fitness than we're starting at. + // In those cases, we don't take the jump, and instead lower the value of g and try again. + for (FLT g = 0.1; g > 0.000000001; g *= 0.99) + { + i++; + FLT point1[4]; + quatcopy(point1, rotOut); + // let's get 3 iterations of gradient descent here. + FLT gradient1[4]; + + //normalize3d(point1, point1); + + getRotationGradientQuaternion(gradient1, lhPoint, point1, obj, g/10000); + getNormalizedAndScaledRotationGradient(gradient1,g); + + FLT point2[4]; + quatadd(point2, gradient1, point1); + quatnormalize(point2,point2); + + //normalize3d(point1, point1); + + FLT gradient2[4]; + getRotationGradientQuaternion(gradient2, lhPoint, point2, obj, g/10000); + getNormalizedAndScaledRotationGradient(gradient2,g); + + FLT point3[4]; + quatadd(point3, gradient2, point2); + + //normalize3d(point1, point1); + + quatnormalize(point3,point3); + + // remember that gradient descent has a tendency to zig-zag when it encounters a narrow valley? + // Well, solving the lighthouse problem presents a very narrow valley, and the zig-zag of a basic + // gradient descent is kinda horrible here. Instead, think about the shape that a zig-zagging + // converging gradient descent makes. Instead of using the gradient as the best indicator of + // the direction we should follow, we're looking at one side of the zig-zag pattern, and specifically + // following *that* vector. As it turns out, this works *amazingly* well. + + FLT specialGradient[4]; + quatsub(specialGradient,point3,point1); + + // The second parameter to this function is very much a tunable parameter. Different values will result + // in a different number of iterations before we get to the minimum. Numbers between 3-10 seem to work well + // It's not clear what would be optimum here. + getNormalizedAndScaledRotationGradient(specialGradient,g/4); + + FLT point4[4]; + quatadd(point4, specialGradient, point3); + quatnormalize(point4,point4); + //normalize3d(point1, point1); + + FLT newMatchFitness = RotationEstimateFitnessQuaternion(lhPoint, point4, obj); + + if (newMatchFitness > lastMatchFitness) + { + + lastMatchFitness = newMatchFitness; + quatcopy(rotOut, point4); +//#ifdef TORI_DEBUG + //printf("+ %8.8f, (%8.8f, %8.8f, %8.8f) %f\n", newMatchFitness, point4[0], point4[1], point4[2], point4[3]); +//#endif + g *= 1.02; + printf("+"); + WhereIsTheTrackedObjectQuaternion(rotOut, lhPoint); + } + else + { +//#ifdef TORI_DEBUG + //printf("- , %f\n", point4[3]); +//#endif + g *= 0.7; + printf("-"); + } + + + } + printf("Ri=%3d Fitness=%3f ", i, lastMatchFitness); +} + + +void SolveForRotation(FLT rotOut[4], TrackedObject *obj, Point lh) +{ + + // Step 1, create initial quaternion for guess. + // This should have the lighthouse directly facing the tracked object. + Point trackedObjRelativeToLh = { .x = -lh.x,.y = -lh.y,.z = -lh.z }; + FLT theta = atan2(-lh.x, -lh.y); + FLT zAxis[4] = { 0, 0, 1 , theta-LINMATHPI/2}; + FLT quat1[4]; + quatfromaxisangle(quat1, zAxis, theta); + + //quatfrom2vectors(0,0) + // not correcting for phi, but that's less important. + + + // Step 2, optimize the axis/ angle to match the data. + RefineRotationEstimateAxisAngle(rotOut, lh, zAxis, obj); + + + //// Step 2, optimize the quaternion to match the data. + //RefineRotationEstimateQuaternion(rotOut, lh, quat1, obj); + + //WhereIsTheTrackedObjectQuaternion(rotOut, lh); + +} + + +static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *obj, SurviveObject *so, char doLogOutput, int lh, int setLhCalibration) +{ + ToriData *toriData = so->PoserData; + + //printf("Solving for Lighthouse\n"); + + //printf("obj->numSensors = %d;\n", obj->numSensors); + + //for (int i=0; i < obj->numSensors; i++) + //{ + // printf("obj->sensor[%d].normal.x = %f;\n", i, obj->sensor[i].normal.x); + // printf("obj->sensor[%d].normal.y = %f;\n", i, obj->sensor[i].normal.y); + // printf("obj->sensor[%d].normal.z = %f;\n", i, obj->sensor[i].normal.z); + // printf("obj->sensor[%d].point.x = %f;\n", i, obj->sensor[i].point.x); + // printf("obj->sensor[%d].point.y = %f;\n", i, obj->sensor[i].point.y); + // printf("obj->sensor[%d].point.z = %f;\n", i, obj->sensor[i].point.z); + // printf("obj->sensor[%d].phi = %f;\n", i, obj->sensor[i].phi); + // printf("obj->sensor[%d].theta = %f;\n\n", i, obj->sensor[i].theta); + //} + PointsAndAngle pna[MAX_POINT_PAIRS]; + + volatile size_t sizeNeeded = sizeof(pna); + + Point avgNorm = { 0 }; + + FLT smallestAngle = 20.0; + FLT largestAngle = 0; + + size_t pnaCount = 0; + for (unsigned int i = 0; i < obj->numSensors; i++) + { + for (unsigned int j = 0; j < i; j++) + { + if (pnaCount < MAX_POINT_PAIRS) + { + pna[pnaCount].a = obj->sensor[i].point; + pna[pnaCount].b = obj->sensor[j].point; + + pna[pnaCount].angle = angleBetweenSensors(&obj->sensor[i], &obj->sensor[j]); + //pna[pnaCount].angle = pythAngleBetweenSensors2(&obj->sensor[i], &obj->sensor[j]); + pna[pnaCount].tanAngle = FLT_TAN(pna[pnaCount].angle); + + if (pna[pnaCount].angle < smallestAngle) + { + smallestAngle = pna[pnaCount].angle; + } + + if (pna[pnaCount].angle > largestAngle) + { + largestAngle = pna[pnaCount].angle; + } + + double pythAngle = sqrt(SQUARED(obj->sensor[i].phi - obj->sensor[j].phi) + SQUARED(obj->sensor[i].theta - obj->sensor[j].theta)); + + pna[pnaCount].rotation = GetRotationMatrixForTorus(pna[pnaCount].a, pna[pnaCount].b); + pna[pnaCount].invRotation = inverseM33(pna[pnaCount].rotation); + pna[pnaCount].ai = i; + pna[pnaCount].bi = j; + + + + pnaCount++; + } + } + + avgNorm.x += obj->sensor[i].normal.x; + avgNorm.y += obj->sensor[i].normal.y; + avgNorm.z += obj->sensor[i].normal.z; + } + avgNorm.x = avgNorm.x / obj->numSensors; + avgNorm.y = avgNorm.y / obj->numSensors; + avgNorm.z = avgNorm.z / obj->numSensors; + + FLT avgNormF[3] = { avgNorm.x, avgNorm.y, avgNorm.z }; + + + FILE *logFile = NULL; + if (doLogOutput) + { + logFile = fopen("pointcloud2.pcd", "wb"); + writePcdHeader(logFile); + writeAxes(logFile); + } + + + // Point refinedEstimageGd = RefineEstimateUsingModifiedGradientDescent1(initialEstimate, pna, pnaCount, logFile); + + + // arbitrarily picking a value of 8 meters out to start from. + // intentionally picking the direction of the average normal vector of the sensors that see the lighthouse + // since this is least likely to pick the incorrect "mirror" point that would send us + // back into the search for the correct point (see "if (a1 > M_PI / 2)" below) + Point p1 = getNormalizedAndScaledVector(avgNorm, 8); + + // if the last lighthouse position has been populated (extremely rare it would be 0) + if (toriData->lastLhPos[lh].x != 0) + { + p1.x = toriData->lastLhPos[lh].x; + p1.y = toriData->lastLhPos[lh].y; + p1.z = toriData->lastLhPos[lh].z; + } + + Point refinedEstimateGd = RefineEstimateUsingModifiedGradientDescent1(p1, pna, pnaCount, logFile); + + FLT pf1[3] = { refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z }; + + FLT a1 = anglebetween3d(pf1, avgNormF); + + if (a1 > M_PI / 2) + { + Point p2 = { .x = -refinedEstimateGd.x,.y = -refinedEstimateGd.y,.z = -refinedEstimateGd.z }; + refinedEstimateGd = RefineEstimateUsingModifiedGradientDescent1(p2, pna, pnaCount, logFile); + + //FLT pf2[3] = { refinedEstimageGd2.x, refinedEstimageGd2.y, refinedEstimageGd2.z }; + + //FLT a2 = anglebetween3d(pf2, avgNormF); + + } + + FLT fitGd = getPointFitness(refinedEstimateGd, pna, pnaCount, 0); + + FLT distance = FLT_SQRT(SQUARED(refinedEstimateGd.x) + SQUARED(refinedEstimateGd.y) + SQUARED(refinedEstimateGd.z)); + printf(" la(% 04.4f) SnsrCnt(%2d) LhPos:(% 04.4f, % 04.4f, % 04.4f) Dist: % 08.8f ", largestAngle, (int)obj->numSensors, refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z, distance); + //printf("Distance is %f, Fitness is %f\n", distance, fitGd); + + FLT rot[4]; // this is axis/ angle rotation, not a quaternion! + + if (toriData->lastLhRotAxisAngle[lh][0] != 0) + { + rot[0] = toriData->lastLhRotAxisAngle[lh][0]; + rot[1] = toriData->lastLhRotAxisAngle[lh][1]; + rot[2] = toriData->lastLhRotAxisAngle[lh][2]; + rot[3] = toriData->lastLhRotAxisAngle[lh][3]; + } + + + SolveForRotation(rot, obj, refinedEstimateGd); + FLT objPos[3]; + + { + toriData->lastLhRotAxisAngle[lh][0] = rot[0]; + toriData->lastLhRotAxisAngle[lh][1] = rot[1]; + toriData->lastLhRotAxisAngle[lh][2] = rot[2]; + toriData->lastLhRotAxisAngle[lh][3] = rot[3]; + } + + WhereIsTheTrackedObjectAxisAngle(objPos, rot, refinedEstimateGd); + + + FLT rotQuat[4]; + + quatfromaxisangle(rotQuat, rot, rot[3]); + + //{ + FLT tmpPos[3] = {refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z}; + + quatrotatevector(tmpPos, rotQuat, tmpPos); + //} + + //static int foo = 0; + + //if (0 == foo) + if (setLhCalibration) + { + //foo = 1; + if (so->ctx->bsd[lh].PositionSet) + { + printf("Warning: resetting base station calibration data"); + } + + FLT invRot[4]; + quatgetreciprocal(invRot, rotQuat); + + so->ctx->bsd[lh].Pose.Pos[0] = refinedEstimateGd.x; + so->ctx->bsd[lh].Pose.Pos[1] = refinedEstimateGd.y; + so->ctx->bsd[lh].Pose.Pos[2] = refinedEstimateGd.z; + so->ctx->bsd[lh].Pose.Rot[0] = invRot[0]; + so->ctx->bsd[lh].Pose.Rot[1] = invRot[1]; + so->ctx->bsd[lh].Pose.Rot[2] = invRot[2]; + so->ctx->bsd[lh].Pose.Rot[3] = invRot[3]; + so->ctx->bsd[lh].PositionSet = 1; + } + + FLT wcPos[3]; // position in wold coordinates + + quatrotatevector(wcPos, so->ctx->bsd[lh].Pose.Rot, objPos); + + FLT newOrientation[4]; + quatrotateabout(newOrientation, rotQuat, so->ctx->bsd[lh].Pose.Rot ); + + wcPos[0] += so->ctx->bsd[lh].Pose.Pos[0]; + wcPos[1] += so->ctx->bsd[lh].Pose.Pos[1]; + wcPos[2] += so->ctx->bsd[lh].Pose.Pos[2]; + + so->OutPose.Pos[0] = wcPos[0]; + so->OutPose.Pos[1] = wcPos[1]; + so->OutPose.Pos[2] = wcPos[2]; + + so->OutPose.Rot[0] = newOrientation[0]; + so->OutPose.Rot[1] = newOrientation[1]; + so->OutPose.Rot[2] = newOrientation[2]; + so->OutPose.Rot[3] = newOrientation[3]; + + printf(" <% 04.4f, % 04.4f, % 04.4f > ", wcPos[0], wcPos[1], wcPos[2]); + + if (logFile) + { + updateHeader(logFile); + fclose(logFile); + } + + + toriData->lastLhPos[lh].x = refinedEstimateGd.x; + toriData->lastLhPos[lh].y = refinedEstimateGd.y; + toriData->lastLhPos[lh].z = refinedEstimateGd.z; + + return refinedEstimateGd; +} + + + + + + + + +static void QuickPose(SurviveObject *so, int lh) +{ + + + ToriData * td = so->PoserData; + + if (! so->ctx->bsd[lh].PositionSet) + { + // we don't know where we are! Augh!!! + return; + } + + //for (int i=0; i < so->nr_locations; i++) + //{ + // FLT x0=td->oldAngles[i][0][0][td->angleIndex[0][0]]; + // FLT y0=td->oldAngles[i][1][0][td->angleIndex[0][1]]; + // FLT x1=td->oldAngles[i][0][1][td->angleIndex[1][0]]; + // FLT y1=td->oldAngles[i][1][1][td->angleIndex[1][1]]; + // //printf("%2d: %8.8f, %8.8f %8.8f, %8.8f \n", + // // i, + // // x0, + // // y0, + // // x1, + // // y1 + // // ); + // printf("%2d: %8.8f, %8.8f \n", + // i, + // x0, + // y0 + // ); + //} + //printf("\n"); + + TrackedObject *to; + + to = malloc(sizeof(TrackedObject) + (SENSORS_PER_OBJECT * sizeof(TrackedSensor))); + + { + int sensorCount = 0; + + //// TODO: remove, for debug purposes only! + //FLT downQuat[4]; + //FLT negZ[3] = { 0,0,-1 }; + ////quatfrom2vectors(downQuat, negZ, td->down); + //quatfrom2vectors(downQuat, td->down, negZ); + //// end TODO + + + for (int i = 0; i < so->nr_locations; i++) + { + int angleIndex0 = (td->angleIndex[lh][0] + 1 + OLD_ANGLES_BUFF_LEN) % OLD_ANGLES_BUFF_LEN; + int angleIndex1 = (td->angleIndex[lh][1] + 1 + OLD_ANGLES_BUFF_LEN) % OLD_ANGLES_BUFF_LEN; + if (td->oldAngles[i][0][lh][angleIndex0] != 0 && td->oldAngles[i][1][lh][angleIndex1] != 0) + { + FLT norm[3] = { so->sensor_normals[i * 3 + 0] , so->sensor_normals[i * 3 + 1] , so->sensor_normals[i * 3 + 2] }; + FLT point[3] = { so->sensor_locations[i * 3 + 0] , so->sensor_locations[i * 3 + 1] , so->sensor_locations[i * 3 + 2] }; + + // TODO: remove these two lines!!! + //quatrotatevector(norm, downQuat, norm); + //quatrotatevector(point, downQuat, point); + + to->sensor[sensorCount].normal.x = norm[0]; + to->sensor[sensorCount].normal.y = norm[1]; + to->sensor[sensorCount].normal.z = norm[2]; + to->sensor[sensorCount].point.x = point[0]; + to->sensor[sensorCount].point.y = point[1]; + to->sensor[sensorCount].point.z = point[2]; + to->sensor[sensorCount].theta = td->oldAngles[i][0][lh][angleIndex0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal) + to->sensor[sensorCount].phi = td->oldAngles[i][1][lh][angleIndex1] + LINMATHPI / 2; // lighthouse 0, angle 1 (vertical) + + + sensorCount++; + } + } + to->numSensors = sensorCount; + + if (sensorCount > 4) + { + FLT pos[3], quat[4]; + + SolveForLighthouse(pos, quat, to, so, 0, lh, 0); + printf("!\n"); + } + + + } + + + free(to); + +} + + + +int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) +{ + PoserType pt = poserData->pt; + SurviveContext * ctx = so->ctx; + ToriData * td = so->PoserData; + + + if (!td) + { + so->PoserData = td = malloc(sizeof(ToriData)); + memset(td, 0, sizeof(ToriData)); + } + + switch( pt ) + { + case POSERDATA_IMU: + { + PoserDataIMU * tmpImu = (PoserDataIMU*)poserData; + + // store off data we can use for figuring out what direction is down when doing calibration. + //TODO: looks like the data mask isn't getting set correctly. + //if (tmpImu->datamask & 1) // accelerometer data is present + { + td->down[0] = td->down[0] * 0.98 + 0.02 * tmpImu->accel[0]; + td->down[1] = td->down[1] * 0.98 + 0.02 * tmpImu->accel[1]; + td->down[2] = td->down[2] * 0.98 + 0.02 * tmpImu->accel[2]; + } + //printf( "IMU:%s (%f %f %f) (%f %f %f)\n", so->codename, tmpImu->accel[0], tmpImu->accel[1], tmpImu->accel[2], tmpImu->gyro[0], tmpImu->gyro[1], tmpImu->gyro[2] ); + //printf( "Down: (%f %f %f)\n", td->down[0], td->down[1], td->down[2] ); + break; + } + case POSERDATA_LIGHT: + { + PoserDataLight * l = (PoserDataLight*)poserData; + + if (l->lh >= NUM_LIGHTHOUSES || l->lh < 0) + { + // should never happen. Famous last words... + break; + } + int axis = l->acode & 0x1; + //printf( "LIG:%s %d @ %f rad, %f s (AC %d) (TC %d)\n", so->codename, l->sensor_id, l->angle, l->length, l->acode, l->timecode ); + if ((td->lastAxis[l->lh] != (l->acode & 0x1)) ) + { + + + if (0 == l->lh && axis) // only once per full cycle... + { + static unsigned int counter = 1; + + counter++; + + // let's just do this occasionally for now... + if (counter % 4 == 0) + QuickPose(so, 0); + } + // axis changed, time to increment the circular buffer index. + td->angleIndex[l->lh][axis]++; + td->angleIndex[l->lh][axis] = td->angleIndex[l->lh][axis] % OLD_ANGLES_BUFF_LEN; + + // and clear out the data. + for (int i=0; i < SENSORS_PER_OBJECT; i++) + { + td->oldAngles[i][axis][l->lh][td->angleIndex[l->lh][axis]] = 0; + } + + td->lastAxis[l->lh] = axis; + } + + td->oldAngles[l->sensor_id][axis][l->lh][td->angleIndex[l->lh][axis]] = l->angle; + break; + } + case POSERDATA_FULL_SCENE: + { + TrackedObject *to; + + PoserDataFullScene * fs = (PoserDataFullScene*)poserData; + + to = malloc(sizeof(TrackedObject) + (SENSORS_PER_OBJECT * sizeof(TrackedSensor))); + + // if we rotate the internal reference frame of of the tracked object from having -z being arbitrary + // to being the down direction as defined by the accelerometer, then when we have come up + // with world coordinate system, it will have Z oriented correctly. + + // let's get the quaternion that represents this rotation. + FLT downQuat[4]; + FLT negZ[3] = { 0,0,1 }; + //quatfrom2vectors(downQuat, negZ, td->down); + quatfrom2vectors(downQuat, td->down, negZ); + + { + int sensorCount = 0; + + + for (int i = 0; i < so->nr_locations; i++) + { + if (fs->lengths[i][0][0] != -1 && fs->lengths[i][0][1] != -1) //lh 0 + { + FLT norm[3] = { so->sensor_normals[i * 3 + 0] , so->sensor_normals[i * 3 + 1] , so->sensor_normals[i * 3 + 2] }; + FLT point[3] = { so->sensor_locations[i * 3 + 0] , so->sensor_locations[i * 3 + 1] , so->sensor_locations[i * 3 + 2] }; + + //quatrotatevector(norm, downQuat, norm); + //quatrotatevector(point, downQuat, point); + + to->sensor[sensorCount].normal.x = norm[0]; + to->sensor[sensorCount].normal.y = norm[1]; + to->sensor[sensorCount].normal.z = norm[2]; + to->sensor[sensorCount].point.x = point[0]; + to->sensor[sensorCount].point.y = point[1]; + to->sensor[sensorCount].point.z = point[2]; + to->sensor[sensorCount].theta = fs->angles[i][0][0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal) + to->sensor[sensorCount].phi = fs->angles[i][0][1] + LINMATHPI / 2; // lighthouse 0, angle 1 (vertical) + + sensorCount++; + } + } + to->numSensors = sensorCount; + + FLT pos[3], quat[4]; + + SolveForLighthouse(pos, quat, to, so, 0, 0, 1); + } + { + int sensorCount = 0; + int lh = 1; + + for (int i = 0; i < so->nr_locations; i++) + { + if (fs->lengths[i][lh][0] != -1 && fs->lengths[i][lh][1] != -1) + { + FLT norm[3] = { so->sensor_normals[i * 3 + 0] , so->sensor_normals[i * 3 + 1] , so->sensor_normals[i * 3 + 2] }; + FLT point[3] = { so->sensor_locations[i * 3 + 0] , so->sensor_locations[i * 3 + 1] , so->sensor_locations[i * 3 + 2] }; + + //quatrotatevector(norm, downQuat, norm); + //quatrotatevector(point, downQuat, point); + + to->sensor[sensorCount].normal.x = norm[0]; + to->sensor[sensorCount].normal.y = norm[1]; + to->sensor[sensorCount].normal.z = norm[2]; + to->sensor[sensorCount].point.x = point[0]; + to->sensor[sensorCount].point.y = point[1]; + to->sensor[sensorCount].point.z = point[2]; + to->sensor[sensorCount].theta = fs->angles[i][lh][0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal) + to->sensor[sensorCount].phi = fs->angles[i][lh][1] + LINMATHPI / 2; // lighthosue 0, angle 1 (vertical) + sensorCount++; + } + } + + to->numSensors = sensorCount; + + FLT pos[3], quat[4]; + + SolveForLighthouse(pos, quat, to, so, 0, 1, 1); + } + + free(to); + //printf( "Full scene data.\n" ); + break; + } + case POSERDATA_DISASSOCIATE: + { + free( so->PoserData ); + so->PoserData = NULL; + //printf( "Need to disassociate.\n" ); + break; + } + } + return 0; +} + + +REGISTER_LINKTIME( PoserTurveyTori ); + diff --git a/src/survive_cal.c b/src/survive_cal.c index 0eb9446..ae92bad 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -30,6 +30,11 @@ int mkdir(const char *); #define DRPTS_NEEDED_FOR_AVG ((int)(DRPTS*3/4)) + //at stage 1, is when it branches to stage two or stage 7 + //stage 2 checks for the presence of two watchmen and an HMD visible to both lighthouses. + //Stage 3 collects a bunch of data for statistical averageing + //stage 4 does the calculation for the poses (NOT DONE!) + //Stage 5 = System Calibrate.d static void handle_calibration( struct SurviveCalData *cd ); @@ -117,33 +122,51 @@ void survive_cal_install( struct SurviveContext * ctx ) cd->stage = 1; cd->ctx = ctx; - cd->poseobjects[0] = survive_get_so_by_name( ctx, "HMD" ); - cd->poseobjects[1] = survive_get_so_by_name( ctx, "WM0" ); - cd->poseobjects[2] = survive_get_so_by_name( ctx, "WM1" ); + cd->numPoseObjects = 0; - if( cd->poseobjects[0] == 0 || cd->poseobjects[1] == 0 || cd->poseobjects[2] == 0 ) + const char * RequiredTrackersForCal = config_read_str( ctx->global_config_values, "RequiredTrackersForCal", "HMD,WM0,WM1" ); + const uint32_t AllowAllTrackersForCal = config_read_uint32( ctx->global_config_values, "AllowAllTrackersForCal", 0 ); + size_t requiredTrackersFound = 0; + + for (int j=0; j < ctx->objs_ct; j++) { - SV_ERROR( "Error: cannot find all devices needed for calibration." ); - free( cd ); - return; + // Add the tracker if we allow all trackers for calibration, or if it's in the list + // of required trackers. + int isRequiredTracker = strstr(RequiredTrackersForCal, ctx->objs[j]->codename) != NULL; + + if (isRequiredTracker) + { + requiredTrackersFound++; + } + + if (AllowAllTrackersForCal || isRequiredTracker) + { + if (MAX_DEVICES_TO_CAL > cd->numPoseObjects) + { + cd->poseobjects[j] = ctx->objs[j]; + cd->numPoseObjects++; + + SV_INFO("Calibration is using %s", cd->poseobjects[j]->codename); + } + else + { + SV_INFO("Calibration is NOT using %s; device count exceeds MAX_DEVICES_TO_CAL", ctx->objs[j]->codename); + } + } + } -//XXX TODO MWTourney, work on your code here. -/* - if( !cd->hmd ) - { - cd->hmd = survive_get_so_by_name( ctx, "TR0" ); + // If we want to mandate that certain devices have been found - if( !cd->hmd ) + if (strlen(RequiredTrackersForCal) > 0) + { + if (requiredTrackersFound != ((strlen(RequiredTrackersForCal) + 1) / 4)) { - SV_ERROR( "Error: cannot find any devices labeled HMD. Required for calibration" ); + SV_ERROR( "Error: Did not find all devices required for calibration." ); free( cd ); return; } - SV_INFO( "HMD not found, calibrating using Tracker" ); } -*/ - const char * DriverName; const char * PreferredPoser = config_read_str( ctx->global_config_values, "ConfigPoser", "PoserCharlesSlow" ); @@ -166,7 +189,7 @@ void survive_cal_install( struct SurviveContext * ctx ) } -void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ) +void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lh) { struct SurviveContext * ctx = so->ctx; struct SurviveCalData * cd = ctx->calptr; @@ -184,8 +207,10 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int //Collecting OOTX data. if( sensor_id < 0 ) { + //fprintf(stderr, "%s\n", so->codename); int lhid = -sensor_id-1; - if( lhid < NUM_LIGHTHOUSES && so->codename[0] == 'H' ) + // Take the OOTX data from the first device. (if using HMD, WM0, WM1 only, this will be HMD) + if( lhid < NUM_LIGHTHOUSES && so == cd->poseobjects[0] ) { uint8_t dbit = (acode & 2)>>1; ootx_pump_bit( &cd->ootx_decoders[lhid], dbit ); @@ -193,14 +218,33 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int int i; for( i = 0; i < NUM_LIGHTHOUSES; i++ ) if( ctx->bsd[i].OOTXSet == 0 ) break; - if( i == NUM_LIGHTHOUSES ) cd->stage = 2; //If all lighthouses have their OOTX set, move on. + if( i == NUM_LIGHTHOUSES ) cd->stage = 2; //TODO: Make this configuratble to allow single lighthouse. } break; + case 3: //Look for light sync lengths. + { + if( acode >= -2 ) break; + else if( acode < -4 ) break; + int lh = (-acode) - 3; + + for (int i=0; i < cd->numPoseObjects; i++) + { + if( strcmp( so->codename, cd->poseobjects[i]->codename ) == 0 ) + { + sensor_id += i*32; + } + } + + cd->all_sync_times[sensor_id][lh][cd->all_sync_counts[sensor_id][lh]++] = length; + break; + } + } + } -void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle ) +void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh ) { struct SurviveContext * ctx = so->ctx; struct SurviveCalData * cd = ctx->calptr; @@ -208,14 +252,18 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin if( !cd ) return; int sensid = sensor_id; - if( strcmp( so->codename, "WM0" ) == 0 ) - sensid += 32; - if( strcmp( so->codename, "WM1" ) == 0 ) - sensid += 64; + + for (int i=0; i < cd->numPoseObjects; i++) + { + if( strcmp( so->codename, cd->poseobjects[i]->codename ) == 0 ) + { + sensid += i*32; + } + } if( sensid >= MAX_SENSORS_TO_CAL || sensid < 0 ) return; - int lighthouse = acode>>2; + int lighthouse = lh; int axis = acode & 1; switch( cd->stage ) @@ -259,7 +307,8 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin int min_peaks = PTS_BEFORE_COMMON; int i, j, k; cd->found_common = 1; - for( i = 0; i < MAX_SENSORS_TO_CAL/SENSORS_PER_OBJECT; i++ ) + for( i = 0; i < cd->numPoseObjects; i++ ) + //for( i = 0; i < MAX_SENSORS_TO_CAL/SENSORS_PER_OBJECT; i++ ) for( j = 0; j < NUM_LIGHTHOUSES; j++ ) { int sensors_visible = 0; @@ -272,6 +321,7 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin if( sensors_visible < MIN_SENSORS_VISIBLE_PER_LH_FOR_CAL ) { //printf( "Dev %d, LH %d not enough visible points found.\n", i, j ); + reset_calibration( cd ); cd->found_common = 0; return; } @@ -332,6 +382,8 @@ static void reset_calibration( struct SurviveCalData * cd ) cd->found_common = 0; cd->times_found_common = 0; cd->stage = 2; + + memset( cd->all_sync_counts, 0, sizeof( cd->all_sync_counts ) ); } static void handle_calibration( struct SurviveCalData *cd ) @@ -357,9 +409,45 @@ static void handle_calibration( struct SurviveCalData *cd ) #else mkdir( "calinfo", 0755 ); #endif + int sen, axis, lh; + FLT temp_syncs[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES]; + + //Just to get it out of the way early, we'll calculate the sync-pulse-lengths here. + FILE * sync_time_info = fopen( "calinfo/synctime.csv", "w" ); + + for( sen = 0; sen < MAX_SENSORS_TO_CAL; sen++ ) + for( lh = 0; lh < NUM_LIGHTHOUSES; lh++ ) + { + int count = cd->all_sync_counts[sen][lh]; + int i; + double totaltime; + + totaltime = 0; + + if( count < 20 ) continue; + for( i = 0; i < count; i++ ) + { + totaltime += cd->all_sync_times[sen][lh][i]; + } + FLT avg = totaltime/count; + + double stddev = 0.0; + for( i = 0; i < count; i++ ) + { + stddev += (cd->all_sync_times[sen][lh][i] - avg)*(cd->all_sync_times[sen][lh][i] - avg); + } + stddev /= count; + + fprintf( sync_time_info, "%d %d %f %d %f\n", sen, lh, totaltime/count, count, stddev ); + } + + fclose( sync_time_info ); + + + + FILE * hists = fopen( "calinfo/histograms.csv", "w" ); FILE * ptinfo = fopen( "calinfo/ptinfo.csv", "w" ); - int sen, axis, lh; for( sen = 0; sen < MAX_SENSORS_TO_CAL; sen++ ) for( lh = 0; lh < NUM_LIGHTHOUSES; lh++ ) for( axis = 0; axis < 2; axis++ ) @@ -440,7 +528,7 @@ static void handle_calibration( struct SurviveCalData *cd ) FLT stddevlen = 0; #define HISTOGRAMSIZE 31 - #define HISTOGRAMBINANG 0.00001 //TODO: Tune + #define HISTOGRAMBINANG ((3.14159)/400000.0) //TODO: Tune int histo[HISTOGRAMSIZE]; memset( histo, 0, sizeof( histo ) ); @@ -498,11 +586,11 @@ static void handle_calibration( struct SurviveCalData *cd ) int obj; //Poses of lighthouses relative to objects. - SurvivePose objphl[POSE_OBJECTS][NUM_LIGHTHOUSES]; + SurvivePose objphl[MAX_POSE_OBJECTS][NUM_LIGHTHOUSES]; FILE * fobjp = fopen( "calinfo/objposes.csv", "w" ); - for( obj = 0; obj < POSE_OBJECTS; obj++ ) + for( obj = 0; obj < cd->numPoseObjects; obj++ ) { int i, j; PoserDataFullScene fsd; @@ -525,6 +613,7 @@ static void handle_calibration( struct SurviveCalData *cd ) fsd.lengths[i][j][1] = cd->avglens[dataindex+1]; fsd.angles[i][j][0] = cd->avgsweeps[dataindex+0]; fsd.angles[i][j][1] = cd->avgsweeps[dataindex+1]; + fsd.synctimes[i][j] = temp_syncs[i][j]; } int r = cd->ConfigPoserFn( cd->poseobjects[obj], (PoserData*)&fsd ); diff --git a/src/survive_cal.h b/src/survive_cal.h index 3d62051..ae644d1 100644 --- a/src/survive_cal.h +++ b/src/survive_cal.h @@ -29,15 +29,19 @@ int survive_cal_get_status( SurviveContext * ctx, char * description, int descri //void survive_cal_teardown( struct SurviveContext * ctx ); //Called from survive_default_light_process -void survive_cal_light( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ); -void survive_cal_angle( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle ); +void survive_cal_light( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lighthouse); +void survive_cal_angle( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh ); -#define MAX_SENSORS_TO_CAL 96 +#define MAX_SENSORS_PER_DEVICE 32 +#define MAX_DEVICES_TO_CAL 3 +#define MAX_SENSORS_TO_CAL (MAX_SENSORS_PER_DEVICE * MAX_DEVICES_TO_CAL) #define MIN_PTS_BEFORE_CAL 24 -#define DRPTS 128 -#define POSE_OBJECTS 3 + +#define DRPTS 32 //Number of samples required in collection phase. + +#define MAX_POSE_OBJECTS 10 #define MAX_CAL_PT_DAT (MAX_SENSORS_TO_CAL*NUM_LIGHTHOUSES*2) struct SurviveCalData @@ -54,6 +58,9 @@ struct SurviveCalData int8_t found_common; int8_t times_found_common; + FLT all_sync_times[MAX_SENSORS_TO_CAL][NUM_LIGHTHOUSES][DRPTS]; + int16_t all_sync_counts[MAX_SENSORS_TO_CAL][NUM_LIGHTHOUSES]; + //For camfind (4+) //Index is calculated with: int dataindex = sen*(2*NUM_LIGHTHOUSES)+lh*2+axis; FLT avgsweeps[MAX_CAL_PT_DAT]; @@ -64,7 +71,9 @@ struct SurviveCalData int senid_of_checkpt; //This is a point on a watchman that can be used to check the lh solution. - SurviveObject * poseobjects[POSE_OBJECTS]; + SurviveObject * poseobjects[MAX_POSE_OBJECTS]; + + size_t numPoseObjects; PoserCB ConfigPoserFn; diff --git a/src/survive_config.c b/src/survive_config.c index 0810280..3a83902 100644 --- a/src/survive_config.c +++ b/src/survive_config.c @@ -175,7 +175,13 @@ const char* config_set_str(config_group *cg, const char *tag, const char* value) if (cv == NULL) cv = next_unused_entry(cg); sstrcpy(&(cv->tag), tag); - sstrcpy(&(cv->data), value); + + if (NULL != value){ + sstrcpy(&(cv->data), value); + } + else { + sstrcpy(&(cv->data), ""); + } cv->type = CONFIG_STRING; return value; @@ -354,12 +360,17 @@ int parse_uint32(char* tag, char** values, uint16_t count) { } void handle_tag_value(char* tag, char** values, uint8_t count) { - print_json_value(tag,values,count); + + //Uncomment for more debugging of input configuration. + //print_json_value(tag,values,count); + config_group* cg = cg_stack[cg_stack_head]; + if (NULL != *values){ if (parse_uint32(tag,values,count) > 0) return; //parse integers first, stricter rules if (parse_floats(tag,values,count) > 0) return; + } //should probably also handle string arrays config_set_str(cg,tag,values[0]); diff --git a/src/survive_data.c b/src/survive_data.c index 4a2cfb6..157650d 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -4,21 +4,546 @@ #include "survive_internal.h" #include <stdint.h> #include <string.h> +#include <math.h> /* for sqrt */ + +//#define USE_TURVEYBIGUATOR + +#ifdef USE_TURVEYBIGUATOR + +static const float tau_table[33] = { 0, 0, 0, 1.151140982, 1.425, 1.5712213707, 1.656266074, 1.7110275587, 1.7490784054, + 1.7770229476, 1.798410005, 1.8153056661, 1.8289916275, 1.8403044103, 1.8498129961, 1.8579178211, + 1.864908883, 1.8710013691, 1.8763583296, 1.881105575, 1.885341741, 1.8891452542, 1.8925792599, + 1.8956951735, 1.8985352854, 1.9011347009, 1.9035228046, 1.9057243816, 1.9077604832, 1.9096491058, + 1.9114057255, 1.9130437248, 1.914574735 +}; + +typedef struct +{ + unsigned int sweep_time[SENSORS_PER_OBJECT]; + uint16_t sweep_len[SENSORS_PER_OBJECT]; //might want to align this to cache lines, will be hot for frequent access +} lightcaps_sweep_data; + +typedef struct +{ + int recent_sync_time; + int activeLighthouse; + int activeSweepStartTime; + int activeAcode; + +// int lh_pulse_len[NUM_LIGHTHOUSES]; + int lh_start_time[NUM_LIGHTHOUSES]; + int lh_max_pulse_length[NUM_LIGHTHOUSES]; + int8_t lh_acode[NUM_LIGHTHOUSES]; + int current_lh; // used knowing which sync pulse we're looking at. + +} lightcap2_per_sweep_data; + +typedef struct +{ + double acode_offset; +} lightcap2_global_data; + +typedef struct +{ + lightcaps_sweep_data sweep; + lightcap2_per_sweep_data per_sweep; + lightcap2_global_data global; +} lightcap2_data; + + +//static lightcap2_global_data lcgd = { 0 }; + +int handle_lightcap2_getAcodeFromSyncPulse(SurviveObject * so, int pulseLen) +{ + double oldOffset = ((lightcap2_data*)so->disambiguator_data)->global.acode_offset; + + int modifiedPulseLen = pulseLen - (int)oldOffset; + + double newOffset = (((pulseLen) + 250) % 500) - 250; + + ((lightcap2_data*)so->disambiguator_data)->global.acode_offset = oldOffset * 0.9 + newOffset * 0.1; + +//fprintf(stderr, " %f\n", oldOffset); +#define ACODE_OFFSET 0 + if (pulseLen < 3250 - ACODE_OFFSET) return 0; + if (pulseLen < 3750 - ACODE_OFFSET) return 1; + if (pulseLen < 4250 - ACODE_OFFSET) return 2; + if (pulseLen < 4750 - ACODE_OFFSET) return 3; + if (pulseLen < 5250 - ACODE_OFFSET) return 4; + if (pulseLen < 5750 - ACODE_OFFSET) return 5; + if (pulseLen < 6250 - ACODE_OFFSET) return 6; + return 7; +} + +uint8_t remove_outliers(SurviveObject *so) { + return 0; // disabling this for now because it seems remove almost all the points for wired watchman and wired tracker. + lightcap2_data *lcd = so->disambiguator_data; + + uint32_t sum = 0; + uint8_t non_zero_count = 0; + uint32_t mean = 0; + + uint16_t* min = NULL; + uint16_t* max = NULL; + uint8_t found_first = 0; + + //future: https://gcc.gnu.org/projects/tree-ssa/vectorization.html#vectorizab + + for (uint8_t i = 0; i < SENSORS_PER_OBJECT; i++) + { + sum += lcd->sweep.sweep_len[i]; + if (lcd->sweep.sweep_len[i] > 0) ++non_zero_count; + } + + if (non_zero_count==0) return 0; + + mean = sum/non_zero_count; + + float standard_deviation = 0.0f; + sum = 0; + for (uint8_t i = 0; i < SENSORS_PER_OBJECT; i++) + { + uint16_t len = lcd->sweep.sweep_len[i]; + if (len > 0) { + sum += (len - mean)*(len - mean); + + if (found_first==0) { + max = min = lcd->sweep.sweep_len + i; + found_first=1; + } else { + if(lcd->sweep.sweep_len[i] < *min) min=lcd->sweep.sweep_len + i; + if(lcd->sweep.sweep_len[i] > *max) max=lcd->sweep.sweep_len + i; + } + } + } + standard_deviation = sqrtf( ((float)sum)/((float)non_zero_count) ); + +// printf("%f\n", standard_deviation); + + float tau_test = standard_deviation; + + if (non_zero_count > 2) tau_test = standard_deviation*tau_table[non_zero_count]; + +// uint8_t removed_outliers = 0; + + uint32_t d1 = abs(*min - mean); + uint32_t d2 = abs(*max - mean); + + if (d1>d2) { + if (d1 > tau_test) { + *min = 0; + return 1; + } + } + else if (d2>tau_test) { + *max = 0; + return 1; + } + + return 0; +/* + for (uint8_t i = 0; i < SENSORS_PER_OBJECT; i++) + { + uint16_t len = lcd->sweep.sweep_len[i]; + if (len == 0) continue; + + if ( abs(len-mean) > tau_test ) + { +// fprintf(stderr, "removing %d\n", len); + lcd->sweep.sweep_len[i] = 0; + removed_outliers = 1; + } + } +*/ +// return removed_outliers; +} + +void handle_lightcap2_process_sweep_data(SurviveObject *so) +{ + lightcap2_data *lcd = so->disambiguator_data; + + while(remove_outliers(so)); + + // look at all of the sensors we found, and process the ones that were hit. + // TODO: find the sensor(s) with the longest pulse length, and assume + // those are the "highest quality". Then, reject any pulses that are sufficiently + // different from those values, assuming that they are reflections. + { + unsigned int longest_pulse = 0; + unsigned int timestamp_of_longest_pulse = 0; + for (int i = 0; i < SENSORS_PER_OBJECT; i++) + { + if (lcd->sweep.sweep_len[i] > longest_pulse) + { + longest_pulse = lcd->sweep.sweep_len[i]; + timestamp_of_longest_pulse = lcd->sweep.sweep_time[i]; + } + } + + int allZero = 1; + for (int q=0; q< 32; q++) + if (lcd->sweep.sweep_len[q] != 0) + allZero=0; + //if (!allZero) + // printf("a[%d]l[%d] ", lcd->per_sweep.activeAcode & 5, lcd->per_sweep.activeLighthouse); + for (int i = 0; i < SENSORS_PER_OBJECT; i++) + { + + { + static int counts[SENSORS_PER_OBJECT][2] = {0}; + +// if (lcd->per_sweep.activeLighthouse == 0 && !allZero) + if (lcd->per_sweep.activeLighthouse > -1 && !allZero) + { + if (lcd->sweep.sweep_len[i] != 0) + { + //printf("%d ", i); + //counts[i][lcd->per_sweep.activeAcode & 1] ++; + } + else + { + counts[i][lcd->per_sweep.activeAcode & 1] =0; + } + + //if (counts[i][0] > 10 && counts[i][1] > 10) + //{ + //printf("%d(%d,%d), ", i, counts[i][0], counts[i][1]); + //} + } + } + + + if (lcd->sweep.sweep_len[i] != 0) // if the sensor was hit, process it + { +//printf("%4d\n", lcd->sweep.sweep_len[i]); + int offset_from = lcd->sweep.sweep_time[i] - lcd->per_sweep.activeSweepStartTime + lcd->sweep.sweep_len[i] / 2; + +// if (offset_from < 380000 && offset_from > 70000) + { + //if (longest_pulse *10 / 8 < lcd->sweep.sweep_len[i]) + { + so->ctx->lightproc(so, i, lcd->per_sweep.activeAcode, offset_from, lcd->sweep.sweep_time[i], lcd->sweep.sweep_len[i], lcd->per_sweep.activeLighthouse); + } + } + } + } + //if (!allZero) + // printf(" ..:..\n"); + +// if (!allZero) printf("\n"); + } + + // clear out sweep data (could probably limit this to only after a "first" sync. + // this is slightly more robust, so doing it here for now. + memset(&(((lightcap2_data*)so->disambiguator_data)->sweep), 0, sizeof(lightcaps_sweep_data)); +} +void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) +{ + //fprintf(stderr, "%6.6d %4.4d \n", le->timestamp - so->recent_sync_time, le->length); + lightcap2_data *lcd = so->disambiguator_data; + + //static unsigned int recent_sync_time = 0; + //static unsigned int recent_sync_count = -1; + //static unsigned int activeSweepStartTime; + + int acode = handle_lightcap2_getAcodeFromSyncPulse(so, le->length); //acode for this sensor reading + + // Process any sweep data we have + handle_lightcap2_process_sweep_data(so); + + int time_since_last_sync = (le->timestamp - lcd->per_sweep.recent_sync_time); + + +// fprintf(stderr, " %2d %8d %d\n", le->sensor_id, time_since_last_sync, le->length); + // need to store up sync pulses, so we can take the earliest starting time for all sensors. + if (time_since_last_sync < 2400) + { + lcd->per_sweep.recent_sync_time = le->timestamp; + // it's the same sync pulse; +// so->sync_set_number = 1; + so->recent_sync_time = le->timestamp; + +// lcd->per_sweep.lh_pulse_len[lcd->per_sweep.current_lh] = le->length; +// lcd->per_sweep.lh_start_time[lcd->per_sweep.current_lh] = le->timestamp; + + if (le->length > lcd->per_sweep.lh_max_pulse_length[lcd->per_sweep.current_lh]) { + lcd->per_sweep.lh_max_pulse_length[lcd->per_sweep.current_lh] = le->length; + lcd->per_sweep.lh_start_time[lcd->per_sweep.current_lh] = le->timestamp; + lcd->per_sweep.lh_acode[lcd->per_sweep.current_lh] = acode; + } + +/* + //this stuff should probably be happening on the sweep so that we can filter out erroneous a codes + if (!(acode >> 2 & 1)) // if the skip bit is not set + { + lcd->per_sweep.activeLighthouse = lcd->per_sweep.current_lh; + lcd->per_sweep.activeSweepStartTime = le->timestamp; + lcd->per_sweep.activeAcode = acode; + } + else + { + //this causes the master lighthouse to be ignored from the HMD + lcd->per_sweep.activeLighthouse = -1; + lcd->per_sweep.activeSweepStartTime = 0; + lcd->per_sweep.activeAcode = 0; + } + */ + } + else if (time_since_last_sync < 24000) + { + lcd->per_sweep.activeLighthouse = -1; + + lcd->per_sweep.recent_sync_time = le->timestamp; + // I do believe we are lighthouse B + lcd->per_sweep.current_lh = 1; +// lcd->per_sweep.lh_pulse_len[lcd->per_sweep.current_lh] = le->length; + lcd->per_sweep.lh_start_time[lcd->per_sweep.current_lh] = le->timestamp; + lcd->per_sweep.lh_max_pulse_length[lcd->per_sweep.current_lh] = le->length; + lcd->per_sweep.lh_acode[lcd->per_sweep.current_lh] = acode; + +/* + if (!(acode >> 2 & 1)) // if the skip bit is not set + { + if (lcd->per_sweep.activeLighthouse != -1) + { + static int pulseWarningCount=0; + + if (pulseWarningCount < 5) + { + pulseWarningCount++; + // hmm, it appears we got two non-skip pulses at the same time. That should never happen + fprintf(stderr, "WARNING: Two non-skip pulses received on the same cycle!\n"); + } + } + + lcd->per_sweep.activeLighthouse = 1; + lcd->per_sweep.activeSweepStartTime = le->timestamp; + lcd->per_sweep.activeAcode = acode; + } + */ + + } + else if (time_since_last_sync > 370000) + { + // XXX CAUTION: if we lose sight of a lighthouse then, the remaining lighthouse will default to master + //this should probably be fixed. Maybe some kind of timing based guess at which lighthouse. + + // looks like this is the first sync pulse. Cool! + + // first, send out the sync pulse data for the last round (for OOTX decoding + { + if (lcd->per_sweep.lh_max_pulse_length[0] != 0) + { + so->ctx->lightproc( + so, + -1, + handle_lightcap2_getAcodeFromSyncPulse(so, lcd->per_sweep.lh_max_pulse_length[0]), + lcd->per_sweep.lh_max_pulse_length[0], + lcd->per_sweep.lh_start_time[0], + 0, + 0); + } + if (lcd->per_sweep.lh_max_pulse_length[1] != 0) + { + so->ctx->lightproc( + so, + -2, + handle_lightcap2_getAcodeFromSyncPulse(so, lcd->per_sweep.lh_max_pulse_length[1]), + lcd->per_sweep.lh_max_pulse_length[1], + lcd->per_sweep.lh_start_time[1], + 0, + 1); + } + } + + //fprintf(stderr, "************************************ Reinitializing Disambiguator!!!\n"); + // initialize here. + memset(&lcd->per_sweep, 0, sizeof(lcd->per_sweep)); + lcd->per_sweep.activeLighthouse = -1; + + for (uint8_t i=0; i < NUM_LIGHTHOUSES;++i) { + lcd->per_sweep.lh_acode[i] = -1; + } + + lcd->per_sweep.recent_sync_time = le->timestamp; + // I do believe we are lighthouse A + lcd->per_sweep.current_lh = 0; +// lcd->per_sweep.lh_pulse_len[lcd->per_sweep.current_lh] = le->length; + lcd->per_sweep.lh_start_time[lcd->per_sweep.current_lh] = le->timestamp; + lcd->per_sweep.lh_max_pulse_length[lcd->per_sweep.current_lh] = le->length; + lcd->per_sweep.lh_acode[lcd->per_sweep.current_lh] = acode; + +// int acode = handle_lightcap2_getAcodeFromSyncPulse(so, le->length); + +/* + if (!(acode >> 2 & 1)) // if the skip bit is not set + { + lcd->per_sweep.activeLighthouse = 0; + lcd->per_sweep.activeSweepStartTime = le->timestamp; + lcd->per_sweep.activeAcode = acode; + } + */ + } + +// printf("%d %d\n", acode, lcd->per_sweep.activeLighthouse ); +} + +void handle_lightcap2_sweep(SurviveObject * so, LightcapElement * le ) +{ + lightcap2_data *lcd = so->disambiguator_data; + + // If we see multiple "hits" on the sweep for a given sensor, + // assume that the longest (i.e. strongest signal) is most likely + // the non-reflected signal. + + //if (le->length < 80) + //{ + // // this is a low-quality read. Better to throw it out than to use it. + // //fprintf(stderr, "%2d %d\n", le->sensor_id, le->length); + // return; + //} + //fprintf(stderr, "%2d %d\n", le->sensor_id, le->length); + //fprintf(stderr, "."); + + lcd->per_sweep.activeLighthouse = -1; + lcd->per_sweep.activeSweepStartTime = 0; + lcd->per_sweep.activeAcode = 0; + + for (uint8_t i=0; i < NUM_LIGHTHOUSES;++i) { + int acode = lcd->per_sweep.lh_acode[i]; + if ( (acode>=0) && !(acode >> 2 & 1)) { + lcd->per_sweep.activeLighthouse = i; + lcd->per_sweep.activeSweepStartTime = lcd->per_sweep.lh_start_time[i]; + lcd->per_sweep.activeAcode = acode; + } + } + + if (lcd->per_sweep.activeLighthouse < 0) { + fprintf(stderr, "WARNING: No active lighthouse!\n"); + fprintf(stderr, " %2d %8d %d %d\n", le->sensor_id, le->length,lcd->per_sweep.lh_acode[0],lcd->per_sweep.lh_acode[1]); + return; + } + + if (lcd->sweep.sweep_len[le->sensor_id] < le->length) + { + lcd->sweep.sweep_len[le->sensor_id] = le->length; + lcd->sweep.sweep_time[le->sensor_id] = le->timestamp; + } +} + +void handle_lightcap2( SurviveObject * so, LightcapElement * le ) +{ + SurviveContext * ctx = so->ctx; + + if (so->disambiguator_data == NULL) + { + fprintf(stderr, "Initializing Disambiguator Data\n"); + so->disambiguator_data = malloc(sizeof(lightcap2_data)); + memset(so->disambiguator_data, 0, sizeof(lightcap2_data)); + } + + if( le->sensor_id > SENSORS_PER_OBJECT ) + { + return; + } + + if (le->length > 6750) + { + // Should never get a reading so high. Odd. + return; + } +// if (le->length >= 2750) + if (le->length >= 2500) + { + // Looks like a sync pulse, process it! + handle_lightcap2_sync(so, le); + return; + } + + // must be a sweep pulse, process it! + handle_lightcap2_sweep(so, le); + +} + + +#endif + + +int32_t decode_acode(uint32_t length, int32_t main_divisor) { + //+50 adds a small offset and seems to help always get it right. + //Check the +50 in the future to see how well this works on a variety of hardware. + if( !main_divisor ) return -1; + int32_t acode = (length+main_divisor+50)/(main_divisor*2); + if( acode & 1 ) return -1; + + return (acode>>1) - 6; +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +///////The charles disambiguator. Don't use this, mostly here for debugging./////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + + +void HandleOOTX( SurviveContext * ctx, SurviveObject * so ) +{ + int32_t main_divisor = so->timebase_hz / 384000; //125 @ 48 MHz. + + int32_t acode_array[2] = + { + decode_acode(so->last_sync_length[0],main_divisor), + decode_acode(so->last_sync_length[1],main_divisor) + }; + + + int32_t delta1 = so->last_sync_time[0] - so->recent_sync_time; + int32_t delta2 = so->last_sync_time[1] - so->last_sync_time[0]; + + //printf( "%p %p %d %d %d %p\n", ctx, so, so->last_sync_time[0], acode_array, so->last_sync_length[0], ctx->lightproc ); + if( acode_array[0] >= 0 ) ctx->lightproc( so, -1, acode_array[0], delta1, so->last_sync_time[0], so->last_sync_length[0], 0 ); + if( acode_array[1] >= 0 ) ctx->lightproc( so, -2, acode_array[1], delta2, so->last_sync_time[1], so->last_sync_length[1], 1 ); + + so->recent_sync_time = so->last_sync_time[1]; + + so->did_handle_ootx = 1; +} + //This is the disambiguator function, for taking light timing and figuring out place-in-sweep for a given photodiode. void handle_lightcap( SurviveObject * so, LightcapElement * le ) { SurviveContext * ctx = so->ctx; - //int32_t deltat = (uint32_t)le->timestamp - (uint32_t)so->last_master_time; - //if( so->codename[0] != 'H' ) +#ifdef USE_TURVEYBIGUATOR + handle_lightcap2(so,le); + return; +#else + //printf( "LE%3d%6d%12d\n", le->sensor_id, le->length, le->timestamp ); + + //int32_t deltat = (uint32_t)le->timestamp - (uint32_t)so->last_master_time; if( le->sensor_id > SENSORS_PER_OBJECT ) { return; } +#if 0 + if( so->codename[0] == 'H' ) + { + static int lt; + static int last; + if( le->length > 1000 ) + { + int dl = le->timestamp - lt; + lt = le->timestamp; + if( dl > 10000 || dl < -10000 ) + printf( "+++%s %3d %5d %9d ", so->codename, le->sensor_id, le->length, dl ); + if( dl > 100000 ) printf(" \n" ); + } + last=le->length; + } +#endif + so->tsl = le->timestamp; if( le->length < 20 ) return; ///Assuming 20 is an okay value for here. @@ -27,6 +552,9 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) //unified driver. int ssn = so->sync_set_number; //lighthouse number if( ssn < 0 ) ssn = 0; +#ifdef DEBUG + if( ssn >= NUM_LIGHTHOUSES ) { SV_INFO( "ALGORITHMIC WARNING: ssn exceeds NUM_LIGHTHOUSES" ); } +#endif int last_sync_time = so->last_sync_time [ssn]; int last_sync_length = so->last_sync_length[ssn]; int32_t delta = le->timestamp - last_sync_time; //Handle time wrapping (be sure to be int32) @@ -44,17 +572,39 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) { int is_new_pulse = delta > so->pulselength_min_sync /*1500*/ + last_sync_length; -// printf("m sync %d %d %d %d\n", le->sensor_id, so->last_sync_time[ssn], le->timestamp, delta); - so->did_handle_ootx = 0; + if( is_new_pulse ) { int is_master_sync_pulse = delta > so->pulse_in_clear_time /*40000*/; + int is_pulse_from_same_lh_as_last_sweep; + int tp = delta % ( so->timecenter_ticks * 2); + is_pulse_from_same_lh_as_last_sweep = tp < so->pulse_synctime_slack && tp > -so->pulse_synctime_slack; + + if( !so->did_handle_ootx ) + { + HandleOOTX( ctx, so ); + } + if( !is_master_sync_pulse ) + { + so->did_handle_ootx = 0; + } + - if( is_master_sync_pulse ) + if( is_master_sync_pulse ) //Could also be called by slave if no master was seen. { - ssn = so->sync_set_number = 0; + ssn = so->sync_set_number = is_pulse_from_same_lh_as_last_sweep?(so->sync_set_number):0; //If repeated lighthouse, just back off one. + if( ssn < 0 ) { SV_INFO( "SEVERE WARNING: Pulse codes for tracking not able to be backed out.\n" ); ssn = 0; } + if( ssn != 0 ) + { + //If it's the slave that is repeated, be sure to zero out its sync info. + so->last_sync_length[0] = 0; + } + else + { + so->last_sync_length[1] = 0; + } so->last_sync_time[ssn] = le->timestamp; so->last_sync_length[ssn] = le->length; } @@ -65,6 +615,7 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) else { ssn = ++so->sync_set_number; + if( so->sync_set_number >= NUM_LIGHTHOUSES ) { SV_INFO( "Warning. Received an extra, unassociated sync pulse." ); @@ -89,9 +640,20 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) } } } - } +#if 0 + //Extra tidbit for storing length-of-sync-pulses, if you want to try to use this to determine AoI or distance to LH. + //We don't actually use this anywhere, and I doubt we ever will? Though, it could be useful at a later time to improve tracking. + { + int32_t main_divisor = so->timebase_hz / 384000; //125 @ 48 MHz. + int base_station = is_new_pulse; + printf( "%s %d %d %d\n", so->codename, le->sensor_id, so->sync_set_number, le->length ); //XXX sync_set_number is wrong here. + ctx->lightproc( so, le->sensor_id, -3 - so->sync_set_number, 0, le->timestamp, le->length, base_station); //XXX sync_set_number is wrong here. + } +#endif + } + //Any else- statements below here are //See if this is a valid actual pulse. else if( le->length < so->pulse_max_for_sweep && delta > so->pulse_in_clear_time && ssn >= 0 ) @@ -109,73 +671,20 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) //Make sure it fits nicely into a divisible-by-500 time. int32_t main_divisor = so->timebase_hz / 384000; //125 @ 48 MHz. - - int32_t acode_array[2] = - { - (so->last_sync_length[0]+main_divisor+50)/(main_divisor*2), //+50 adds a small offset and seems to help always get it right. - (so->last_sync_length[1]+main_divisor+50)/(main_divisor*2), //Check the +50 in the future to see how well this works on a variety of hardware. - }; - - //XXX: TODO: Capture error count here. - if( acode_array[0] & 1 ) return; - if( acode_array[1] & 1 ) return; - - acode_array[0] = (acode_array[0]>>1) - 6; - acode_array[1] = (acode_array[1]>>1) - 6; - - - int acode = acode_array[0]; + int acode = decode_acode(so->last_sync_length[0],main_divisor); if( !so->did_handle_ootx ) - { - int32_t delta1 = so->last_sync_time[0] - so->recent_sync_time; - int32_t delta2 = so->last_sync_time[1] - so->last_sync_time[0]; - - ctx->lightproc( so, -1, acode_array[0], delta1, so->last_sync_time[0], so->last_sync_length[0] ); - ctx->lightproc( so, -2, acode_array[1], delta2, so->last_sync_time[1], so->last_sync_length[1] ); - - so->recent_sync_time = so->last_sync_time[1]; - - //Throw out everything if our sync pulses look like they're bad. - - int32_t center_1 = so->timecenter_ticks*2 - so->pulse_synctime_offset; - int32_t center_2 = so->pulse_synctime_offset; - int32_t slack = so->pulse_synctime_slack; - - if( delta1 < center_1 - slack || delta1 > center_1 + slack ) - { - //XXX: TODO: Count faults. - so->sync_set_number = -1; - return; - } - - if( delta2 < center_2 - slack || delta2 > center_2 + slack ) - { - //XXX: TODO: Count faults. - so->sync_set_number = -1; - return; - } - - so->did_handle_ootx = 1; - } - - - if (acode > 3) { - if( ssn == 0 ) - { - //SV_INFO( "Warning: got a slave marker but only got a master sync." ); - //This happens too frequently. Consider further examination. - } - dl = so->last_sync_time[1]; - tpco = so->last_sync_length[1]; - } + HandleOOTX( ctx, so ); int32_t offset_from = le->timestamp - dl + le->length/2; //Make sure pulse is in valid window - if( offset_from < 380000 && offset_from > 70000 ) + if( offset_from < so->timecenter_ticks*2-so->pulse_in_clear_time && offset_from > so->pulse_in_clear_time ) { - ctx->lightproc( so, le->sensor_id, acode, offset_from, le->timestamp, le->length ); + int whichlh; + if( acode < 0 ) whichlh = 1; + else whichlh = !(acode>>2); + ctx->lightproc( so, le->sensor_id, acode, offset_from, le->timestamp, le->length, whichlh ); } } else @@ -183,6 +692,7 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) //printf( "FAIL %d %d - %d = %d\n", le->length, so->last_photo_time, le->timestamp, so->last_photo_time - le->timestamp ); //Runt pulse, or no sync pulses available. } +#endif } diff --git a/src/survive_process.c b/src/survive_process.c index 8dc849a..3af2da9 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -6,16 +6,19 @@ //XXX TODO: Once data is avialble in the context, use the stuff here to handle converting from time codes to //proper angles, then from there perform the rest of the solution. -void survive_default_light_process( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ) +void survive_default_light_process( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lh) { SurviveContext * ctx = so->ctx; - int base_station = acode >> 2; + int base_station = lh; int axis = acode & 1; if( ctx->calptr ) { - survive_cal_light( so, sensor_id, acode, timeinsweep, timecode, length ); + survive_cal_light( so, sensor_id, acode, timeinsweep, timecode, length, lh); } + //We don't use sync times, yet. + if( acode < -1 ) return; + if( base_station > NUM_LIGHTHOUSES ) return; //No loner need sync information past this point. @@ -34,16 +37,16 @@ void survive_default_light_process( SurviveObject * so, int sensor_id, int acode #endif FLT length_sec = length / (FLT)so->timebase_hz; - ctx->angleproc( so, sensor_id, acode, timecode, length_sec, angle ); + ctx->angleproc( so, sensor_id, acode, timecode, length_sec, angle, lh); } -void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle ) +void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh) { SurviveContext * ctx = so->ctx; if( ctx->calptr ) { - survive_cal_angle( so, sensor_id, acode, timecode, length, angle ); + survive_cal_angle( so, sensor_id, acode, timecode, length, angle, lh ); } if( so->PoserFn ) { @@ -54,6 +57,7 @@ void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode .timecode = timecode, .length = length, .angle = angle, + .lh = lh, }; so->PoserFn( so, (PoserData *)&l ); } diff --git a/src/survive_vive.c b/src/survive_vive.c index cdc319d..9a3cb03 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -22,6 +22,8 @@ #include <malloc.h> // for alloca #endif +#include "json_helpers.h" + #ifdef HIDAPI #if defined(WINDOWS) || defined(WIN32) || defined (_WIN32) #include <windows.h> @@ -39,48 +41,60 @@ struct SurviveViveData; const short vidpids[] = { - 0x0bb4, 0x2c87, 0, //The main HTC HMD device - 0x28de, 0x2000, 0, //Valve lighthouse + 0x0bb4, 0x2c87, 0, //Valve HMD Button and face proximity sensor + 0x28de, 0x2000, 0, //Valve HMD IMU & Lighthouse Sensors 0x28de, 0x2101, 0, //Valve Watchman 0x28de, 0x2101, 1, //Valve Watchman 0x28de, 0x2022, 0, //HTC Tracker + 0x28de, 0x2012, 0, //Valve Watchman, USB connected #ifdef HIDAPI - 0x28de, 0x2000, 1, //Valve lighthouse(B) (only used on HIDAPI, for lightcap) + 0x28de, 0x2000, 1, //Valve HMD lighthouse(B) (only used on HIDAPI, for lightcap) + 0x28de, 0x2022, 1, //HTC Tracker (only used on HIDAPI, for lightcap) + 0x28de, 0x2012, 1, //Valve Watchman, USB connected (only used on HIDAPI, for lightcap) #endif }; //length MAX_USB_INTERFACES*2 const char * devnames[] = { "HMD", - "Lighthouse", + "HMD IMU & LH", "Watchman 1", "Watchman 2", "Tracker 0", + "Wired Watchman 1", #ifdef HIDAPI - "Lightcap", + "HMD Lightcap", + "Tracker 0 Lightcap", + "Wired Watchman 1 Lightcap", #endif }; //length MAX_USB_INTERFACES #define USB_DEV_HMD 0 -#define USB_DEV_LIGHTHOUSE 1 +#define USB_DEV_HMD_IMU_LH 1 #define USB_DEV_WATCHMAN1 2 #define USB_DEV_WATCHMAN2 3 #define USB_DEV_TRACKER0 4 +#define USB_DEV_W_WATCHMAN1 5 // Wired Watchman attached via USB #ifdef HIDAPI -#define USB_DEV_LIGHTHOUSEB 5 -#define MAX_USB_DEVS 6 +#define USB_DEV_HMD_IMU_LHB 6 +#define USB_DEV_TRACKER0_LIGHTCAP 7 +#define USB_DEV_W_WATCHMAN1_LIGHTCAP 8 +#define MAX_USB_DEVS 9 #else -#define MAX_USB_DEVS 5 +#define MAX_USB_DEVS 6 #endif #define USB_IF_HMD 0 -#define USB_IF_LIGHTHOUSE 1 +#define USB_IF_HMD_IMU_LH 1 #define USB_IF_WATCHMAN1 2 #define USB_IF_WATCHMAN2 3 #define USB_IF_TRACKER0 4 -#define USB_IF_LIGHTCAP 5 -#define MAX_INTERFACES 6 +#define USB_IF_W_WATCHMAN1 5 +#define USB_IF_LIGHTCAP 6 +#define USB_IF_TRACKER0_LIGHTCAP 7 +#define USB_IF_W_WATCHMAN1_LIGHTCAP 8 +#define MAX_INTERFACES 9 typedef struct SurviveUSBInterface SurviveUSBInterface; typedef struct SurviveViveData SurviveViveData; @@ -133,10 +147,10 @@ void survive_data_cb( SurviveUSBInterface * si ); //USB Subsystem void survive_usb_close( SurviveContext * t ); -int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject *wm0, SurviveObject * wm1, SurviveObject * tr0 ); +int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject *wm0, SurviveObject * wm1, SurviveObject * tr0 , SurviveObject * ww0 ); int survive_usb_poll( SurviveContext * ctx ); int survive_get_config( char ** config, SurviveViveData * ctx, int devno, int iface, int send_extra_magic ); -int survive_vive_send_magic(struct SurviveContext * ctx, void * drv, int magic_code, void * data, int datalen ); +int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, void * data, int datalen ); #ifdef HIDAPI void * HAPIReceiver( void * v ) @@ -169,8 +183,8 @@ void * HAPIReceiver( void * v ) #else static void handle_transfer(struct libusb_transfer* transfer) { - struct SurviveUSBInterface * iface = transfer->user_data; - struct SurviveContext * ctx = iface->ctx; + SurviveUSBInterface * iface = transfer->user_data; + SurviveContext * ctx = iface->ctx; if( transfer->status != LIBUSB_TRANSFER_COMPLETED ) { @@ -299,9 +313,9 @@ static inline int hid_get_feature_report_timeout(USBHANDLE device, uint16_t ifac return -1; } -int survive_usb_init( struct SurviveViveData * sv, struct SurviveObject * hmd, struct SurviveObject *wm0, struct SurviveObject * wm1, struct SurviveObject * tr0 ) +int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject *wm0, SurviveObject * wm1, SurviveObject * tr0 , SurviveObject * ww0 ) { - struct SurviveContext * ctx = sv->ctx; + SurviveContext * ctx = sv->ctx; #ifdef HIDAPI if( !GlobalRXUSBMutx ) { @@ -332,7 +346,8 @@ int survive_usb_init( struct SurviveViveData * sv, struct SurviveObject * hmd, s if (cur_dev->vendor_id == vendor_id && cur_dev->product_id == product_id) { - if( menum == enumid ) + if( cur_dev->interface_number == enumid || + cur_dev->interface_number == -1 && menum == enumid) { path_to_open = cur_dev->path; break; @@ -358,6 +373,7 @@ int survive_usb_init( struct SurviveViveData * sv, struct SurviveObject * hmd, s wchar_t wstr[255]; res = hid_get_serial_number_string(handle, wstr, 255); + printf("Found %s. ", devnames[i]); wprintf(L"Serial Number String: (%d) %s for %04x:%04x@%d (Dev: %p)\n", wstr[0], wstr,vendor_id, product_id, menum, handle); sv->udev[i] = handle; @@ -459,15 +475,23 @@ int survive_usb_init( struct SurviveViveData * sv, struct SurviveObject * hmd, s #endif if( sv->udev[USB_DEV_HMD] && AttachInterface( sv, hmd, USB_IF_HMD, sv->udev[USB_DEV_HMD], 0x81, survive_data_cb, "Mainboard" ) ) { return -6; } - if( sv->udev[USB_DEV_LIGHTHOUSE] && AttachInterface( sv, hmd, USB_IF_LIGHTHOUSE, sv->udev[USB_DEV_LIGHTHOUSE], 0x81, survive_data_cb, "Lighthouse" ) ) { return -7; } + if( sv->udev[USB_DEV_HMD_IMU_LH] && AttachInterface( sv, hmd, USB_IF_HMD_IMU_LH, sv->udev[USB_DEV_HMD_IMU_LH], 0x81, survive_data_cb, "Lighthouse" ) ) { return -7; } if( sv->udev[USB_DEV_WATCHMAN1] && AttachInterface( sv, wm0, USB_IF_WATCHMAN1, sv->udev[USB_DEV_WATCHMAN1], 0x81, survive_data_cb, "Watchman 1" ) ) { return -8; } if( sv->udev[USB_DEV_WATCHMAN2] && AttachInterface( sv, wm1, USB_IF_WATCHMAN2, sv->udev[USB_DEV_WATCHMAN2], 0x81, survive_data_cb, "Watchman 2")) { return -9; } if( sv->udev[USB_DEV_TRACKER0] && AttachInterface( sv, tr0, USB_IF_TRACKER0, sv->udev[USB_DEV_TRACKER0], 0x81, survive_data_cb, "Tracker 1")) { return -10; } + if( sv->udev[USB_DEV_W_WATCHMAN1] && AttachInterface( sv, ww0, USB_IF_W_WATCHMAN1, sv->udev[USB_DEV_W_WATCHMAN1], 0x81, survive_data_cb, "Wired Watchman 1")) { return -11; } #ifdef HIDAPI //Tricky: use other interface for actual lightcap. XXX THIS IS NOT YET RIGHT!!! - if( sv->udev[USB_DEV_LIGHTHOUSEB] && AttachInterface( sv, hmd, USB_IF_LIGHTCAP, sv->udev[USB_DEV_LIGHTHOUSEB], 0x82, survive_data_cb, "Lightcap")) { return -12; } + if( sv->udev[USB_DEV_HMD_IMU_LHB] && AttachInterface( sv, hmd, USB_IF_LIGHTCAP, sv->udev[USB_DEV_HMD_IMU_LHB], 0x82, survive_data_cb, "Lightcap")) { return -12; } + + // This is a HACK! But it works. Need to investigate further + sv->uiface[USB_DEV_TRACKER0_LIGHTCAP].actual_len = 64; + if( sv->udev[USB_DEV_TRACKER0_LIGHTCAP] && AttachInterface( sv, tr0, USB_IF_TRACKER0_LIGHTCAP, sv->udev[USB_DEV_TRACKER0_LIGHTCAP], 0x82, survive_data_cb, "Tracker 1 Lightcap")) { return -13; } + if( sv->udev[USB_DEV_W_WATCHMAN1_LIGHTCAP] && AttachInterface( sv, ww0, USB_IF_W_WATCHMAN1_LIGHTCAP, sv->udev[USB_DEV_W_WATCHMAN1_LIGHTCAP], 0x82, survive_data_cb, "Wired Watchman 1 Lightcap")) { return -13; } #else - if( sv->udev[USB_DEV_LIGHTHOUSE] && AttachInterface( sv, hmd, USB_IF_LIGHTCAP, sv->udev[USB_DEV_LIGHTHOUSE], 0x82, survive_data_cb, "Lightcap")) { return -12; } + if( sv->udev[USB_DEV_HMD_IMU_LH] && AttachInterface( sv, hmd, USB_IF_LIGHTCAP, sv->udev[USB_DEV_HMD_IMU_LH], 0x82, survive_data_cb, "Lightcap")) { return -12; } + if( sv->udev[USB_DEV_TRACKER0] && AttachInterface( sv, ww0, USB_IF_TRACKER0_LIGHTCAP, sv->udev[USB_DEV_TRACKER0], 0x82, survive_data_cb, "Tracker 0 Lightcap")) { return -13; } + if( sv->udev[USB_DEV_W_WATCHMAN1] && AttachInterface( sv, ww0, USB_IF_W_WATCHMAN1_LIGHTCAP, sv->udev[USB_DEV_W_WATCHMAN1], 0x82, survive_data_cb, "Wired Watchman 1 Lightcap")) { return -13; } #endif SV_INFO( "All enumerated devices attached." ); @@ -477,10 +501,10 @@ int survive_usb_init( struct SurviveViveData * sv, struct SurviveObject * hmd, s return 0; } -int survive_vive_send_magic(struct SurviveContext * ctx, void * drv, int magic_code, void * data, int datalen ) +int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, void * data, int datalen ) { int r; - struct SurviveViveData * sv = drv; + SurviveViveData * sv = drv; printf( "*CALLING %p %p\n", ctx, sv ); //XXX TODO: Handle haptics, etc. @@ -497,27 +521,100 @@ int survive_vive_send_magic(struct SurviveContext * ctx, void * drv, int magic_c if( r != sizeof( vive_magic_power_on ) ) return 5; } - if (sv->udev[USB_DEV_LIGHTHOUSE]) + if (sv->udev[USB_DEV_HMD_IMU_LH]) + { + static uint8_t vive_magic_enable_lighthouse[5] = { 0x04 }; + r = update_feature_report( sv->udev[USB_DEV_HMD_IMU_LH], 0, vive_magic_enable_lighthouse, sizeof( vive_magic_enable_lighthouse ) ); + if( r != sizeof( vive_magic_enable_lighthouse ) ) return 5; + + static uint8_t vive_magic_enable_lighthouse2[5] = { 0x07, 0x02 }; //Switch to 0x25 mode (able to get more light updates) + r = update_feature_report( sv->udev[USB_DEV_HMD_IMU_LH], 0, vive_magic_enable_lighthouse2, sizeof( vive_magic_enable_lighthouse2 ) ); + if( r != sizeof( vive_magic_enable_lighthouse2 ) ) return 5; + } + + if (sv->udev[USB_DEV_W_WATCHMAN1]) + { + static uint8_t vive_magic_power_on[5] = { 0x04 }; + r = update_feature_report( sv->udev[USB_DEV_W_WATCHMAN1], 0, vive_magic_power_on, sizeof( vive_magic_power_on ) ); + if( r != sizeof( vive_magic_power_on ) ) return 5; + } + +#ifdef HIDAPI + if (sv->udev[USB_DEV_W_WATCHMAN1_LIGHTCAP]) + { + static uint8_t vive_magic_enable_lighthouse[5] = { 0x04 }; + r = update_feature_report( sv->udev[USB_DEV_W_WATCHMAN1_LIGHTCAP], 0, vive_magic_enable_lighthouse, sizeof( vive_magic_enable_lighthouse ) ); + if( r != sizeof( vive_magic_enable_lighthouse ) ) return 5; + + static uint8_t vive_magic_enable_lighthouse2[5] = { 0x07, 0x02 }; //Switch to 0x25 mode (able to get more light updates) + r = update_feature_report( sv->udev[USB_DEV_W_WATCHMAN1_LIGHTCAP], 0, vive_magic_enable_lighthouse2, sizeof( vive_magic_enable_lighthouse2 ) ); + if( r != sizeof( vive_magic_enable_lighthouse2 ) ) return 5; + } + +#else + if (sv->udev[USB_DEV_W_WATCHMAN1]) + { + static uint8_t vive_magic_enable_lighthouse[5] = { 0x04 }; + r = update_feature_report( sv->udev[USB_DEV_W_WATCHMAN1], 0, vive_magic_enable_lighthouse, sizeof( vive_magic_enable_lighthouse ) ); + if( r != sizeof( vive_magic_enable_lighthouse ) ) return 5; + + static uint8_t vive_magic_enable_lighthouse2[5] = { 0x07, 0x02 }; //Switch to 0x25 mode (able to get more light updates) + r = update_feature_report( sv->udev[USB_DEV_W_WATCHMAN1], 0, vive_magic_enable_lighthouse2, sizeof( vive_magic_enable_lighthouse2 ) ); + if( r != sizeof( vive_magic_enable_lighthouse2 ) ) return 5; + } + +#endif + + if (sv->udev[USB_DEV_TRACKER0]) + { + static uint8_t vive_magic_power_on[5] = { 0x04 }; + r = update_feature_report( sv->udev[USB_DEV_TRACKER0], 0, vive_magic_power_on, sizeof( vive_magic_power_on ) ); + if( r != sizeof( vive_magic_power_on ) ) return 5; + } +//#ifdef HIDAPI +// if (sv->udev[USB_DEV_TRACKER0_LIGHTCAP]) +// { +// static uint8_t vive_magic_enable_lighthouse[5] = { 0x04 }; +// r = update_feature_report( sv->udev[USB_DEV_TRACKER0_LIGHTCAP], 0, vive_magic_enable_lighthouse, sizeof( vive_magic_enable_lighthouse ) ); +// if( r != sizeof( vive_magic_enable_lighthouse ) ) return 5; +// +// static uint8_t vive_magic_enable_lighthouse2[5] = { 0x07, 0x02 }; //Switch to 0x25 mode (able to get more light updates) +// r = update_feature_report( sv->udev[USB_DEV_TRACKER0_LIGHTCAP], 0, vive_magic_enable_lighthouse2, sizeof( vive_magic_enable_lighthouse2 ) ); +// if( r != sizeof( vive_magic_enable_lighthouse2 ) ) return 5; +// } +//#else + if (sv->udev[USB_DEV_TRACKER0]) { static uint8_t vive_magic_enable_lighthouse[5] = { 0x04 }; - r = update_feature_report( sv->udev[USB_DEV_LIGHTHOUSE], 0, vive_magic_enable_lighthouse, sizeof( vive_magic_enable_lighthouse ) ); + r = update_feature_report( sv->udev[USB_DEV_TRACKER0], 0, vive_magic_enable_lighthouse, sizeof( vive_magic_enable_lighthouse ) ); if( r != sizeof( vive_magic_enable_lighthouse ) ) return 5; static uint8_t vive_magic_enable_lighthouse2[5] = { 0x07, 0x02 }; //Switch to 0x25 mode (able to get more light updates) - r = update_feature_report( sv->udev[USB_DEV_LIGHTHOUSE], 0, vive_magic_enable_lighthouse2, sizeof( vive_magic_enable_lighthouse2 ) ); + r = update_feature_report( sv->udev[USB_DEV_TRACKER0], 0, vive_magic_enable_lighthouse2, sizeof( vive_magic_enable_lighthouse2 ) ); if( r != sizeof( vive_magic_enable_lighthouse2 ) ) return 5; } +//#endif + #if 0 - for( i = 0; i < 256; i++ ) + for( int i = 0; i < 256; i++ ) { static uint8_t vive_controller_haptic_pulse[64] = { 0xff, 0x8f, 0xff, 0, 0, 0, 0, 0, 0, 0 }; - r = update_feature_report( sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_haptic_pulse, sizeof( vive_controller_haptic_pulse ) ); + //r = update_feature_report( sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_haptic_pulse, sizeof( vive_controller_haptic_pulse ) ); + r = update_feature_report( sv->udev[USB_DEV_W_WATCHMAN1_LIGHTCAP], 0, vive_controller_haptic_pulse, sizeof( vive_controller_haptic_pulse ) ); SV_INFO( "UCR: %d", r ); if( r != sizeof( vive_controller_haptic_pulse ) ) return 5; OGUSleep( 1000 ); } #endif + + //if (sv->udev[USB_DEV_TRACKER0]) + //{ + // static uint8_t vive_magic_power_on[64] = { 0x04, 0x78, 0x29, 0x38 }; + // r = update_feature_report( sv->udev[USB_DEV_TRACKER0], 0, vive_magic_power_on, sizeof( vive_magic_power_on ) ); + // if( r != sizeof( vive_magic_power_on ) ) return 5; + //} + SV_INFO( "Powered unit on." ); } else @@ -553,7 +650,7 @@ int survive_vive_send_magic(struct SurviveContext * ctx, void * drv, int magic_c return 0; } -void survive_vive_usb_close( struct SurviveViveData * sv ) +void survive_vive_usb_close( SurviveViveData * sv ) { int i; #ifdef HIDAPI @@ -578,18 +675,19 @@ void survive_vive_usb_close( struct SurviveViveData * sv ) #endif } -int survive_vive_usb_poll( struct SurviveContext * ctx, void * v ) +int survive_vive_usb_poll( SurviveContext * ctx, void * v ) { #ifdef HIDAPI OGUnlockMutex( GlobalRXUSBMutx ); OGUSleep( 100 ); OGUnlockMutex( GlobalRXUSBMutx ); + return 0; #else - struct SurviveViveData * sv = v; + SurviveViveData * sv = v; int r = libusb_handle_events( sv->usbctx ); if( r ) { - struct SurviveContext * ctx = sv->ctx; + SurviveContext * ctx = sv->ctx; SV_ERROR( "Libusb poll failed." ); } return r; @@ -598,9 +696,9 @@ int survive_vive_usb_poll( struct SurviveContext * ctx, void * v ) } -int survive_get_config( char ** config, struct SurviveViveData * sv, int devno, int iface, int send_extra_magic ) +int survive_get_config( char ** config, SurviveViveData * sv, int devno, int iface, int send_extra_magic ) { - struct SurviveContext * ctx = sv->ctx; + SurviveContext * ctx = sv->ctx; int ret, count = 0, size = 0; uint8_t cfgbuff[64]; uint8_t compressed_data[8192]; @@ -629,11 +727,9 @@ int survive_get_config( char ** config, struct SurviveViveData * sv, int devno, #else //Switch mode to pull config? - SV_INFO( "XXX TODO See if this can be update_feature_report" ); for( k = 0; k < 10; k++ ) { - int rk = libusb_control_transfer(dev, LIBUSB_REQUEST_TYPE_CLASS | LIBUSB_RECIPIENT_INTERFACE | LIBUSB_ENDPOINT_OUT, - 0x09, 0x300 | cfgbuff_send[0], iface, cfgbuff_send, 64, 1000 ); + update_feature_report( dev, iface, cfgbuff_send, 64 ); OGUSleep(1000); } #endif @@ -643,6 +739,7 @@ int survive_get_config( char ** config, struct SurviveViveData * sv, int devno, OGUSleep(1000); } + // Send Report 16 to prepare the device for reading config info memset( cfgbuff, 0, sizeof( cfgbuff ) ); cfgbuff[0] = 0x10; if( ( ret = hid_get_feature_report_timeout( dev, iface, cfgbuff, sizeof( cfgbuff ) ) ) < 0 ) @@ -652,6 +749,7 @@ int survive_get_config( char ** config, struct SurviveViveData * sv, int devno, } + // Now do a bunch of Report 17 until there are no bytes left cfgbuff[1] = 0xaa; cfgbuff[0] = 0x11; do @@ -716,7 +814,36 @@ int survive_get_config( char ** config, struct SurviveViveData * sv, int devno, #define POP2 (*(((uint16_t*)((readdata+=2)-2)))) #define POP4 (*(((uint32_t*)((readdata+=4)-4)))) -static void handle_watchman( struct SurviveObject * w, uint8_t * readdata ) +void calibrate_acc(SurviveObject* so, FLT* agm) { + if (so->acc_bias != NULL) { + agm[0] -= so->acc_bias[0]; + agm[1] -= so->acc_bias[1]; + agm[2] -= so->acc_bias[2]; + } + + if (so->acc_scale != NULL) { + agm[0] *= so->acc_scale[0]; + agm[1] *= so->acc_scale[1]; + agm[2] *= so->acc_scale[2]; + } +} + +void calibrate_gyro(SurviveObject* so, FLT* agm) { + if (so->gyro_bias != NULL) { + agm[0] -= so->gyro_bias[0]; + agm[1] -= so->gyro_bias[1]; + agm[2] -= so->gyro_bias[2]; + } + + if (so->gyro_scale != NULL) { + agm[0] *= so->gyro_scale[0]; + agm[1] *= so->gyro_scale[1]; + agm[2] *= so->gyro_scale[2]; + } +} + + +static void handle_watchman( SurviveObject * w, uint8_t * readdata ) { uint8_t startread[29]; @@ -739,7 +866,6 @@ static void handle_watchman( struct SurviveObject * w, uint8_t * readdata ) int propset = 0; int doimu = 0; - if( (type & 0xf0) == 0xf0 ) { propset |= 4; @@ -794,6 +920,21 @@ static void handle_watchman( struct SurviveObject * w, uint8_t * readdata ) readdata[4], readdata[5], readdata[6], 0,0,0 }; +// if (w->acc_scale) printf("%f %f %f\n",w->acc_scale[0],w->acc_scale[1],w->acc_scale[2]); + calibrate_acc(w, agm); + + //I don't understand where these numbers come from but the data from the WMD seems to max out at 255... + agm[0]*=(1.0f/255.0f); + agm[1]*=(1.0f/255.0f); + agm[2]*=(1.0f/255.0f); + + calibrate_gyro(w, agm+3); + + //I don't understand where these numbers come from but the data from the WMD seems to max out at 255... + agm[3]*=(1.0f/255.0f); + agm[4]*=(1.0f/255.0f); + agm[5]*=(1.0f/255.0f); + w->ctx->imuproc( w, 3, agm, (time1<<24)|(time2<<16)|readdata[0], 0 ); int16_t * k = (int16_t *)readdata+1; @@ -815,10 +956,9 @@ static void handle_watchman( struct SurviveObject * w, uint8_t * readdata ) *readdata = type; //Put 'type' back on stack. uint8_t * mptr = readdata + qty-3-1; //-3 for timecode, -1 to -//#define DEBUG_WATCHMAN #ifdef DEBUG_WATCHMAN printf( "_%s ", w->codename); - for( i = 0; i < qty; i++ ) + for(int i = 0; i < qty; i++ ) { printf( "%02x ", readdata[i] ); } @@ -883,11 +1023,12 @@ static void handle_watchman( struct SurviveObject * w, uint8_t * readdata ) LightcapElement les[10]; int lese = 0; //les's end + //Second, go through all LEDs and extract the lightevent from them. { uint8_t *marked; marked = alloca(nrtime); - memset( marked, 0, sizeof( marked ) ); + memset( marked, 0, nrtime ); int i, parpl = 0; timecount--; int timepl = 0; @@ -900,8 +1041,20 @@ static void handle_watchman( struct SurviveObject * w, uint8_t * readdata ) led >>= 3; while( marked[timepl] ) timepl++; + +#ifdef DEBUG_WATCHMAN + int i; + printf( "TP %d TC: %d : ", timepl, timecount ); + for( i = 0; i < nrtime; i++ ) + { + printf( "%d", marked[i] ); + } + printf( "\n" ); +#endif + if( timepl > timecount ) { fault = 3; goto end; } //Ran off max of list. uint32_t endtime = times[timepl++]; + int end = timepl + adv; if( end > timecount ) { fault = 4; goto end; } //end referencing off list if( marked[end] > 0 ) { fault = 5; goto end; } //Already marked trying to be used. @@ -945,12 +1098,11 @@ static void handle_watchman( struct SurviveObject * w, uint8_t * readdata ) end: { SurviveContext * ctx = w->ctx; - SV_INFO( "Light decoding fault: %d\n", fault ); + SV_INFO( "Light decoding fault: %d", fault ); } } } - void survive_data_cb( SurviveUSBInterface * si ) { int size = si->actual_len; @@ -982,7 +1134,7 @@ void survive_data_cb( SurviveUSBInterface * si ) { case USB_IF_HMD: { - struct SurviveObject * headset = obj; + SurviveObject * headset = obj; readdata+=2; headset->buttonmask = POP1; //Lens headset->axis2 = POP2; //Lens Separation @@ -996,7 +1148,9 @@ void survive_data_cb( SurviveUSBInterface * si ) headset->ison = 1; break; } - case USB_IF_LIGHTHOUSE: + case USB_IF_HMD_IMU_LH: + case USB_IF_W_WATCHMAN1: + case USB_IF_TRACKER0: { int i; //printf( "%d -> ", size ); @@ -1018,6 +1172,23 @@ void survive_data_cb( SurviveUSBInterface * si ) acceldata[3], acceldata[4], acceldata[5], 0,0,0 }; + agm[0]*=(float)(1./8192.0); + agm[1]*=(float)(1./8192.0); + agm[2]*=(float)(1./8192.0); + calibrate_acc(obj, agm); + + //1G for accelerometer, from MPU6500 datasheet + //this can change if the firmware changes the sensitivity. + + + agm[3]*=(float)((1./32.768)*(3.14159/180.)); + agm[4]*=(float)((1./32.768)*(3.14159/180.)); + agm[5]*=(float)((1./32.768)*(3.14159/180.)); + calibrate_gyro(obj, agm+3); + + //65.5 deg/s for gyroscope, from MPU6500 datasheet + //1000 deg/s for gyroscope, from MPU6500 datasheet + ctx->imuproc( obj, 3, agm, timecode, code ); } } @@ -1048,22 +1219,6 @@ void survive_data_cb( SurviveUSBInterface * si ) } break; } - case USB_IF_TRACKER0: - { - SurviveObject * w = obj; - if( id == 32 ) - { - // TODO: Looks like this will need to be handle_tracker, since - // it appears the interface is sufficiently different. - // More work needd to reverse engineer it. - handle_watchman( w, readdata); - } - else - { - SV_INFO( "Unknown tracker code %d\n", id ); - } - break; - } case USB_IF_LIGHTCAP: { int i; @@ -1073,7 +1228,25 @@ void survive_data_cb( SurviveUSBInterface * si ) le.sensor_id = POP1; le.length = POP2; le.timestamp = POP4; - if( le.sensor_id == 0xff ) break; + if( le.sensor_id > 0xfd ) continue; + handle_lightcap( obj, &le ); + } + break; + } + case USB_IF_W_WATCHMAN1_LIGHTCAP: + case USB_IF_TRACKER0_LIGHTCAP: + { + int i=0; + for( i = 0; i < 7; i++ ) + { + unsigned short *sensorId = (unsigned short *)readdata; + unsigned short *length = (unsigned short *)(&(readdata[2])); + unsigned long *time = (unsigned long *)(&(readdata[4])); + LightcapElement le; + le.sensor_id = (uint8_t)POP2; + le.length = POP2; + le.timestamp = POP4; + if( le.sensor_id > 0xfd ) continue; // handle_lightcap( obj, &le ); } break; @@ -1199,6 +1372,40 @@ printf( "Loading config: %d\n", len ); break; } } + + + if (jsoneq(ct0conf, tk, "acc_bias") == 0) { + int32_t count = (tk+1)->size; + FLT* values = NULL; + if ( parse_float_array(ct0conf, tk+2, &values, count) >0 ) { + so->acc_bias = values; + so->acc_bias[0] *= .125; //XXX Wat? Observed by CNL. Biasing by more than this seems to hose things. + so->acc_bias[1] *= .125; + so->acc_bias[2] *= .125; + } + } + if (jsoneq(ct0conf, tk, "acc_scale") == 0) { + int32_t count = (tk+1)->size; + FLT* values = NULL; + if ( parse_float_array(ct0conf, tk+2, &values, count) >0 ) { + so->acc_scale = values; + } + } + + if (jsoneq(ct0conf, tk, "gyro_bias") == 0) { + int32_t count = (tk+1)->size; + FLT* values = NULL; + if ( parse_float_array(ct0conf, tk+2, &values, count) >0 ) { + so->gyro_bias = values; + } + } + if (jsoneq(ct0conf, tk, "gyro_scale") == 0) { + int32_t count = (tk+1)->size; + FLT* values = NULL; + if ( parse_float_array(ct0conf, tk+2, &values, count) >0 ) { + so->gyro_scale = values; + } + } } } else @@ -1238,6 +1445,12 @@ int survive_vive_close( SurviveContext * ctx, void * driver ) return 0; } +void init_SurviveObject(SurviveObject* so) { + so->acc_scale = NULL; + so->acc_bias = NULL; + so->gyro_scale = NULL; + so->gyro_bias = NULL; +} int DriverRegHTCVive( SurviveContext * ctx ) { @@ -1246,8 +1459,15 @@ int DriverRegHTCVive( SurviveContext * ctx ) SurviveObject * wm0 = calloc( 1, sizeof( SurviveObject ) ); SurviveObject * wm1 = calloc( 1, sizeof( SurviveObject ) ); SurviveObject * tr0 = calloc( 1, sizeof( SurviveObject ) ); + SurviveObject * ww0 = calloc( 1, sizeof( SurviveObject ) ); SurviveViveData * sv = calloc( 1, sizeof( SurviveViveData ) ); + init_SurviveObject(hmd); + init_SurviveObject(wm0); + init_SurviveObject(wm1); + init_SurviveObject(tr0); + init_SurviveObject(ww0); + sv->ctx = ctx; #ifdef _WIN32 @@ -1271,32 +1491,50 @@ int DriverRegHTCVive( SurviveContext * ctx ) tr0->ctx = ctx; memcpy( tr0->codename, "TR0", 4 ); memcpy( tr0->drivername, "HTC", 4 ); + ww0->ctx = ctx; + memcpy( ww0->codename, "WW0", 4 ); + memcpy( ww0->drivername, "HTC", 4 ); //USB must happen last. - if( r = survive_usb_init( sv, hmd, wm0, wm1, tr0) ) + if( r = survive_usb_init( sv, hmd, wm0, wm1, tr0, ww0) ) { //TODO: Cleanup any libUSB stuff sitting around. goto fail_gracefully; } //Next, pull out the config stuff. - if( sv->udev[USB_DEV_HMD] && LoadConfig( sv, hmd, 1, 0, 0 )) { SV_INFO( "HMD config issue." ); } + if( sv->udev[USB_DEV_HMD_IMU_LH] && LoadConfig( sv, hmd, 1, 0, 0 )) { SV_INFO( "HMD config issue." ); } if( sv->udev[USB_DEV_WATCHMAN1] && LoadConfig( sv, wm0, 2, 0, 1 )) { SV_INFO( "Watchman 0 config issue." ); } if( sv->udev[USB_DEV_WATCHMAN2] && LoadConfig( sv, wm1, 3, 0, 1 )) { SV_INFO( "Watchman 1 config issue." ); } - if( sv->udev[USB_DEV_TRACKER0] && LoadConfig( sv, tr0, 4, 0, 1 )) { SV_INFO( "Tracker 0 config issue." ); } + if( sv->udev[USB_DEV_TRACKER0] && LoadConfig( sv, tr0, 4, 0, 0 )) { SV_INFO( "Tracker 0 config issue." ); } + if( sv->udev[USB_DEV_W_WATCHMAN1] && LoadConfig( sv, ww0, 5, 0, 0 )) { SV_INFO( "Wired Watchman 0 config issue." ); } hmd->timebase_hz = wm0->timebase_hz = wm1->timebase_hz = 48000000; + tr0->timebase_hz = ww0->timebase_hz = hmd->timebase_hz; + hmd->pulsedist_max_ticks = wm0->pulsedist_max_ticks = wm1->pulsedist_max_ticks = 500000; + tr0->pulsedist_max_ticks = ww0->pulsedist_max_ticks = hmd->pulsedist_max_ticks; + hmd->pulselength_min_sync = wm0->pulselength_min_sync = wm1->pulselength_min_sync = 2200; + tr0->pulselength_min_sync = ww0->pulselength_min_sync = hmd->pulselength_min_sync; + hmd->pulse_in_clear_time = wm0->pulse_in_clear_time = wm1->pulse_in_clear_time = 35000; + tr0->pulse_in_clear_time = ww0->pulse_in_clear_time = hmd->pulse_in_clear_time; + hmd->pulse_max_for_sweep = wm0->pulse_max_for_sweep = wm1->pulse_max_for_sweep = 1800; + tr0->pulse_max_for_sweep = ww0->pulse_max_for_sweep = hmd->pulse_max_for_sweep; hmd->pulse_synctime_offset = wm0->pulse_synctime_offset = wm1->pulse_synctime_offset = 20000; + tr0->pulse_synctime_offset = ww0->pulse_synctime_offset = hmd->pulse_synctime_offset; + hmd->pulse_synctime_slack = wm0->pulse_synctime_slack = wm1->pulse_synctime_slack = 5000; + tr0->pulse_synctime_slack = ww0->pulse_synctime_slack = hmd->pulse_synctime_slack; hmd->timecenter_ticks = hmd->timebase_hz / 240; wm0->timecenter_ticks = wm0->timebase_hz / 240; wm1->timecenter_ticks = wm1->timebase_hz / 240; + tr0->timecenter_ticks = tr0->timebase_hz / 240; + ww0->timecenter_ticks = ww0->timebase_hz / 240; /* int i; int locs = hmd->nr_locations; @@ -1320,10 +1558,11 @@ int DriverRegHTCVive( SurviveContext * ctx ) */ //Add the drivers. - if( sv->udev[USB_DEV_HMD] ) { survive_add_object( ctx, hmd ); } + if( sv->udev[USB_DEV_HMD_IMU_LH] ) { survive_add_object( ctx, hmd ); } if( sv->udev[USB_DEV_WATCHMAN1] ) { survive_add_object( ctx, wm0 ); } if( sv->udev[USB_DEV_WATCHMAN2] ) { survive_add_object( ctx, wm1 ); } if( sv->udev[USB_DEV_TRACKER0] ) { survive_add_object( ctx, tr0 ); } + if( sv->udev[USB_DEV_W_WATCHMAN1] ) { survive_add_object( ctx, ww0 ); } survive_add_driver( ctx, sv, survive_vive_usb_poll, survive_vive_close, survive_vive_send_magic ); @@ -1333,6 +1572,7 @@ fail_gracefully: free( wm0 ); free( wm1 ); free( tr0 ); + free( ww0 ); survive_vive_usb_close( sv ); free( sv ); return -1; @@ -1,14 +1,41 @@ +#ifdef __linux__ #include <unistd.h> +#endif #include <stdio.h> #include <stdlib.h> #include <stdint.h> #include <survive.h> #include <os_generic.h> -#include <DrawFunctions.h> +#include <CNFGFunctions.h> struct SurviveContext * ctx; +void HandleKey( int keycode, int bDown ) +{ + if( !bDown ) return; + + if( keycode == 'O' || keycode == 'o' ) + { + survive_send_magic(ctx,1,0,0); + } + if( keycode == 'F' || keycode == 'f' ) + { + survive_send_magic(ctx,0,0,0); + } +} + +void HandleButton( int x, int y, int button, int bDown ) +{ +} + +void HandleMotion( int x, int y, int mask ) +{ +} + +void HandleDestroy() +{ +} static void dump_iface( struct SurviveObject * so, const char * prefix ) @@ -56,6 +83,7 @@ int main() dump_iface( survive_get_so_by_name( ctx, "WM0" ), "WM0" ); dump_iface( survive_get_so_by_name( ctx, "WM1" ), "WM1" ); dump_iface( survive_get_so_by_name( ctx, "TR0" ), "TR0" ); + dump_iface( survive_get_so_by_name( ctx, "WW0" ), "WW0" ); while(survive_poll(ctx) == 0) { diff --git a/useful_files/FunctionalSystem.dia b/useful_files/FunctionalSystem.dia Binary files differnew file mode 100644 index 0000000..d231ef5 --- /dev/null +++ b/useful_files/FunctionalSystem.dia diff --git a/useful_files/FunctionalSystem.png b/useful_files/FunctionalSystem.png Binary files differnew file mode 100644 index 0000000..030f464 --- /dev/null +++ b/useful_files/FunctionalSystem.png diff --git a/useful_files/sample.config.json b/useful_files/sample.config.json new file mode 100644 index 0000000..e12594a --- /dev/null +++ b/useful_files/sample.config.json @@ -0,0 +1,8 @@ +"_Comment0":"This file must be named config.json and placed in the same directory as the executable", +"_Comment1":"Valid Posers Are: PoserCharlesSlow, PoserDaveOrtho, PoserDummy, PoserOctavioRadii, PoserTurveyTori", +"DefaultPoser":"PoserTurveyTori", +"ConfigPoser":"PoserTurveyTori", +"_Comment2":"RequiredTrackersForCal takes a comma separated list of devices required for calibration to pass. Valid options are: HMD,WM0,WM1,TR0,WW0", +"RequiredTrackersForCal":"", +"_Comment3":"If set, AllowAllTrackersForCal will use all trackers for calibration. Otherwise only required trackers will be used.", +"AllowAllTrackersForCal":"1", diff --git a/winbuild/build_tcc.bat b/winbuild/build_tcc.bat index 46f4beb..5be1361 100644 --- a/winbuild/build_tcc.bat +++ b/winbuild/build_tcc.bat @@ -7,7 +7,7 @@ set SR=..\src\ set RD=..\redist\ set SOURCES=%SR%ootx_decoder.c %SR%poser_charlesslow.c %SR%poser_daveortho.c %SR%poser_dummy.c %SR%survive.c %SR%survive_cal.c %SR%survive_config.c %SR%survive_data.c %SR%survive_driverman.c %SR%survive_process.c %SR%survive_vive.c set REDIST=%RD%crc32.c %RD%linmath.c %RD%puff.c %RD%jsmn.c %RD%json_helpers.c %RD%symbol_enumerator.c -set EXEC=..\calibrate.c %RD%WinDriver.c %RD%os_generic.c %RD%DrawFunctions.c +set EXEC=..\calibrate.c %RD%CNFGWinDriver.c %RD%os_generic.c %RD%CNFGFunctions.c set CFLAGS=-DNOZLIB -DTCC -DWINDOWS -DHIDAPI -DWIN32 -DRUNTIME_SYMNUM -O0 -g -rdynamic -I..\redist -I..\include\libsurvive -I..\src -I. set LDFLAGS=-lkernel32 -lgdi32 -luser32 -lsetupapi -ldbghelp @echo on diff --git a/winbuild/data_recorder/data_recorder.vcxproj b/winbuild/data_recorder/data_recorder.vcxproj new file mode 100644 index 0000000..59a1e77 --- /dev/null +++ b/winbuild/data_recorder/data_recorder.vcxproj @@ -0,0 +1,178 @@ +<?xml version="1.0" encoding="utf-8"?> +<Project DefaultTargets="Build" ToolsVersion="15.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003"> + <ItemGroup Label="ProjectConfigurations"> + <ProjectConfiguration Include="Debug|Win32"> + <Configuration>Debug</Configuration> + <Platform>Win32</Platform> + </ProjectConfiguration> + <ProjectConfiguration Include="Release|Win32"> + <Configuration>Release</Configuration> + <Platform>Win32</Platform> + </ProjectConfiguration> + <ProjectConfiguration Include="Debug|x64"> + <Configuration>Debug</Configuration> + <Platform>x64</Platform> + </ProjectConfiguration> + <ProjectConfiguration Include="Release|x64"> + <Configuration>Release</Configuration> + <Platform>x64</Platform> + </ProjectConfiguration> + </ItemGroup> + <PropertyGroup Label="Globals"> + <VCProjectVersion>15.0</VCProjectVersion> + <ProjectGuid>{265C4E2C-66CB-4954-97CA-194D69B5A673}</ProjectGuid> + <Keyword>Win32Proj</Keyword> + <RootNamespace>data_recorder</RootNamespace> + <WindowsTargetPlatformVersion>10.0.14393.0</WindowsTargetPlatformVersion> + </PropertyGroup> + <Import Project="$(VCTargetsPath)\Microsoft.Cpp.Default.props" /> + <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'" Label="Configuration"> + <ConfigurationType>Application</ConfigurationType> + <UseDebugLibraries>true</UseDebugLibraries> + <PlatformToolset>v141</PlatformToolset> + <CharacterSet>MultiByte</CharacterSet> + </PropertyGroup> + <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'" Label="Configuration"> + <ConfigurationType>Application</ConfigurationType> + <UseDebugLibraries>false</UseDebugLibraries> + <PlatformToolset>v141</PlatformToolset> + <WholeProgramOptimization>true</WholeProgramOptimization> + <CharacterSet>MultiByte</CharacterSet> + </PropertyGroup> + <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'" Label="Configuration"> + <ConfigurationType>Application</ConfigurationType> + <UseDebugLibraries>true</UseDebugLibraries> + <PlatformToolset>v141</PlatformToolset> + <CharacterSet>MultiByte</CharacterSet> + </PropertyGroup> + <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'" Label="Configuration"> + <ConfigurationType>Application</ConfigurationType> + <UseDebugLibraries>false</UseDebugLibraries> + <PlatformToolset>v141</PlatformToolset> + <WholeProgramOptimization>true</WholeProgramOptimization> + <CharacterSet>MultiByte</CharacterSet> + </PropertyGroup> + <Import Project="$(VCTargetsPath)\Microsoft.Cpp.props" /> + <ImportGroup Label="ExtensionSettings"> + </ImportGroup> + <ImportGroup Label="Shared"> + </ImportGroup> + <ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'"> + <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" /> + </ImportGroup> + <ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|Win32'"> + <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" /> + </ImportGroup> + <ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Debug|x64'"> + <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" /> + </ImportGroup> + <ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|x64'"> + <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" /> + </ImportGroup> + <PropertyGroup Label="UserMacros" /> + <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'"> + <LinkIncremental>true</LinkIncremental> + </PropertyGroup> + <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'"> + <LinkIncremental>true</LinkIncremental> + </PropertyGroup> + <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'"> + <LinkIncremental>false</LinkIncremental> + </PropertyGroup> + <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'"> + <LinkIncremental>false</LinkIncremental> + </PropertyGroup> + <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'"> + <ClCompile> + <PrecompiledHeader> + </PrecompiledHeader> + <WarningLevel>Level3</WarningLevel> + <Optimization>Disabled</Optimization> + <PreprocessorDefinitions>USE_DOUBLE;RUNTIME_SYMNUMX;HIDAPI;_CRT_SECURE_NO_WARNINGS;_DEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions> + <AdditionalIncludeDirectories>..\..\windows;..\..\include\libsurvive;..\..\redist;</AdditionalIncludeDirectories> + </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;%(AdditionalDependencies)</AdditionalDependencies> + <GenerateDebugInformation>true</GenerateDebugInformation> + <LinkTimeCodeGeneration>UseFastLinkTimeCodeGeneration</LinkTimeCodeGeneration> + </Link> + <ProjectReference> + <UseLibraryDependencyInputs>true</UseLibraryDependencyInputs> + </ProjectReference> + </ItemDefinitionGroup> + <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'"> + <ClCompile> + <PrecompiledHeader> + </PrecompiledHeader> + <WarningLevel>Level3</WarningLevel> + <Optimization>Disabled</Optimization> + <PreprocessorDefinitions>USE_DOUBLE;HIDAPI;_CRT_SECURE_NO_WARNINGS;WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions> + <AdditionalIncludeDirectories>..\..\windows;..\..\include\libsurvive;..\..\redist;</AdditionalIncludeDirectories> + </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;%(AdditionalDependencies)</AdditionalDependencies> + <GenerateDebugInformation>true</GenerateDebugInformation> + <LinkTimeCodeGeneration>UseFastLinkTimeCodeGeneration</LinkTimeCodeGeneration> + </Link> + <ProjectReference> + <UseLibraryDependencyInputs>true</UseLibraryDependencyInputs> + </ProjectReference> + </ItemDefinitionGroup> + <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'"> + <ClCompile> + <WarningLevel>Level3</WarningLevel> + <PrecompiledHeader> + </PrecompiledHeader> + <Optimization>MaxSpeed</Optimization> + <FunctionLevelLinking>true</FunctionLevelLinking> + <IntrinsicFunctions>true</IntrinsicFunctions> + <PreprocessorDefinitions>USE_DOUBLE;HIDAPI;_CRT_SECURE_NO_WARNINGS;WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions> + <AdditionalIncludeDirectories>..\..\windows;..\..\include\libsurvive;..\..\redist;</AdditionalIncludeDirectories> + </ClCompile> + <Link> + <SubSystem>Console</SubSystem> + <EnableCOMDATFolding>true</EnableCOMDATFolding> + <OptimizeReferences>true</OptimizeReferences> + <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> + </Link> + <ProjectReference> + <UseLibraryDependencyInputs>true</UseLibraryDependencyInputs> + </ProjectReference> + </ItemDefinitionGroup> + <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'"> + <ClCompile> + <WarningLevel>Level3</WarningLevel> + <PrecompiledHeader> + </PrecompiledHeader> + <Optimization>MaxSpeed</Optimization> + <FunctionLevelLinking>true</FunctionLevelLinking> + <IntrinsicFunctions>true</IntrinsicFunctions> + <PreprocessorDefinitions>USE_DOUBLE;HIDAPI;_CRT_SECURE_NO_WARNINGS;NDEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions> + <AdditionalIncludeDirectories>..\..\windows;..\..\include\libsurvive;..\..\redist;</AdditionalIncludeDirectories> + </ClCompile> + <Link> + <SubSystem>Console</SubSystem> + <EnableCOMDATFolding>true</EnableCOMDATFolding> + <OptimizeReferences>true</OptimizeReferences> + <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> + </Link> + <ProjectReference> + <UseLibraryDependencyInputs>true</UseLibraryDependencyInputs> + </ProjectReference> + </ItemDefinitionGroup> + <ItemGroup> + <ProjectReference Include="..\libsurvive\libsurvive.vcxproj"> + <Project>{435cfd2c-8724-42ee-8fde-71ef7d2c6b2f}</Project> + </ProjectReference> + </ItemGroup> + <ItemGroup> + <ClCompile Include="..\..\data_recorder.c" /> + </ItemGroup> + <Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" /> + <ImportGroup Label="ExtensionTargets"> + </ImportGroup> +</Project>
\ No newline at end of file diff --git a/winbuild/data_recorder/data_recorder.vcxproj.filters b/winbuild/data_recorder/data_recorder.vcxproj.filters new file mode 100644 index 0000000..c696f0f --- /dev/null +++ b/winbuild/data_recorder/data_recorder.vcxproj.filters @@ -0,0 +1,22 @@ +<?xml version="1.0" encoding="utf-8"?> +<Project ToolsVersion="4.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003"> + <ItemGroup> + <Filter Include="Source Files"> + <UniqueIdentifier>{4FC737F1-C7A5-4376-A066-2A32D752A2FF}</UniqueIdentifier> + <Extensions>cpp;c;cc;cxx;def;odl;idl;hpj;bat;asm;asmx</Extensions> + </Filter> + <Filter Include="Header Files"> + <UniqueIdentifier>{93995380-89BD-4b04-88EB-625FBE52EBFB}</UniqueIdentifier> + <Extensions>h;hh;hpp;hxx;hm;inl;inc;xsd</Extensions> + </Filter> + <Filter Include="Resource Files"> + <UniqueIdentifier>{67DA6AB6-F800-4c08-8B7A-83BB121AAD01}</UniqueIdentifier> + <Extensions>rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms</Extensions> + </Filter> + </ItemGroup> + <ItemGroup> + <ClCompile Include="..\..\data_recorder.c"> + <Filter>Source Files</Filter> + </ClCompile> + </ItemGroup> +</Project>
\ No newline at end of file diff --git a/winbuild/libsurvive.sln b/winbuild/libsurvive.sln index 8924631..b525975 100644 --- a/winbuild/libsurvive.sln +++ b/winbuild/libsurvive.sln @@ -7,6 +7,10 @@ Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "calibrate", "calibrate\cali EndProject Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "libsurvive", "libsurvive\libsurvive.vcxproj", "{435CFD2C-8724-42EE-8FDE-71EF7D2C6B2F}" EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "data_recorder", "data_recorder\data_recorder.vcxproj", "{265C4E2C-66CB-4954-97CA-194D69B5A673}" +EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "test", "test\test.vcxproj", "{EF083B79-F1D7-408A-9902-502B9A0639E0}" +EndProject Global GlobalSection(SolutionConfigurationPlatforms) = preSolution Debug|x64 = Debug|x64 @@ -31,6 +35,22 @@ Global {435CFD2C-8724-42EE-8FDE-71EF7D2C6B2F}.Release|x64.Build.0 = Release|x64 {435CFD2C-8724-42EE-8FDE-71EF7D2C6B2F}.Release|x86.ActiveCfg = Release|Win32 {435CFD2C-8724-42EE-8FDE-71EF7D2C6B2F}.Release|x86.Build.0 = Release|Win32 + {265C4E2C-66CB-4954-97CA-194D69B5A673}.Debug|x64.ActiveCfg = Debug|x64 + {265C4E2C-66CB-4954-97CA-194D69B5A673}.Debug|x64.Build.0 = Debug|x64 + {265C4E2C-66CB-4954-97CA-194D69B5A673}.Debug|x86.ActiveCfg = Debug|Win32 + {265C4E2C-66CB-4954-97CA-194D69B5A673}.Debug|x86.Build.0 = Debug|Win32 + {265C4E2C-66CB-4954-97CA-194D69B5A673}.Release|x64.ActiveCfg = Release|x64 + {265C4E2C-66CB-4954-97CA-194D69B5A673}.Release|x64.Build.0 = Release|x64 + {265C4E2C-66CB-4954-97CA-194D69B5A673}.Release|x86.ActiveCfg = Release|Win32 + {265C4E2C-66CB-4954-97CA-194D69B5A673}.Release|x86.Build.0 = Release|Win32 + {EF083B79-F1D7-408A-9902-502B9A0639E0}.Debug|x64.ActiveCfg = Debug|x64 + {EF083B79-F1D7-408A-9902-502B9A0639E0}.Debug|x64.Build.0 = Debug|x64 + {EF083B79-F1D7-408A-9902-502B9A0639E0}.Debug|x86.ActiveCfg = Debug|Win32 + {EF083B79-F1D7-408A-9902-502B9A0639E0}.Debug|x86.Build.0 = Debug|Win32 + {EF083B79-F1D7-408A-9902-502B9A0639E0}.Release|x64.ActiveCfg = Release|x64 + {EF083B79-F1D7-408A-9902-502B9A0639E0}.Release|x64.Build.0 = Release|x64 + {EF083B79-F1D7-408A-9902-502B9A0639E0}.Release|x86.ActiveCfg = Release|Win32 + {EF083B79-F1D7-408A-9902-502B9A0639E0}.Release|x86.Build.0 = Release|Win32 EndGlobalSection GlobalSection(SolutionProperties) = preSolution HideSolutionNode = FALSE diff --git a/winbuild/libsurvive/libsurvive.vcxproj b/winbuild/libsurvive/libsurvive.vcxproj index 996f342..c794382 100644 --- a/winbuild/libsurvive/libsurvive.vcxproj +++ b/winbuild/libsurvive/libsurvive.vcxproj @@ -139,8 +139,9 @@ <Text Include="ReadMe.txt" /> </ItemGroup> <ItemGroup> + <ClCompile Include="..\..\redist\CNFGFunctions.c" /> + <ClCompile Include="..\..\redist\CNFGWinDriver.c" /> <ClCompile Include="..\..\redist\crc32.c" /> - <ClCompile Include="..\..\redist\DrawFunctions.c" /> <ClCompile Include="..\..\redist\hid-windows.c" /> <ClCompile Include="..\..\redist\jsmn.c" /> <ClCompile Include="..\..\redist\json_helpers.c" /> @@ -148,11 +149,12 @@ <ClCompile Include="..\..\redist\os_generic.c" /> <ClCompile Include="..\..\redist\puff.c" /> <ClCompile Include="..\..\redist\symbol_enumerator.c" /> - <ClCompile Include="..\..\redist\WinDriver.c" /> <ClCompile Include="..\..\src\ootx_decoder.c" /> <ClCompile Include="..\..\src\poser_charlesslow.c" /> <ClCompile Include="..\..\src\poser_daveortho.c" /> <ClCompile Include="..\..\src\poser_dummy.c" /> + <ClCompile Include="..\..\src\poser_octavioradii.c" /> + <ClCompile Include="..\..\src\poser_turveytori.c" /> <ClCompile Include="..\..\src\survive.c" /> <ClCompile Include="..\..\src\survive_cal.c" /> <ClCompile Include="..\..\src\survive_config.c" /> @@ -166,8 +168,8 @@ <ClInclude Include="..\..\include\libsurvive\poser.h" /> <ClInclude Include="..\..\include\libsurvive\survive.h" /> <ClInclude Include="..\..\include\libsurvive\survive_types.h" /> + <ClInclude Include="..\..\redist\CNFGFunctions.h" /> <ClInclude Include="..\..\redist\crc32.h" /> - <ClInclude Include="..\..\redist\DrawFunctions.h" /> <ClInclude Include="..\..\redist\jsmn.h" /> <ClInclude Include="..\..\redist\json_helpers.h" /> <ClInclude Include="..\..\redist\linmath.h" /> diff --git a/winbuild/libsurvive/libsurvive.vcxproj.filters b/winbuild/libsurvive/libsurvive.vcxproj.filters index 8be02f1..e7d44e2 100644 --- a/winbuild/libsurvive/libsurvive.vcxproj.filters +++ b/winbuild/libsurvive/libsurvive.vcxproj.filters @@ -60,12 +60,6 @@ <ClCompile Include="..\..\redist\puff.c"> <Filter>Source Files</Filter> </ClCompile> - <ClCompile Include="..\..\redist\DrawFunctions.c"> - <Filter>Source Files</Filter> - </ClCompile> - <ClCompile Include="..\..\redist\WinDriver.c"> - <Filter>Source Files</Filter> - </ClCompile> <ClCompile Include="..\..\redist\json_helpers.c"> <Filter>Source Files</Filter> </ClCompile> @@ -84,6 +78,18 @@ <ClCompile Include="..\..\redist\hid-windows.c"> <Filter>Source Files</Filter> </ClCompile> + <ClCompile Include="..\..\redist\CNFGFunctions.c"> + <Filter>Source Files</Filter> + </ClCompile> + <ClCompile Include="..\..\redist\CNFGWinDriver.c"> + <Filter>Source Files</Filter> + </ClCompile> + <ClCompile Include="..\..\src\poser_turveytori.c"> + <Filter>Source Files</Filter> + </ClCompile> + <ClCompile Include="..\..\src\poser_octavioradii.c"> + <Filter>Source Files</Filter> + </ClCompile> </ItemGroup> <ItemGroup> <ClInclude Include="..\..\src\ootx_decoder.h"> @@ -110,9 +116,6 @@ <ClInclude Include="..\..\redist\os_generic.h"> <Filter>Header Files</Filter> </ClInclude> - <ClInclude Include="..\..\redist\DrawFunctions.h"> - <Filter>Header Files</Filter> - </ClInclude> <ClInclude Include="..\..\redist\json_helpers.h"> <Filter>Header Files</Filter> </ClInclude> @@ -131,5 +134,8 @@ <ClInclude Include="..\hidapi.h"> <Filter>Header Files</Filter> </ClInclude> + <ClInclude Include="..\..\redist\CNFGFunctions.h"> + <Filter>Header Files</Filter> + </ClInclude> </ItemGroup> </Project>
\ No newline at end of file diff --git a/winbuild/test/test.vcxproj b/winbuild/test/test.vcxproj new file mode 100644 index 0000000..e6ee3fb --- /dev/null +++ b/winbuild/test/test.vcxproj @@ -0,0 +1,178 @@ +<?xml version="1.0" encoding="utf-8"?> +<Project DefaultTargets="Build" ToolsVersion="15.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003"> + <ItemGroup Label="ProjectConfigurations"> + <ProjectConfiguration Include="Debug|Win32"> + <Configuration>Debug</Configuration> + <Platform>Win32</Platform> + </ProjectConfiguration> + <ProjectConfiguration Include="Release|Win32"> + <Configuration>Release</Configuration> + <Platform>Win32</Platform> + </ProjectConfiguration> + <ProjectConfiguration Include="Debug|x64"> + <Configuration>Debug</Configuration> + <Platform>x64</Platform> + </ProjectConfiguration> + <ProjectConfiguration Include="Release|x64"> + <Configuration>Release</Configuration> + <Platform>x64</Platform> + </ProjectConfiguration> + </ItemGroup> + <PropertyGroup Label="Globals"> + <VCProjectVersion>15.0</VCProjectVersion> + <ProjectGuid>{EF083B79-F1D7-408A-9902-502B9A0639E0}</ProjectGuid> + <Keyword>Win32Proj</Keyword> + <RootNamespace>test</RootNamespace> + <WindowsTargetPlatformVersion>10.0.14393.0</WindowsTargetPlatformVersion> + </PropertyGroup> + <Import Project="$(VCTargetsPath)\Microsoft.Cpp.Default.props" /> + <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'" Label="Configuration"> + <ConfigurationType>Application</ConfigurationType> + <UseDebugLibraries>true</UseDebugLibraries> + <PlatformToolset>v141</PlatformToolset> + <CharacterSet>Unicode</CharacterSet> + </PropertyGroup> + <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'" Label="Configuration"> + <ConfigurationType>Application</ConfigurationType> + <UseDebugLibraries>false</UseDebugLibraries> + <PlatformToolset>v141</PlatformToolset> + <WholeProgramOptimization>true</WholeProgramOptimization> + <CharacterSet>Unicode</CharacterSet> + </PropertyGroup> + <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'" Label="Configuration"> + <ConfigurationType>Application</ConfigurationType> + <UseDebugLibraries>true</UseDebugLibraries> + <PlatformToolset>v141</PlatformToolset> + <CharacterSet>Unicode</CharacterSet> + </PropertyGroup> + <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'" Label="Configuration"> + <ConfigurationType>Application</ConfigurationType> + <UseDebugLibraries>false</UseDebugLibraries> + <PlatformToolset>v141</PlatformToolset> + <WholeProgramOptimization>true</WholeProgramOptimization> + <CharacterSet>Unicode</CharacterSet> + </PropertyGroup> + <Import Project="$(VCTargetsPath)\Microsoft.Cpp.props" /> + <ImportGroup Label="ExtensionSettings"> + </ImportGroup> + <ImportGroup Label="Shared"> + </ImportGroup> + <ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'"> + <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" /> + </ImportGroup> + <ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|Win32'"> + <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" /> + </ImportGroup> + <ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Debug|x64'"> + <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" /> + </ImportGroup> + <ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|x64'"> + <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" /> + </ImportGroup> + <PropertyGroup Label="UserMacros" /> + <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'"> + <LinkIncremental>true</LinkIncremental> + </PropertyGroup> + <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'"> + <LinkIncremental>true</LinkIncremental> + </PropertyGroup> + <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'"> + <LinkIncremental>false</LinkIncremental> + </PropertyGroup> + <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'"> + <LinkIncremental>false</LinkIncremental> + </PropertyGroup> + <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'"> + <ClCompile> + <PrecompiledHeader> + </PrecompiledHeader> + <WarningLevel>Level3</WarningLevel> + <Optimization>Disabled</Optimization> + <PreprocessorDefinitions>USE_DOUBLE;RUNTIME_SYMNUMX;HIDAPI;_CRT_SECURE_NO_WARNINGS;_DEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions> + <AdditionalIncludeDirectories>..\..\windows;..\..\include\libsurvive;..\..\redist;</AdditionalIncludeDirectories> + </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;%(AdditionalDependencies)</AdditionalDependencies> + <GenerateDebugInformation>true</GenerateDebugInformation> + <LinkTimeCodeGeneration>UseFastLinkTimeCodeGeneration</LinkTimeCodeGeneration> + </Link> + <ProjectReference> + <UseLibraryDependencyInputs>true</UseLibraryDependencyInputs> + </ProjectReference> + </ItemDefinitionGroup> + <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'"> + <ClCompile> + <PrecompiledHeader> + </PrecompiledHeader> + <WarningLevel>Level3</WarningLevel> + <Optimization>Disabled</Optimization> + <PreprocessorDefinitions>USE_DOUBLE;HIDAPI;_CRT_SECURE_NO_WARNINGS;WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions> + <AdditionalIncludeDirectories>..\..\windows;..\..\include\libsurvive;..\..\redist;</AdditionalIncludeDirectories> + </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;%(AdditionalDependencies)</AdditionalDependencies> + <GenerateDebugInformation>true</GenerateDebugInformation> + <LinkTimeCodeGeneration>UseFastLinkTimeCodeGeneration</LinkTimeCodeGeneration> + </Link> + <ProjectReference> + <UseLibraryDependencyInputs>true</UseLibraryDependencyInputs> + </ProjectReference> + </ItemDefinitionGroup> + <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'"> + <ClCompile> + <WarningLevel>Level3</WarningLevel> + <PrecompiledHeader> + </PrecompiledHeader> + <Optimization>MaxSpeed</Optimization> + <FunctionLevelLinking>true</FunctionLevelLinking> + <IntrinsicFunctions>true</IntrinsicFunctions> + <PreprocessorDefinitions>USE_DOUBLE;HIDAPI;_CRT_SECURE_NO_WARNINGS;WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions> + <AdditionalIncludeDirectories>..\..\windows;..\..\include\libsurvive;..\..\redist;</AdditionalIncludeDirectories> + </ClCompile> + <Link> + <SubSystem>Console</SubSystem> + <EnableCOMDATFolding>true</EnableCOMDATFolding> + <OptimizeReferences>true</OptimizeReferences> + <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> + </Link> + <ProjectReference> + <UseLibraryDependencyInputs>true</UseLibraryDependencyInputs> + </ProjectReference> + </ItemDefinitionGroup> + <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'"> + <ClCompile> + <WarningLevel>Level3</WarningLevel> + <PrecompiledHeader> + </PrecompiledHeader> + <Optimization>MaxSpeed</Optimization> + <FunctionLevelLinking>true</FunctionLevelLinking> + <IntrinsicFunctions>true</IntrinsicFunctions> + <PreprocessorDefinitions>USE_DOUBLE;HIDAPI;_CRT_SECURE_NO_WARNINGS;NDEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions> + <AdditionalIncludeDirectories>..\..\windows;..\..\include\libsurvive;..\..\redist;</AdditionalIncludeDirectories> + </ClCompile> + <Link> + <SubSystem>Console</SubSystem> + <EnableCOMDATFolding>true</EnableCOMDATFolding> + <OptimizeReferences>true</OptimizeReferences> + <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> + </Link> + <ProjectReference> + <UseLibraryDependencyInputs>true</UseLibraryDependencyInputs> + </ProjectReference> + </ItemDefinitionGroup> + <ItemGroup> + <ProjectReference Include="..\libsurvive\libsurvive.vcxproj"> + <Project>{435cfd2c-8724-42ee-8fde-71ef7d2c6b2f}</Project> + </ProjectReference> + </ItemGroup> + <ItemGroup> + <ClCompile Include="..\..\test.c" /> + </ItemGroup> + <Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" /> + <ImportGroup Label="ExtensionTargets"> + </ImportGroup> +</Project>
\ No newline at end of file diff --git a/winbuild/test/test.vcxproj.filters b/winbuild/test/test.vcxproj.filters new file mode 100644 index 0000000..60c3446 --- /dev/null +++ b/winbuild/test/test.vcxproj.filters @@ -0,0 +1,22 @@ +<?xml version="1.0" encoding="utf-8"?> +<Project ToolsVersion="4.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003"> + <ItemGroup> + <Filter Include="Source Files"> + <UniqueIdentifier>{4FC737F1-C7A5-4376-A066-2A32D752A2FF}</UniqueIdentifier> + <Extensions>cpp;c;cc;cxx;def;odl;idl;hpj;bat;asm;asmx</Extensions> + </Filter> + <Filter Include="Header Files"> + <UniqueIdentifier>{93995380-89BD-4b04-88EB-625FBE52EBFB}</UniqueIdentifier> + <Extensions>h;hh;hpp;hxx;hm;inl;inc;xsd</Extensions> + </Filter> + <Filter Include="Resource Files"> + <UniqueIdentifier>{67DA6AB6-F800-4c08-8B7A-83BB121AAD01}</UniqueIdentifier> + <Extensions>rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms</Extensions> + </Filter> + </ItemGroup> + <ItemGroup> + <ClCompile Include="..\..\test.c"> + <Filter>Source Files</Filter> + </ClCompile> + </ItemGroup> +</Project>
\ No newline at end of file |
