From b13b5f9af2c2803f919d8897cd3ed5d24831cad3 Mon Sep 17 00:00:00 2001
From: Joshua Allen <axlecrusher@gmail.com>
Date: Tue, 7 Mar 2017 21:48:30 -0500
Subject: null after free

---
 src/survive_config.c | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/survive_config.c b/src/survive_config.c
index f30bf87..8e0241a 100644
--- a/src/survive_config.c
+++ b/src/survive_config.c
@@ -25,8 +25,8 @@ void init_config_entry(config_entry* ce) {
 }
 
 void destroy_config_entry(config_entry* ce) {
-	if (ce->tag!=NULL) free(ce->tag);
-	if (ce->data!=NULL) free(ce->data);
+	if (ce->tag!=NULL) { free(ce->tag); ce->tag=NULL; }
+	if (ce->data!=NULL) { free(ce->data); ce->data=NULL; }
 }
 
 void init_config_group(config_group *cg, uint16_t count) {
-- 
cgit v1.3.1


From 5eeecb19eb884baf4781280a9c8e1c394fe9c669 Mon Sep 17 00:00:00 2001
From: cnlohr <lohr85@gmail.com>
Date: Wed, 8 Mar 2017 19:13:02 -0500
Subject: update with poser in tree

---
 Makefile          | 15 ++++++++++-----
 src/poser_dummy.c | 52 ++++++++++++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 62 insertions(+), 5 deletions(-)
 create mode 100644 src/poser_dummy.c

diff --git a/Makefile b/Makefile
index 3c8431b..1a1712d 100644
--- a/Makefile
+++ b/Makefile
@@ -3,27 +3,32 @@ all : lib data_recorder test calibrate calibrate_client
 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
+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)
+
+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)
 
-data_recorder : data_recorder.c lib/libsurvive.so redist/os_generic.o redist/DrawFunctions.o redist/XDriver.o
+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 redist/DrawFunctions.c redist/XDriver.c
+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 redist/DrawFunctions.c redist/XDriver.c
+calibrate_client :  calibrate_client.c lib/libsurvive.so redist/os_generic.c $(GRAPHICS_LOFI)
 	gcc -o $@ $^ $(LDFLAGS) $(CFLAGS)
 
 lib:
 	mkdir lib
 
-lib/libsurvive.so : src/survive.o src/survive_usb.o src/survive_data.o src/survive_process.o redist/jsmn.o src/ootx_decoder.o redist/linmath.o src/survive_driverman.o src/survive_vive.o src/survive_config.o redist/json_helpers.o src/PoserDummy.o $(DEBUGSTUFF) $(CALS)
+lib/libsurvive.so : $(LIBSURVIVE_O)
 	gcc -o $@ $^ $(LDFLAGS) -shared
 
 clean :
diff --git a/src/poser_dummy.c b/src/poser_dummy.c
new file mode 100644
index 0000000..67f8edb
--- /dev/null
+++ b/src/poser_dummy.c
@@ -0,0 +1,52 @@
+#include <survive.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+typedef struct
+{
+	int something;
+	//Stuff
+} DummyData;
+
+int PoserDummy( 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;
+		//printf( "Full scene data.\n" );
+		break;
+	}
+	case POSERDATA_DISASSOCIATE:
+	{
+		free( dd );
+		so->PoserData = 0;
+		//printf( "Need to disassociate.\n" );
+		break;
+	}
+	}
+
+}
+
+
+REGISTER_LINKTIME( PoserDummy );
+
-- 
cgit v1.3.1


From 26f55787820874629519c589f7c7c1ba1334ef54 Mon Sep 17 00:00:00 2001
From: Joshua Allen <axlecrusher@gmail.com>
Date: Wed, 8 Mar 2017 21:08:30 -0500
Subject: return null if no file is opened

---
 redist/json_helpers.c | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/redist/json_helpers.c b/redist/json_helpers.c
