aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--Makefile32
-rw-r--r--calibrate.c80
-rwxr-xr-xdave/AffineSolvebin65416 -> 65424 bytes
-rw-r--r--dave/AffineSolve.c2
-rw-r--r--dave/ptinfo.csv157
-rw-r--r--include/libsurvive/survive.h7
-rw-r--r--redist/dclapack.h (renamed from dave/dclapack.h)0
-rw-r--r--redist/linmath.c167
-rw-r--r--redist/linmath.h9
-rw-r--r--redist/lintest.c112
-rw-r--r--src/poser_charlesslow.c343
-rw-r--r--src/poser_daveortho.c460
-rwxr-xr-xsrc/survive.c2
-rwxr-xr-xsrc/survive_cal.c168
-rw-r--r--src/survive_cal.h6
-rw-r--r--src/survive_cal_lhfind.c13
-rw-r--r--src/survive_config.c2
-rw-r--r--src/survive_data.c1
-rwxr-xr-xsrc/survive_vive.c4
-rw-r--r--tools/lighthousefind_radii/Makefile10
-rw-r--r--tools/lighthousefind_radii/lighthousefind_radii.c557
-rw-r--r--tools/lighthousefind_tori/torus_localizer.c141
22 files changed, 1972 insertions, 301 deletions
diff --git a/Makefile b/Makefile
index 1a1712d..29b2164 100644
--- a/Makefile
+++ b/Makefile
@@ -1,35 +1,41 @@
all : lib data_recorder test calibrate calibrate_client
+CC:=gcc
+
CFLAGS:=-Iinclude/libsurvive -I. -fPIC -g -O0 -Iredist -flto -DUSE_DOUBLE -std=gnu99
LDFLAGS:=-lpthread -lusb-1.0 -lz -lX11 -lm -flto -g
-CALS:=src/survive_cal_lhfind.o src/survive_cal.o
-POSERS:=src/poser_dummy.o
+POSERS:=src/poser_dummy.o src/poser_daveortho.o src/poser_charlesslow.o
REDISTS:=redist/json_helpers.o redist/linmath.o redist/jsmn.o
-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
-LIBSURVIVE_O:=$(CALS) $(POSERS) $(REDISTS) $(LIBSURVIVE_CORE)
+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_O:=$(POSERS) $(REDISTS) $(LIBSURVIVE_CORE)
+LIBSURVIVE_C:=$(LIBSURVIVE_O:.o=.c)
GRAPHICS_LOFI:=redist/DrawFunctions.o redist/XDriver.o
# unused: redist/crc32.c
-test : test.c lib/libsurvive.so redist/os_generic.o
- gcc -o $@ $^ $(LDFLAGS) $(CFLAGS)
+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)
+ $(CC) -o $@ $^ $(LDFLAGS) $(CFLAGS)
-data_recorder : data_recorder.c lib/libsurvive.so redist/os_generic.c $(GRAPHICS_LOFI)
- gcc -o $@ $^ $(LDFLAGS) $(CFLAGS)
+calibrate : calibrate.c ./lib/libsurvive.so redist/os_generic.c $(GRAPHICS_LOFI)
+ $(CC) -o $@ $^ $(LDFLAGS) $(CFLAGS)
-calibrate : calibrate.c lib/libsurvive.so redist/os_generic.c $(GRAPHICS_LOFI)
- gcc -o $@ $^ $(LDFLAGS) $(CFLAGS)
+calibrate_client : calibrate_client.c ./lib/libsurvive.so redist/os_generic.c $(GRAPHICS_LOFI)
+ $(CC) -o $@ $^ $(LDFLAGS) $(CFLAGS)
-calibrate_client : calibrate_client.c lib/libsurvive.so redist/os_generic.c $(GRAPHICS_LOFI)
- gcc -o $@ $^ $(LDFLAGS) $(CFLAGS)
+## Still not working!!! Don't use.
+static_calibrate : calibrate.c redist/os_generic.c redist/XDriver.c redist/DrawFunctions.c $(LIBSURVIVE_C)
+ tcc -o $@ $^ $(CFLAGS) $(LDFLAGS) -DTCC
lib:
mkdir lib
lib/libsurvive.so : $(LIBSURVIVE_O)
- gcc -o $@ $^ $(LDFLAGS) -shared
+ $(CC) -o $@ $^ $(LDFLAGS) -shared
clean :
rm -rf *.o src/*.o *~ src/*~ test data_recorder lib/libsurvive.so redist/*.o redist/*~
diff --git a/calibrate.c b/calibrate.c
index 82da7a7..667089a 100644
--- a/calibrate.c
+++ b/calibrate.c
@@ -13,6 +13,7 @@
#include "src/survive_config.h"
struct SurviveContext * ctx;
+int quit = 0;
void HandleKey( int keycode, int bDown )
{
@@ -26,6 +27,10 @@ void HandleKey( int keycode, int bDown )
{
survive_send_magic(ctx,0,0,0);
}
+ if( keycode == 'Q' || keycode == 'q' )
+ {
+ quit = 1;
+ }
}
void HandleButton( int x, int y, int button, int bDown )
@@ -36,9 +41,12 @@ void HandleMotion( int x, int y, int mask )
{
}
-int bufferpts[32*2*3];
+//int bufferpts[32*2*3][2];
+int bufferpts[32*2*3][2];
+
+
char buffermts[32*128*3];
-int buffertimeto[32*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 )
{
@@ -54,25 +62,25 @@ void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int
if( acode == 0 || acode == 2 ) //data = 0
{
- bufferpts[jumpoffset*2+0] = (timeinsweep-100000)/500;
- buffertimeto[jumpoffset] = 0;
+ bufferpts[jumpoffset*2+0][0] = (timeinsweep-100000)/500;
+ buffertimeto[jumpoffset][0] = 0;
}
if( acode == 1 || acode == 3 ) //data = 1
{
- bufferpts[jumpoffset*2+1] = (timeinsweep-100000)/500;
- buffertimeto[jumpoffset] = 0;
+ bufferpts[jumpoffset*2+1][0] = (timeinsweep-100000)/500;
+ buffertimeto[jumpoffset][0] = 0;
}
if( acode == 4 || acode == 6 ) //data = 0
{
- bufferpts[jumpoffset*2+0] = (timeinsweep-100000)/500;
- buffertimeto[jumpoffset] = 0;
+ bufferpts[jumpoffset*2+0][1] = (timeinsweep-100000)/500;
+ buffertimeto[jumpoffset][1] = 0;
}
if( acode == 5 || acode == 7 ) //data = 1
{
- bufferpts[jumpoffset*2+1] = (timeinsweep-100000)/500;
- buffertimeto[jumpoffset] = 0;
+ bufferpts[jumpoffset*2+1][1] = (timeinsweep-100000)/500;
+ buffertimeto[jumpoffset][1] = 0;
}
}
@@ -93,6 +101,7 @@ void my_angle_process( struct SurviveObject * so, int sensor_id, int acode, uint
survive_default_angle_process( so, sensor_id, acode, timecode, length, angle );
}
+char* sensor_name[32];
void * GuiThread( void * v )
{
@@ -108,23 +117,35 @@ void * GuiThread( void * v )
CNFGColor( 0xFFFFFF );
CNFGGetDimensions( &screenx, &screeny );
- int i;
+ int i,nn;
for( i = 0; i < 32*3; i++ )
{
- if( buffertimeto[i] < 50 )
+ for( nn = 0; nn < 2; nn++ )
{
- uint32_t color = i * 3231349;
- uint8_t r = color & 0xff;
- uint8_t g = (color>>8) & 0xff;
- uint8_t b = (color>>16) & 0xff;
- r = (r * (5-buffertimeto[i])) / 5 ;
- g = (g * (5-buffertimeto[i])) / 5 ;
- b = (b * (5-buffertimeto[i])) / 5 ;
- CNFGColor( (b<<16) | (g<<8) | r );
- CNFGTackRectangle( bufferpts[i*2+0], bufferpts[i*2+1], bufferpts[i*2+0] + 5, bufferpts[i*2+1] + 5 );
- CNFGPenX = bufferpts[i*2+0]; CNFGPenY = bufferpts[i*2+1];
- CNFGDrawText( buffermts, 2 );
- buffertimeto[i]++;
+ if( buffertimeto[i][nn] < 50 )
+ {
+ uint32_t color = i * 3231349;
+ uint8_t r = 0xff;
+ uint8_t g = 0x00;
+ uint8_t b = 0xff;
+
+ if (nn==0) b = 0; //lighthouse B
+ if (nn==1) r = 0; //lighthouse C
+
+// 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 );
+ 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 );
+
+ if (i<32) {
+ CNFGPenX = bufferpts[i*2+0][nn]+5; CNFGPenY = bufferpts[i*2+1][nn]+5;
+ CNFGDrawText( sensor_name[i], 2 );
+ }
+ buffertimeto[i][nn]++;
+ }
}
}
@@ -148,6 +169,12 @@ int main()
{
ctx = survive_init( 0 );
+ uint8_t i =0;
+ for (i=0;i<32;++i) {
+ sensor_name[i] = malloc(3);
+ sprintf(sensor_name[i],"%d",i);
+ }
+
survive_install_light_fn( ctx, my_light_process );
survive_install_imu_fn( ctx, my_imu_process );
survive_install_angle_fn( ctx, my_angle_process );
@@ -163,10 +190,13 @@ int main()
return 1;
}
- while(survive_poll(ctx) == 0)
+ while(survive_poll(ctx) == 0 && !quit)
{
//Do stuff.
}
+
+ survive_close( ctx );
+
printf( "Returned\n" );
}
diff --git a/dave/AffineSolve b/dave/AffineSolve
index cc4d26e..98a9590 100755
--- a/dave/AffineSolve
+++ b/dave/AffineSolve
Binary files differ
diff --git a/dave/AffineSolve.c b/dave/AffineSolve.c
index 1c873d9..4fba56b 100644
--- a/dave/AffineSolve.c
+++ b/dave/AffineSolve.c
@@ -11,7 +11,7 @@
#include <math.h>
#include "dclapack.h"
#include <linmath.h>
-#define LH_ID 1
+#define LH_ID 0
#define NUM_HMD 32
#define MAX_POINTS 128
diff --git a/dave/ptinfo.csv b/dave/ptinfo.csv
index 2dbef65..f4c5ea9 100644
--- a/dave/ptinfo.csv
+++ b/dave/ptinfo.csv
@@ -1,88 +1,69 @@
-0 0 0 1024 0.054304 8.087056 7.006970 0.000005 0.000095 49.911564
-0 0 1 1024 -0.091073 7.633667 2.124995 0.000002 0.000032 28.309638
-4 0 0 1024 0.046903 7.663818 9.134277 0.000014 0.000274 87.982354
-4 0 1 1024 -0.086459 8.770345 2.726730 0.000011 0.000192 29.506709
-5 0 0 1024 0.047980 4.033020 8.087671 0.000017 0.000218 50.499310
-5 0 1 1024 -0.077373 7.318115 2.557700 0.000005 0.000082 24.065775
-6 0 0 1024 0.062184 9.521423 7.429031 0.000003 0.000035 38.676732
-6 0 1 1024 -0.073973 9.523905 1.995981 0.000002 0.000029 26.113657
-7 0 0 1024 0.056254 9.999064 7.497290 0.000003 0.000044 38.734072
-7 0 1 1024 -0.079104 9.753560 2.103774 0.000004 0.000051 33.619688
-8 1 0 1024 -0.006291 9.665955 1.987640 0.000002 0.000028 15.453922
-8 1 1 1024 -0.117330 10.177124 1.600708 0.000001 0.000018 11.038096
-9 0 0 1025 0.062143 8.118984 6.820550 0.000004 0.000055 40.521475
-9 0 1 1023 -0.085365 8.509694 1.896965 0.000003 0.000091 21719098.365862
-9 1 0 1024 0.000557 6.315043 1.998499 0.000010 0.000087 21.686142
-9 1 1 1024 -0.113388 7.437968 1.836344 0.000002 0.000035 18.467548
-10 1 0 1024 -0.009943 9.343363 1.995316 0.000002 0.000031 17.584078
-10 1 1 1024 -0.124325 9.785502 1.689753 0.000002 0.000038 11.692163
-11 1 1 1024 -0.118290 6.393616 2.064949 0.000007 0.000127 19.804200
-12 1 0 1024 -0.014712 6.424642 2.678035 0.000011 0.000116 31.494265
-12 1 1 1024 -0.109233 7.731120 2.279953 0.000007 0.000190 18.323990
-15 0 0 1024 0.060565 6.858704 7.184426 0.000008 0.000088 44.052520
-15 0 1 1024 -0.094258 6.385498 2.018832 0.000010 0.000091 29.446135
-15 1 0 1024 -0.006425 3.219747 2.549749 0.000022 0.000239 26.656846
-15 1 1 1024 -0.106455 6.234049 1.960842 0.000006 0.000076 13.899017
-17 0 0 1024 0.066623 8.772827 6.714717 0.000003 0.000034 33.014899
-17 0 1 1024 -0.060226 8.462301 2.015937 0.000002 0.000022 32.443223
-23 1 1 1024 -0.155699 4.259481 2.133325 0.000009 0.000119 14.051344
-24 1 0 1024 0.004956 2.748250 3.060253 0.000032 0.000273 24.108629
-24 1 1 1024 -0.156870 7.030355 1.539771 0.000003 0.000047 11.937263
-27 1 0 1024 -0.004080 5.997660 2.117311 0.000010 0.000096 25.358188
-27 1 1 1024 -0.157007 8.123576 1.820049 0.000003 0.000063 11.321934
-28 1 0 1024 -0.007485 3.633423 2.815398 0.000022 0.000202 29.154675
-28 1 1 1024 -0.148765 6.027649 1.957405 0.000006 0.000081 13.776313
-29 1 0 1024 -0.006364 9.275757 2.122339 0.000002 0.000026 16.821591
-29 1 1 1024 -0.140631 9.910583 1.611250 0.000002 0.000023 13.197373
-30 0 0 1024 0.072943 3.733805 6.264887 0.000006 0.000071 35.965622
-30 0 1 1024 -0.052506 4.387878 3.479865 0.000009 0.000093 44.349223
-30 1 0 1024 0.008406 5.021810 2.308727 0.000008 0.000085 15.880164
-30 1 1 1024 -0.147555 8.227193 1.637723 0.000002 0.000022 13.866484
-31 1 0 1024 0.000033 9.308573 1.886001 0.000002 0.000017 19.936102
-31 1 1 1024 -0.145700 9.870667 1.610662 0.000001 0.000017 14.105948
-32 1 0 1016 -0.101837 5.457534 2.442544 0.000008 0.000085 16.869970
-32 1 1 1018 -0.100882 6.643766 1.484100 0.000002 0.000027 17.016439
-33 1 0 1019 -0.107913 5.854473 2.030354 0.000008 0.000111 17.656553
-33 1 1 1018 -0.104750 6.514285 1.579995 0.000002 0.000026 18.046024
-34 1 0 1020 -0.108257 6.882782 1.880494 0.000003 0.000038 14.125138
-34 1 1 1018 -0.110099 6.917526 1.892384 0.000002 0.000016 13.833723
-35 1 0 1022 -0.121796 8.334801 1.807161 0.000002 0.000028 14.423969
-35 1 1 1018 -0.107517 7.105231 1.554912 0.000002 0.000022 15.379143
-36 1 0 1021 -0.111978 8.395752 1.902573 0.000002 0.000021 12.684395
-36 1 1 1018 -0.103989 7.755075 1.513899 0.000002 0.000017 15.735186
-37 1 0 1022 -0.119498 8.144121 2.128056 0.000001 0.000016 11.799663
-37 1 1 1018 -0.086472 6.760928 1.440899 0.000002 0.000032 16.451576
-38 1 0 1022 -0.126362 8.366418 1.890943 0.000002 0.000024 15.547028
-38 1 1 1018 -0.099977 7.628377 1.647596 0.000003 0.000027 13.428394
-39 1 0 1022 -0.128867 8.546498 1.952486 0.000002 0.000020 13.761684
-39 1 1 1018 -0.091108 8.340987 1.392215 0.000002 0.000030 15.791367
-41 1 0 858 -0.117675 8.663534 2.024164 0.000002 0.000016 13.786543
-41 1 1 852 -0.083109 7.402558 1.346682 0.000001 0.000017 14.923250
-42 1 0 1020 -0.110949 8.397712 2.080674 0.000002 0.000028 16.436445
-42 1 1 1017 -0.080808 7.596423 1.468899 0.000002 0.000024 14.864047
-44 0 0 1021 0.098696 8.919217 4.402947 0.000003 0.000022 29.326257
-44 0 1 1023 -0.134452 8.670434 2.040941 0.000002 0.000027 25.185552
-54 0 0 1021 0.095352 9.054522 4.727093 0.000004 0.000049 27.342838
-54 0 1 1023 -0.158791 8.836144 2.087028 0.000003 0.000027 22.937738
-68 0 0 912 -0.126928 4.094275 2.984245 0.000000 0.000005 31.298089
-68 0 1 916 0.529440 2.825646 2.494563 0.000008 0.000066 26.030924
-68 1 0 916 0.282512 3.705604 1.662663 0.000004 0.000042 16.517113
-68 1 1 916 -0.382934 2.543668 1.671447 0.000012 0.000142 16.902817
-70 1 0 919 0.273220 2.914037 1.975858 0.000007 0.000072 16.422591
-70 1 1 911 -0.380586 2.538534 2.661676 0.000018 0.000189 21.306309
-71 1 0 927 0.270648 4.495415 1.832304 0.000003 0.000023 15.326381
-71 1 1 911 -0.374629 3.509147 1.960478 0.000007 0.000067 14.023731
-72 0 0 922 -0.139030 4.265365 3.092647 0.000001 0.000008 36.803851
-72 0 1 937 0.507338 4.919313 2.656878 0.000005 0.000087 28.489847
-72 1 0 918 0.273990 4.384191 1.657099 0.000003 0.000025 16.250508
-72 1 1 901 -0.365903 4.259850 1.754957 0.000004 0.000068 12.246038
-73 0 0 909 -0.131142 4.841928 3.053060 0.000001 0.000013 35.932353
-73 0 1 939 0.512391 5.133209 2.178137 0.000004 0.000025 26.796286
-73 1 0 915 0.278769 2.556967 1.691021 0.000006 0.000050 15.318204
-74 0 0 919 -0.135500 5.774937 2.990444 0.000000 0.000004 38.680779
-74 0 1 919 0.517127 8.180404 2.131625 0.000001 0.000012 24.272923
-74 1 0 916 0.282418 1.150973 1.692552 0.000023 0.000215 15.138841
-76 0 0 916 -0.136047 5.623431 3.027347 0.000001 0.000015 40.599860
-76 0 1 913 0.537758 7.682777 2.005741 0.000001 0.000008 23.670972
-86 0 0 925 -0.141096 6.131959 3.056302 0.000001 0.000009 37.086914
-86 0 1 929 0.512093 8.391079 2.034693 0.000001 0.000008 26.888878
+0 0 0 1024 0.041750 3.356344 1.828998 0.000002 0.000021 49.839622
+0 0 1 1024 0.109967 4.153035 1.883659 0.000002 0.000017 18.750043
+0 1 1 838 -0.207362 0.514121 2.011177 0.000005 0.000146 20.057562
+4 1 0 1024 0.131400 2.764364 2.038156 0.000003 0.000066 21.627449
+4 1 1 1024 -0.201195 2.843648 1.844738 0.000003 0.000079 20.542771
+6 0 0 1024 0.023004 4.443075 1.777077 0.000001 0.000026 47.120957
+6 0 1 1024 0.113830 5.225403 1.653571 0.000001 0.000022 20.887187
+6 1 0 1024 0.146652 2.742981 2.012739 0.000001 0.000012 21.375524
+6 1 1 1024 -0.211721 2.358521 1.862361 0.000001 0.000015 20.053232
+7 1 0 1024 0.140333 3.198140 1.997439 0.000001 0.000018 18.166517
+7 1 1 1024 -0.208156 2.908244 1.892947 0.000001 0.000013 17.755360
+8 0 0 1024 0.036998 5.118429 1.829670 0.000001 0.000012 39.822422
+8 0 1 1024 0.126802 6.585714 1.734635 0.000002 0.000021 20.020662
+9 0 0 1024 0.035700 4.496134 1.846496 0.000001 0.000012 45.663926
+9 0 1 1024 0.115987 5.774577 1.682445 0.000002 0.000027 19.873340
+10 0 0 1024 0.032992 4.960815 1.840030 0.000002 0.000034 49.867023
+10 0 1 1024 0.134889 6.200765 1.801494 0.000004 0.000056 19.108826
+15 0 0 1024 0.046206 3.663981 1.957251 0.000001 0.000022 51.132112
+15 0 1 1024 0.119199 4.899963 1.761027 0.000002 0.000060 24.645105
+16 1 0 1024 0.167758 2.818949 2.055596 0.000001 0.000010 19.765362
+16 1 1 1024 -0.210253 2.650065 1.684485 0.000001 0.000010 20.046716
+17 0 0 1024 0.008123 4.042155 1.907654 0.000001 0.000016 44.942365
+17 0 1 1024 0.119126 5.346334 1.616305 0.000002 0.000038 23.873406
+17 1 0 1024 0.160551 2.538859 2.078290 0.000001 0.000011 18.503004
+17 1 1 1024 -0.212818 1.963074 1.653989 0.000001 0.000010 20.818517
+18 1 0 1024 0.169252 1.834696 2.095149 0.000003 0.000073 20.457114
+18 1 1 1024 -0.202781 1.857198 1.804126 0.000003 0.000083 25.433970
+19 1 0 1024 0.177362 2.038411 2.536636 0.000008 0.000194 22.828018
+19 1 1 1024 -0.204779 2.259420 2.005833 0.000009 0.000221 27.081790
+23 0 0 1025 -0.010332 3.968150 1.862948 0.000004 0.000068 45.777934
+23 0 1 1023 0.128696 6.093495 1.773672 0.000004 0.004550 19300251.477888
+24 0 0 993 -0.006030 4.369734 1.921640 0.000005 0.000129 48.222013
+24 0 1 992 0.138082 6.460706 1.786190 0.000005 0.000155 22.781613
+27 0 1 1024 0.147109 2.936829 2.604852 0.000022 0.000474 28.721504
+29 0 0 1024 0.016187 5.032145 1.921979 0.000003 0.000047 47.194243
+29 0 1 1024 0.140979 6.596395 1.815706 0.000005 0.000090 24.794637
+30 0 0 1024 0.000282 4.738342 1.756708 0.000001 0.000040 47.044401
+30 0 1 1024 0.128872 6.677734 1.748380 0.000002 0.000039 19.083389
+31 0 0 1024 0.007667 5.235433 1.898046 0.000001 0.000019 47.785930
+31 0 1 1024 0.137390 6.697835 1.762878 0.000002 0.000024 20.161993
+32 0 0 1019 -0.221131 4.109073 1.687081 0.000001 0.000008 39.847161
+32 0 1 1022 0.145009 4.683627 1.672801 0.000001 0.000004 18.169256
+33 0 0 1020 -0.217002 4.494730 1.552900 0.000001 0.000006 38.840936
+33 0 1 1021 0.150320 4.659607 1.476978 0.000001 0.000006 20.903208
+34 0 0 1019 -0.220660 4.394218 1.499063 0.000001 0.000007 36.572525
+34 0 1 1020 0.155998 4.846773 1.572007 0.000001 0.000008 20.786276
+35 0 0 1020 -0.207471 4.282680 1.416071 0.000000 0.000006 37.591605
+35 0 1 1020 0.158539 3.864747 1.512890 0.000001 0.000005 19.809991
+36 0 0 1020 -0.213648 1.838133 1.606175 0.000001 0.000014 42.311387
+36 0 1 1020 0.151255 1.933946 1.611633 0.000001 0.000010 20.868507
+36 1 0 1017 0.357442 1.409087 2.339893 0.000001 0.000009 25.252360
+37 0 0 1019 -0.199851 4.357601 1.549965 0.000001 0.000006 41.234504
+37 0 1 1022 0.139227 4.483753 1.584095 0.000001 0.000008 19.873120
+38 0 0 1019 -0.200025 2.547739 1.691084 0.000001 0.000006 40.679205
+38 0 1 1020 0.153593 3.013930 1.747689 0.000001 0.000006 18.072496
+39 0 1 1022 0.147554 2.405292 1.636333 0.000001 0.000009 16.334833
+40 0 1 1022 0.134857 1.219056 1.504621 0.000004 0.000039 18.954468
+40 1 0 1020 0.341554 2.105331 2.496244 0.000000 0.000007 25.552542
+40 1 1 1020 -0.141616 2.311785 2.012586 0.000001 0.000007 22.408698
+41 1 0 1020 0.344930 2.430515 2.448566 0.000000 0.000003 27.764761
+41 1 1 1021 -0.149021 2.758386 1.892542 0.000000 0.000006 25.399428
+42 0 0 1020 -0.205661 1.525102 1.704018 0.000002 0.000045 38.874329
+42 0 1 1022 0.131522 1.181813 1.789474 0.000003 0.000023 19.790870
+42 1 0 1018 0.349915 3.181974 2.424394 0.000000 0.000005 27.068381
+42 1 1 1021 -0.147299 2.959639 1.892314 0.000000 0.000004 23.454554
+54 1 0 1019 0.346022 2.557082 2.295552 0.000000 0.000006 25.170448
+54 1 1 1020 -0.141488 2.866462 2.025173 0.000001 0.000006 25.737504
+55 1 0 1018 0.350800 1.716110 2.326650 0.000001 0.000013 26.234609
+55 1 1 1021 -0.145191 0.853330 1.730182 0.000001 0.000021 26.866658
diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h
index 03249e9..5f2d711 100644
--- a/include/libsurvive/survive.h
+++ b/include/libsurvive/survive.h
@@ -26,8 +26,8 @@ struct SurviveObject
//Pose Information, also "poser" field.
FLT PoseConfidence; //0..1
- SurvivePose OutPose;
- SurvivePose FromLHPose[NUM_LIGHTHOUSES]; //Optionally filled out by poser, contains computed position from each lighthouse.
+ SurvivePose OutPose; //Final pose? (some day, one can dream!)
+ SurvivePose FromLHPose[NUM_LIGHTHOUSES]; //Filled out by poser, contains computed position from each lighthouse.
void * PoserData; //Initialized to zero, configured by poser, can be anything the poser wants.
PoserCB PoserFn;
@@ -92,7 +92,6 @@ struct SurviveContext
//Calibration data:
BaseStationData bsd[NUM_LIGHTHOUSES];
-
SurviveCalData * calptr; //If and only if the calibration subsystem is attached.
SurviveObject ** objs;
@@ -140,7 +139,7 @@ void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode
void RegisterDriver( const char * name, void * data );
#define REGISTER_LINKTIME( func ) \
- void __attribute__((constructor)) Register##func() { RegisterDriver( #func, &func ); }
+ void __attribute__((constructor)) LTRegister##func() { RegisterDriver( #func, &func ); }
diff --git a/dave/dclapack.h b/redist/dclapack.h
index 4e209d3..4e209d3 100644
--- a/dave/dclapack.h
+++ b/redist/dclapack.h
diff --git a/redist/linmath.c b/redist/linmath.c
index 72c00a4..41fa18f 100644
--- a/redist/linmath.c
+++ b/redist/linmath.c
@@ -516,150 +516,27 @@ void matrix44copy(FLT * mout, const FLT * minm )
memcpy( mout, minm, sizeof( FLT ) * 16 );
}
-///////////////////////////////////////Matrix Rotations////////////////////////////////////
-////Originally from Stack Overflow
-////Under cc by-sa 3.0
-//// http://stackoverflow.com/questions/23166898/efficient-way-to-calculate-a-3x3-rotation-matrix-from-the-rotation-defined-by-tw
-//// Copyright 2014 by Campbell Barton
-//// Copyright 2017 by Michael Turvey
-//
-///**
-//* Calculate a rotation matrix from 2 normalized vectors.
-//*
-//* v1 and v2 must be unit length.
-//*/
-//void rotation_between_vecs_to_mat3(FLT m[3][3], const FLT v1[3], const FLT v2[3])
-//{
-// FLT axis[3];
-// /* avoid calculating the angle */
-// FLT angle_sin;
-// FLT angle_cos;
-//
-// cross3d(axis, v1, v2);
-//
-// angle_sin = normalize_v3(axis);
-// angle_cos = dot3d(v1, v2);
-//
-// if (angle_sin > FLT_EPSILON) {
-// axis_calc:
-// axis_angle_normalized_to_mat3_ex(m, axis, angle_sin, angle_cos);
-// }
-// else {
-// /* Degenerate (co-linear) vectors */
-// if (angle_cos > 0.0f) {
-// /* Same vectors, zero rotation... */
-// unit_m3(m);
-// }
-// else {
-// /* Colinear but opposed vectors, 180 rotation... */
-// get_orthogonal_vector(axis, v1);
-// normalize_v3(axis);
-// angle_sin = 0.0f; /* sin(M_PI) */
-// angle_cos = -1.0f; /* cos(M_PI) */
-// goto axis_calc;
-// }
-// }
-//}
-
-//void get_orthogonal_vector(FLT out[3], const FLT in[3])
-//{
-//#ifdef USE_DOUBLE
-// const FLT x = fabs(in[0]);
-// const FLT y = fabs(in[1]);
-// const FLT z = fabs(in[2]);
-//#else
-// const FLT x = fabsf(in[0]);
-// const FLT y = fabsf(in[1]);
-// const FLT z = fabsf(in[2]);
-//#endif
-//
-// if (x > y && x > z)
-// {
-// // x is dominant
-// out[0] = -in[1] - in[2];
-// out[1] = in[0];
-// out[2] = in[0];
-// }
-// else if (y > z)
-// {
-// // y is dominant
-// out[0] = in[1];
-// out[1] = -in[0] - in[2];
-// out[2] = in[1];
-// }
-// else
-// {
-// // z is dominant
-// out[0] = in[2];
-// out[1] = in[2];
-// out[2] = -in[0] - in[1];
-// }
-//}
-//
-//void unit_m3(FLT mat[3][3])
-//{
-// mat[0][0] = 1;
-// mat[0][1] = 0;
-// mat[0][2] = 0;
-// mat[1][0] = 0;
-// mat[1][1] = 1;
-// mat[1][2] = 0;
-// mat[2][0] = 0;
-// mat[2][1] = 0;
-// mat[2][2] = 1;
-//}
-
-
-//FLT normalize_v3(FLT vect[3])
-//{
-// FLT distance = dot3d(vect, vect);
-//
-// if (distance < 1.0e-35f)
-// {
-// // distance is too short, just go to zero.
-// vect[0] = 0;
-// vect[1] = 0;
-// vect[2] = 0;
-// distance = 0;
-// }
-// else
-// {
-// distance = FLT_SQRT((FLT)distance);
-// scale3d(vect, vect, 1.0f / distance);
-// }
-//
-// return distance;
-//}
-
-///* axis must be unit length */
-//void axis_angle_normalized_to_mat3_ex(
-// FLT mat[3][3], const FLT axis[3],
-// const FLT angle_sin, const FLT angle_cos)
-//{
-// FLT nsi[3], ico;
-// FLT n_00, n_01, n_11, n_02, n_12, n_22;
-//
-// ico = (1.0f - angle_cos);
-// nsi[0] = axis[0] * angle_sin;
-// nsi[1] = axis[1] * angle_sin;
-// nsi[2] = axis[2] * angle_sin;
-//
-// n_00 = (axis[0] * axis[0]) * ico;
-// n_01 = (axis[0] * axis[1]) * ico;
-// n_11 = (axis[1] * axis[1]) * ico;
-// n_02 = (axis[0] * axis[2]) * ico;
-// n_12 = (axis[1] * axis[2]) * ico;
-// n_22 = (axis[2] * axis[2]) * ico;
-//
-// mat[0][0] = n_00 + angle_cos;
-// mat[0][1] = n_01 + nsi[2];
-// mat[0][2] = n_02 - nsi[1];
-// mat[1][0] = n_01 - nsi[2];
-// mat[1][1] = n_11 + angle_cos;
-// mat[1][2] = n_12 + nsi[0];
-// mat[2][0] = n_02 + nsi[1];
-// mat[2][1] = n_12 - nsi[0];
-// mat[2][2] = n_22 + angle_cos;
-//}
+void matrix44transpose(FLT * mout, const FLT * minm )
+{
+ mout[0] = minm[0];
+ mout[1] = minm[4];
+ mout[2] = minm[8];
+ mout[3] = minm[12];
+
+ mout[4] = minm[1];
+ mout[5] = minm[5];
+ mout[6] = minm[9];
+ mout[7] = minm[13];
+
+ mout[8] = minm[2];
+ mout[9] = minm[6];
+ mout[10] = minm[10];
+ mout[11] = minm[14];
+ mout[12] = minm[3];
+ mout[13] = minm[7];
+ mout[14] = minm[11];
+ mout[15] = minm[15];
+
+}
diff --git a/redist/linmath.h b/redist/linmath.h
index 676d182..66a38ed 100644
--- a/redist/linmath.h
+++ b/redist/linmath.h
@@ -89,9 +89,14 @@ void quatevenproduct( FLT * q, FLT * qa, FLT * qb );
void quatoddproduct( FLT * outvec3, FLT * qa, FLT * qb );
void quatslerp( FLT * q, const FLT * qa, const FLT * qb, FLT t );
void quatrotatevector( FLT * vec3out, const FLT * quat, const FLT * vec3in );
-
void quatfrom2vectors(FLT *q, const FLT *src, const FLT *dest);
+//Poses are Position: [x, y, z] Quaternion: [q, x, y, z]
+//XXX TODO Write these!
+void ApplyPoseToPoint( FLT * pout, const FLT * pin, const FLT * pose );
+void InvertPose( FLT * poseout, const FLT * pose );
+
+
// Matrix Stuff
typedef struct
@@ -105,7 +110,7 @@ Matrix3x3 inverseM33(const Matrix3x3 mat);
void matrix44copy(FLT * mout, const FLT * minm );
-
+void matrix44transpose(FLT * mout, const FLT * minm );
#endif
diff --git a/redist/lintest.c b/redist/lintest.c
new file mode 100644
index 0000000..fa5a9d7
--- /dev/null
+++ b/redist/lintest.c
@@ -0,0 +1,112 @@
+#include "linmath.h"
+#include <stdio.h>
+
+int main()
+{
+#if 1
+
+#define NONTRANSPOSED_DAVE
+#ifdef NONTRANSPOSED_DAVE
+ FLT pLH1[3] = {-0.396888, 3.182945, -0.568622};
+ FLT qLH1[4] = {0.668640, -0.576296, 0.103727, -0.458305};
+ FLT pNLH1[3] = { 0.113572, 2.791495, -1.495652 }; //1M +x
+ FLT qNLH1[4] = { 0.807419, 0.372818, -0.451339, 0.073308 };
+
+
+ FLT pLH2[3] = {0.195579, 3.193770, -0.424473};
+ FLT qLH2[4] = {0.401849, 0.104771, 0.580441, 0.700449};
+ FLT pNLH2[3] = {-0.183505, 3.356293, 0.695688, };
+ FLT qNLH2[4] = {-0.237438, 0.405213, 0.270438, 0.840410 };
+#else
+
+ FLT pLH1[3] = {-0.321299, 3.130532, -0.786460};
+ FLT qLH1[4] = {0.794180, 0.336117, -0.485668, -0.142934};
+ FLT pNLH1[3] = { 0.113572, 2.791495, -1.495652 }; //1M +x
+ FLT qNLH1[4] = { 0.807419, 0.372818, -0.451339, 0.073308 };
+
+ FLT pLH2[3] = {0.153580, 3.251673, -0.190491};
+ FLT qLH2[4] = {0.217017, 0.482214, 0.306568, 0.791448 };
+ FLT pNLH2[3] = {-0.175330, 3.351943, 0.669623 };
+ FLT qNLH2[4] = {0.257241, 0.394159, 0.292555, 0.832392 };
+#endif
+
+ FLT pOut1[3];
+ FLT pOut2[3];
+
+ qLH1[0] *= -1;
+ qLH2[0] *= -1;
+
+ quatrotatevector( pOut1, qLH1, pLH1 );
+ quatrotatevector( pOut2, qLH2, pLH2 );
+
+ printf( "%f %f %f\n", PFTHREE( pOut1 ) );
+ printf( "%f %f %f\n", PFTHREE( pOut2 ) );
+
+// qLH1[1]*=-1;
+// qLH2[0]*=-1;
+
+/*
+ sub3d( pOut1, pLH1, pNLH1 );
+ sub3d( pOut2, pLH2, pNLH2 );
+
+
+ printf( "%f %f %f\n", PFTHREE( pOut1 ) );
+ printf( "%f %f %f\n", PFTHREE( pOut2 ) );
+
+ quatrotatevector( pOut1, qLH1, pOut1 );
+ quatrotatevector( pOut2, qLH2, pOut2 );
+
+ printf( "%f %f %f\n", PFTHREE( pOut1 ) );
+ printf( "%f %f %f\n", PFTHREE( pOut2 ) );
+*/
+ return -1;
+
+#endif
+
+#if 0
+
+ FLT e[3] = { 1,1,3.14 };
+ FLT q[4];
+ FLT m[16];
+ FLT pt[3] = { 1, 1, 1 };
+
+ q[0] = 0;
+ q[1] = 0;
+ q[2] = 0;
+ q[3] = 1;
+
+ quatrotatevector( pt, q, pt );
+ printf( "%f %f %f\n", PFTHREE( pt ) );
+ printf( "\n" );
+
+ quatfromeuler( q, e );
+ printf( "%f %f %f %f\n\n", PFFOUR( q ) );
+ quattomatrix(m,q);
+ printf( "%f %f %f %f\n", PFFOUR( &m[0] ) );
+ printf( "%f %f %f %f\n", PFFOUR( &m[4] ) );
+ printf( "%f %f %f %f\n", PFFOUR( &m[8] ) );
+ printf( "%f %f %f %f\n\n", PFFOUR( &m[12] ) );
+ quatfrommatrix(q,m );
+ printf( "%f %f %f %f\n\n", PFFOUR( q ) );
+ quattoeuler( e,q );
+ printf( "E: %f %f %f\n", e[0], e[1], e[2] );
+
+
+ FLT pfromlh[3] = { 0, 1, 0 };
+ FLT p[3] = { 0, 1, 0 };
+ quatrotatevector( p, q, p );
+ printf( "%f %f %f\n", PFTHREE( p ) );
+ printf( "Flipping rotation\n" );
+ q[0] *= -1; //Wow that was easy.
+ quatrotatevector( p, q, p );
+ printf( "%f %f %f\n", PFTHREE( p ) );
+
+ //Try setting up a pose.
+// FLT mypose[7] = { 0, 0, 10, q[0], q[1], q[2], q[3] );
+// ApplyPoseToPoint( FLT * pout, const FLT * pin, const FLT * pose );
+//void InvertPose( FLT * poseout, const FLT * pose );
+
+#endif
+
+}
+
diff --git a/src/poser_charlesslow.c b/src/poser_charlesslow.c
new file mode 100644
index 0000000..3cc7d9d
--- /dev/null
+++ b/src/poser_charlesslow.c
@@ -0,0 +1,343 @@
+#include "survive_cal.h"
+#include <math.h>
+#include <string.h>
+#include "linmath.h"
+#include <survive.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <dclapack.h>
+#include <linmath.h>
+
+static int LH_ID;
+
+typedef struct
+{
+ int something;
+ //Stuff
+} DummyData;
+
+
+static FLT RunOpti( SurviveObject * so, PoserDataFullScene * fs, int lh, int print, FLT * LighthousePos, FLT * LighthouseQuat );
+
+int PoserCharlesSlow( SurviveObject * so, PoserData * pd )
+{
+ PoserType pt = pd->pt;
+ SurviveContext * ctx = so->ctx;
+ DummyData * dd = so->PoserData;
+
+ if( !dd ) so->PoserData = dd = malloc( sizeof( DummyData ) );
+
+ 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;
+ //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 );
+ break;
+ }
+ case POSERDATA_FULL_SCENE:
+ {
+ PoserDataFullScene * fs = (PoserDataFullScene*)pd;
+
+ int lh, cycle;
+ FLT dz, dy, dx;
+ for( lh = 0; lh < 2; lh++ )
+ {
+ FLT beste = 1e20;
+
+ FLT LighthousePos[3];
+ FLT LighthouseQuat[4];
+
+ LighthousePos[0] = 0;
+ LighthousePos[1] = 0;
+ LighthousePos[2] = 0;
+ LighthouseQuat[0] = 1;
+ LighthouseQuat[1] = 0;
+ LighthouseQuat[2] = 0;
+ LighthouseQuat[3] = 0;
+
+ FLT bestxyz[3];
+ memcpy( bestxyz, LighthousePos, sizeof( LighthousePos ) );
+
+ //STAGE1 1: Detemine vectoral position from lighthouse to target. Does not determine lighthouse-target distance.
+ //This also is constantly optimizing the lighthouse quaternion for optimal spotting.
+ FLT fullrange = 5; //Maximum search space for positions. (Relative to HMD)
+
+
+ //Sweep whole area 30 times
+ for( cycle = 0; cycle < 30; cycle ++ )
+ {
+
+ //Adjust position, one axis at a time, over and over until we zero in.
+ {
+ FLT bestxyzrunning[3];
+ beste = 1e20;
+
+ FILE * f;
+ if( cycle == 0 )
+ {
+ char filename[1024];
+ sprintf( filename, "calinfo/%d_lighthouse.dat", lh );
+ f = fopen( filename, "wb" );
+ }
+
+ //We split the space into this many groups (times 2) and
+ //if we're on the first cycle, we want to do a very linear
+ //search. As we refine our search we can then use a more
+ //binary search technique.
+ FLT splits = 4;
+ if( cycle == 0 ) splits = 32;
+ if( cycle == 1 ) splits = 13;
+ if( cycle == 2 ) splits = 10;
+ if( cycle == 3 ) splits = 8;
+ if( cycle == 4 ) splits = 5;
+
+ //Wwe search throug the whole space.
+ for( dz = 0; dz < fullrange; dz += fullrange/splits )
+ for( dy = -fullrange; dy < fullrange; dy += fullrange/splits )
+ for( dx = -fullrange; dx < fullrange; dx += fullrange/splits )
+ {
+ //Specificially adjust one axis at a time, searching for the best.
+ memcpy( LighthousePos, bestxyz, sizeof( LighthousePos ) );
+ LighthousePos[0] += dx; //These are adjustments to the "best" from last frame.
+ LighthousePos[1] += dy;
+ LighthousePos[2] += dz;
+
+ FLT ft;
+ //Try refining the search for the best orientation several times.
+ ft = RunOpti(so, fs, lh, 0, LighthousePos, LighthouseQuat);
+ if( cycle == 0 )
+ {
+ float sk = ft*10.;
+ if( sk > 1 ) sk = 1;
+ uint8_t cell = (1.0 - sk) * 255;
+ FLT epsilon = 0.1;
+
+ if( dz == 0 ) { /* Why is dz special? ? */
+ if ( dx > -epsilon && dx < epsilon )
+ cell = 255;
+ if ( dy > -epsilon && dy < epsilon )
+ cell = 128;
+ }
+
+ fprintf( f, "%c", cell );
+ }
+
+ if( ft < beste ) { beste = ft; memcpy( bestxyzrunning, LighthousePos, sizeof( LighthousePos ) ); }
+ }
+
+ if( cycle == 0 )
+ {
+ fclose( f );
+ }
+ memcpy( bestxyz, bestxyzrunning, sizeof( bestxyz ) );
+
+ //Print out the quality of the lock this time.
+ FLT dist = sqrt(bestxyz[0]*bestxyz[0] + bestxyz[1]*bestxyz[1] + bestxyz[2]*bestxyz[2]);
+ printf( "%f %f %f (%f) = %f\n", bestxyz[0], bestxyz[1], bestxyz[2], dist, beste );
+ }
+
+ //Every cycle, tighten up the search area.
+ fullrange *= 0.25;
+ }
+
+ if( beste > 0.1 )
+ {
+ //Error too high
+ SV_ERROR( "LH: %d / Best E %f Error too high\n", lh, beste );
+ return -1;
+ }
+
+ RunOpti(so, fs, lh, 1, LighthousePos, LighthouseQuat);
+
+ ctx->bsd[lh].PositionSet = 1;
+ copy3d( ctx->bsd[lh].Pose.Pos, LighthousePos );
+ quatcopy( ctx->bsd[lh].Pose.Rot, LighthouseQuat );
+#define ALT_COORDS
+#ifdef ALT_COORDS
+ so->FromLHPose[lh].Pos[0] = LighthousePos[0];
+ so->FromLHPose[lh].Pos[1] = LighthousePos[1];
+ so->FromLHPose[lh].Pos[2] = LighthousePos[2];
+ so->FromLHPose[lh].Rot[0] =-LighthouseQuat[0];
+ so->FromLHPose[lh].Rot[1] = LighthouseQuat[1];
+ so->FromLHPose[lh].Rot[2] = LighthouseQuat[2];
+ so->FromLHPose[lh].Rot[3] = LighthouseQuat[3];
+
+ quatrotatevector( so->FromLHPose[lh].Pos, so->FromLHPose[lh].Rot, so->FromLHPose[lh].Pos );
+#else
+ so->FromLHPose[lh].Pos[0] = LighthousePos[0];
+ so->FromLHPose[lh].Pos[1] = LighthousePos[1];
+ so->FromLHPose[lh].Pos[2] = LighthousePos[2];
+ so->FromLHPose[lh].Rot[0] = LighthouseQuat[0];
+ so->FromLHPose[lh].Rot[1] = LighthouseQuat[1];
+ so->FromLHPose[lh].Rot[2] = LighthouseQuat[2];
+ so->FromLHPose[lh].Rot[3] = LighthouseQuat[3];
+#endif
+ }
+
+ break;
+ }
+ case POSERDATA_DISASSOCIATE:
+ {
+ free( dd );
+ so->PoserData = 0;
+ //printf( "Need to disassociate.\n" );
+ break;
+ }
+ }
+
+}
+
+
+REGISTER_LINKTIME( PoserCharlesSlow );
+
+
+
+static FLT RunOpti( SurviveObject * hmd, PoserDataFullScene * fs, int lh, int print, FLT * LighthousePos, FLT * LighthouseQuat )
+{
+ int i, p;
+ FLT UsToTarget[3];
+ FLT LastUsToTarget[3];
+ FLT mux = .9;
+ quatsetnone( LighthouseQuat );
+ FLT * hmd_points = hmd->sensor_locations;
+ FLT * hmd_normals = hmd->sensor_normals;
+
+ int first = 1, second = 0;
+
+ //First check to see if this is a valid viewpoint.
+ //If a sensor is pointed away from where we are testing a possible lighthouse position.
+ //BUT We get data from that light house, then we KNOW this is not a possible
+ //lighthouse position.
+ for( p = 0; p < 32; p++ )
+ {
+ int dataindex = p*(2*NUM_LIGHTHOUSES)+lh*2;
+ if( fs->lengths[p][lh][0] < 0 || fs->lengths[p][lh][1] < 0 ) continue;
+ FLT me_to_dot[3];
+ sub3d( me_to_dot, LighthousePos, &hmd_points[p*3] );
+ float dot = dot3d( &hmd_normals[p*3], me_to_dot );
+ if( dot < -.01 ) { return 1000; }
+ }
+ int iters = 6;
+
+ //Iterate over a refinement of the quaternion that constitutes the
+ //lighthouse.
+ for( i = 0; i < iters; i++ )
+ {
+ first = 1;
+ for( p = 0; p < 32; p++ )
+ {
+ int dataindex = p*(2*NUM_LIGHTHOUSES)+lh*2;
+ if( fs->lengths[p][lh][0] < 0 || fs->lengths[p][lh][1] < 0 ) continue;
+
+ //Find out where our ray shoots forth from.
+ FLT ax = fs->angles[p][lh][0];
+ FLT ay = fs->angles[p][lh][1];
+ //NOTE: Inputs may never be output with cross product.
+ //Create a fictitious normalized ray. Imagine the lighthouse is pointed
+ //straight in the +z direction, this is the lighthouse ray to the point.
+ FLT RayShootOut[3] = { sin(ax), sin(ay), 0 };
+ RayShootOut[2] = sqrt( 1 - (RayShootOut[0]*RayShootOut[0] + RayShootOut[1]*RayShootOut[1]) );
+ FLT RayShootOutWorld[3];
+
+ quatnormalize( LighthouseQuat, LighthouseQuat );
+ //Rotate that ray by the current rotation estimation.
+ quatrotatevector( RayShootOutWorld, LighthouseQuat, RayShootOut );
+
+ //Find a ray from us to the target point.
+ sub3d( UsToTarget, &hmd_points[p*3], LighthousePos );
+ if( magnitude3d( UsToTarget ) < 0.0001 ) { continue; }
+ normalize3d( UsToTarget, UsToTarget );
+
+ FLT RotatedLastUs[3];
+ quatnormalize( LighthouseQuat, LighthouseQuat );
+ quatrotatevector( RotatedLastUs, LighthouseQuat, LastUsToTarget );
+
+ //Rotate the lighthouse around this axis to point at the HMD.
+ //If it's the first time, the axis is synthesized, if it's after that, use most recent point.
+ FLT ConcatQuat[4];
+ FLT AxisToRotate[3];
+ if( first )
+ {
+ cross3d( AxisToRotate, RayShootOutWorld, UsToTarget );
+ if( magnitude3d(AxisToRotate) < 0.0001 ) break;
+ normalize3d( AxisToRotate, AxisToRotate );
+ //Don't need to worry about being negative, cross product will fix it.
+ FLT RotateAmount = anglebetween3d( RayShootOutWorld, UsToTarget );
+ quatfromaxisangle( ConcatQuat, AxisToRotate, RotateAmount );
+ quatnormalize( ConcatQuat, ConcatQuat );
+ }
+ else
+ {
+ FLT Target[3];
+ FLT Actual[3];
+
+ copy3d( AxisToRotate, LastUsToTarget );
+ //Us to target = normalized ray from us to where we should be.
+ //RayShootOut = where we would be pointing.
+ sub3d( Target, UsToTarget, AxisToRotate ); //XXX XXX XXX WARNING THIS MESSES STUFF UP.
+ sub3d( Actual, RayShootOutWorld, AxisToRotate );
+ if( magnitude3d( Actual ) < 0.0001 || magnitude3d( Target ) < 0.0001 ) { continue; }
+ normalize3d( Target, Target );
+ normalize3d( Actual, Actual );
+
+ cross3d( AxisToRotate, Actual, Target ); //XXX Check: AxisToRotate should be equal to LastUsToTarget.
+ if( magnitude3d( AxisToRotate ) < 0.000001 ) { continue; }
+ normalize3d( AxisToRotate,AxisToRotate );
+
+ //printf( "%f %f %f === %f %f %f : ", PFTHREE( AxisToRotate ), PFTHREE( LastUsToTarget ) );
+ FLT RotateAmount = anglebetween3d( Actual, Target ) * mux;
+ //printf( "FA: %f (O:%f)\n", acos( dot3d( Actual, Target ) ), RotateAmount );
+ quatfromaxisangle( ConcatQuat, AxisToRotate, RotateAmount );
+ quatnormalize( ConcatQuat, ConcatQuat );
+ }
+
+
+ quatnormalize( ConcatQuat, ConcatQuat );
+ quatnormalize( LighthouseQuat, LighthouseQuat );
+ quatrotateabout( LighthouseQuat, ConcatQuat, LighthouseQuat ); //Checked. This appears to be
+
+ mux = mux * 0.94;
+ if( second ) { second = 0; }
+ if( first ) { first = 0; second = 1; }
+ copy3d( LastUsToTarget, RayShootOutWorld );
+ }
+ }
+
+ //Step 2: Determine error.
+ float errorsq = 0.0;
+ int count = 0;
+ for( p = 0; p < 32; p++ )
+ {
+ int dataindex = p*(2*NUM_LIGHTHOUSES)+lh*2;
+ if( fs->lengths[p][lh][0] < 0 || fs->lengths[p][lh][1] < 0 ) continue;
+
+ //Find out where our ray shoots forth from.
+ FLT ax = fs->angles[p][lh][0];
+ FLT ay = fs->angles[p][lh][1];
+ FLT RayShootOut[3] = { sin(ax), sin(ay), 0 };
+ RayShootOut[2] = sqrt( 1 - (RayShootOut[0]*RayShootOut[0] + RayShootOut[1]*RayShootOut[1]) );
+
+ //Rotate that ray by the current rotation estimation.
+ quatrotatevector( RayShootOut, LighthouseQuat, RayShootOut );
+
+ //Point-line distance.
+ //Line defined by LighthousePos & Direction: RayShootOut
+
+ //Find a ray from us to the target point.
+ sub3d( UsToTarget, &hmd_points[p*3], LighthousePos );
+ FLT xproduct[3];
+ cross3d( xproduct, UsToTarget, RayShootOut );
+ FLT dist = magnitude3d( xproduct );
+ errorsq += dist*dist;
+ //if( print ) printf( "%f (%d(%d/%d))\n", dist, p, cd->ctsweeps[dataindex+0], cd->ctsweeps[dataindex+1] );
+ }
+ if( print ) printf( " = %f\n", sqrt( errorsq ) );
+ return sqrt(errorsq);
+}
diff --git a/src/poser_daveortho.c b/src/poser_daveortho.c
new file mode 100644
index 0000000..9f3b55a
--- /dev/null
+++ b/src/poser_daveortho.c
@@ -0,0 +1,460 @@
+#include "survive_cal.h"
+#include <math.h>
+#include <string.h>
+#include "linmath.h"
+#include <survive.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <dclapack.h>
+#include <linmath.h>
+
+static int LH_ID;
+
+void OrthoSolve(
+ FLT T[4][4], // OUTPUT: 4x4 transformation matrix
+ FLT S_out[2][SENSORS_PER_OBJECT], // OUTPUT: array of screenspace points
+ FLT S_in[2][SENSORS_PER_OBJECT], // INPUT: array of screenspace points
+ FLT X_in[3][SENSORS_PER_OBJECT], // INPUT: array of offsets
+ int nPoints);
+
+
+typedef struct
+{
+ int something;
+ //Stuff
+} DummyData;
+
+int PoserDaveOrtho( SurviveObject * so, PoserData * pd )
+{
+ PoserType pt = pd->pt;
+ SurviveContext * ctx = so->ctx;
+ DummyData * dd = so->PoserData;
+
+ if( !dd ) so->PoserData = dd = malloc( sizeof( DummyData ) );
+
+ 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;
+ //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 );
+ break;
+ }
+ case POSERDATA_FULL_SCENE:
+ {
+ PoserDataFullScene * fs = (PoserDataFullScene*)pd;
+
+ for( LH_ID = 0; LH_ID < 2; LH_ID++ )
+ {
+ int i;
+ int max_hits = 0;
+ FLT S_in[2][SENSORS_PER_OBJECT];
+ FLT X_in[3][SENSORS_PER_OBJECT];
+ for( i = 0; i < SENSORS_PER_OBJECT; i++ )
+ {
+ //Load all our valid points into something the LHFinder can use.
+ if( fs->lengths[i][LH_ID][0] > 0 )
+ {
+ S_in[0][max_hits] = fs->angles[i][LH_ID][0];
+ S_in[1][max_hits] = fs->angles[i][LH_ID][1];
+ X_in[0][max_hits] = so->sensor_locations[i*3+0];
+ X_in[1][max_hits] = so->sensor_locations[i*3+1];
+ X_in[2][max_hits] = so->sensor_locations[i*3+2];
+ max_hits++;
+ }
+
+ }
+ FLT tOut[4][4];
+ FLT S_out[2][SENSORS_PER_OBJECT];
+ OrthoSolve( tOut, S_out, S_in, X_in, max_hits );
+
+ //Now, we need to solve where we are as a function of where
+ //the lighthouses are.
+ FLT quat[4];
+ FLT posoff[3] = { tOut[0][3], tOut[1][3], tOut[2][3] };
+ FLT MT[4][4];
+
+ //matrix44transpose( MT, &tOut[0][0] );
+ matrix44copy( &MT[0][0], &tOut[0][0] );
+
+ quatfrommatrix( quat, &MT[0][0] );
+
+
+ //printf( "QUAT: %f %f %f %f = %f\n", quat[0], quat[1], quat[2], quat[3], quatmagnitude(quat) );
+ //quat[2] -= 0.005; //fixes up lh0 in test data set.
+ quatnormalize( quat, quat );
+ printf( "QUAT: %f %f %f %f = %f [%f %f %f]\n", quat[0], quat[1], quat[2], quat[3], quatmagnitude(quat), posoff[0], posoff[1], posoff[2] );
+
+ for( i = 0; i < max_hits;i++ )
+ {
+ FLT pt[3] = { X_in[0][i], X_in[1][i], X_in[2][i] };
+ quatrotatevector( pt, quat, pt );
+ add3d( pt, pt, posoff );
+ printf( "OUT %f %f %f ANGLE %f %f AOUT %f %f\n",
+ pt[0], pt[1], pt[2],
+ S_in[0][i], S_in[1][i], atan2( pt[0], pt[1] ), atan2( pt[2], pt[1] ) );
+ }
+
+ so->FromLHPose[LH_ID].Pos[0] = posoff[0];
+ so->FromLHPose[LH_ID].Pos[1] = posoff[1];
+ so->FromLHPose[LH_ID].Pos[2] = posoff[2];
+ so->FromLHPose[LH_ID].Rot[0] = quat[0];
+ so->FromLHPose[LH_ID].Rot[1] = quat[1];
+ so->FromLHPose[LH_ID].Rot[2] = quat[2];
+ so->FromLHPose[LH_ID].Rot[3] = quat[3];
+ }
+
+ break;
+ }
+ case POSERDATA_DISASSOCIATE:
+ {
+ free( dd );
+ so->PoserData = 0;
+ //printf( "Need to disassociate.\n" );
+ break;
+ }
+ }
+
+}
+
+
+REGISTER_LINKTIME( PoserDaveOrtho );
+
+
+
+
+
+#define PRINT_MAT(A,M,N) { \
+ int m,n; \
+ printf(#A "\n"); \
+ for (m=0; m<M; m++) { \
+ for (n=0; n<N; n++) { \
+ printf("%f\t", A[m][n]); \
+ } \
+ printf("\n"); \
+ } \
+}
+
+#define CrossProduct(ox,oy,oz,a,b,c,x,y,z) { \
+ ox=(b)*(z)-(c)*(y); \
+ oy=(c)*(x)-(a)*(z); \
+ oz=(a)*(y)-(b)*(x); }
+
+void OrthoSolve(
+ FLT T[4][4], // OUTPUT: 4x4 transformation matrix
+ FLT S_out[2][SENSORS_PER_OBJECT], // OUTPUT: array of screenspace points
+ FLT S_in[2][SENSORS_PER_OBJECT], // INPUT: array of screenspace points
+ FLT X_in[3][SENSORS_PER_OBJECT], // INPUT: array of offsets
+ int nPoints)
+{
+ int i,j,k;
+ FLT R[3][3]; // OUTPUT: 3x3 rotation matrix
+ FLT trans[3]; // INPUT: x,y,z translation vector
+
+ //--------------------
+ // Remove the center of the HMD offsets, and the screen space
+ //--------------------
+ FLT xbar[3] = {0.0, 0.0, 0.0};
+ FLT sbar[2] = {0.0, 0.0};
+ FLT S[2][SENSORS_PER_OBJECT];
+ FLT X[3][SENSORS_PER_OBJECT];
+ FLT inv_nPoints = 1.0 / nPoints;
+ for (i=0; i<nPoints; i++) {
+ xbar[0] += X_in[0][i];
+ xbar[1] += X_in[1][i];
+ xbar[2] += X_in[2][i];
+ sbar[0] += S_in[0][i];
+ sbar[1] += S_in[1][i];
+ }
+ for (j=0; j<3; j++) { xbar[j] *= inv_nPoints; }
+ for (j=0; j<2; j++) { sbar[j] *= inv_nPoints; }
+ for (i=0; i<nPoints; i++) {
+ X[0][i] = X_in[0][i] - xbar[0];
+ X[1][i] = X_in[1][i] - xbar[1];
+ X[2][i] = X_in[2][i] - xbar[2];
+ S[0][i] = S_in[0][i] - sbar[0];
+ S[1][i] = S_in[1][i] - sbar[1];
+ }
+
+ //--------------------
+ // Solve for the morph matrix
+ // S = M X
+ // thus
+ // (SX^t)(XX^t)^-1 = M
+ //--------------------
+ FLT Xt[SENSORS_PER_OBJECT][3];
+ FLT XXt[3][3];
+ FLT invXXt[3][3];
+ FLT SXt[2][3];
+ FLT M[2][3]; // Morph matrix! (2 by 3)
+ TRANSP(X,Xt,3,nPoints);
+ MUL(X,Xt,XXt,3,nPoints,3);
+ MUL(S,Xt,SXt,2,nPoints,3);
+ INV(XXt,invXXt,3);
+ MUL(SXt,invXXt,M,2,3,3);
+//PRINT(M,2,3);
+
+// Double checking work
+FLT S_morph[2][SENSORS_PER_OBJECT];
+MUL(M,X,S_morph,2,3,nPoints);
+for (i=0; i<nPoints; i++) { S_morph[0][i]+=sbar[0]; S_morph[1][i]+=sbar[1]; }
+
+ //--------------------
+ // Solve for the non-trivial vector
+ // uf -- vector that goes into the camera
+ //--------------------
+ FLT uM[3][3] = {
+ { M[0][0], M[0][1], M[0][2] },
+ { M[1][0], M[1][1], M[1][2] },
+ { 3.14567, -1.2345, 4.32567 } }; // Morph matrix with appended row
+//PRINT(uM,3,3);
+// ToDo: Pick a number for the bottom that is NOT linearly separable with M[0] and M[1]
+ FLT B[3][1] = { {0.0}, {0.0}, {1.0} };
+ FLT inv_uM[3][3];
+ FLT uf[3][1];
+ INV(uM,inv_uM,3);
+ MUL(inv_uM,B,uf,3,3,1);
+
+ //--------------------
+ // Solve for unit length vector
+ // f that goes into the camera
+ //--------------------
+ FLT uf_len = sqrt( uf[0][0]*uf[0][0] + uf[1][0]*uf[1][0] + uf[2][0]*uf[2][0] );
+ FLT f[3][1] = { {uf[0][0]/uf_len}, {uf[1][0]/uf_len}, {uf[2][0]/uf_len} };
+//PRINT(uf,3,1);
+//PRINT(f,3,1);
+
+//FLT check[3][1];
+//MUL(uM,uf,check,3,3,1);
+//PRINT(check,3,1);
+
+ //--------------------
+ // take cross products to get vectors u,r
+ //--------------------
+ FLT u[3][1], r[3][1];
+ CrossProduct(u[0][0],u[1][0],u[2][0],f[0][0],f[1][0],f[2][0],1.0,0.0,0.0);
+ FLT inv_ulen = 1.0 / sqrt( u[0][0]*u[0][0] + u[1][0]*u[1][0] + u[2][0]*u[2][0] );
+ u[0][0]*=inv_ulen; u[1][0]*=inv_ulen; u[2][0]*=inv_ulen;
+ CrossProduct(r[0][0],r[1][0],r[2][0],f[0][0],f[1][0],f[2][0],u[0][0],u[1][0],u[2][0]);
+//PRINT(u,3,1);
+//PRINT(r,3,1);
+
+ //--------------------
+ // Use morph matrix to get screen space
+ // uhat,rhat
+ //--------------------
+ FLT uhat[2][1], rhat[2][1], fhat[2][1];
+ MUL(M,f,fhat,2,3,1);
+ MUL(M,u,uhat,2,3,1);
+ MUL(M,r,rhat,2,3,1);
+ FLT fhat_len = sqrt( fhat[0][0]*fhat[0][0] + fhat[1][0]*fhat[1][0] );
+ FLT uhat_len = sqrt( uhat[0][0]*uhat[0][0] + uhat[1][0]*uhat[1][0] );
+ FLT rhat_len = sqrt( rhat[0][0]*rhat[0][0] + rhat[1][0]*rhat[1][0] );
+ FLT urhat_len = 0.5 * (uhat_len + rhat_len);
+/*
+printf("fhat %f %f (len %f)\n", fhat[0][0], fhat[1][0], fhat_len);
+printf("uhat %f %f (len %f)\n", uhat[0][0], uhat[1][0], uhat_len);
+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);
+
+ //--------------------
+ // Rescale the axies to be of the proper length
+ //--------------------
+ FLT x[3][1] = { {M[0][0]*ydist}, {0.0}, {M[1][0]*ydist} };
+ FLT y[3][1] = { {M[0][1]*ydist}, {0.0}, {M[1][1]*ydist} };
+ FLT z[3][1] = { {M[0][2]*ydist}, {0.0}, {M[1][2]*ydist} };
+ // we know the distance into (or out of) the camera for the z axis,
+ // but we don't know which direction . . .
+ FLT x_y = sqrt(1.0 - x[0][0]*x[0][0] - x[2][0]*x[2][0]);
+ FLT y_y = sqrt(1.0 - y[0][0]*y[0][0] - y[2][0]*y[2][0]);
+ FLT z_y = sqrt(1.0 - z[0][0]*z[0][0] - z[2][0]*z[2][0]);
+
+ 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];
+ FLT yz_dot2 = y[0][0]*z[0][0] + y[2][0]*z[2][0];
+ FLT zx_dot2 = z[0][0]*x[0][0] + z[2][0]*x[2][0];
+ for (i=0;i<2;i++) {
+ for (j=0;j<2;j++) {
+ for(k=0;k<2;k++) {
+
+ // Calculate the error term
+ FLT xy_dot = xy_dot2 + x_y*y_y;
+ FLT yz_dot = yz_dot2 + y_y*z_y;
+ FLT zx_dot = zx_dot2 + z_y*x_y;
+ FLT err = _ABS(xy_dot) + _ABS(yz_dot) + _ABS(zx_dot);
+
+ // Calculate the handedness
+ 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);
+
+ // 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; }
+ if ( i == 0 && j == 1 && k == 0) { x[1][0]=x_y; y[1][0]=y_y; z[1][0]=z_y; bestErr=err; }
+ z_y = -z_y;
+ }
+ y_y = -y_y;
+ }
+ x_y = -x_y;
+ }
+ printf("bestErr %f\n", bestErr);
+*/
+
+ //-------------------------
+ // A test version of the rescaling to the proper length
+ //-------------------------
+ FLT ydist2 = ydist;
+ FLT bestBestErr = 9999.0;
+ FLT bestYdist = 0;
+ for (ydist2=ydist-0.1; ydist2<ydist+0.1; ydist2+=0.0001)
+ {
+ FLT x2[3][1] = { {M[0][0]*ydist2}, {0.0}, {M[1][0]*ydist2} };
+ FLT y2[3][1] = { {M[0][1]*ydist2}, {0.0}, {M[1][1]*ydist2} };
+ FLT z2[3][1] = { {M[0][2]*ydist2}, {0.0}, {M[1][2]*ydist2} };
+
+ // we know the distance into (or out of) the camera for the z axis,
+ // but we don't know which direction . . .
+ FLT x_y = sqrt(1.0 - x2[0][0]*x2[0][0] - x2[2][0]*x2[2][0]);
+ FLT y_y = sqrt(1.0 - y2[0][0]*y2[0][0] - y2[2][0]*y2[2][0]);
+ FLT z_y = sqrt(1.0 - z2[0][0]*z2[0][0] - z2[2][0]*z2[2][0]);
+
+ if( x_y != x_y ) x_y = 0;
+ 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 );
+
+ // Exhaustively flip the minus sign of the z axis until we find the right one . . .
+ FLT bestErr = 9999.0;
+ FLT xy_dot2 = x2[0][0]*y2[0][0] + x2[2][0]*y2[2][0];
+ FLT yz_dot2 = y2[0][0]*z2[0][0] + y2[2][0]*z2[2][0];
+ FLT zx_dot2 = z2[0][0]*x2[0][0] + z2[2][0]*x2[2][0];
+ for (i=0;i<2;i++) {
+ for (j=0;j<2;j++) {
+ for(k=0;k<2;k++) {
+
+ // Calculate the error term
+ FLT xy_dot = xy_dot2 + x_y*y_y;
+ FLT yz_dot = yz_dot2 + y_y*z_y;
+ FLT zx_dot = zx_dot2 + z_y*x_y;
+ FLT err = _ABS(xy_dot) + _ABS(yz_dot) + _ABS(zx_dot);
+
+ // Calculate the handedness
+ 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);
+
+ // 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; }
+ z_y = -z_y;
+ }
+ y_y = -y_y;
+ }
+ x_y = -x_y;
+ }
+ printf("ydist2 %f bestErr %f\n",ydist2,bestErr);
+
+ if (bestErr < bestBestErr) {
+ memcpy(x,x2,3*sizeof(FLT));
+ memcpy(y,y2,3*sizeof(FLT));
+ memcpy(z,z2,3*sizeof(FLT));
+ bestBestErr = bestErr;
+ bestYdist = ydist2;
+ }
+ }
+ 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];
+ FLT y1 = x[1][0]*X[0][i] + y[1][0]*X[1][i] + z[1][0]*X[2][i];
+ FLT z1 = x[2][0]*X[0][i] + y[2][0]*X[1][i] + z[2][0]*X[2][i];
+ printf("x1z1 %f %f y1 %f\n", x1, z1, y1);
+ }
+*/
+/*
+ //--------------------
+ // Combine uhat and rhat to figure out the unit x-vector
+ //--------------------
+ FLT xhat[2][1] = { {0.0}, {1.0} };
+ FLT urhat[2][2] = {
+ {uhat[0][0], uhat[1][0]},
+ {rhat[0][0], rhat[1][0]} };
+ FLT inv_urhat[2][2];
+ FLT ab[2][1];
+ INV(urhat,inv_urhat,2);
+ MUL(inv_urhat,xhat,ab,2,2,1);
+PRINT(ab,2,1);
+ FLT a = ab[0][0], b = ab[1][0];
+
+ //-------------------
+ // calculate the xyz coordinate system
+ //-------------------
+ FLT y[3][1] = { {f[0][0]}, {f[1][0]}, {f[2][0]} };
+ FLT x[3][1] = { {a*u[0][0] + b*r[0][0]}, {a*u[1][0] + b*r[1][0]}, {a*u[2][0] + b*r[2][0]} };
+ FLT inv_xlen = 1.0 / sqrt( x[0][0]*x[0][0] + x[1][0]*x[1][0] + x[2][0]*x[2][0] );
+ x[0][0]*=inv_xlen; x[1][0]*=inv_xlen; x[2][0]*=inv_xlen;
+ FLT z[3][1];
+ CrossProduct(z[0][0],z[1][0],z[2][0],x[0][0],x[1][0],x[2][0],y[0][0],y[1][0],y[2][0]);
+*/
+ // Store into the rotation matrix
+ for (i=0; i<3; i++) { R[i][0] = x[i][0]; R[i][1] = y[i][0]; R[i][2] = z[i][0]; }
+//PRINT(R,3,3);
+
+ //-------------------
+ // Calculate the translation of the centroid
+ //-------------------
+ trans[0]=tan(sbar[0]); trans[1]=1.0; trans[2]=tan(sbar[1]);
+ FLT inv_translen = ydist / sqrt( trans[0]*trans[0] + trans[1]*trans[1] + trans[2]*trans[2] );
+ trans[0]*=inv_translen; trans[1]*=inv_translen; trans[2]*=inv_translen;
+
+ //-------------------
+ // Add in the centroid point
+ //-------------------
+ trans[0] -= xbar[0]*R[0][0] + xbar[1]*R[0][1] + xbar[2]*R[0][2];
+ trans[1] -= xbar[0]*R[1][0] + xbar[1]*R[1][1] + xbar[2]*R[1][2];
+ trans[2] -= xbar[0]*R[2][0] + xbar[1]*R[2][1] + xbar[2]*R[2][2];
+ FLT transdist = sqrt( trans[0]*trans[0] + trans[1]*trans[1] + trans[2]*trans[2] );
+
+ //-------------------
+ // Pack into the 4x4 transformation matrix
+ //-------------------
+ T[0][0]=R[0][0]; T[0][1]=R[0][1]; T[0][2]=R[0][2]; T[0][3]=trans[0];
+ T[1][0]=R[1][0]; T[1][1]=R[1][1]; T[1][2]=R[1][2]; T[1][3]=trans[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);
+ //-------------------
+ // Plot the output points
+ //-------------------
+ for (i=0; i<nPoints; i++) {
+ FLT Tx = T[0][0]*X_in[0][i] + T[0][1]*X_in[1][i] + T[0][2]*X_in[2][i] + T[0][3];
+ FLT Ty = T[1][0]*X_in[0][i] + T[1][1]*X_in[1][i] + T[1][2]*X_in[2][i] + T[1][3];
+ FLT Tz = T[2][0]*X_in[0][i] + T[2][1]*X_in[1][i] + T[2][2]*X_in[2][i] + T[2][3];
+ S_out[0][i] = atan2(Tx, Ty); // horiz
+ 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]);
+ }
+
+}
+
diff --git a/src/survive.c b/src/survive.c
index 09eb432..6d49d55 100755
--- a/src/survive.c
+++ b/src/survive.c
@@ -53,7 +53,7 @@ SurviveContext * survive_init( int headless )
}
i = 0;
- const char * PreferredPoser = config_read_str( ctx->global_config_values, "DefualtPoser", "PoserDummy" );
+ const char * PreferredPoser = config_read_str( ctx->global_config_values, "DefaultPoser", "PoserDummy" );
PoserCB PreferredPoserCB = 0;
const char * FirstPoser = 0;
printf( "Available posers:\n" );
diff --git a/src/survive_cal.c b/src/survive_cal.c
index ad518b3..1a3be2d 100755
--- a/src/survive_cal.c
+++ b/src/survive_cal.c
@@ -14,6 +14,7 @@
#include <string.h>
#include <sys/stat.h>
#include <sys/types.h>
+#include <linmath.h>
#include "survive_config.h"
@@ -22,11 +23,16 @@
#define NEEDED_TIMES_OF_COMMON 5
#define DRPTS_NEEDED_FOR_AVG ((int)(DRPTS*3/4))
+
+
+
static void handle_calibration( struct SurviveCalData *cd );
static void reset_calibration( struct SurviveCalData * cd );
void ootx_packet_clbk_d(ootx_decoder_context *ct, ootx_packet* packet)
{
+ static uint8_t lighthouses_completed = 0;
+
SurviveContext * ctx = (SurviveContext*)(ct->user);
SurviveCalData * cd = ctx->calptr;
int id = ct->user1;
@@ -53,7 +59,11 @@ void ootx_packet_clbk_d(ootx_decoder_context *ct, ootx_packet* packet)
b->OOTXSet = 1;
config_set_lighthouse(ctx->lh_config,b,id);
-// config_save("config.json");
+ lighthouses_completed++;
+
+ if (lighthouses_completed >= NUM_LIGHTHOUSES) {
+ config_save(ctx, "config.json");
+ }
}
int survive_cal_get_status( struct SurviveContext * ctx, char * description, int description_length )
@@ -115,6 +125,23 @@ void survive_cal_install( struct SurviveContext * ctx )
SV_INFO( "HMD not found, calibrating using Tracker" );
}
+
+
+ const char * DriverName;
+ const char * PreferredPoser = config_read_str( ctx->global_config_values, "ConfigPoser", "PoserDaveOrtho" );
+ PoserCB PreferredPoserCB = 0;
+ const char * FirstPoser = 0;
+ printf( "Available posers:\n" );
+ i = 0;
+ while( ( DriverName = GetDriverNameMatching( "Poser", i++ ) ) )
+ {
+ PoserCB p = GetDriver( DriverName );
+ if( !PreferredPoserCB ) PreferredPoserCB = p;
+ int ThisPoser = strcmp( DriverName, PreferredPoser ) == 0;
+ if( ThisPoser ) PreferredPoserCB = p;
+ }
+ cd->ConfigPoserFn = PreferredPoserCB;
+ printf( "Got config poser: %p\n", cd->ConfigPoserFn );
ootx_packet_clbk = ootx_packet_clbk_d;
ctx->calptr = cd;
@@ -207,7 +234,7 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin
if( cd->peak_counts >= PTS_BEFORE_COMMON )
{
- int tfc = cd->times_found_common;
+/* int tfc = cd->times_found_common;
if( cd->found_common )
{
if( tfc >= NEEDED_TIMES_OF_COMMON )
@@ -229,7 +256,12 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin
SV_INFO( "Stage 2 bad - redoing. %d %d %d", cd->peak_counts, cd->found_common, tfc );
reset_calibration( cd );
cd->times_found_common = 0;
- }
+ }*/
+
+ SV_INFO( "Stage 2 moving to stage 3. %d %d", cd->peak_counts, cd->found_common );
+ reset_calibration( cd );
+ cd->stage = 3;
+ cd->found_common = 1;
}
break;
@@ -287,7 +319,7 @@ static void handle_calibration( struct SurviveCalData *cd )
for( axis = 0; axis < 2; axis++ )
{
int dpmax = cd->all_counts[sen][lh][axis];
- if( dpmax < 50 ) continue;
+ if( dpmax < MIN_PTS_BEFORE_CAL ) continue;
int i;
FLT sumsweepangle = 0;
@@ -416,8 +448,10 @@ static void handle_calibration( struct SurviveCalData *cd )
}
fclose( hists );
fclose( ptinfo );
-
+/*
//Comb through data and make sure we still have a sensor on a WM that
+ //We don't do this anymore.
+
int bcp_senid = 0;
int bcp_count = 0;
for( sen = 0; sen < MAX_SENSORS_TO_CAL; sen++ )
@@ -440,13 +474,129 @@ static void handle_calibration( struct SurviveCalData *cd )
reset_calibration( cd );
return;
}
-
cd->senid_of_checkpt = bcp_senid;
+*/
+
+ int i, j;
+ PoserDataFullScene fsd;
+ fsd.pt = POSERDATA_FULL_SCENE;
+ for( j = 0; j < NUM_LIGHTHOUSES; j++ )
+ for( i = 0; i < SENSORS_PER_OBJECT; i++ )
+ {
+ int gotdata = 0;
+
+ int dataindex = i*(2*NUM_LIGHTHOUSES)+j*2+0;
- if( survive_cal_lhfind( cd ) == 0 )
+ if( cd->ctsweeps[dataindex+0] < DRPTS_NEEDED_FOR_AVG ||
+ cd->ctsweeps[dataindex+1] < DRPTS_NEEDED_FOR_AVG )
+ {
+ fsd.lengths[i][j][0] = -1;
+ fsd.lengths[i][j][1] = -1;
+ continue;
+ }
+ fsd.lengths[i][j][0] = cd->avglens[dataindex+0];
+ 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];
+ }
+
+ cd->ConfigPoserFn( cd->hmd, (PoserData*)&fsd );
+ if( 1 )
{
- SV_INFO( "Stage 4 succeeded." );
- cd->stage = 5;
+ static int notfirstcal = 0;
+ SV_INFO( "Stage 4 succeeded. Inverting %d", notfirstcal );
+
+ if( !notfirstcal )
+ {
+ // XXX This part is /all/ wrong.
+ // XXX This part is /all/ wrong.
+ // XXX This part is /all/ wrong.
+
+ //OK! We've arrived. Now, we have to get the LH's pose from.
+ int lh;
+ for( lh = 0; lh < NUM_LIGHTHOUSES; lh++ )
+ {
+ SurvivePose * objfromlh = &cd->hmd->FromLHPose[lh];
+ SurvivePose * lhp = &ctx->bsd[lh].Pose;
+
+
+/* lhp->Pos[0] = objfromlh->Pos[0];
+ lhp->Pos[1] = objfromlh->Pos[1];
+ lhp->Pos[2] = objfromlh->Pos[2];*/
+
+ lhp->Rot[0] =-objfromlh->Rot[0];
+ lhp->Rot[1] = objfromlh->Rot[1];
+ lhp->Rot[2] = objfromlh->Rot[2];
+ lhp->Rot[3] = objfromlh->Rot[3];
+
+ quatrotatevector( lhp->Pos, lhp->Rot, objfromlh->Pos );
+
+ //Write lhp from the inverse of objfromlh
+ //quatrotatevector( lhp->Pos, lhp->Rot, lhp->Pos );
+
+
+ fprintf( stderr, "%f, %f, %f\n", objfromlh->Pos[0], objfromlh->Pos[1], objfromlh->Pos[2] );
+ fprintf( stderr, "%f, %f, %f, %f\n", objfromlh->Rot[0], objfromlh->Rot[1], objfromlh->Rot[2], objfromlh->Rot[3] );
+
+ /*
+ -0.204066 3.238746 -0.856369
+ 0.812203 -0.264897 0.505599 0.120520
+ -0.204066 3.238746 -0.856369
+
+ 0.020036 3.162476 -0.117896
+ -0.322354 0.450869 0.346281 0.756898
+ 0.020036 3.162476 -0.117896
+ */
+
+ /* Facing up, moved -x 1m.
+ ====> 0.446818 -0.309120 -0.747630 ====> -0.222356 -0.701865 -0.558656
+ ====> -0.341064 0.099785 0.887015 ====> 0.619095 0.727263 0.029786
+ IN PLACE, but rotated 90 * up.
+ ====> 0.374516 -0.370583 -0.606996 ====> -0.120238 -0.670330 -0.426896
+ ====> -0.231758 0.070437 0.765982 ====> 0.497615 0.625761 0.078759
+ */
+
+ printf( "\n" );
+
+ }
+ notfirstcal = 1;
+ }
+ else
+ {
+ for( lh = 0; lh < NUM_LIGHTHOUSES; lh++ )
+ {
+ SurvivePose * objfromlh = &cd->hmd->FromLHPose[lh];
+ SurvivePose * lhp = &ctx->bsd[lh].Pose;
+
+ FLT pos[3];
+ quatrotatevector( pos, lhp->Rot, objfromlh->Pos );
+
+ pos[0] -= lhp->Pos[0];
+ pos[1] -= lhp->Pos[1];
+ pos[2] -= lhp->Pos[2];
+
+ //FLT rot[4] = {
+ // [0],
+ // lhp->Rot[1],
+ // lhp->Rot[2],
+ // lhp->Rot[3] };
+ //quatrotatevector( pos, lhp->Rot, pos );
+
+ fprintf( stderr, "%f, %f, %f\n", objfromlh->Pos[0], objfromlh->Pos[1], objfromlh->Pos[2] );
+ fprintf( stderr, "%f, %f, %f, %f\n", objfromlh->Rot[0], objfromlh->Rot[1], objfromlh->Rot[2], objfromlh->Rot[3] );
+ fprintf( stderr, "%f, %f, %f\n", lhp->Pos[0], lhp->Pos[1], lhp->Pos[2] );
+ fprintf( stderr, "%f, %f, %f, %f\n", lhp->Rot[0], lhp->Rot[1], lhp->Rot[2], lhp->Rot[3] );
+
+ fprintf( stderr, "====> %f %f %f\n",
+ pos[0], pos[1], pos[2] );
+
+ }
+ }
+ fprintf( stderr, "\n" );
+
+
+ reset_calibration( cd );
+// cd->stage = 5;
}
else
{
diff --git a/src/survive_cal.h b/src/survive_cal.h
index dd2a1e2..ce61962 100644
--- a/src/survive_cal.h
+++ b/src/survive_cal.h
@@ -33,7 +33,9 @@ void survive_cal_light( SurviveObject * so, int sensor_id, int acode, int timein
void survive_cal_angle( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle );
#define MAX_SENSORS_TO_CAL 96
-#define DRPTS 1024
+
+#define MIN_PTS_BEFORE_CAL 24
+#define DRPTS 32
#define MAX_CAL_PT_DAT (MAX_SENSORS_TO_CAL*NUM_LIGHTHOUSES*2)
struct SurviveCalData
{
@@ -61,6 +63,8 @@ struct SurviveCalData
SurviveObject * hmd;
+ PoserCB ConfigPoserFn;
+
//Stage:
// 0: Idle
// 1: Collecting OOTX data.
diff --git a/src/survive_cal_lhfind.c b/src/survive_cal_lhfind.c
index a1bb2cc..cc32154 100644
--- a/src/survive_cal_lhfind.c
+++ b/src/survive_cal_lhfind.c
@@ -129,13 +129,15 @@ int survive_cal_lhfind( SurviveCalData * cd )
fullrange *= 0.25;
}
- if( beste > 0.01 )
+ if( beste > 0.1 )
{
//Error too high
SV_ERROR( "LH: %d / Best E %f Error too high\n", lh, beste );
return -1;
}
+ RunOpti(cd, lh, 1, LighthousePos, LighthouseQuat);
+
cd->ctx->bsd[lh].PositionSet = 1;
copy3d( cd->ctx->bsd[lh].Pose.Pos, LighthousePos );
quatcopy( cd->ctx->bsd[lh].Pose.Rot, LighthouseQuat );
@@ -198,6 +200,7 @@ static FLT RunOpti( SurviveCalData * cd, int lh, int print, FLT * LighthousePos,
RayShootOut[2] = sqrt( 1 - (RayShootOut[0]*RayShootOut[0] + RayShootOut[1]*RayShootOut[1]) );
FLT RayShootOutWorld[3];
+ quatnormalize( LighthouseQuat, LighthouseQuat );
//Rotate that ray by the current rotation estimation.
quatrotatevector( RayShootOutWorld, LighthouseQuat, RayShootOut );
@@ -207,6 +210,7 @@ static FLT RunOpti( SurviveCalData * cd, int lh, int print, FLT * LighthousePos,
normalize3d( UsToTarget, UsToTarget );
FLT RotatedLastUs[3];
+ quatnormalize( LighthouseQuat, LighthouseQuat );
quatrotatevector( RotatedLastUs, LighthouseQuat, LastUsToTarget );
//Rotate the lighthouse around this axis to point at the HMD.
@@ -221,6 +225,7 @@ static FLT RunOpti( SurviveCalData * cd, int lh, int print, FLT * LighthousePos,
//Don't need to worry about being negative, cross product will fix it.
FLT RotateAmount = anglebetween3d( RayShootOutWorld, UsToTarget );
quatfromaxisangle( ConcatQuat, AxisToRotate, RotateAmount );
+ quatnormalize( ConcatQuat, ConcatQuat );
}
else
{
@@ -244,9 +249,13 @@ static FLT RunOpti( SurviveCalData * cd, int lh, int print, FLT * LighthousePos,
FLT RotateAmount = anglebetween3d( Actual, Target ) * mux;
//printf( "FA: %f (O:%f)\n", acos( dot3d( Actual, Target ) ), RotateAmount );
quatfromaxisangle( ConcatQuat, AxisToRotate, RotateAmount );
+ quatnormalize( ConcatQuat, ConcatQuat );
}
- quatrotateabout( LighthouseQuat, ConcatQuat, LighthouseQuat ); //Chekcked. This appears to be
+
+ quatnormalize( ConcatQuat, ConcatQuat );
+ quatnormalize( LighthouseQuat, LighthouseQuat );
+ quatrotateabout( LighthouseQuat, ConcatQuat, LighthouseQuat ); //Checked. This appears to be
mux = mux * 0.94;
if( second ) { second = 0; }
diff --git a/src/survive_config.c b/src/survive_config.c
index 3f0f199..10da9a6 100644
--- a/src/survive_config.c
+++ b/src/survive_config.c
@@ -209,8 +209,6 @@ const FLT* config_set_float_a(config_group *cg, const char *tag, const FLT* valu
assert(ptr!=NULL);
cv->data = ptr;
- printf("float array\n");
-
memcpy(cv->data,values,sizeof(FLT)*count);
cv->type = CONFIG_FLOAT_ARRAY;
cv->elements = count;
diff --git a/src/survive_data.c b/src/survive_data.c
index 63cc5c2..6824b0f 100644
--- a/src/survive_data.c
+++ b/src/survive_data.c
@@ -12,7 +12,6 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le )
//int32_t deltat = (uint32_t)le->timestamp - (uint32_t)so->last_master_time;
//if( so->codename[0] != 'H' )
- //printf( "*** %s %d %d %d %d %d\n", so->codename, le->sensor_id, le->type, le->length, le->timestamp, le->timestamp-so->tsl );
if( le->sensor_id > SENSORS_PER_OBJECT )
{
diff --git a/src/survive_vive.c b/src/survive_vive.c
index 993125a..0cae6f0 100755
--- a/src/survive_vive.c
+++ b/src/survive_vive.c
@@ -93,8 +93,7 @@ void survive_usb_close( SurviveContext * t );
int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject *wm0, SurviveObject * wm1, SurviveObject * tr0 );
int survive_usb_poll( SurviveContext * ctx );
int survive_get_config( char ** config, SurviveViveData * ctx, int devno, int interface, int send_extra_magic );
-
-
+int survive_vive_send_magic(struct SurviveContext * ctx, void * drv, int magic_code, void * data, int datalen );
static void handle_transfer(struct libusb_transfer* transfer)
@@ -306,6 +305,7 @@ int survive_usb_init( struct SurviveViveData * sv, struct SurviveObject * hmd, s
SV_INFO( "All enumerated devices attached." );
+ survive_vive_send_magic(ctx, sv, 1, 0, 0 );
//libUSB initialized. Continue.
return 0;
diff --git a/tools/lighthousefind_radii/Makefile b/tools/lighthousefind_radii/Makefile
new file mode 100644
index 0000000..b60e07c
--- /dev/null
+++ b/tools/lighthousefind_radii/Makefile
@@ -0,0 +1,10 @@
+all : lighthousefind_radii
+
+CFLAGS:=-g -O4 -DUSE_DOUBLE -I../../redist -flto
+LDFLAGS:=$(CFLAGS) -lm
+
+lighthousefind : lighthousefind.o ../../redist/linmath.c
+ gcc -o $@ $^ $(LDFLAGS)
+
+clean :
+ rm -rf *.o *~ lighthousefind_radii
diff --git a/tools/lighthousefind_radii/lighthousefind_radii.c b/tools/lighthousefind_radii/lighthousefind_radii.c
new file mode 100644
index 0000000..8a684cb
--- /dev/null
+++ b/tools/lighthousefind_radii/lighthousefind_radii.c
@@ -0,0 +1,557 @@
+#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;
+int LoadData( char Camera, const char * FileData );
+
+//Values used for RunTest()
+FLT LighthousePos[3] = { 0, 0, 0 };
+FLT LighthouseQuat[4] = { 1, 0, 0, 0 };
+
+FLT RunTest( int print );
+void PrintOpti();
+
+#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.
+} 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))
+
+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);
+}
+
+FLT calculateFitness(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);
+}
+
+#define MAX_RADII 32
+
+// note gradientOut will be of the same degree as numRadii
+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;
+}
+
+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 RefineEstimateUsingGradientDescent(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) ", i, newMatchFitness, g);
+#endif
+ g = g * 1.05;
+ }
+ else
+ {
+#ifdef RADII_DEBUG
+// printf("-");
+ printf("- %d %0.9f (%0.9f) ", i, newMatchFitness, g);
+#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("\ni=%d\n", i);
+}
+
+void SolveForLighthouse(Point *objPosition, FLT *objOrientation, TrackedObject *obj)
+{
+ FLT estimate[MAX_RADII];
+
+ for (size_t i = 0; i < MAX_RADII; i++)
+ {
+ estimate[i] = 2.4;
+ }
+
+ SensorAngles angles[MAX_RADII];
+ PointPair pairs[MAX_POINT_PAIRS];
+
+ size_t pairCount = 0;
+
+ obj->numSensors = 7; // 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 (size_t i = 0; i < obj->numSensors - 1; i++)
+ {
+ for (size_t 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++;
+ }
+ }
+
+
+ RefineEstimateUsingGradientDescent(estimate, angles, estimate, obj->numSensors, pairs, pairCount, NULL);
+
+ // we should now have an estimate of the radii.
+
+ for (size_t i = 0; i < obj->numSensors; 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)
+
+ getc(stdin);
+ return;
+}
+
+static void runTheNumbers()
+{
+ TrackedObject *to;
+
+ to = malloc(sizeof(TrackedObject) + (PTS * sizeof(TrackedSensor)));
+
+ int sensorCount = 0;
+
+ for (int i = 0; i < PTS; i++)
+ {
+ // if there are enough valid counts for both the x and y sweeps for sensor i
+ if ((hmd_point_counts[2 * i] > MIN_HITS_FOR_VALID) &&
+ (hmd_point_counts[2 * i + 1] > MIN_HITS_FOR_VALID))
+ {
+ to->sensor[sensorCount].point.x = hmd_points[i * 3 + 0];
+ to->sensor[sensorCount].point.y = hmd_points[i * 3 + 1];
+ to->sensor[sensorCount].point.z = hmd_points[i * 3 + 2];
+ to->sensor[sensorCount].normal.x = hmd_norms[i * 3 + 0];
+ to->sensor[sensorCount].normal.y = hmd_norms[i * 3 + 1];
+ to->sensor[sensorCount].normal.z = hmd_norms[i * 3 + 2];
+ to->sensor[sensorCount].theta = hmd_point_angles[i * 2 + 0] + LINMATHPI / 2;
+ to->sensor[sensorCount].phi = hmd_point_angles[i * 2 + 1] + LINMATHPI / 2;
+ sensorCount++;
+ }
+ }
+
+ to->numSensors = sensorCount;
+
+ printf("Using %d sensors to find lighthouse.\n", sensorCount);
+
+ Point lh;
+ for (int i = 0; i < 1; i++)
+ {
+ SolveForLighthouse(&lh, NULL, to);
+ //(0.156754, -2.403268, 2.280167)
+ //assert(fabs((lh.x / 0.1419305302702402) - 1) < 0.00001);
+ //assert(fabs((lh.y / 2.5574949720325431) - 1) < 0.00001);
+ //assert(fabs((lh.z / 2.2451193935772080) - 1) < 0.00001);
+ //assert(lh.x > 0);
+ //assert(lh.y > 0);
+ //assert(lh.z > 0);
+ }
+
+ printf("(%f, %f, %f)\n", lh.x, lh.y, lh.z);
+
+ //printTrackedObject(to);
+ free(to);
+}
+
+int main( int argc, char ** argv )
+{
+
+ if( argc != 3 )
+ {
+ fprintf( stderr, "Error: usage: camfind [camera (L or R)] [datafile]\n" );
+ exit( -1 );
+ }
+
+ //Load either 'L' (LH1) or 'R' (LH2) data.
+ if( LoadData( argv[1][0], argv[2] ) ) return 5;
+
+ runTheNumbers();
+
+
+
+}
+
+
+
+
+
+int LoadData( char Camera, const char * datafile )
+{
+
+ //First, read the positions of all the sensors on the HMD.
+ FILE * f = fopen( "HMD_points.csv", "r" );
+ int pt = 0;
+ if( !f ) { fprintf( stderr, "error: can't open hmd points.\n" ); return -5; }
+ while(!feof(f) && !ferror(f) && pt < PTS)
+ {
+ float fa, fb, fc;
+ int r = fscanf( f,"%g %g %g\n", &fa, &fb, &fc );
+ hmd_points[pt*3+0] = fa;
+ hmd_points[pt*3+1] = fb;
+ hmd_points[pt*3+2] = fc;
+ pt++;
+ if( r != 3 )
+ {
+ fprintf( stderr, "Not enough entries on line %d of points\n", pt );
+ return -8;
+ }
+ }
+ if( pt < PTS )
+ {
+ fprintf( stderr, "Not enough points.\n" );
+ return -9;
+ }
+ fclose( f );
+ printf( "Loaded %d points\n", pt );
+
+ //Read all the normals on the HMD into hmd_norms.
+ f = fopen( "HMD_normals.csv", "r" );
+ int nrm = 0;
+ if( !f ) { fprintf( stderr, "error: can't open hmd points.\n" ); return -5; }
+ while(!feof(f) && !ferror(f) && nrm < PTS)
+ {
+ float fa, fb, fc;
+ int r = fscanf( f,"%g %g %g\n", &fa, &fb, &fc );
+ hmd_norms[nrm*3+0] = fa;
+ hmd_norms[nrm*3+1] = fb;
+ hmd_norms[nrm*3+2] = fc;
+ nrm++;
+ if( r != 3 )
+ {
+ fprintf( stderr, "Not enough entries on line %d of normals\n", nrm );
+ return -8;
+ }
+ }
+ if( nrm < PTS )
+ {
+ fprintf( stderr, "Not enough points.\n" );
+ return -9;
+ }
+ if( nrm != pt )
+ {
+ fprintf( stderr, "point/normal counts disagree.\n" );
+ return -9;
+ }
+ fclose( f );
+ printf( "Loaded %d norms\n", nrm );
+
+ //Actually load the processed data!
+ int xck = 0;
+ f = fopen( datafile, "r" );
+ if( !f )
+ {
+ fprintf( stderr, "Error: cannot open %s\n", datafile );
+ exit (-11);
+ }
+ int lineno = 0;
+ while( !feof( f ) )
+ {
+ //Format:
+ // HMD LX 0 3433 173656.227498 327.160210 36.342361 2.990936
+ lineno++;
+ char devn[10];
+ char inn[10];
+ int id;
+ int pointct;
+ FLT avgTime;
+ FLT avgLen;
+ FLT stddevTime;
+ FLT stddevLen;
+ int ct = fscanf( f, "%9s %9s %d %d %lf %lf %lf %lf\n", devn, inn, &id, &pointct, &avgTime, &avgLen, &stddevTime, &stddevLen );
+ if( ct == 0 ) continue;
+ if( ct != 8 )
+ {
+ fprintf( stderr, "Malformatted line, %d in processed_data.txt\n", lineno );
+ }
+ if( strcmp( devn, "HMD" ) != 0 ) continue;
+
+ if( inn[0] != Camera ) continue;
+
+ int isy = inn[1] == 'Y';
+
+ hmd_point_angles[id*2+isy] = ( avgTime - 200000 ) / 200000 * 3.1415926535/2.0;
+ hmd_point_counts[id*2+isy] = pointct;
+ }
+ fclose( f );
+
+
+ int targpd;
+ int maxhits = 0;
+
+ for( targpd = 0; targpd < PTS; targpd++ )
+ {
+ int hits = hmd_point_counts[targpd*2+0];
+ if( hits > hmd_point_counts[targpd*2+1] ) hits = hmd_point_counts[targpd*2+1];
+ //Need an X and a Y lock.
+
+ if( hits > maxhits ) { maxhits = hits; best_hmd_target = targpd; }
+ }
+ if( maxhits < MIN_HITS_FOR_VALID )
+ {
+ fprintf( stderr, "Error: Not enough data for a primary fix.\n" );
+ }
+
+ return 0;
+}
+
diff --git a/tools/lighthousefind_tori/torus_localizer.c b/tools/lighthousefind_tori/torus_localizer.c
index fd74b22..894bbce 100644
--- a/tools/lighthousefind_tori/torus_localizer.c
+++ b/tools/lighthousefind_tori/torus_localizer.c
@@ -7,12 +7,12 @@
#include "visualization.h"
-static double distance(Point a, Point b)
+static FLT distance(Point a, Point b)
{
- double x = a.x - b.x;
- double y = a.y - b.y;
- double z = a.z - b.z;
- return sqrt(x*x + y*y + z*z);
+ 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)
@@ -457,6 +457,137 @@ static Point RefineEstimateUsingModifiedGradientDescent1(Point initialEstimate,
}
+// 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 RotationEstimateFitness(Point lhPoint, FLT *quaternion, TrackedObject *obj)
+{
+ 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 t1[3] = { 1, tan(theta), 0 };
+ FLT t2[3] = { 1, tan(theta), 1 };
+
+ FLT tNorm[3];
+
+ // the normal is the cross of two vectors on the plane.
+ cross3d(tNorm, t1, t2);
+
+ // distance for this plane is d= fabs(A*x + B*y)/sqrt(A^2+B^2) (z term goes away since this plane is "vertical")
+ // where A is
+ //FLT d =
+ }
+}
+
+static Point RefineRotationEstimate(Point initialEstimate, PointsAndAngle *pna, size_t pnaCount, FILE *logFile)
+{
+ int i = 0;
+ FLT lastMatchFitness = getPointFitness(initialEstimate, pna, pnaCount);
+ 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 = getNormalizedVector(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 = getNormalizedVector(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 = getNormalizedVector(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);
+
+ 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;
+
+ }
+
+
+ }
+ printf("\ni=%d\n", i);
+
+ return lastPoint;
+}
+
+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[3] = { 0, 0, 1 };
+ FLT quat1[4];
+ quatfromaxisangle(quat1, zAxis, theta);
+ // not correcting for phi, but that's less important.
+
+ // Step 2, optimize the quaternion to match the data.
+
+}
+
+
Point SolveForLighthouse(TrackedObject *obj, char doLogOutput)
{
PointsAndAngle pna[MAX_POINT_PAIRS];