index b7ccb40..4a3ba55 100644
--- a/redist/json_helpers.c
+++ b/redist/json_helpers.c
@@ -76,6 +76,7 @@ uint32_t JSON_STRING_LEN;
 
 char* load_file_to_mem(const char* path) {
 	FILE * f = fopen( path, "r" );
+	if (f==NULL) return NULL;
 	fseek( f, 0, SEEK_END );
 	int len = ftell( f );
 	fseek( f, 0, SEEK_SET );
@@ -116,7 +117,10 @@ static uint16_t json_load_array(const char* JSON_STRING, jsmntok_t* tokens, uint
 
 void json_load_file(const char* path) {
 	uint32_t i = 0;
+
 	char* JSON_STRING = load_file_to_mem(path);
+	if (JSON_STRING==NULL) return;
+
 	JSON_STRING_LEN = strlen(JSON_STRING);
 
 	jsmn_parser parser;
-- 
cgit v1.3.1


From d69cf82a079f93ae1109f70fd011a0efa75b44b5 Mon Sep 17 00:00:00 2001
From: Joshua Allen <axlecrusher@gmail.com>
Date: Wed, 8 Mar 2017 21:17:50 -0500
Subject: shoehorn configuration into SurviveContext

---
 calibrate.c                  |  1 -
 calibrate_client.c           |  4 ++--
 include/libsurvive/survive.h |  5 +++++
 src/survive.c                | 21 ++++++++++++++++++++-
 src/survive_cal.c            |  2 +-
 src/survive_config.c         | 45 ++++++++++++++++++++++++++++++--------------
 src/survive_config.h         | 20 ++++++++++++--------
 7 files changed, 71 insertions(+), 27 deletions(-)

diff --git a/calibrate.c b/calibrate.c
index c557251..82da7a7 100644
--- a/calibrate.c
+++ b/calibrate.c
@@ -147,7 +147,6 @@ void * GuiThread( void * v )
 int main()
 {
 	ctx = survive_init( 0 );
-	config_init();
 
 	survive_install_light_fn( ctx,  my_light_process );
 	survive_install_imu_fn( ctx,  my_imu_process );
diff --git a/calibrate_client.c b/calibrate_client.c
index b15b9db..f28520b 100644
--- a/calibrate_client.c
+++ b/calibrate_client.c
@@ -141,7 +141,7 @@ void * GuiThread( void * v )
 
 int main()
 {
-
+/*
 	config_init();
 	config_read("config.json");
 	config_set_str(&global_config_values, "Hello","World!");
@@ -156,7 +156,7 @@ int main()
 
 
 //	config_save("config.json");
-
+*/
 	
 	ctx = survive_init( 1 );
 
diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h
index 7fd6046..03249e9 100644
--- a/include/libsurvive/survive.h
+++ b/include/libsurvive/survive.h
@@ -77,6 +77,8 @@ struct BaseStationData
 	FLT fcalgibmag[2];
 };
 
+struct config_group;
+
 struct SurviveContext
 {
 	text_feedback_func faultfunction;
@@ -85,6 +87,9 @@ struct SurviveContext
 	imu_process_func imuproc;
 	angle_process_func angleproc;
 
+	struct config_group* global_config_values;
+	struct config_group* lh_config; //lighthouse configs
+
 	//Calibration data:
 	BaseStationData bsd[NUM_LIGHTHOUSES];
 
diff --git a/src/survive.c b/src/survive.c
index efa5d82..09eb432 100644
--- a/src/survive.c
+++ b/src/survive.c
@@ -8,6 +8,8 @@
 #include <string.h>
 #include <zlib.h>
 
+#include "survive_config.h"
+
 static void survivefault( struct SurviveContext * ctx, const char * fault )
 {
 	fprintf( stderr, "Error: %s\n", fault );
@@ -26,6 +28,14 @@ SurviveContext * survive_init( int headless )
 	int i = 0;
 	SurviveContext * ctx = calloc( 1, sizeof( SurviveContext ) );
 
+	ctx->global_config_values = malloc( sizeof(config_group) );
+	ctx->lh_config = malloc( sizeof(config_group) * NUM_LIGHTHOUSES);
+
+	init_config_group(ctx->global_config_values,10);
+	init_config_group(ctx->lh_config,10);
+
+	config_read(ctx, "config.json");
+
 	ctx->faultfunction = survivefault;
 	ctx->notefunction = survivenote;
 
@@ -43,7 +53,7 @@ SurviveContext * survive_init( int headless )
 	}
 
 	i = 0;
-	const char * PreferredPoser = "PoserDummy"; //config_read_str( cg, "DefualtPoser", "PoserDummy" ); /XXX Axlecrusher, can you add config stuff for this?
+	const char * PreferredPoser = config_read_str( ctx->global_config_values, "DefualtPoser", "PoserDummy" );
 	PoserCB PreferredPoserCB = 0;
 	const char * FirstPoser = 0;
 	printf( "Available posers:\n" );
@@ -169,11 +179,20 @@ void survive_close( SurviveContext * ctx )
 		ctx->drivercloses[i]( ctx, ctx->drivers[i] );
 	}
 
+
+	config_save(ctx, "config.json");
+
+	destroy_config_group(ctx->global_config_values);
+	destroy_config_group(ctx->lh_config);
+
 	free( ctx->objs );
 	free( ctx->drivers );
 	free( ctx->driverpolls );
 	free( ctx->drivermagics );
 	free( ctx->drivercloses );
+	free( ctx->global_config_values );
+	free( ctx->lh_config );
+
 	free( ctx );
 }
 
diff --git a/src/survive_cal.c b/src/survive_cal.c
index 2acf51c..06914eb 100644
--- a/src/survive_cal.c
+++ b/src/survive_cal.c
@@ -52,7 +52,7 @@ void ootx_packet_clbk_d(ootx_decoder_context *ct, ootx_packet* packet)
 	b->fcalgibmag[1] = v6.fcal_1_gibmag;
 	b->OOTXSet = 1;
 
-	config_set_lighthouse(b,id);
+	config_set_lighthouse(ctx->lh_config,b,id);
 //	config_save("config.json");
 }
 
diff --git a/src/survive_config.c b/src/survive_config.c
index 4de6a3a..de0530b 100644
--- a/src/survive_config.c
+++ b/src/survive_config.c
@@ -9,8 +9,8 @@
 #define MAX_CONFIG_ENTRIES 100
 #define MAX_LIGHTHOUSES 2
 
-config_group global_config_values;
-config_group lh_config[MAX_LIGHTHOUSES]; //lighthouse configs
+//config_group global_config_values;
+//config_group lh_config[MAX_LIGHTHOUSES]; //lighthouse configs
 
 //static uint16_t used_entries = 0;
 
@@ -40,6 +40,17 @@ void init_config_group(config_group *cg, uint16_t count) {
 	}
 }
 
+void destroy_config_group(config_group* cg) {
+	uint16_t i = 0;
+	if (cg->config_entries==NULL) return;
+
+	for (i=0;i<cg->max_entries;++i) {
+		destroy_config_entry(cg->config_entries+i);
+	}
+
+	free(cg->config_entries);
+}
+
 void resize_config_group(config_group *cg, uint16_t count) {
 	uint16_t i = 0;
 
@@ -57,6 +68,7 @@ void resize_config_group(config_group *cg, uint16_t count) {
 	}
 }
 
+/*
 void config_init() {
 	uint16_t i = 0;
 	init_config_group(&global_config_values, MAX_CONFIG_ENTRIES);
@@ -64,9 +76,9 @@ void config_init() {
 		init_config_group(lh_config+i, 9);
 	}
 }
+*/
 
-
-void config_set_lighthouse(BaseStationData* bsd, uint8_t idx) {
+void config_set_lighthouse(config_group* lh_config, BaseStationData* bsd, uint8_t idx) {
 	config_group *cg = lh_config+idx;
 	config_set_uint32(cg,"index", idx);
 	config_set_uint32(cg,"id", bsd->BaseStationID);
@@ -123,7 +135,7 @@ FLT config_read_float(config_group *cg, const char *tag, const FLT def) {
 	return config_set_float(cg, tag, def);
 }
 
-uint16_t config_read_float_array(config_group *cg, const char *tag, FLT** values, const FLT* def, uint16_t count) {
+uint16_t config_read_float_array(config_group *cg, const char *tag, const FLT** values, const FLT* def, uint16_t count) {
 	config_entry *cv = find_config_entry(cg, tag);
 
 	if (cv != NULL) {
@@ -236,14 +248,17 @@ void write_config_group(FILE* f, config_group *cg, char *tag) {
 	}
 }
 
-void config_save(const char* path) {
+//struct SurviveContext;
+SurviveContext* survive_context;
+
+void config_save(SurviveContext* sctx, const char* path) {
 	uint16_t i = 0;
 
 	FILE* f = fopen(path, "w");
 
-	write_config_group(f,&global_config_values, NULL);
-	write_config_group(f,lh_config, "lighthouse0");
-	write_config_group(f,lh_config+1, "lighthouse1");
+	write_config_group(f,sctx->global_config_values, NULL);
+	write_config_group(f,sctx->lh_config, "lighthouse0");
+	write_config_group(f,sctx->lh_config+1, "lighthouse1");
 
 	fclose(f);
 }
@@ -261,11 +276,11 @@ uint8_t cg_stack_head = 0;
 void handle_config_group(char* tag) {
 	cg_stack_head++;
 	if (strcmp("lighthouse0",tag) == 0) {
-		cg_stack[cg_stack_head] = lh_config;
+		cg_stack[cg_stack_head] = survive_context->lh_config;
 	} else if (strcmp("lighthouse1",tag) == 0) {
-		cg_stack[cg_stack_head] = lh_config+1;
+		cg_stack[cg_stack_head] = survive_context->lh_config+1;
 	} else {
-		cg_stack[cg_stack_head] = &global_config_values;
+		cg_stack[cg_stack_head] = survive_context->global_config_values;
 	}
 }
 
@@ -343,12 +358,14 @@ void handle_tag_value(char* tag, char** values, uint16_t count) {
 //	else if (count>1) config_set_str
 }
 
-void config_read(const char* path) {
+void config_read(SurviveContext* sctx, const char* path) {
+	survive_context = sctx;
+
 	json_begin_object = handle_config_group;
 	json_end_object = pop_config_group;
 	json_tag_value = handle_tag_value;
 
-	cg_stack[0] = &global_config_values;
+	cg_stack[0] = sctx->global_config_values;
 
 	json_load_file(path);
 
diff --git a/src/survive_config.h b/src/survive_config.h
index cd16c76..c8c7762 100644
--- a/src/survive_config.h
+++ b/src/survive_config.h
@@ -26,29 +26,33 @@ typedef struct {
 	uint32_t elements;
 } config_entry;
 
-typedef struct {
+typedef struct config_group {
 	config_entry *config_entries;
 	uint16_t	used_entries;
 	uint16_t	max_entries;
 } config_group;
 
-extern config_group global_config_values;
-extern config_group lh_config[2]; //lighthouse configs
+//extern config_group global_config_values;
+//extern config_group lh_config[2]; //lighthouse configs
 
+void init_config_group(config_group *cg, uint16_t count);
+void destroy_config_group(config_group* cg);
 
-void config_init();
+//void config_init();
 //void config_open(const char* path, const char* mode);
-void config_read(const char* path);
+
 //void config_write_lighthouse(struct BaseStationData* bsd, uint8_t length);
-void config_set_lighthouse(BaseStationData* bsd, uint8_t idx);
+void config_set_lighthouse(config_group* lh_config, BaseStationData* bsd, uint8_t idx);
+
+void config_read(SurviveContext* sctx, const char* path);
+void config_save(SurviveContext* sctx, const char* path);
 
-void config_save(const char* path);
 const FLT config_set_float(config_group *cg, const char *tag, const FLT value);
 const uint32_t config_set_uint32(config_group *cg, const char *tag, const uint32_t value);
 const char* config_set_str(config_group *cg, const char *tag, const char* value);
 
 FLT config_read_float(config_group *cg, const char *tag, const FLT def);
-uint16_t config_read_float_array(config_group *cg, const char *tag, FLT** values, const FLT* def, uint16_t count);
+uint16_t config_read_float_array(config_group *cg, const char *tag, const FLT** values, const FLT* def, uint16_t count);
 uint32_t config_read_uint32(config_group *cg, const char *tag, const uint32_t def);
 const char* config_read_str(config_group *cg, const char *tag, const char *def);
 
-- 
cgit v1.3.1


From e7994264c36713b485b51f5505707bd8743b9e4d Mon Sep 17 00:00:00 2001
From: Joshua Allen <axlecrusher@gmail.com>
Date: Wed, 8 Mar 2017 21:19:38 -0500
Subject: comment out unneeded #defines

---
 src/survive_config.c | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/survive_config.c b/src/survive_config.c
index de0530b..38c24e1 100644
--- a/src/survive_config.c
+++ b/src/survive_config.c
@@ -6,8 +6,8 @@
 
 #include <errno.h>
 
-#define MAX_CONFIG_ENTRIES 100
-#define MAX_LIGHTHOUSES 2
+//#define MAX_CONFIG_ENTRIES 100
+//#define MAX_LIGHTHOUSES 2
 
 //config_group global_config_values;
 //config_group lh_config[MAX_LIGHTHOUSES]; //lighthouse configs
-- 
cgit v1.3.1


From 9e1eda90ff4c426750eed202c03808f6406988bb Mon Sep 17 00:00:00 2001
From: Joshua Allen <axlecrusher@gmail.com>
Date: Wed, 8 Mar 2017 21:37:20 -0500
Subject: do not malloc if size is 0

---
 src/survive_config.c | 6 +++++-
 1 file changed, 5 insertions(+), 1 deletion(-)

diff --git a/src/survive_config.c b/src/survive_config.c
index 38c24e1..3f0f199 100644
--- a/src/survive_config.c
+++ b/src/survive_config.c
@@ -31,9 +31,13 @@ void destroy_config_entry(config_entry* ce) {
 
 void init_config_group(config_group *cg, uint16_t count) {
 	uint16_t i = 0;
-	cg->config_entries = malloc(count*sizeof(config_entry));
 	cg->used_entries = 0;
 	cg->max_entries = count;
+	cg->config_entries = NULL;
+
+	if (count==0) return;
+
+	cg->config_entries = malloc(count*sizeof(config_entry));
 
 	for (i=0;i<count;++i) {
 		init_config_entry(cg->config_entries+i);
-- 
cgit v1.3.1


From cc988b75ead0b9a64e42b9b814a1cf88db22b07c Mon Sep 17 00:00:00 2001
From: cnlohr <lohr85@gmail.com>
Date: Wed, 8 Mar 2017 21:42:15 -0500
Subject: add vive-rules to useful files folder (About to drop in EDID data)

---
 useful_files/81-vive.rules | 14 ++++++++++++++
 1 file changed, 14 insertions(+)
 create mode 100644 useful_files/81-vive.rules

diff --git a/useful_files/81-vive.rules b/useful_files/81-vive.rules
new file mode 100644
index 0000000..ab38087
--- /dev/null
+++ b/useful_files/81-vive.rules
@@ -0,0 +1,14 @@
+# HTC Vive HID Sensor naming and permissioning
+KERNEL=="hidraw*", SUBSYSTEM=="hidraw", ATTRS{idVendor}=="0bb4", ATTRS{idProduct}=="2c87", TAG+="uaccess"
+KERNEL=="hidraw*", SUBSYSTEM=="hidraw", ATTRS{idVendor}=="28de", ATTRS{idProduct}=="2101", TAG+="uaccess"
+KERNEL=="hidraw*", SUBSYSTEM=="hidraw", ATTRS{idVendor}=="28de", ATTRS{idProduct}=="2000", TAG+="uaccess"
+KERNEL=="hidraw*", SUBSYSTEM=="hidraw", ATTRS{idVendor}=="28de", ATTRS{idProduct}=="1043", TAG+="uaccess"
+KERNEL=="hidraw*", SUBSYSTEM=="hidraw", ATTRS{idVendor}=="28de", ATTRS{idProduct}=="2050", TAG+="uaccess"
+KERNEL=="hidraw*", SUBSYSTEM=="hidraw", ATTRS{idVendor}=="28de", ATTRS{idProduct}=="2011", TAG+="uaccess"
+KERNEL=="hidraw*", SUBSYSTEM=="hidraw", ATTRS{idVendor}=="28de", ATTRS{idProduct}=="2012", TAG+="uaccess"
+SUBSYSTEM=="usb", ATTRS{idVendor}=="0bb4", ATTRS{idProduct}=="2c87", TAG+="uaccess"
+# HTC Camera USB Node
+SUBSYSTEM=="usb", ATTRS{idVendor}=="114d", ATTRS{idProduct}=="8328", TAG+="uaccess"
+# HTC Mass Storage Node
+SUBSYSTEM=="usb", ATTRS{idVendor}=="114d", ATTRS{idProduct}=="8200", TAG+="uaccess"
+SUBSYSTEM=="usb", ATTRS{idVendor}=="114d", ATTRS{idProduct}=="8a12", TAG+="uaccess"
-- 
cgit v1.3.1


From 65256592d4f23215f1d72a2cd20539d22cddd8b3 Mon Sep 17 00:00:00 2001
From: CNLohr <charles@cnlohr.com>
Date: Wed, 8 Mar 2017 21:43:10 -0500
Subject: Add files via upload

---
 useful_files/htc_vive_edid_on_display_port.dat | Bin 0 -> 256 bytes
 1 file changed, 0 insertions(+), 0 deletions(-)
 create mode 100644 useful_files/htc_vive_edid_on_display_port.dat

diff --git a/useful_files/htc_vive_edid_on_display_port.dat b/useful_files/htc_vive_edid_on_display_port.dat
new file mode 100644
index 0000000..c5fdce2
Binary files /dev/null and b/useful_files/htc_vive_edid_on_display_port.dat differ
-- 
cgit v1.3.1


From 5629fd458d1de11f604248422473291e434c289f Mon Sep 17 00:00:00 2001
From: CNLohr <charles@cnlohr.com>
Date: Thu, 9 Mar 2017 00:02:37 -0500
Subject: Delete 81-vive.rules

---
 81-vive.rules | 14 --------------
 1 file changed, 14 deletions(-)
 delete mode 100644 81-vive.rules

diff --git a/81-vive.rules b/81-vive.rules
deleted file mode 100644
index ab38087..0000000
--- a/81-vive.rules
+++ /dev/null
@@ -1,14 +0,0 @@
-# HTC Vive HID Sensor naming and permissioning
-KERNEL=="hidraw*", SUBSYSTEM=="hidraw", ATTRS{idVendor}=="0bb4", ATTRS{idProduct}=="2c87", TAG+="uaccess"
-KERNEL=="hidraw*", SUBSYSTEM=="hidraw", ATTRS{idVendor}=="28de", ATTRS{idProduct}=="2101", TAG+="uaccess"
-KERNEL=="hidraw*", SUBSYSTEM=="hidraw", ATTRS{idVendor}=="28de", ATTRS{idProduct}=="2000", TAG+="uaccess"
-KERNEL=="hidraw*", SUBSYSTEM=="hidraw", ATTRS{idVendor}=="28de", ATTRS{idProduct}=="1043", TAG+="uaccess"
-KERNEL=="hidraw*", SUBSYSTEM=="hidraw", ATTRS{idVendor}=="28de", ATTRS{idProduct}=="2050", TAG+="uaccess"
-KERNEL=="hidraw*", SUBSYSTEM=="hidraw", ATTRS{idVendor}=="28de", ATTRS{idProduct}=="2011", TAG+="uaccess"
-KERNEL=="hidraw*", SUBSYSTEM=="hidraw", ATTRS{idVendor}=="28de", ATTRS{idProduct}=="2012", TAG+="uaccess"
-SUBSYSTEM=="usb", ATTRS{idVendor}=="0bb4", ATTRS{idProduct}=="2c87", TAG+="uaccess"
-# HTC Camera USB Node
-SUBSYSTEM=="usb", ATTRS{idVendor}=="114d", ATTRS{idProduct}=="8328", TAG+="uaccess"
-# HTC Mass Storage Node
-SUBSYSTEM=="usb", ATTRS{idVendor}=="114d", ATTRS{idProduct}=="8200", TAG+="uaccess"
-SUBSYSTEM=="usb", ATTRS{idVendor}=="114d", ATTRS{idProduct}=="8a12", TAG+="uaccess"
-- 
cgit v1.3.1


From 3fa72f4b765457ce369d1aa1495dc36c90a94ebf Mon Sep 17 00:00:00 2001
From: ultramn <dchapm2@umbc.edu>
Date: Wed, 8 Mar 2017 21:03:05 -0800
Subject: Added OrthoSolve, it works great!

---
 dave/AffineSolve.c | 478 +++++++++++++++++++++++++++++++++++++++++++----------
 1 file changed, 395 insertions(+), 83 deletions(-)

diff --git a/dave/AffineSolve.c b/dave/AffineSolve.c
index 36587e6..1eaf633 100644
--- a/dave/AffineSolve.c
+++ b/dave/AffineSolve.c
@@ -1,7 +1,6 @@
 //
 //  main.c
-//  AffineSolve
-//
+//  Aff
 //  Created by user on 3/2/17.
 //  Copyright © 2017 user. All rights reserved.
 //
@@ -10,10 +9,24 @@
 #include <string.h>
 #include <stdlib.h>
 #include <math.h>
+#include "dclapack.h"
 
 #define LH_ID    0
 #define NUM_HMD 32
 
+#define MAX_POINTS 128
+//#define _ABS(a)  ( (a)<=0 ? -(a) : (a) )
+#define _SIGN(a) ( (a)<=0 ? -1.0f : 1.0f )
+#define RANDF  ( (float)rand() / (float)RAND_MAX )
+#define PI 3.14159265358979323846264
+
+#define STEP_SIZE_ROT 1.0
+#define STEP_SIZE_POS 1.0
+#define FALLOFF   0.99999
+#define NITER     2000000
+#define TOO_SMALL 0.0001
+#define ORTHOG_PENALTY   1.0
+
 float hmd_pos[NUM_HMD][3];
 void ReadHmdPoints()
 {
@@ -31,17 +44,35 @@ void ReadHmdPoints()
     fclose(fin);
 }
 
-#define MAX_POINTS 128
-#define _ABS(a)  ( (a)<=0 ? -(a) : (a) )
-#define _SIGN(a) ( (a)<=0 ? -1.0f : 1.0f )
-#define RANDF  ( (float)rand() / (float)RAND_MAX )
+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; }
 
-#define STEP_SIZE_ROT 1.0
-#define STEP_SIZE_POS 1.0
-#define FALLOFF   0.99999
-#define NITER     2000000
-#define TOO_SMALL 0.0001
-#define ORTHOG_PENALTY   1.0
+    // Read ptinfo.csv
+    FILE *fin = fopen("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);
+}
 
 #define PRINT_MAT(A,M,N) { \
     int m,n; \
@@ -54,6 +85,253 @@ void ReadHmdPoints()
     } \
 }
 
+#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(
+    float T[4][4],               // OUTPUT: 4x4 transformation matrix
+    FLOAT S_out[2][MAX_POINTS],  // OUTPUT:  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)
+{
+    int i,j,k;
+    FLOAT R[3][3];           // OUTPUT: 3x3 rotation matrix
+    FLOAT trans[3];          // INPUT:  x,y,z translation vector
+
+    //--------------------
+    // Remove the center of the HMD offsets, and the screen space
+    //--------------------
+    FLOAT xbar[3] = {0.0, 0.0, 0.0};
+    FLOAT sbar[2] = {0.0, 0.0};
+    FLOAT S[2][MAX_POINTS];
+    FLOAT X[3][MAX_POINTS];
+    FLOAT 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
+    //--------------------
+    FLOAT Xt[MAX_POINTS][3];
+    FLOAT XXt[3][3];
+    FLOAT invXXt[3][3];
+    FLOAT SXt[2][3];
+    FLOAT 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
+FLOAT S_morph[2][MAX_POINTS];
+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
+    //--------------------
+    FLOAT 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]
+    FLOAT B[3][1] = { {0.0}, {0.0}, {1.0} };
+    FLOAT inv_uM[3][3];
+    FLOAT 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
+    //--------------------
+    FLOAT uf_len = sqrt( uf[0][0]*uf[0][0] + uf[1][0]*uf[1][0] + uf[2][0]*uf[2][0] );
+    FLOAT 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);
+
+//FLOAT check[3][1];
+//MUL(uM,uf,check,3,3,1);
+//PRINT(check,3,1);
+
+    //--------------------
+    // take cross products to get vectors u,r
+    //--------------------
+    FLOAT 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);
+    FLOAT 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
+    //--------------------
+    FLOAT 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);
+    FLOAT fhat_len = sqrt( fhat[0][0]*fhat[0][0] + fhat[1][0]*fhat[1][0] );
+    FLOAT uhat_len = sqrt( uhat[0][0]*uhat[0][0] + uhat[1][0]*uhat[1][0] );
+    FLOAT rhat_len = sqrt( rhat[0][0]*rhat[0][0] + rhat[1][0]*rhat[1][0] );
+    FLOAT 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);
+
+    FLOAT ydist1 = 1.0 /  uhat_len; //0.25*PI / uhat_len;
+    FLOAT ydist2 = 1.0 /  rhat_len; //0.25*PI / rhat_len;
+    FLOAT 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
+    //--------------------
+    FLOAT x[3][1] = { {M[0][0]*ydist}, {0.0}, {M[1][0]*ydist} };
+    FLOAT y[3][1] = { {M[0][1]*ydist}, {0.0}, {M[1][1]*ydist} };
+    FLOAT 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 . . .
+    FLOAT x_y = sqrt(1.0 - x[0][0]*x[0][0] - x[2][0]*x[2][0]);
+    FLOAT y_y = sqrt(1.0 - y[0][0]*y[0][0] - y[2][0]*y[2][0]);
+    FLOAT z_y = sqrt(1.0 - z[0][0]*z[0][0] - z[2][0]*z[2][0]);
+
+    // Exhaustively flip the minus sign of the z axis until we find the right one . . .
+    FLOAT bestErr = 9999.0;
+    FLOAT xy_dot2 = x[0][0]*y[0][0] + x[2][0]*y[2][0];
+    FLOAT yz_dot2 = y[0][0]*z[0][0] + y[2][0]*z[2][0];
+    FLOAT 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
+                FLOAT xy_dot = xy_dot2 + x_y*y_y;
+                FLOAT yz_dot = yz_dot2 + y_y*z_y;
+                FLOAT zx_dot = zx_dot2 + z_y*x_y;
+                FLOAT err = _ABS(xy_dot) + _ABS(yz_dot) + _ABS(zx_dot);
+                
+                // Calculate the handedness
+                FLOAT cx,cy,cz;
+                CrossProduct(cx,cy,cz,x[0][0],x_y,x[2][0],y[0][0],y_y,y[2][0]);
+                FLOAT hand = cx*z[0][0] + cy*y_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; }
+                z_y = -z_y;
+            }
+            y_y = -y_y;
+        }
+        x_y = -x_y;
+    }
+    printf("bestErr %f\n", bestErr);
+
+    for (i=0; i<nPoints; i++) {
+        float x1 = x[0][0]*X[0][i] + y[0][0]*X[1][i] + z[0][0]*X[2][i];
+        float y1 = x[1][0]*X[0][i] + y[1][0]*X[1][i] + z[1][0]*X[2][i];
+        float 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
+    //--------------------
+    FLOAT xhat[2][1]  = { {0.0}, {1.0} };
+    FLOAT urhat[2][2] = {
+        {uhat[0][0], uhat[1][0]},
+        {rhat[0][0], rhat[1][0]} };
+    FLOAT inv_urhat[2][2];
+    FLOAT ab[2][1];
+    INV(urhat,inv_urhat,2);
+    MUL(inv_urhat,xhat,ab,2,2,1);
+PRINT(ab,2,1);
+    FLOAT a = ab[0][0], b = ab[1][0];
+
+    //-------------------
+    // calculate the xyz coordinate system
+    //-------------------
+    FLOAT y[3][1] = { {f[0][0]}, {f[1][0]}, {f[2][0]} };
+    FLOAT 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]} };
+    FLOAT 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;
+    FLOAT 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]);
+    FLOAT 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];
+    FLOAT 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;
+
+    //-------------------
+    // Plot the output points
+    //-------------------
+    for (i=0; i<nPoints; i++) {
+        float 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];
+        float 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];
+        float 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] = asin(Tx / Ty);   // horiz
+        S_out[1][i] = asin(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("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
@@ -102,8 +380,8 @@ void AffineSolve(
             errorSq += Ei*Ei;
         }
 
-        printf("%d %f %f %f %f\n", iter, T[0][3], T[1][3], T[2][3], sqrt(errorSq));
-
+//        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)
         {
@@ -228,95 +506,129 @@ void AffineSolve(
             }
         }
     }
-
+    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;
+    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 nPoints = 0;
-    
-    // Read the hmd points
-    ReadHmdPoints();
-    
-    //-------------------------
-    // Read the lighthouse data
-    //-------------------------
-    FILE *fin = fopen("ptinfo.csv", "r");
-    if (fin==NULL) { printf("ERROR: could not open ptinfo.csv for reading\n"); exit(1); }
-    while (!feof(fin))
+    int nPlanes = 0;
+
+    for (sen=0; sen<NUM_HMD; sen++)
     {
-        // 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 (lh == LH_ID && sen < NUM_HMD) {
-            // Set the offset
-            O[nPoints][0] = hmd_pos[sen][0];
-            O[nPoints][1] = hmd_pos[sen][1];
-            O[nPoints][2] = hmd_pos[sen][2];
-            O[nPoints][3] = 1.0;
+        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 == 1) {   // Horizontal
-                N[nPoints][0] = -cos(angle);
-                N[nPoints][1] = -sin(angle);
-                N[nPoints][2] = 0.0;
-                D[nPoints]    = 0.0;
-            } else {           // Vertical
-                N[nPoints][0] = 0.0;
-                N[nPoints][1] = -sin(angle);
-                N[nPoints][2] =  cos(angle);
-                D[nPoints]    = 0.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("pt %d O %.3f %.3f %.3f %.3f  N %.3f %.3f %.3f  D %.3f\n",
-                nPoints,
-                O[nPoints][0], O[nPoints][1], O[nPoints][2], O[nPoints][3], 
-                N[nPoints][0], N[nPoints][1], N[nPoints][2], 
-                D[nPoints]);
-        
-            nPoints++;
         }
     }
-    fclose(fin);
+
     
-    printf("nPoints %d\n", nPoints);
+    printf("nPlanes %d\n", nPlanes);
         
-    // Run the calculation for Tcalc
-    int run;
-    //for (run=0; run<100; run++) {
-
-        // Initialize Tcalc to the identity matrix
-        //memcpy(Tcalc, Torig, 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
-            nPoints, NITER,
-            STEP_SIZE_ROT, STEP_SIZE_POS, FALLOFF,
-            1);
     //}
     
     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"
+    //--------------------------------------------------    
+    
+    // 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...
-    printf("Hello, World!\n");
     return 0;
 }
-- 
cgit v1.3.1


From e711030a76cbefd1cbe6017690076e0bc4987ec5 Mon Sep 17 00:00:00 2001
From: CNLohr <charles@cnlohr.com>
Date: Thu, 9 Mar 2017 00:05:35 -0500
Subject: Update README.md

---
 README.md | 7 +++++++
 1 file changed, 7 insertions(+)

diff --git a/README.md b/README.md
index 9428f39..6e097f8 100644
--- a/README.md
+++ b/README.md
@@ -24,6 +24,13 @@ 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/
 
+## Getting things working
+
+There are two things you should consider doing to your system before running libsurvive.
+
+(1) Install the udev rules:  ```cp usefulfiles/81-vive.rules to /etc/udev/rules.d/``` and reboot.
+(2) If you are running on an NVIDIA Card, you will need to AllowHMD to true.  Add the following line to your /etc/X11/xorg.conf device section:  ```Option "AllowHMD" "yes"```
+
 ## Introduction
 High-performance HTC Vive Library
 
-- 
cgit v1.3.1


From 313f6a01e5855e82eb7546c65e7c3b00ad3a9170 Mon Sep 17 00:00:00 2001
From: ultramn <dchapm2@umbc.edu>
Date: Wed, 8 Mar 2017 21:48:37 -0800
Subject: Added 1 million times runtime just for performance testing

---
 dave/AffineSolve.c | 48 ++++++++++++++++++++++++++----------------------
 1 file changed, 26 insertions(+), 22 deletions(-)

diff --git a/dave/AffineSolve.c b/dave/AffineSolve.c
index 1eaf633..ad9ccb5 100644
--- a/dave/AffineSolve.c
+++ b/dave/AffineSolve.c
@@ -142,7 +142,7 @@ void OrthoSolve(
     MUL(S,Xt,SXt,2,nPoints,3);
     INV(XXt,invXXt,3);
     MUL(SXt,invXXt,M,2,3,3);
-PRINT(M,2,3);
+//PRINT(M,2,3);
 
 // Double checking work
 FLOAT S_morph[2][MAX_POINTS];
@@ -201,15 +201,15 @@ for (i=0; i<nPoints; i++) { S_morph[0][i]+=sbar[0]; S_morph[1][i]+=sbar[1]; }
     FLOAT uhat_len = sqrt( uhat[0][0]*uhat[0][0] + uhat[1][0]*uhat[1][0] );
     FLOAT rhat_len = sqrt( rhat[0][0]*rhat[0][0] + rhat[1][0]*rhat[1][0] );
     FLOAT 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);
-
+/*    
+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);
+*/
     FLOAT ydist1 = 1.0 /  uhat_len; //0.25*PI / uhat_len;
     FLOAT ydist2 = 1.0 /  rhat_len; //0.25*PI / rhat_len;
     FLOAT ydist  = 1.0 / urhat_len;
-    printf("ydist1 %f ydist2 %f ydist %f\n", ydist1, ydist2, ydist);
+//    printf("ydist1 %f ydist2 %f ydist %f\n", ydist1, ydist2, ydist);
 
     //--------------------
     // Rescale the axies to be of the proper length
@@ -243,7 +243,7 @@ for (i=0; i<nPoints; i++) { S_morph[0][i]+=sbar[0]; S_morph[1][i]+=sbar[1]; }
                 FLOAT cx,cy,cz;
                 CrossProduct(cx,cy,cz,x[0][0],x_y,x[2][0],y[0][0],y_y,y[2][0]);
                 FLOAT hand = cx*z[0][0] + cy*y_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; }
@@ -253,15 +253,15 @@ for (i=0; i<nPoints; i++) { S_morph[0][i]+=sbar[0]; S_morph[1][i]+=sbar[1]; }
         }
         x_y = -x_y;
     }
-    printf("bestErr %f\n", bestErr);
-
+//    printf("bestErr %f\n", bestErr);
+/*
     for (i=0; i<nPoints; i++) {
         float x1 = x[0][0]*X[0][i] + y[0][0]*X[1][i] + z[0][0]*X[2][i];
         float y1 = x[1][0]*X[0][i] + y[1][0]*X[1][i] + z[1][0]*X[2][i];
         float 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
@@ -289,7 +289,7 @@ PRINT(ab,2,1);
 */
     // 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);
+//PRINT(R,3,3);
 
     //-------------------
     // Calculate the translation of the centroid
@@ -325,11 +325,11 @@ PRINT(R,3,3);
         S_out[1][i] = asin(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]);
     }
 
-    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);
+//    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(
@@ -602,13 +602,17 @@ int main()
     // 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);
+    int loop;
+    for (loop=0; loop<1000000; 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;
-- 
cgit v1.3.1


From 8701607e71c70e668f5e906a787b1fab325c0fb8 Mon Sep 17 00:00:00 2001
From: ultramn <dchapm2@umbc.edu>
Date: Wed, 8 Mar 2017 21:52:49 -0800
Subject: added affinesolve makefile

---
 dave/Makefile | 1 +
 1 file changed, 1 insertion(+)

diff --git a/dave/Makefile b/dave/Makefile
index ed916f4..af29e1d 100644
--- a/dave/Makefile
+++ b/dave/Makefile
@@ -1,5 +1,6 @@
 all:
 #	gcc -O3 -o kalman_filter kalman_filter.c main.c
 	gcc -O3 -o dclapack_test dclapack_test.c
+	gcc -O3 -o AffineSolve   AffineSolve.c
 clean:
 	rm -f kalman_filter dclapack_test
-- 
cgit v1.3.1


From 48aa2b2a13daf5a007deb3fedf0da4e49aae0fd5 Mon Sep 17 00:00:00 2001
From: ultramn <dchapm2@umbc.edu>
Date: Wed, 8 Mar 2017 21:54:22 -0800
Subject: Added -lm

---
 dave/Makefile | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/dave/Makefile b/dave/Makefile
index af29e1d..fc7dde5 100644
--- a/dave/Makefile
+++ b/dave/Makefile
@@ -1,6 +1,6 @@
 all:
 #	gcc -O3 -o kalman_filter kalman_filter.c main.c
-	gcc -O3 -o dclapack_test dclapack_test.c
-	gcc -O3 -o AffineSolve   AffineSolve.c
+	gcc -O3 -o dclapack_test dclapack_test.c -lm
+	gcc -O3 -o AffineSolve   AffineSolve.c -lm
 clean:
 	rm -f kalman_filter dclapack_test
-- 
cgit v1.3.1


From 6362b6ee00ef5e394b636e2bb63d5e20c18c01f8 Mon Sep 17 00:00:00 2001
From: cnlohr <lohr85@gmail.com>
Date: Thu, 9 Mar 2017 02:38:10 -0500
Subject: switch to atan2 instead of asin.  Take out of performance check and
 put into test mode again.

---
 dave/AffineSolve.c |   9 +++++----
 dave/Makefile      |   3 ++-
 dave/dclapack_test | Bin 12836 -> 12960 bytes
 3 files changed, 7 insertions(+), 5 deletions(-)

diff --git a/dave/AffineSolve.c b/dave/AffineSolve.c
index ad9ccb5..e848ade 100644
--- a/dave/AffineSolve.c
+++ b/dave/AffineSolve.c
@@ -314,6 +314,7 @@ 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);
     //-------------------
     // Plot the output points
     //-------------------
@@ -321,11 +322,11 @@ PRINT(ab,2,1);
         float 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];
         float 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];
         float 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] = asin(Tx / Ty);   // horiz
-        S_out[1][i] = asin(Tz / Ty);   // vert
+        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]);
+        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("xbar %f %f %f\n", xbar[0], xbar[1], xbar[2]);
@@ -603,7 +604,7 @@ int main()
     //--------------------------------------------------    
     
     int loop;
-    for (loop=0; loop<1000000; loop++)
+    for (loop=0; loop<1; loop++)
     {
         // Run OrthoSolve
         OrthoSolve(
diff --git a/dave/Makefile b/dave/Makefile
index fc7dde5..7738850 100644
--- a/dave/Makefile
+++ b/dave/Makefile
@@ -1,6 +1,7 @@
 all:
 #	gcc -O3 -o kalman_filter kalman_filter.c main.c
 	gcc -O3 -o dclapack_test dclapack_test.c -lm
-	gcc -O3 -o AffineSolve   AffineSolve.c -lm
+	gcc  -O0 -g -o AffineSolve   AffineSolve.c -lm #-Wall
+
 clean:
 	rm -f kalman_filter dclapack_test
diff --git a/dave/dclapack_test b/dave/dclapack_test
index 7789e78..bac05e7 100755
Binary files a/dave/dclapack_test and b/dave/dclapack_test differ
-- 
cgit v1.3.1


From 9b792127fe2e2c4590b9e528dfd97e44d890d253 Mon Sep 17 00:00:00 2001
From: ultramn <dchapm2@umbc.edu>
Date: Thu, 9 Mar 2017 16:51:41 -0800
Subject: fixed bug with error calculation

---
 dave/AffineSolve.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/dave/AffineSolve.c b/dave/AffineSolve.c
index e848ade..f29e29e 100644
--- a/dave/AffineSolve.c
+++ b/dave/AffineSolve.c
@@ -242,7 +242,7 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len);
                 // Calculate the handedness
                 FLOAT cx,cy,cz;
                 CrossProduct(cx,cy,cz,x[0][0],x_y,x[2][0],y[0][0],y_y,y[2][0]);
-                FLOAT hand = cx*z[0][0] + cy*y_y + cz*z[2][0];
+                FLOAT 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
-- 
cgit v1.3.1


From 765bf1623299e81f5f95b78a78e7d66997e4ad0c Mon Sep 17 00:00:00 2001
From: cnlohr <lohr85@gmail.com>
Date: Thu, 9 Mar 2017 20:00:42 -0500
Subject: Fix wrong quat functions + Add matrix->quat conversion.

---
 redist/linmath.c | 58 ++++++++++++++++++++++++++++++++++++++++++++------------
 redist/linmath.h |  5 +++--
 2 files changed, 49 insertions(+), 14 deletions(-)

diff --git a/redist/linmath.c b/redist/linmath.c
index 1c5c25b..d76fbba 100644
--- a/redist/linmath.c
+++ b/redist/linmath.c
@@ -152,7 +152,7 @@ FLT quatmagnitude( const FLT * q )
 
 FLT quatinvsqmagnitude( const FLT * q )
 {
-	return ((FLT)1.)/((q[0]*q[0])+(q[1]*q[1])+(q[2]*q[2])+(q[3]*q[3]));
+	return ((FLT)1.)/FLT_SQRT((q[0]*q[0])+(q[1]*q[1])+(q[2]*q[2])+(q[3]*q[3]));
 }
 
 
@@ -168,17 +168,17 @@ void quattomatrix(FLT * matrix44, const FLT * qin)
 	quatnormalize(q, qin);
 
 	//Reduced calulation for speed
-	FLT xx = 2 * q[0] * q[0];
-	FLT xy = 2 * q[0] * q[1];
-	FLT xz = 2 * q[0] * q[2];
-	FLT xw = 2 * q[0] * q[3];
+	FLT xx = 2 * q[1] * q[1];
+	FLT xy = 2 * q[1] * q[2];
+	FLT xz = 2 * q[1] * q[3];
+	FLT xw = 2 * q[1] * q[0];
 
-	FLT yy = 2 * q[1] * q[1];
-	FLT yz = 2 * q[1] * q[2];
-	FLT yw = 2 * q[1] * q[3];
+	FLT yy = 2 * q[2] * q[2];
+	FLT yz = 2 * q[2] * q[3];
+	FLT yw = 2 * q[2] * q[0];
 
-	FLT zz = 2 * q[2] * q[2];
-	FLT zw = 2 * q[2] * q[3];
+	FLT zz = 2 * q[3] * q[3];
+	FLT zw = 2 * q[3] * q[0];
 
 	//opengl major
 	matrix44[0] = 1 - yy - zz;
@@ -202,6 +202,40 @@ void quattomatrix(FLT * matrix44, const FLT * qin)
 	matrix44[15] = 1;
 }
 
+
+void quatfrommatrix( FLT * q, const FLT * matrix44 )
+{
+	//Algorithm from http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
+	float tr = matrix44[0] + matrix44[5] + matrix44[10];
+
+	if (tr > 0) {
+		float S = sqrt(tr+1.0) * 2; // S=4*qw
+		q[0] = 0.25 * S;
+		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])) {
+		float S = sqrt(1.0 + matrix44[0] - matrix44[5] - matrix44[10]) * 2; // S=4*qx
+		q[0] = (matrix44[9] - matrix44[6]) / S;
+		q[1] = 0.25 * S;
+		q[2] = (matrix44[1] + matrix44[4]) / S;
+		q[3] = (matrix44[2] + matrix44[8]) / S;
+	} else if (matrix44[5] > matrix44[10]) {
+		float S = sqrt(1.0 + matrix44[5] - matrix44[0] - matrix44[10]) * 2; // S=4*qy
+		q[0] = (matrix44[2] - matrix44[8]) / S;
+		q[1] = (matrix44[1] + matrix44[4]) / S;
+		q[2] = 0.25 * S;
+		q[3] = (matrix44[6] + matrix44[9]) / S;
+	} else {
+		float S = sqrt(1.0 + matrix44[10] - matrix44[0] - matrix44[5]) * 2; // S=4*qz
+		q[0] = (matrix44[4] - matrix44[1]) / S;
+		q[1] = (matrix44[2] + matrix44[8]) / S;
+		q[2] = (matrix44[6] + matrix44[9]) / S;
+		q[3] = 0.25 * S;
+	}
+}
+
+
 void quattomatrix33(FLT * matrix33, const FLT * qin)
 {
 	FLT q[4];
@@ -270,8 +304,8 @@ void quatrotateabout( FLT * qout, const FLT * a, const FLT * b )
 	FLT q1[4];
 	FLT q2[4];
 
-	quatnormalize( q1, a );
-	quatnormalize( q2, b );
+	//quatnormalize( q1, a );
+	//quatnormalize( q2, b );
 
 	qout[0] = (q1[0]*q2[0])-(q1[1]*q2[1])-(q1[2]*q2[2])-(q1[3]*q2[3]);
 	qout[1] = (q1[0]*q2[1])+(q1[1]*q2[0])+(q1[2]*q2[3])-(q1[3]*q2[2]);
diff --git a/redist/linmath.h b/redist/linmath.h
index caec281..ec20534 100644
--- a/redist/linmath.h
+++ b/redist/linmath.h
@@ -7,8 +7,8 @@
 #define DEFAULT_EPSILON 0.001
 
 //For printf
-#define PFTHREE(x) x[0], x[1], x[2]
-#define PFFOUR(x) x[0], x[1], x[2], x[3]
+#define PFTHREE(x) (x)[0], (x)[1], (x)[2]
+#define PFFOUR(x) (x)[0], (x)[1], (x)[2], (x)[3]
 
 #define LINMATHPI ((FLT)3.141592653589)
 
@@ -76,6 +76,7 @@ FLT quatmagnitude( const FLT * q );
 FLT quatinvsqmagnitude( const FLT * q );
 void quatnormalize( FLT * qout, const FLT * qin );  //Safe for in to be same as out.
 void quattomatrix( FLT * matrix44, const FLT * q );
+void quatfrommatrix( FLT * q, const FLT * matrix44 );
 void quatgetconjugate( FLT * qout, const FLT * qin );
 void quatgetreciprocal( FLT * qout, const FLT * qin );
 void quatsub( FLT * qout, const FLT * a, const FLT * b );
-- 
cgit v1.3.1


From 3e39224bdd65b8a49de44f007add854dc626a209 Mon Sep 17 00:00:00 2001
From: ultramn <dchapm2@umbc.edu>
Date: Thu, 9 Mar 2017 17:35:53 -0800
Subject: Added loop to try all z values

---
 dave/AffineSolve.c | 69 ++++++++++++++++++++++++++++++++++++++++++++++++++----
 1 file changed, 64 insertions(+), 5 deletions(-)

diff --git a/dave/AffineSolve.c b/dave/AffineSolve.c
index f29e29e..1c1d0f1 100644
--- a/dave/AffineSolve.c
+++ b/dave/AffineSolve.c
@@ -206,8 +206,8 @@ 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);
 */
-    FLOAT ydist1 = 1.0 /  uhat_len; //0.25*PI / uhat_len;
-    FLOAT ydist2 = 1.0 /  rhat_len; //0.25*PI / rhat_len;
+//    FLOAT ydist1 = 1.0 /  uhat_len; //0.25*PI / uhat_len;
+//    FLOAT ydist2 = 1.0 /  rhat_len; //0.25*PI / rhat_len;
     FLOAT ydist  = 1.0 / urhat_len;
 //    printf("ydist1 %f ydist2 %f ydist %f\n", ydist1, ydist2, ydist);
 
@@ -223,7 +223,7 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len);
     FLOAT x_y = sqrt(1.0 - x[0][0]*x[0][0] - x[2][0]*x[2][0]);
     FLOAT y_y = sqrt(1.0 - y[0][0]*y[0][0] - y[2][0]*y[2][0]);
     FLOAT z_y = sqrt(1.0 - z[0][0]*z[0][0] - z[2][0]*z[2][0]);
-
+/*
     // Exhaustively flip the minus sign of the z axis until we find the right one . . .
     FLOAT bestErr = 9999.0;
     FLOAT xy_dot2 = x[0][0]*y[0][0] + x[2][0]*y[2][0];
@@ -243,7 +243,7 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len);
                 FLOAT cx,cy,cz;
                 CrossProduct(cx,cy,cz,x[0][0],x_y,x[2][0],y[0][0],y_y,y[2][0]);
                 FLOAT 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; }
@@ -253,7 +253,66 @@ 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
+    //-------------------------
+    FLOAT ydist2;
+    FLOAT bestBestErr = 9999.0;
+    for (ydist2=ydist-0.1; ydist2<ydist+0.1; ydist2+=0.0001)
+    {
+        FLOAT x2[3][1] = { {M[0][0]*ydist2}, {0.0}, {M[1][0]*ydist2} };
+        FLOAT y2[3][1] = { {M[0][1]*ydist2}, {0.0}, {M[1][1]*ydist2} };
+        FLOAT 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 . . .
+        FLOAT x_y = sqrt(1.0 - x2[0][0]*x2[0][0] - x2[2][0]*x2[2][0]);
+        FLOAT y_y = sqrt(1.0 - y2[0][0]*y2[0][0] - y2[2][0]*y2[2][0]);
+        FLOAT z_y = sqrt(1.0 - z2[0][0]*z2[0][0] - z2[2][0]*z2[2][0]);
+
+        // Exhaustively flip the minus sign of the z axis until we find the right one . . .
+        FLOAT bestErr = 9999.0;
+        FLOAT xy_dot2 = x2[0][0]*y2[0][0] + x2[2][0]*y2[2][0];
+        FLOAT yz_dot2 = y2[0][0]*z2[0][0] + y2[2][0]*z2[2][0];
+        FLOAT 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
+                    FLOAT xy_dot = xy_dot2 + x_y*y_y;
+                    FLOAT yz_dot = yz_dot2 + y_y*z_y;
+                    FLOAT zx_dot = zx_dot2 + z_y*x_y;
+                    FLOAT err = _ABS(xy_dot) + _ABS(yz_dot) + _ABS(zx_dot);
+                
+                    // Calculate the handedness
+                    FLOAT cx,cy,cz;
+                    CrossProduct(cx,cy,cz,x2[0][0],x_y,x2[2][0],y2[0][0],y_y,y2[2][0]);
+                    FLOAT 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(FLOAT));
+            memcpy(y,y2,3*sizeof(FLOAT));
+            memcpy(z,z2,3*sizeof(FLOAT));
+            bestBestErr = bestErr;
+        }
+    }
+
+
 /*
     for (i=0; i<nPoints; i++) {
         float x1 = x[0][0]*X[0][i] + y[0][0]*X[1][i] + z[0][0]*X[2][i];
-- 
cgit v1.3.1


From 6541d1e63358324400ab446a4d1457941a0e90cf Mon Sep 17 00:00:00 2001
From: cnlohr <lohr85@gmail.com>
Date: Thu, 9 Mar 2017 20:35:55 -0500
Subject: Update linmath to allow for transforming the pose solver for testing
 purposes.

---
 dave/AffineSolve.c | 50 ++++++++++++++++++++++++++++++++++++++++++++------
 dave/Makefile      |  2 +-
 redist/linmath.c   | 17 +++++++++--------
 redist/linmath.h   |  2 +-
 4 files changed, 55 insertions(+), 16 deletions(-)

diff --git a/dave/AffineSolve.c b/dave/AffineSolve.c
index f29e29e..6e17f26 100644
--- a/dave/AffineSolve.c
+++ b/dave/AffineSolve.c
@@ -10,8 +10,8 @@
 #include <stdlib.h>
 #include <math.h>
 #include "dclapack.h"
-
-#define LH_ID    0
+#include <linmath.h>
+#define LH_ID    1
 #define NUM_HMD 32
 
 #define MAX_POINTS 128
@@ -209,7 +209,7 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len);
     FLOAT ydist1 = 1.0 /  uhat_len; //0.25*PI / uhat_len;
     FLOAT ydist2 = 1.0 /  rhat_len; //0.25*PI / rhat_len;
     FLOAT ydist  = 1.0 / urhat_len;
-//    printf("ydist1 %f ydist2 %f ydist %f\n", ydist1, ydist2, ydist);
+    //printf("ydist1 %f ydist2 %f ydist %f\n", ydist1, ydist2, ydist);
 
     //--------------------
     // Rescale the axies to be of the proper length
@@ -223,6 +223,9 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len);
     FLOAT x_y = sqrt(1.0 - x[0][0]*x[0][0] - x[2][0]*x[2][0]);
     FLOAT y_y = sqrt(1.0 - y[0][0]*y[0][0] - y[2][0]*y[2][0]);
     FLOAT 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 . . .
     FLOAT bestErr = 9999.0;
@@ -243,17 +246,18 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len);
                 FLOAT cx,cy,cz;
                 CrossProduct(cx,cy,cz,x[0][0],x_y,x[2][0],y[0][0],y_y,y[2][0]);
                 FLOAT 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; }
+                //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);
+    printf("bestErr %f\n", bestErr);
 /*
     for (i=0; i<nPoints; i++) {
         float x1 = x[0][0]*X[0][i] + y[0][0]*X[1][i] + z[0][0]*X[2][i];
@@ -329,6 +333,40 @@ PRINT(ab,2,1);
         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]);
     }
 
+
+
+
+	float quat[4];
+	float posoff[3] = { trans[0], trans[1], trans[2] };
+	float MT[4][4];
+	//matrix44transpose( MT, &T[0][0] );
+	matrix44copy( &MT[0][0], &T[0][0] );
+
+//QUAT: 0.657864 -0.305763 0.128486 0.265895 = 0.783252
+//QUAT: 0.657864 0.305763 -0.128486 -0.265895 = 0.783252
+
+
+	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\n", quat[0], quat[1], quat[2], quat[3], quatmagnitude(quat) );
+
+
+	for( i = 0; i <nPoints;i++ )
+	{
+		float pt[3] = { X_in[0][i], X_in[1][i], X_in[2][i] };
+		quatrotatevector( pt, quat, pt );
+		add3d( pt, pt, posoff );
+		printf( "%f %f %f OUT %f %f %f ANGLE %f %f AOUT %f %f\n", 
+			X_in[0][i], X_in[1][i], X_in[2][i], pt[0], pt[1], pt[2],
+			S_in[0][i], S_in[1][i], atan2( pt[0], pt[1] ), atan2( pt[2], pt[1] ) );
+	}
+	quattomatrix( &MT[0][0], quat );
+    PRINT_MAT(MT,4,4);
+
+
+
 //    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);
 }
diff --git a/dave/Makefile b/dave/Makefile
index 7738850..bf62837 100644
--- a/dave/Makefile
+++ b/dave/Makefile
@@ -1,7 +1,7 @@
 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 #-Wall
+	gcc  -O0 -g -o AffineSolve   AffineSolve.c -lm -I../redist ../redist/linmath.c #-Wall
 
 clean:
 	rm -f kalman_filter dclapack_test
diff --git a/redist/linmath.c b/redist/linmath.c
index d76fbba..72c00a4 100644
--- a/redist/linmath.c
+++ b/redist/linmath.c
@@ -3,6 +3,7 @@
 #include "linmath.h"
 #include <math.h>
 #include <float.h>
+#include <string.h>
 
 void cross3d( FLT * out, const FLT * a, const FLT * b )
 {
@@ -299,14 +300,9 @@ void quatadd( FLT * qout, const FLT * a, const FLT * b )
 	qout[3] = a[3] + b[3];
 }
 
-void quatrotateabout( FLT * qout, const FLT * a, const FLT * b )
+void quatrotateabout( FLT * qout, const FLT * q1, const FLT * q2 )
 {
-	FLT q1[4];
-	FLT q2[4];
-
-	//quatnormalize( q1, a );
-	//quatnormalize( q2, b );
-
+	//NOTE: Does not normalize
 	qout[0] = (q1[0]*q2[0])-(q1[1]*q2[1])-(q1[2]*q2[2])-(q1[3]*q2[3]);
 	qout[1] = (q1[0]*q2[1])+(q1[1]*q2[0])+(q1[2]*q2[3])-(q1[3]*q2[2]);
 	qout[2] = (q1[0]*q2[2])-(q1[1]*q2[3])+(q1[2]*q2[0])+(q1[3]*q2[1]);
@@ -396,7 +392,7 @@ void quatrotatevector( FLT * vec3out, const FLT * quat, const FLT * vec3in )
 	vquat[3] = vec3in[2];
 
 	quatrotateabout( tquat, quat, vquat );
-	quatgetreciprocal( qrecp, quat );
+	quatgetconjugate( qrecp, quat );
 	quatrotateabout( vquat, tquat, qrecp );
 
 	vec3out[0] = vquat[1];
@@ -515,6 +511,11 @@ void quatfrom2vectors(FLT *q, const FLT *src, const FLT *dest)
 
 }
 
+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
diff --git a/redist/linmath.h b/redist/linmath.h
index ec20534..676d182 100644
--- a/redist/linmath.h
+++ b/redist/linmath.h
@@ -104,7 +104,7 @@ void rotation_between_vecs_to_m3(Matrix3x3 *m, const FLT v1[3], const FLT v2[3])
 Matrix3x3 inverseM33(const Matrix3x3 mat);
 
 
-
+void matrix44copy(FLT * mout, const FLT * minm );
 
 
 
-- 
cgit v1.3.1


From a956d29b35aa9c8b7cfd869683afb382da65b3ea Mon Sep 17 00:00:00 2001
From: ultramn <dchapm2@umbc.edu>
Date: Thu, 9 Mar 2017 17:40:19 -0800
Subject: Added a loop to check the z values

---
 dave/AffineSolve.c | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/dave/AffineSolve.c b/dave/AffineSolve.c
index 1c1d0f1..d036858 100644
--- a/dave/AffineSolve.c
+++ b/dave/AffineSolve.c
@@ -261,6 +261,7 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len);
     //-------------------------
     FLOAT ydist2;
     FLOAT bestBestErr = 9999.0;
+    FLOAT bestYdist = 0;
     for (ydist2=ydist-0.1; ydist2<ydist+0.1; ydist2+=0.0001)
     {
         FLOAT x2[3][1] = { {M[0][0]*ydist2}, {0.0}, {M[1][0]*ydist2} };
@@ -309,9 +310,10 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len);
             memcpy(y,y2,3*sizeof(FLOAT));
             memcpy(z,z2,3*sizeof(FLOAT));
             bestBestErr = bestErr;
+            bestYdist = ydist2;
         }
     }
-
+    ydist = bestYdist;
 
 /*
     for (i=0; i<nPoints; i++) {
-- 
cgit v1.3.1